mirror of
https://github.com/supleed2/ELEC50003-P1-CW.git
synced 2024-12-22 13:35:49 +00:00
Merge branch 'main' into Integration
This commit is contained in:
GitHub
commit
54d30741e0
2
Control/.gitignore
vendored
2
Control/.gitignore
vendored
|
|
@ -3,4 +3,4 @@
|
|||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
src/credentials.h
|
||||
include/credentials.h
|
||||
|
|
|
|||
21
Control/include/instruction.h
Normal file
21
Control/include/instruction.h
Normal file
|
|
@ -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
|
||||
11
Control/include/status.h
Normal file
11
Control/include/status.h
Normal file
|
|
@ -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
|
||||
|
|
@ -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 command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Control ==> Command
|
||||
int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // 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
|
||||
batteryVoltage = rdoc["bV"];
|
||||
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];
|
||||
|
|
|
|||
|
|
@ -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"];
|
||||
float spd = rdoc["sp"];
|
||||
int distance = rdoc["dist"]; // -1 for emergency stop
|
||||
float spd = rdoc["sp"]; // -1 for emergency stop
|
||||
int currentHeading = rdoc["cH"];
|
||||
bool resetDistanceTravelled = rdoc["rstD"];
|
||||
|
||||
bool commandComplete = 0;
|
||||
float powerUsage_mWh = 0.0;
|
||||
|
|
|
|||
|
|
@ -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"];
|
||||
}
|
||||
|
|
@ -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
|
||||
#define TX1pin 4 // Pin 11 on expansion board, UART1
|
||||
#pragma region Definitions eg pins
|
||||
#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
|
||||
void printFPGAoutput();
|
||||
void returnSensorData();
|
||||
#pragma region Function Declarations
|
||||
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
|
||||
float battery_voltage = 4.0f;
|
||||
int distance_travelled = 0;
|
||||
#pragma region Global variables
|
||||
ControlStatus_t Status;
|
||||
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()
|
||||
{
|
||||
|
|
@ -39,10 +75,25 @@ void setup()
|
|||
esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack
|
||||
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
|
||||
Serial2.begin(9600, SERIAL_8N1, 16, 17); // RX 9, TX 8
|
||||
// Set up remaining communication ports here (Energy, Vision)
|
||||
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
|
||||
Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
|
||||
Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy)
|
||||
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"); });
|
||||
webserver.onNotFound(notFound);
|
||||
webserver.begin();
|
||||
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
|
||||
webserver.onNotFound(notFound); // Set up basic 404NotFound page
|
||||
webserver.begin(); // Start Asynchronous Web Server
|
||||
|
||||
websocketserver.begin();
|
||||
websocketserver.onEvent(webSocketEvent);
|
||||
ticker.attach(0.5, returnSensorData);
|
||||
websocketserver.begin(); // Start Websocket Server
|
||||
websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command
|
||||
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
|
||||
}
|
||||
|
||||
void printFPGAoutput()
|
||||
{ // Print serial communication from FPGA to serial monitor
|
||||
String FPGAoutput;
|
||||
if (Serial1.available())
|
||||
recvFromDrive(); // Update stats from Drive
|
||||
recvFromEnergy(); // Update stats from Energy
|
||||
recvFromVision(); // Update stats from Vision
|
||||
switch (Status)
|
||||
{
|
||||
FPGAoutput = String(Serial1.readStringUntil('\n'));
|
||||
Serial.println(FPGAoutput);
|
||||
}
|
||||
}
|
||||
|
||||
void returnSensorData()
|
||||
{
|
||||
// Collect sensor data here?
|
||||
distance_travelled++;
|
||||
if (battery_voltage < 6)
|
||||
case CS_ERROR:
|
||||
{
|
||||
battery_voltage += 0.2;
|
||||
Serial.println("Rover in error state, rebooting...");
|
||||
exit(1);
|
||||
}
|
||||
else
|
||||
break;
|
||||
case CS_IDLE:
|
||||
{
|
||||
battery_voltage = 4;
|
||||
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
|
||||
}
|
||||
}
|
||||
String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}";
|
||||
// Serial.println(JSON_Data);
|
||||
websocketserver.broadcastTXT(JSON_Data);
|
||||
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
|
||||
|
||||
}
|
||||
break;
|
||||
default:
|
||||
{
|
||||
Serial.println("Unknown rover state, exiting...");
|
||||
exit(1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
delay(500);
|
||||
}
|
||||
|
||||
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);
|
||||
String command = String((char *)(payload));
|
||||
Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal
|
||||
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.
|
||||
DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive.
|
||||
DynamicJsonDocument rdoc(200); // Create instance of DynamicJsonDocument on heap, 200 Bytes
|
||||
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"];
|
||||
int MVM_L_status = doc["MVM_L"];
|
||||
int MVM_R_status = doc["MVM_R"];
|
||||
int MVM_B_status = doc["MVM_B"];
|
||||
RoverInstruction instr;
|
||||
int mode = rdoc["mode"];
|
||||
switch (mode)
|
||||
{
|
||||
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 + '>');
|
||||
|
||||
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
|
||||
queueInstruction(instr); // Put reset command in InstrQueue
|
||||
}
|
||||
if (MVM_B_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
|
||||
break;
|
||||
case 0: // Stop immediately, clear command cache
|
||||
{
|
||||
Serial.println("Emergency stop command received");
|
||||
// instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued
|
||||
// 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");
|
||||
}
|
||||
|
|
@ -1,5 +1,3 @@
|
|||
|
||||
|
||||
#include <stdio.h>
|
||||
#include "I2C_core.h"
|
||||
#include "terasic_includes.h"
|
||||
|
|
@ -12,7 +10,7 @@
|
|||
#include <unistd.h>
|
||||
|
||||
//EEE_IMGPROC defines
|
||||
#define EEE_IMGPROC_MSG_START ('R'<<16 | 'B'<<8 | 'B')
|
||||
#define EEE_IMGPROC_MSG_START ('R' << 16 | 'B' << 8 | 'B')
|
||||
|
||||
//offsets
|
||||
#define EEE_IMGPROC_STATUS 0
|
||||
|
|
@ -26,286 +24,242 @@
|
|||
#define GAIN_STEP 0x040
|
||||
#define DEFAULT_LEVEL 3
|
||||
|
||||
#define MIPI_REG_PHYClkCtl 0x0056
|
||||
#define MIPI_REG_PHYData0Ctl 0x0058
|
||||
#define MIPI_REG_PHYData1Ctl 0x005A
|
||||
#define MIPI_REG_PHYData2Ctl 0x005C
|
||||
#define MIPI_REG_PHYData3Ctl 0x005E
|
||||
#define MIPI_REG_PHYTimDly 0x0060
|
||||
#define MIPI_REG_PHYSta 0x0062
|
||||
#define MIPI_REG_CSIStatus 0x0064
|
||||
#define MIPI_REG_CSIErrEn 0x0066
|
||||
#define MIPI_REG_MDLSynErr 0x0068
|
||||
#define MIPI_REG_FrmErrCnt 0x0080
|
||||
#define MIPI_REG_MDLErrCnt 0x0090
|
||||
#define MIPI_REG_PHYClkCtl 0x0056
|
||||
#define MIPI_REG_PHYData0Ctl 0x0058
|
||||
#define MIPI_REG_PHYData1Ctl 0x005A
|
||||
#define MIPI_REG_PHYData2Ctl 0x005C
|
||||
#define MIPI_REG_PHYData3Ctl 0x005E
|
||||
#define MIPI_REG_PHYTimDly 0x0060
|
||||
#define MIPI_REG_PHYSta 0x0062
|
||||
#define MIPI_REG_CSIStatus 0x0064
|
||||
#define MIPI_REG_CSIErrEn 0x0066
|
||||
#define MIPI_REG_MDLSynErr 0x0068
|
||||
#define MIPI_REG_FrmErrCnt 0x0080
|
||||
#define MIPI_REG_MDLErrCnt 0x0090
|
||||
|
||||
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
|
||||
MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error
|
||||
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
|
||||
MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error
|
||||
|
||||
MipiBridgeRegWrite(0x0082,0x00);
|
||||
MipiBridgeRegWrite(0x0084,0x00);
|
||||
MipiBridgeRegWrite(0x0086,0x00);
|
||||
MipiBridgeRegWrite(0x0088,0x00);
|
||||
MipiBridgeRegWrite(0x008A,0x00);
|
||||
MipiBridgeRegWrite(0x008C,0x00);
|
||||
MipiBridgeRegWrite(0x008E,0x00);
|
||||
MipiBridgeRegWrite(0x0090,0x00);
|
||||
MipiBridgeRegWrite(0x0082, 0x00);
|
||||
MipiBridgeRegWrite(0x0084, 0x00);
|
||||
MipiBridgeRegWrite(0x0086, 0x00);
|
||||
MipiBridgeRegWrite(0x0088, 0x00);
|
||||
MipiBridgeRegWrite(0x008A, 0x00);
|
||||
MipiBridgeRegWrite(0x008C, 0x00);
|
||||
MipiBridgeRegWrite(0x008E, 0x00);
|
||||
MipiBridgeRegWrite(0x0090, 0x00);
|
||||
}
|
||||
|
||||
void mipi_show_error_info(void){
|
||||
void mipi_show_error_info(void)
|
||||
{
|
||||
|
||||
alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt;
|
||||
alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt;
|
||||
|
||||
PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta);
|
||||
SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus);
|
||||
MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr);
|
||||
FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt);
|
||||
MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt);
|
||||
printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr,FrmErrCnt, MDLErrCnt);
|
||||
PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta);
|
||||
SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus);
|
||||
MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr);
|
||||
FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt);
|
||||
MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt);
|
||||
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){
|
||||
printf("FrmErrCnt = %d\n",MipiBridgeRegRead(0x0080));
|
||||
printf("CRCErrCnt = %d\n",MipiBridgeRegRead(0x0082));
|
||||
printf("CorErrCnt = %d\n",MipiBridgeRegRead(0x0084));
|
||||
printf("HdrErrCnt = %d\n",MipiBridgeRegRead(0x0086));
|
||||
printf("EIDErrCnt = %d\n",MipiBridgeRegRead(0x0088));
|
||||
printf("CtlErrCnt = %d\n",MipiBridgeRegRead(0x008A));
|
||||
printf("SoTErrCnt = %d\n",MipiBridgeRegRead(0x008C));
|
||||
printf("SynErrCnt = %d\n",MipiBridgeRegRead(0x008E));
|
||||
printf("MDLErrCnt = %d\n",MipiBridgeRegRead(0x0090));
|
||||
printf("FIFOSTATUS = %d\n",MipiBridgeRegRead(0x00F8));
|
||||
printf("DataType = 0x%04x\n",MipiBridgeRegRead(0x006A));
|
||||
printf("CSIPktLen = %d\n",MipiBridgeRegRead(0x006E));
|
||||
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));
|
||||
printf("HdrErrCnt = %d\n", MipiBridgeRegRead(0x0086));
|
||||
printf("EIDErrCnt = %d\n", MipiBridgeRegRead(0x0088));
|
||||
printf("CtlErrCnt = %d\n", MipiBridgeRegRead(0x008A));
|
||||
printf("SoTErrCnt = %d\n", MipiBridgeRegRead(0x008C));
|
||||
printf("SynErrCnt = %d\n", MipiBridgeRegRead(0x008E));
|
||||
printf("MDLErrCnt = %d\n", MipiBridgeRegRead(0x0090));
|
||||
printf("FIFOSTATUS = %d\n", MipiBridgeRegRead(0x00F8));
|
||||
printf("DataType = 0x%04x\n", MipiBridgeRegRead(0x006A));
|
||||
printf("CSIPktLen = %d\n", MipiBridgeRegRead(0x006E));
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
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);
|
||||
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");
|
||||
usleep(500 * 1000);
|
||||
|
||||
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);
|
||||
|
||||
|
||||
usleep(1000);
|
||||
|
||||
|
||||
// oc_i2c_uninit(I2C_OPENCORES_MIPI_BASE);
|
||||
|
||||
return bSuccess;
|
||||
return bSuccess;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
|
||||
|
||||
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
|
||||
|
||||
printf("DE10-LITE D8M VGA Demo\n");
|
||||
printf("Imperial College EEE2 Project version\n");
|
||||
IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00);
|
||||
IOWR(MIPI_RESET_N_BASE, 0x00, 0x00);
|
||||
|
||||
usleep(2000);
|
||||
IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF);
|
||||
usleep(2000);
|
||||
IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF);
|
||||
|
||||
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()){
|
||||
printf("MIPI_Init Init failed!\r\n");
|
||||
}else{
|
||||
printf("MIPI_Init Init successfully!\r\n");
|
||||
}
|
||||
|
||||
// while(1){
|
||||
mipi_clear_error();
|
||||
usleep(50*1000);
|
||||
mipi_clear_error();
|
||||
usleep(1000*1000);
|
||||
mipi_show_error_info();
|
||||
// mipi_show_error_info_more();
|
||||
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
|
||||
printf("DE10-LITE D8M VGA Demo\n");
|
||||
printf("Imperial College EEE2 Project version\n");
|
||||
IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00);
|
||||
IOWR(MIPI_RESET_N_BASE, 0x00, 0x00);
|
||||
|
||||
usleep(2000);
|
||||
IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF);
|
||||
usleep(2000);
|
||||
IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF);
|
||||
|
||||
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())
|
||||
{
|
||||
printf("MIPI_Init Init failed!\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("MIPI_Init Init successfully!\r\n");
|
||||
}
|
||||
|
||||
// while(1){
|
||||
mipi_clear_error();
|
||||
usleep(50 * 1000);
|
||||
mipi_clear_error();
|
||||
usleep(1000 * 1000);
|
||||
mipi_show_error_info();
|
||||
// mipi_show_error_info_more();
|
||||
printf("\n");
|
||||
// }
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
alt_u16 bin_level = DEFAULT_LEVEL;
|
||||
alt_u8 manual_focus_step = 10;
|
||||
alt_u16 current_focus = 300;
|
||||
int boundingBoxColour = 0;
|
||||
alt_u32 exposureTime = EXPOSURE_INIT;
|
||||
alt_u16 gain = GAIN_INIT;
|
||||
alt_u16 bin_level = DEFAULT_LEVEL;
|
||||
alt_u8 manual_focus_step = 10;
|
||||
alt_u16 current_focus = 300;
|
||||
int boundingBoxColour = 0;
|
||||
alt_u32 exposureTime = EXPOSURE_INIT;
|
||||
alt_u16 gain = GAIN_INIT;
|
||||
|
||||
OV8865SetExposure(exposureTime);
|
||||
OV8865SetGain(gain);
|
||||
Focus_Init();
|
||||
OV8865SetExposure(exposureTime);
|
||||
OV8865SetGain(gain);
|
||||
Focus_Init();
|
||||
|
||||
FILE* ser = fopen("/dev/uart_0", "rb+");
|
||||
if(ser){
|
||||
printf("Opened UART\n");
|
||||
} else {
|
||||
printf("Failed to open UART\n");
|
||||
while (1);
|
||||
FILE *ser = fopen("/dev/uart_0", "rb+");
|
||||
fcntl(ser, F_SETFL, O_NONBLOCK);
|
||||
if (ser)
|
||||
{
|
||||
printf("Opened UART\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Failed to open UART\n");
|
||||
while (1)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
while (1)
|
||||
{
|
||||
|
||||
// touch KEY0 to trigger Auto focus
|
||||
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 (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);
|
||||
}
|
||||
|
||||
while(1){
|
||||
//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
|
||||
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");
|
||||
if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier
|
||||
printf("\n");
|
||||
printf("%08x ", word);
|
||||
}
|
||||
|
||||
// touch KEY0 to trigger Auto focus
|
||||
if((IORD(KEY_BASE,0)&0x03) == 0x02){
|
||||
//Update the bounding box colour
|
||||
boundingBoxColour = ((boundingBoxColour + 1) & 0xff);
|
||||
IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour));
|
||||
|
||||
current_focus = Focus_Window(320,240);
|
||||
}
|
||||
// touch KEY1 to ZOOM
|
||||
if((IORD(KEY_BASE,0)&0x03) == 0x01){
|
||||
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);
|
||||
//Process input commands
|
||||
int in = getchar();
|
||||
switch (in)
|
||||
{
|
||||
case 'e':
|
||||
{
|
||||
exposureTime += EXPOSURE_STEP;
|
||||
OV8865SetExposure(exposureTime);
|
||||
printf("\nExposure = %x ", exposureTime);
|
||||
break;
|
||||
}
|
||||
case 'd':
|
||||
{
|
||||
exposureTime -= EXPOSURE_STEP;
|
||||
OV8865SetExposure(exposureTime);
|
||||
printf("\nExposure = %x ", exposureTime);
|
||||
break;
|
||||
}
|
||||
case 't':
|
||||
{
|
||||
gain += GAIN_STEP;
|
||||
OV8865SetGain(gain);
|
||||
printf("\nGain = %x ", gain);
|
||||
break;
|
||||
}
|
||||
case 'g':
|
||||
{
|
||||
gain -= GAIN_STEP;
|
||||
OV8865SetGain(gain);
|
||||
printf("\nGain = %x ", gain);
|
||||
break;
|
||||
}
|
||||
case 'r':
|
||||
{
|
||||
current_focus += manual_focus_step;
|
||||
if (current_focus > 1023)
|
||||
current_focus = 1023;
|
||||
OV8865_FOCUS_Move_to(current_focus);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
#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
|
||||
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");
|
||||
if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier
|
||||
printf("\n");
|
||||
printf("%08x ",word);
|
||||
}
|
||||
|
||||
//Update the bounding box colour
|
||||
boundingBoxColour = ((boundingBoxColour + 1) & 0xff);
|
||||
IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour));
|
||||
|
||||
//Process input commands
|
||||
int in = getchar();
|
||||
switch (in) {
|
||||
case 'e': {
|
||||
exposureTime += EXPOSURE_STEP;
|
||||
OV8865SetExposure(exposureTime);
|
||||
printf("\nExposure = %x ", exposureTime);
|
||||
break;}
|
||||
case 'd': {
|
||||
exposureTime -= EXPOSURE_STEP;
|
||||
OV8865SetExposure(exposureTime);
|
||||
printf("\nExposure = %x ", exposureTime);
|
||||
break;}
|
||||
case 't': {
|
||||
gain += GAIN_STEP;
|
||||
OV8865SetGain(gain);
|
||||
printf("\nGain = %x ", gain);
|
||||
break;}
|
||||
case 'g': {
|
||||
gain -= GAIN_STEP;
|
||||
OV8865SetGain(gain);
|
||||
printf("\nGain = %x ", gain);
|
||||
break;}
|
||||
case 'r': {
|
||||
current_focus += manual_focus_step;
|
||||
if(current_focus >1023) current_focus = 1023;
|
||||
OV8865_FOCUS_Move_to(current_focus);
|
||||
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;
|
||||
//Main loop delay
|
||||
usleep(10000);
|
||||
};
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue