Merge branch 'main' into Integration

Control/.gitignore

@@ -3,4 +3,4 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
-src/credentials.h
+include/credentials.h

Control/include/instruction.h

@@ -0,0 +1,21 @@
+#ifndef INSTRUCTION_H
+#define INSTRUCTION_H
+
+typedef enum {
+    INSTR_RESET = -1,
+    INSTR_STOP,
+    INSTR_MOVE,
+    INSTR_CHARGE
+} instr_t;
+
+typedef struct instruction
+{
+    int id;
+    int instr;
+    int heading;
+    int distance;
+    float speed;
+    int charge;
+} RoverInstruction;
+
+#endif

Control/include/status.h

@@ -0,0 +1,11 @@
+#ifndef CONTROL_STATUS_H
+#define CONTROL_STATUS_H
+
+typedef enum {
+    CS_ERROR = -1,
+    CS_IDLE,
+    CS_MOVING,
+    CS_CHARGING
+} ControlStatus_t;
+
+#endif

Control/ref/command.cpp

@@ -3,8 +3,10 @@
 #include <string>
 
 #define WebSocket 0
-int batteryVoltage, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Command ==> Control
+int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command
-int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Control ==> Command
+float batteryVoltage, batteryLevel, batteryCycles; // Info Control ==> Command
+int command_id, mode, reqHeading, reqDistance, reqCharge; // Info Command ==> Control
+float reqSpeed; // Info Command ==> Control
 
 void setup() {}
 
@@ -12,7 +14,10 @@ void loop()
 {
 	DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
 	deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket
+	state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging
 	batteryVoltage = rdoc["bV"];
+	batteryLevel = rdoc["bL"];
+	batteryCycles = rdoc["bC"];
 	totalTripDistance = rdoc["tD"];
 	currentHeading = rdoc["cH"];
 	current_x = rdoc["pos"][0];

Control/ref/drive.cpp

@@ -18,9 +18,10 @@ void loop()
     DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
     deserializeJson(rdoc, Serial1); // Take JSON input from UART1
     int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
-    int distance = rdoc["dist"];
+    int distance = rdoc["dist"]; // -1 for emergency stop
-    float spd = rdoc["sp"];
+    float spd = rdoc["sp"]; // -1 for emergency stop
     int currentHeading = rdoc["cH"];
+    bool resetDistanceTravelled = rdoc["rstD"];
 
     bool commandComplete = 0;
     float powerUsage_mWh = 0.0;

Control/ref/vision.cpp

@@ -1 +1,49 @@
+#include <Arduino.h>
+#include <ArduinoJson.h>
+#include <string>
+
 const int ARDUINO_IO[16] = {-1/*RX*/, -1/*RX*/, 23, 22, 21, 19, 18, 5, 17, 16, 14, 4, 15, 2, 13, 12}; // Expansion board mapping
+#define RXpin ARDUINO_IO[11] // Define your RX pin here
+#define TXpin ARDUINO_IO[10] // Define your TX pin here
+FILE* SerialUART;
+
+void setup()
+{
+    Serial.begin(115200);                          // Set up hardware UART0 (Connected to USB port)
+}
+
+void loop()
+{
+    int command = getc(SerialUART);
+    // command char, used for controlling exposure/focus/gain settings:
+    // e = increase exposure
+    // d = decrease exposure
+    // r = increase focus
+    // f = decrease focus
+    // t = increase gain
+    // g = decrease gain
+
+    // Bounding Box edges
+    int bb_left = 0;
+    int bb_right = 0;
+    int bb_top = 0;
+    int bb_bottom = 0;
+    // Weighted average of detected pixels coordinates
+    int centre_x = 0;
+    int centre_y = 0;
+    // Heading from DE10-Lite magnetometer
+    float heading = 0.0;
+
+    // Build hardcode JSON packet on DE10-Lite using fprintf() as space is minimal and library would be too large.
+    // fprintf(SerialUART, "{\"bb\":[%d,%d,%d,%d],\"cen\":[%d,%d],\"cH\":%d\"}", bb_left, bb_right, bb_top, bb_bottom, centre_x, centre_y, heading);
+
+    DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
+    deserializeJson(rdoc, SerialUART);
+    bb_left = rdoc["bb"][0];
+    bb_right = rdoc["bb"][1];
+    bb_top = rdoc["bb"][2];
+    bb_bottom = rdoc["bb"][3];
+    centre_x = rdoc["cen"][0];
+    centre_y = rdoc["cen"][1];
+    heading = rdoc["cH"];
+}

Control/src/main.cpp

@@ -1,37 +1,73 @@
+#pragma region Includes
 #include <Arduino.h>
 #include <string>
 #include <SoftwareSerial.h> // Software Serial not currently needed
+#include <SoftwareSerial.h>
 #include <AsyncTCP.h>
 #include <ESPAsyncWebServer.h>
 #include <ESPmDNS.h>
 #include "TickerV2.h"
 #include <WebSocketsServer.h>
-#include <credentials.h>
+#include "credentials.h"
 #include <ArduinoJson.h>
 #include <SPIFFS.h>
+#include "status.h"
+#include "instruction.h"
+#include <queue>
+#pragma endregion
 
-// Enable extra debugging info
+#pragma region Enable extra debugging info for ESP32
 #undef LOG_LOCAL_LEVEL
 #define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE
 #include "esp_log.h"
+#pragma endregion
 
-#define RX1pin 14 // Pin 10 on expansion board, UART1
+#pragma region Definitions eg pins
-#define TX1pin 4  // Pin 11 on expansion board, UART1
+#define RX1pin 18 // Pin 6 on expansion board, UART1
+#define TX1pin 5  // Pin 7 on expansion board, UART1
+#define RX2pin 17 // Pin 8 on expansion board, UART2
+#define TX2pin 16 // Pin 9 on expansion board, UART2
+#define RX3pin 14 // Pin 10 on expansion board, UART3
+#define TX3pin 4  // Pin 11 on expansion board, UART3
+#pragma endregion
 
-// Function Declarations
+#pragma region Function Declarations
-void printFPGAoutput();
-void returnSensorData();
 void notFound(AsyncWebServerRequest *request);
 void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
+void queueInstruction(RoverInstruction instruction);
+void sendToCommand();
+void sendToDrive(RoverInstruction instruction);
+void recvFromDrive();
+void sendToEnergy(bool instruction);
+void recvFromEnergy();
+void sendToVision();
+void recvFromVision();
+void emergencyStop();
+#pragma endregion
 
-// Global objects
+#pragma region Global objects
 AsyncWebServer webserver(80);
 WebSocketsServer websocketserver(81);
 Ticker ticker;
+SoftwareSerial Serial3;
+std::queue<RoverInstruction> InstrQueue;
+#pragma endregion
 
-// Global variables
+#pragma region Global variables
-float battery_voltage = 4.0f;
+ControlStatus_t Status;
-int distance_travelled = 0;
+float batteryVoltage;
+float batteryLevel;
+float batteryCycles;
+int odometer;
+int heading;
+int xpos, ypos;
+int signalStrength;
+int lastExecutedCommand, lastCompletedCommand;
+bool driveCommandComplete;
+int bb_left, bb_right, bb_top, bb_bottom;
+int bb_centre_x, bb_centre_y;
+float chargeGoal;
+#pragma endregion
 
 void setup()
 {
@@ -40,9 +76,24 @@ void setup()
 	esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client
 
 	Serial.begin(115200);							   // Set up hardware UART0 (Connected to USB port)
-	Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1
+	Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin);   // Set up hardware UART1 (Connected to Drive)
-	Serial2.begin(9600, SERIAL_8N1, 16, 17); 		 // RX 9, TX 8
+	Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin);   // Set up hardware UART2 (Connected to Energy)
-	// Set up remaining communication ports here (Energy, Vision)
+	Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision)
+
+	// Set global variable startup values
+	Status = CS_IDLE;
+	batteryVoltage = 0;
+	batteryLevel = 0;
+	batteryCycles = 0;
+	odometer = 0;
+	heading = 0;
+	xpos = 0;
+	ypos = 0;
+	signalStrength = 0;
+	lastExecutedCommand = 0;
+	lastCompletedCommand = 0;
+	driveCommandComplete = 1;
+	chargeGoal = 0;
 
 	if (!SPIFFS.begin(true)) // Mount SPIFFS
 	{
@@ -52,11 +103,11 @@ void setup()
 	Serial.println("SPIFFS mounted");
 
 	WiFi.begin(WIFI_SSID, WIFI_PW);
-	while (WiFi.status() != WL_CONNECTED)
+	while (WiFi.status() != WL_CONNECTED) // Wait for ESP32 to connect to AP in "credentials.h"
 	{
 		delay(500);
 	}
-	while (!MDNS.begin("rover"))
+	while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/"
 	{
 		Serial.println("Error setting up mDNS, retrying in 5s");
 		delay(5000);
@@ -64,56 +115,118 @@ void setup()
 	Serial.println("mDNS set up, access Control Panel at 'rover.local/'");
 
 	webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
-				 { request->send(SPIFFS, "/index.html", "text/html"); });
+				 { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
 	webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
-				 { request->send(SPIFFS, "/favicon.ico", "image/png"); });
+				 { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
-	webserver.onNotFound(notFound);
+	webserver.onNotFound(notFound);										   // Set up basic 404NotFound page
-	webserver.begin();
+	webserver.begin();													   // Start Asynchronous Web Server
 
-	websocketserver.begin();
+	websocketserver.begin();				 // Start Websocket Server
-	websocketserver.onEvent(webSocketEvent);
+	websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command
-	ticker.attach(0.5, returnSensorData);
+	ticker.attach(0.5, sendToCommand);		 // Set up recurring function to forward rover status to Command
 }
 
 void loop()
 {
-	printFPGAoutput();
-
-	String FPGAinput; // Forward serial monitor input to FPGA
-	if (Serial.available())
-	{
-		FPGAinput = String(Serial.readStringUntil('\n'));
-		Serial1.println(FPGAinput);
-	}
-
 	websocketserver.loop(); // Handle incoming client connections
+	recvFromDrive();		// Update stats from Drive
+	recvFromEnergy();		// Update stats from Energy
+	recvFromVision();		// Update stats from Vision
+	switch (Status)
+	{
+	case CS_ERROR:
+	{
+		Serial.println("Rover in error state, rebooting...");
+		exit(1);
 	}
+	break;
+	case CS_IDLE:
+	{
+		if (!InstrQueue.empty()) // If Rover idle and InstrQueue NOT empty: Do the next command in the queue
+		{
+			RoverInstruction *instr = &InstrQueue.front(); // Get next command
+			switch (instr->instr)						   // Determine command type
+			{
+			case INSTR_RESET: // Reset telemetry values (zeroing position/distance)
+			{
+				odometer = 0;
+				xpos = 0;
+				ypos = 0;
+				DynamicJsonDocument tdoc(128);
+				tdoc["rstD"] = 1;
+				serializeJson(tdoc, Serial1); // Send reset odometer signal to Drive
+			}
+			break;
+			case INSTR_STOP: // Emergency stop
+			{
+				Status = CS_ERROR;
+				while (1)
+				{
+					Serial.println("Emergency Stop should not get queued, hold and print");
+					delay(1000);
+				}
+			}
+			break;
+			case INSTR_MOVE: // Normal movement
+			{
+				Status = CS_MOVING; // Set moving state
+				driveCommandComplete = 0;
+				sendToDrive(*instr); // Forward to Drive handler
+			}
+			break;
+			case INSTR_CHARGE: // Normal charge
+			{
+				Status = CS_CHARGING;			   // Set charging state
+				chargeGoal = (float)instr->charge; // Set charging goal
+				sendToEnergy(1);				   // Forward to Energy handler
+			}
+			break;
+			default:
+			{
+				Serial.println("Unknown instruction type in queue, skipping...");
+			}
+			break;
+			}
+			lastExecutedCommand = instr->id; // Update tracker of last processed command
+		}
+	}
+	break;
+	case CS_MOVING:
+	{
+		if (driveCommandComplete) // If movement command complete:
+		{
+			Status = CS_IDLE;							// Set rover state back to idle
+			lastCompletedCommand = lastExecutedCommand; // Update last completed command
+		}
+		else // If movement command NOT complete:
+		{	 // Send (up to date) current heading to Drive
+			DynamicJsonDocument tdoc(128);
+			tdoc["rH"] = -1;
+			tdoc["cH"] = heading;
+			serializeJson(tdoc, Serial1);
+		}
+	}
+	break;
+	case CS_CHARGING:
+	{
+		if (batteryLevel >= chargeGoal) // Compare batteryLevel to chargeGoal
+		{
+			Status = CS_IDLE;
+			lastCompletedCommand = lastExecutedCommand; // Update last completed command
+			sendToEnergy(0); // Stop charging if goal reached
+		}
+		// Otherwise continue charging, no change
 		
-void printFPGAoutput()
-{ // Print serial communication from FPGA to serial monitor
-	String FPGAoutput;
-	if (Serial1.available())
-	{
-		FPGAoutput = String(Serial1.readStringUntil('\n'));
-		Serial.println(FPGAoutput);
 	}
-}
+	break;
-
+	default:
-void returnSensorData()
 	{
-	// Collect sensor data here?
+		Serial.println("Unknown rover state, exiting...");
-	distance_travelled++;
+		exit(1);
-	if (battery_voltage < 6)
-	{
-		battery_voltage += 0.2;
 	}
-	else
+	break;
-	{
-		battery_voltage = 4;
 	}
-	String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}";
+	delay(500);
-	// Serial.println(JSON_Data);
-	websocketserver.broadcastTXT(JSON_Data);
 }
 
 void notFound(AsyncWebServerRequest *request)
@@ -136,13 +249,13 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 		Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
 	}
 	break;
-	case WStype_TEXT:
+	case WStype_TEXT: // MSG received from command panel
 	{
-		// Serial.printf("Client[%u] sent Text: %s\n", num, payload);
+		Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal
-		String command = String((char *)(payload));
+		String command = String((char *)(payload));				   // Convert received command to string type
 
-		DynamicJsonDocument doc(200);								//creating an instance of a DynamicJsonDocument allocating 200bytes on the heap.
+		DynamicJsonDocument rdoc(200);								 // Create instance of DynamicJsonDocument on heap, 200 Bytes
-		DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive.
+		DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors
 		if (error)
 		{
 			Serial.print("deserializeJson() failed: ");
@@ -150,28 +263,60 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			return;
 		}
 
-		int MVM_F_status = doc["MVM_F"];
+		RoverInstruction instr;
-		int MVM_L_status = doc["MVM_L"];
+		int mode = rdoc["mode"];
-		int MVM_R_status = doc["MVM_R"];
+		switch (mode)
-		int MVM_B_status = doc["MVM_B"];
+		{
+		case -1: // Add to queue, reset x/y/odometer (telemetry data)
+		{
+			Serial.println("Reset telemetry command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_RESET;
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
 
-		Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>');
+			queueInstruction(instr); // Put reset command in InstrQueue
-
-		Serial.println("UpArrow=" + String(MVM_F_status));
-
-		if (MVM_F_status == 1){
-			DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
-			tdoc["rH"] = 0;
-			tdoc["dist"] = 100;
-			tdoc["sp"] = 1;
-			serializeJson(tdoc, Serial2);  // Build JSON and send on UART1
 		}
-		if (MVM_B_status == 1){
+		break;
-			DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
+		case 0: // Stop immediately, clear command cache
-			tdoc["rH"] = 0;
+		{
-			tdoc["dist"] = -100;
+			Serial.println("Emergency stop command received");
-			tdoc["sp"] = 1;
+			// instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued
-			serializeJson(tdoc, Serial2);  // Build JSON and send on UART1
+			// Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+
+			emergencyStop();
+		}
+		break;
+		case 1: // Normal movement command, added to end of command cache
+		{
+			Serial.println("Normal movement command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_MOVE;
+			instr.heading = rdoc["rH"];
+			instr.distance = rdoc["rD"];
+			instr.speed = rdoc["rS"];
+			// Ignore rdoc["rC"]
+
+			queueInstruction(instr); // Put movement command in InstrQueue
+		}
+		break;
+		case 2: // Normal charge command, results in no motion, added to end of command cache
+		{
+			Serial.println("Normal charge command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_CHARGE;
+			instr.charge = rdoc["rC"];
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"]
+      
+			queueInstruction(instr); // Put charge command in InstrQueue
+		}
+		break;
+		default:
+		{
+			// Default case, print and continue
+			Serial.println("Unknown Command type received, ignoring");
+			// Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+		}
+		break;
 		}
 	}
 	break;
@@ -185,5 +330,109 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 		Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting"));
 		exit(1);
 	}
+	break;
 	}
 }
+
+void queueInstruction(RoverInstruction instruction)
+{
+	InstrQueue.push(instruction);
+}
+
+void sendToCommand()
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["st"] = Status;
+	tdoc["bV"] = batteryVoltage;
+	tdoc["bL"] = batteryLevel;
+	tdoc["bC"] = batteryCycles;
+	tdoc["tD"] = odometer;
+	tdoc["cH"] = heading;
+	tdoc["pos"][0] = xpos;
+	tdoc["pos"][1] = ypos;
+	tdoc["rssi"] = signalStrength;
+	tdoc["LCCid"] = lastCompletedCommand;
+	String JSON_Data;
+	serializeJson(tdoc, JSON_Data);
+	websocketserver.broadcastTXT(JSON_Data);
+}
+
+void sendToDrive(RoverInstruction instruction)
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["rH"] = instruction.heading;
+	tdoc["dist"] = instruction.distance;
+	tdoc["sp"] = instruction.speed;
+	tdoc["cH"] = heading;
+	serializeJson(tdoc, Serial1);
+}
+
+void recvFromDrive() // Update telemetry data and state info from Drive packet
+{
+	if (Serial1.available()) // Check for input from UART1 (Connected to Drive)
+	{
+		DynamicJsonDocument rdoc(1024);
+		deserializeJson(rdoc, Serial1);
+		driveCommandComplete = rdoc["comp"];
+		odometer = rdoc["mm"];
+		xpos = rdoc["pos"][0];
+		ypos = rdoc["pos"][1];
+	}
+}
+
+void sendToEnergy(bool instruction)
+{
+	DynamicJsonDocument tdoc(128);
+	tdoc["ch"] = instruction; // Start charging
+	serializeJson(tdoc, Serial2);
+}
+
+void recvFromEnergy() // Update telemetry data and state info from Energy packet
+{
+	if (Serial2.available()) // Check for input from UART2 (Connected to Energy)
+	{
+		DynamicJsonDocument rdoc(1024);
+		deserializeJson(rdoc, Serial2);
+		batteryLevel = rdoc["soc"];
+		batteryVoltage = rdoc["mV"];
+		batteryCycles = rdoc["cyc"];
+	}
+}
+
+void sendToVision()
+{
+	Serial3.print("R"); // Request new data from Vision
+}
+
+void recvFromVision() // Update bounding box and obstacle detection data from Vision packet
+{
+	if (Serial3.available()) // Check for input from UART3 (Connected to Vision)
+	{
+		DynamicJsonDocument rdoc(1024);
+		deserializeJson(rdoc, Serial3);
+		bb_left = rdoc["bb"][0];
+		bb_right = rdoc["bb"][1];
+		bb_top = rdoc["bb"][2];
+		bb_bottom = rdoc["bb"][3];
+		bb_centre_x = rdoc["cen"][0];
+		bb_centre_y = rdoc["cen"][1];
+		heading = rdoc["cH"];
+	}
+}
+
+void emergencyStop()
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["rH"] = heading;
+	tdoc["dist"] = -1;
+	tdoc["sp"] = -1;
+	tdoc["cH"] = heading;
+	serializeJson(tdoc, Serial1); // Send stop signals to Drive
+	sendToEnergy(0);			  // Send stop signal to Energy
+	while (InstrQueue.size())
+	{
+		InstrQueue.pop(); // Clear Instruction Queue
+	}
+	Status = CS_IDLE; // Reset rover to idle state
+	Serial.println("Instruction Queue cleared");
+}

Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c

@@ -1,5 +1,3 @@
-
-
 #include <stdio.h>
 #include "I2C_core.h"
 #include "terasic_includes.h"
@@ -39,7 +37,8 @@
 #define MIPI_REG_FrmErrCnt 0x0080
 #define MIPI_REG_MDLErrCnt 0x0090
 
-void mipi_clear_error(void){
+void mipi_clear_error(void)
+{
     MipiBridgeRegWrite(MIPI_REG_CSIStatus, 0x01FF); // clear error
     MipiBridgeRegWrite(MIPI_REG_MDLSynErr, 0x0000); // clear error
     MipiBridgeRegWrite(MIPI_REG_FrmErrCnt, 0x0000); // clear error
@@ -55,7 +54,8 @@ void mipi_clear_error(void){
     MipiBridgeRegWrite(0x0090, 0x00);
 }
 
-void mipi_show_error_info(void){
+void mipi_show_error_info(void)
+{
 
     alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt;
 
@@ -67,7 +67,8 @@ void mipi_show_error_info(void){
     printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt);
 }
 
-void mipi_show_error_info_more(void){
+void mipi_show_error_info_more(void)
+{
     printf("FrmErrCnt = %d\n", MipiBridgeRegRead(0x0080));
     printf("CRCErrCnt = %d\n", MipiBridgeRegRead(0x0082));
     printf("CorErrCnt = %d\n", MipiBridgeRegRead(0x0084));
@@ -82,46 +83,31 @@ void mipi_show_error_info_more(void){
     printf("CSIPktLen = %d\n", MipiBridgeRegRead(0x006E));
 }
 
-
+bool MIPI_Init(void)
-
+{
-bool MIPI_Init(void){
     bool bSuccess;
 
-
     bSuccess = oc_i2c_init_ex(I2C_OPENCORES_MIPI_BASE, 50 * 1000 * 1000, 400 * 1000); //I2C: 400K
     if (!bSuccess)
+    {
         printf("failed to init MIPI- Bridge i2c\r\n");
+    }
 
     usleep(50 * 1000);
     MipiBridgeInit();
 
     usleep(500 * 1000);
 
-//	bSuccess = oc_i2c_init_ex(I2C_OPENCORES_CAMERA_BASE, 50*1000*1000,400*1000); //I2C: 400K
-//	if (!bSuccess)
-//		printf("failed to init MIPI- Camera i2c\r\n");
-
     MipiCameraInit();
     MIPI_BIN_LEVEL(DEFAULT_LEVEL);
-//    OV8865_FOCUS_Move_to(340);
-
-//    oc_i2c_uninit(I2C_OPENCORES_CAMERA_BASE);  // Release I2C bus , due to two I2C master shared!
-
 
     usleep(1000);
 
-
-//    oc_i2c_uninit(I2C_OPENCORES_MIPI_BASE);
-
     return bSuccess;
 }
 
-
-
-
 int main()
 {
-
     fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
 
     printf("DE10-LITE D8M VGA Demo\n");
@@ -137,14 +123,15 @@ int main()
     printf("Image Processor ID: %x\n", IORD(0x42000, EEE_IMGPROC_ID));
     //printf("Image Processor ID: %x\n",IORD(EEE_IMGPROC_0_BASE,EEE_IMGPROC_ID)); //Don't know why this doesn't work - definition is in system.h in BSP
 
-
     usleep(2000);
 
-
     // MIPI Init
-   if (!MIPI_Init()){
+    if (!MIPI_Init())
+    {
         printf("MIPI_Init Init failed!\r\n");
-  }else{
+    }
+    else
+    {
         printf("MIPI_Init Init successfully!\r\n");
     }
 
@@ -158,29 +145,6 @@ int main()
     printf("\n");
     //   }
 
-
-#if 0  // focus sweep
-	    printf("\nFocus sweep\n");
- 	 	alt_u16 ii= 350;
- 	    alt_u8  dir = 0;
- 	 	while(1){
- 	 		if(ii< 50) dir = 1;
- 	 		else if (ii> 1000) dir =0;
-
- 	 		if(dir) ii += 20;
- 	 		else    ii -= 20;
-
- 	    	printf("%d\n",ii);
- 	     OV8865_FOCUS_Move_to(ii);
- 	     usleep(50*1000);
- 	    }
-#endif
-
-
-
-
-
-
     //////////////////////////////////////////////////////////
     alt_u16 bin_level = DEFAULT_LEVEL;
     alt_u8 manual_focus_step = 10;
@@ -194,66 +158,43 @@ int main()
     Focus_Init();
 
     FILE *ser = fopen("/dev/uart_0", "rb+");
-        if(ser){
+    fcntl(ser, F_SETFL, O_NONBLOCK);
+    if (ser)
+    {
         printf("Opened UART\n");
-        } else {
+    }
+    else
+    {
         printf("Failed to open UART\n");
-        	while (1);
+        while (1)
+        {
+        }
     }
 
-  while(1){
+    while (1)
+    {
 
         // touch KEY0 to trigger Auto focus
-	   if((IORD(KEY_BASE,0)&0x03) == 0x02){
+        if ((IORD(KEY_BASE, 0) & 0x03) == 0x02)
+        {
 
             current_focus = Focus_Window(320, 240);
         }
         // touch KEY1 to ZOOM
-	         if((IORD(KEY_BASE,0)&0x03) == 0x01){
+        if ((IORD(KEY_BASE, 0) & 0x03) == 0x01)
-	      	   if(bin_level == 3 )bin_level = 1;
+        {
-	      	   else bin_level ++;
+            if (bin_level == 3)
+                bin_level = 1;
+            else
+                bin_level++;
             printf("set bin level to %d\n", bin_level);
             MIPI_BIN_LEVEL(bin_level);
             usleep(500000);
-
         }
 
-
-	#if 0
-       if((IORD(KEY_BASE,0)&0x0F) == 0x0E){
-
-    	   current_focus = Focus_Window(320,240);
-       }
-
-       // touch KEY1 to trigger Manual focus  - step
-       if((IORD(KEY_BASE,0)&0x0F) == 0x0D){
-
-    	   if(current_focus > manual_focus_step) current_focus -= manual_focus_step;
-    	   else current_focus = 0;
-    	   OV8865_FOCUS_Move_to(current_focus);
-
-       }
-
-       // touch KEY2 to trigger Manual focus  + step
-       if((IORD(KEY_BASE,0)&0x0F) == 0x0B){
-    	   current_focus += manual_focus_step;
-    	   if(current_focus >1023) current_focus = 1023;
-    	   OV8865_FOCUS_Move_to(current_focus);
-       }
-
-       // touch KEY3 to ZOOM
-       if((IORD(KEY_BASE,0)&0x0F) == 0x07){
-    	   if(bin_level == 3 )bin_level = 1;
-    	   else bin_level ++;
-    	   printf("set bin level to %d\n",bin_level);
-    	   MIPI_BIN_LEVEL(bin_level);
-    	 	usleep(500000);
-
-       }
-	#endif
-
         //Read messages from the image processor and print them on the terminal
-       while ((IORD(0x42000,EEE_IMGPROC_STATUS)>>8) & 0xff) { 	//Find out if there are words to read
+        while ((IORD(0x42000, EEE_IMGPROC_STATUS) >> 8) & 0xff)
+        {                                              //Find out if there are words to read
             int word = IORD(0x42000, EEE_IMGPROC_MSG); //Get next word from message buffer
             if (fwrite(&word, 4, 1, ser) != 1)
                 printf("Error writing to UART");
@@ -268,44 +209,57 @@ int main()
 
         //Process input commands
         int in = getchar();
-       switch (in) {
+        switch (in)
-       	   case 'e': {
+        {
+        case 'e':
+        {
             exposureTime += EXPOSURE_STEP;
             OV8865SetExposure(exposureTime);
             printf("\nExposure = %x ", exposureTime);
-       	   	   break;}
+            break;
-       	   case 'd': {
+        }
+        case 'd':
+        {
             exposureTime -= EXPOSURE_STEP;
             OV8865SetExposure(exposureTime);
             printf("\nExposure = %x ", exposureTime);
-       	   	   break;}
+            break;
-       	   case 't': {
+        }
+        case 't':
+        {
             gain += GAIN_STEP;
             OV8865SetGain(gain);
             printf("\nGain = %x ", gain);
-       	   	   break;}
+            break;
-       	   case 'g': {
+        }
+        case 'g':
+        {
             gain -= GAIN_STEP;
             OV8865SetGain(gain);
             printf("\nGain = %x ", gain);
-       	   	   break;}
+            break;
-       	   case 'r': {
+        }
-        	   current_focus += manual_focus_step;
+        case 'r':
-        	   if(current_focus >1023) current_focus = 1023;
+        {
-        	   OV8865_FOCUS_Move_to(current_focus);
+            current_focus += manual_focus_step;
-        	   printf("\nFocus = %x ",current_focus);
+            if (current_focus > 1023)
-       	   	   break;}
+                current_focus = 1023;
-       	   case 'f': {
+            OV8865_FOCUS_Move_to(current_focus);
-        	   if(current_focus > manual_focus_step) current_focus -= manual_focus_step;
+            printf("\nFocus = %x ", current_focus);
-        	   OV8865_FOCUS_Move_to(current_focus);
+            break;
-        	   printf("\nFocus = %x ",current_focus);
+        }
-       	   	   break;}
+        case 'f':
+        {
+            if (current_focus > manual_focus_step)
+                current_focus -= manual_focus_step;
+            OV8865_FOCUS_Move_to(current_focus);
+            printf("\nFocus = %x ", current_focus);
+            break;
+        }
         }
-
 
         //Main loop delay
         usleep(10000);
-
     };
     return 0;
 }