Merge pull request #14 from ubcuas/failsafe_guided

Merge all code till now
ubcuas · Jul 14, 2024 · bb749a0 · bb749a0
2 parents 8898594 + 05897c9
commit bb749a0
Show file tree

Hide file tree

Showing 12 changed files with 148 additions and 112 deletions.
diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp
@@ -52,8 +52,13 @@ void setup() {
     peerInfo.encrypt = false;
     // Register parachute boards
     memcpy(peerInfo.peer_addr, ADDRESS_1, 6);
+    if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); }
     memcpy(peerInfo.peer_addr, ADDRESS_2, 6);
     if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); }
+    memcpy(peerInfo.peer_addr, ADDRESS_3, 6);
+    if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); }
+    memcpy(peerInfo.peer_addr, ADDRESS_4, 6);
+    if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); }
 
     digitalWrite(LED_RED, LOW); // Red LED off indicates comminucation established
 
@@ -80,11 +85,7 @@ char buffer[100]; // For printing things
 void loop() {
 
     // Read drop point and bottle number from Pi (blocking)
-    // drop_data = recieveData();
-    drop_data.lon = -123.0734507; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
-    drop_data.lat = 49.2444132;
-    drop_data.heading = 0.0;
-    drop_data.bottleID = 2;
+    drop_data = recieveData();
 
     bool notDropped = true;
     snprintf(buffer, sizeof(buffer), "Received data from Pi: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer);
@@ -96,19 +97,12 @@ void loop() {
     // Serial.println("Wind: " + String(windspeed) + ", " + String(wind_heading));
     // calc_des_drop_state(windspeed, wind_heading, drop_data, &des_drop_data);
 
-    calc_des_drop_state(0, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-    // Serial.println(latitudeToMeters(des_drop_data.lat)-latitudeToMeters(drop_data.lat));
-    // Serial.println(latitudeToMeters(des_drop_data.lon)-latitudeToMeters(drop_data.lon));
+    calc_des_drop_state(0, 0, drop_data, &des_drop_data);
 
     // Send desired drop point to Pi
     snprintf(buffer, sizeof(buffer), "%.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID);
     PiSerial.print(buffer);
-    Serial.print("Sent desired drop point to Pi: "); Serial.print(buffer);
-
-    // Send message to parachutes (try 3 times; this is time sensitive so nothing we can do if it fails)
-    for (int i = 0; i < 3; i++) {
-        broadcastMessage(des_drop_data);
-    }
+    Serial.print("Sent desired drop point to Pi (not): "); Serial.print(buffer);
 
     // Wait to get close enough to desired drop point
     digitalWrite(LED_RED, HIGH);
@@ -119,25 +113,21 @@ void loop() {
         double lon = (double) msg.global_position_int.lon / 10000000.0;
         // Serial.println("Coords: " + String(lat, 8) + ", " + String(lon, 8));
 
-        if (FAILSAFE_MODE) {
-            if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the right bottle
-                des_drop_data.bottleID = 1; 
-                notDropped = false;
-                Serial.println("Failsafe mode: Right bottle dropped.");
-            } 
-            if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the left bottle
-                des_drop_data.bottleID = 2; 
-                notDropped = false;
-                Serial.println("Failsafe mode: Left bottle dropped.");
-            } 
-        }
-        else {
-            double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon);
-            Serial.println("Dist: " + String(dist_from_drop_point));
-            if (dist_from_drop_point < RELEASE_MARGIN) {
-                notDropped = false;
-                Serial.println("Reached drop point");
-            }
+        if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the left bottle
+            des_drop_data.bottleID = 1; 
+            notDropped = false;
+            Serial.println("Failsafe mode: Right bottle dropped.");
+        } 
+        if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the right bottle
+            des_drop_data.bottleID = 2; 
+            notDropped = false;
+            Serial.println("Failsafe mode: Left bottle dropped.");
+        } 
+        double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon);
+        Serial.println("Dist: " + String(dist_from_drop_point));
+        if (dist_from_drop_point < RELEASE_MARGIN) {
+            notDropped = false;
+            Serial.println("Reached drop point");
         }
     }
     digitalWrite(LED_RED, LOW);

diff --git a/src/SUAS_Link_2024/src/params.h b/src/SUAS_Link_2024/src/params.h
@@ -19,20 +19,20 @@ extern HardwareSerial CubeSerial;
 
 #define M_PER_LAT_DEG 111320.0 // 111320 meters per degree of latitude (short distance approx)
 
-#define AIRCRAFT_SPEED 7.0 // m/s
-#define DRIFT_FACTOR 0.0 // TODO: find this from testing, it's arbitrary rn!!
+#define AIRCRAFT_SPEED 12.0 // m/s
+#define DRIFT_FACTOR 1.0 // TODO: find this from testing, it's arbitrary rn!!
 #define RELEASE_DELAY 0.0 // s // TODO!!!
-#define RELEASE_MARGIN 6.0 // Tolerance margin from desired drop point that we will trigger release
+#define RELEASE_MARGIN 5.0 // Tolerance margin from desired drop point that we will trigger release
 
 #define WINDOW_SIZE 10
 
-#define FAILSAFE_MODE true
+#define FAILSAFE_MODE false
 
 // MAC Address of responder - edit as required
 static const uint8_t ADDRESS_1[] = {0xD8, 0xBC, 0x38, 0xE4, 0x9E, 0x5C}; // Parachute #1
 static const uint8_t ADDRESS_2[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xFC}; // Parachute #2
-// static const uint8_t ADDRESS_3[] = {0x, 0x, 0x, 0x, 0x, 0x};
-// static const uint8_t ADDRESS_4[] = {0x, 0x, 0x, 0x, 0x, 0x};
-// static const uint8_t ADDRESS_5[] = {0x, 0x, 0x, 0x, 0x, 0x};
+static const uint8_t ADDRESS_3[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xE4}; // Parachute #3
+static const uint8_t ADDRESS_4[] = {0x94, 0xE6, 0x86, 0x92, 0xA8, 0xCC}; // Parachute #4
+// static const uint8_t ADDRESS_ALL[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 #endif
diff --git a/src/main/ConfigParse.cpp b/src/main/ConfigParse.cpp
@@ -31,6 +31,9 @@ void ConfigParser::createDefaultConfigFile(fs::SDFS fs){
     PID["KD"] = 0.0;
     PID["PID_ControlRate"] = 100.0;
 
+    //IMU Parameters
+    doc["IMU_HeadOffset"] = 180.0;
+
     // Print enable
     doc["Print_Enable"] = true;
     doc["BufferSize"] = 512;
@@ -137,6 +140,9 @@ ConfigParseStatus ConfigParser::parseConfigFile(fs::SDFS fs, ConfigData_t *confi
     configData->PID.KD = doc["PID"]["KD"];
     configData->PID.PID_ControlRate = doc["PID"]["PID_ControlRate"];
 
+    //IMU Parameters
+     configData->IMU_HeadOffset = doc["IMU_HeadOffset"];
+
     //Printing Parameters
     configData->Print_Enable = doc["Print_Enable"];
     configData->BufferSize = doc["BufferSize"];
@@ -185,6 +191,7 @@ void ConfigParser::getConfigNoSD(ConfigData_t *ConfigData){
     ConfigData->PID.KI = 0.0;
     ConfigData->PID.KD = 0.0;
     ConfigData->PID.PID_ControlRate = 100.0;
+    ConfigData->IMU_HeadOffset = 180.0;
 
     ConfigData->Print_Enable = true;
     ConfigData->BufferSize = 512;
@@ -228,6 +235,8 @@ void ConfigParser::printConfigData(ConfigData_t *configData){
     Serial.printf("PID KI: %f\n", configData->PID.KI);
     Serial.printf("PID KD: %f\n", configData->PID.KD);
     Serial.printf("PID Control Rate: %f\n", configData->PID.PID_ControlRate);
+
+    Serial.printf("IMU Head Offset: %lf(deg)\n", configData->IMU_HeadOffset);
 
     Serial.printf("Print Enable: %d\n", configData->Print_Enable);
     Serial.printf("Buffer Size: %d\n", configData->BufferSize);

diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h
@@ -74,6 +74,7 @@ typedef struct{
     double ACC_Z_STD;
     double BARO_ALT_STD;
     double GPS_POS_STD;
+    double IMU_HeadOffset;
     PIDConfig_t PID;
     bool Print_Enable;
     uint16_t BufferSize;

diff --git a/src/main/Reciever.cpp b/src/main/Reciever.cpp
@@ -27,7 +27,8 @@ bool InitESPNow(uint8_t Bottle_id) {
 
 // Callback function that will be executed when data is received
 void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
-  memcpy(&myData, (datapacket*)incomingData, sizeof(myData));
+  datapacket temp_storage;
+  memcpy(&temp_storage, (datapacket*)incomingData, sizeof(temp_storage));
   Serial.print("Bytes received: ");
   Serial.println(len);
   Serial.print("lat: ");
@@ -41,8 +42,9 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
   Serial.println();
 
   // Send a response
-  if (myData.bottleID == Bottle_ID){
+  if (temp_storage.bottleID == Bottle_ID){
     response.bottleID = Bottle_ID;
+    memcpy(&myData, (datapacket*)incomingData, sizeof(myData));
     esp_err_t result = esp_now_send(mac, (uint8_t *) &response, sizeof(response));
     if (result == ESP_OK) {
       Serial.println("Response sent");

diff --git a/src/main/SDCard.cpp b/src/main/SDCard.cpp
@@ -90,9 +90,21 @@ namespace SDCard
             }
         }
 
-        // create a new base file
+        // Check if directory exists
+        if (!SD.exists(LOG_DIR)) {
+            // Create directory if it does not exist
+            if (SD.mkdir(LOG_DIR)) {
+                Serial.println("Directory created");
+            } else {
+                Serial.println("Failed to create directory");
+                SDStatus = SDCARD_ERROR;
+                return SDCARD_ERROR;
+            }
+        }
+
+        // Create a new base file
         File file = SD.open((String(LOG_DIR) + "/" + String(LOG_FILE_BASE) + String(LOG_FILE_EXT)), FILE_WRITE);
-        if(!file){
+        if (!file) {
             Serial.println("Failed to create new file");
             SDStatus = SDCARD_ERROR;
             return SDCARD_ERROR;

diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp
@@ -12,6 +12,7 @@ QueueHandle_t steeringQueue = NULL;
 
 #define SERVO_CONTROL_PERIOD 10 // ms
 volatile int PID_Control_Period = SERVO_CONTROL_PERIOD;
+bool servo_enable = false;
 
 void steering_setup(double acquireRate, double kp, double ki, double kd) {
 
@@ -31,16 +32,6 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) {
   PID_Control_Period = (1.0/acquireRate)*1000;
   // pid.setDeadband(DIST_PER_TICK);
 
-  // Initialize servos
-  Serial.println("Initializing servos");
-  servo_1.attach(SERVO_1);
-  servo_1.setPeriodHertz(SERVO_FREQ);
-  servo_2.attach(SERVO_2);
-  servo_2.setPeriodHertz(SERVO_FREQ);
-  // No spin
-  servo_1.write(90);
-  servo_2.write(90);
-
   // Create queue
   steeringQueue = xQueueCreate(1, sizeof(AngleData));  // this queue will take the yaw value
   if (steeringQueue == NULL) {
@@ -67,7 +58,19 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) {
   } else {
     Serial.println("Servo control task created");
   }
+}
+
 
+void init_servos() {
+  // Initialize servos
+  Serial.println("Initializing servos");
+  servo_1.attach(SERVO_1);
+  servo_1.setPeriodHertz(SERVO_FREQ);
+  servo_2.attach(SERVO_2);
+  servo_2.setPeriodHertz(SERVO_FREQ);
+  // No spin
+  servo_1.write(90);
+  servo_2.write(90);
 }
 
 
@@ -90,11 +93,22 @@ double angle_diff(double angle1, double angle2) {
  */
 void servo_control(AngleData data) {
 
-  if (data.desiredForward == -1){
-    do_nothing();
+  // Serial,printf("Data Packet: %lf \t %lf \t %lf\n", data.currentYaw, data.desiredForward, data.desiredYaw);
+
+  if (data.desiredForward == -1.0){
+    if (servo_enable) {
+      servo_1.detach();
+      servo_2.detach();
+      servo_enable = false;
+    }
     return;
   }
 
+  if (!servo_enable) {
+    servo_enable = true;
+    init_servos();
+  }
+
   SteeringData des_lengths;
   SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)};
 
@@ -111,7 +125,7 @@ void servo_control(AngleData data) {
     des_lengths.l2 = output;
   }
 
-  Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2);    //- -- Comment me when testing is done
+  // Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2);    //- -- Comment me when testing is done
 
   // Servo 1
   if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) {
@@ -145,13 +159,13 @@ void servoControlTask(void *pvParameters) {
     // initialize the incoming data
     incomingData.desiredYaw = 0;
     incomingData.currentYaw = 0;
-    incomingData.desiredForward = 0;
+    incomingData.desiredForward = -1.0;
 
     while (true) {
         // Check if we have incoming data
         if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) {
             // Successfully received data
-            Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward);
+            // Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward);
         }
         // Call servo_control function here
         servo_control(incomingData); // Modify parameters as needed

diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h
@@ -39,6 +39,9 @@ extern double current_heading;
 extern volatile bool forward_1;
 extern volatile bool forward_2;
 
+extern bool servo_enable;
+
+void init_servos();
 void steering_setup(double acquireRate, double kp, double ki, double kd);
 double angle_diff(double angle1, double angle2);
 void  servo_control(AngleData data);

diff --git a/src/main/gpsLowLevel.h b/src/main/gpsLowLevel.h
@@ -26,7 +26,7 @@ typedef struct{
 
 class gpsLowLevel {
 public:
-    gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 2, uint8_t TXPin = 4);
+    gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 4, uint8_t TXPin = 2);
 
     enum GPS_Status {
         GPS_OK,