What worked last night

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
 
@@ -81,10 +86,11 @@ void loop() {
 
     // Read drop point and bottle number from Pi (blocking)
     // drop_data = recieveData();
-    drop_data.lon = -123.072022; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
-    drop_data.lat = 49.244365;
+
+    drop_data.lon = -76.5207332; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
+    drop_data.lat = 38.3022803;
     drop_data.heading = 0.0;
-    drop_data.bottleID = 2;
+    drop_data.bottleID = 4;
 
     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 +102,19 @@ 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(1, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    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));
 
     // 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);
+    Serial.print("Sent desired drop point to Pi (not): "); 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);
-    // }
+    for (int i = 0; i < 3; i++) {
+        broadcastMessage(des_drop_data);
+    }
 
     // Wait to get close enough to desired drop point
     digitalWrite(LED_RED, HIGH);
@@ -126,16 +132,16 @@ void loop() {
                 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
-                drop_data.lon = msg.global_position_int.lon / 10000000.0; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
-                drop_data.lat = msg.global_position_int.lat / 10000000.0;
-                drop_data.heading = 0.0;
-                drop_data.bottleID = 2;
-                snprintf(buffer, sizeof(buffer), "Received data from failsafe activation: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer);
-                calc_des_drop_state(1, 0, drop_data, &des_drop_data);
-                snprintf(buffer, sizeof(buffer), "Destination: %.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); Serial.print(buffer);
-                for (int i = 0; i < 3; i++) {
-                    broadcastMessage(des_drop_data);
-                }
+                // drop_data.lon = msg.global_position_int.lon / 10000000.0; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
+                // drop_data.lat = msg.global_position_int.lat / 10000000.0;
+                // drop_data.heading = 0.0;
+                // drop_data.bottleID = 2;
+                // snprintf(buffer, sizeof(buffer), "Received data from failsafe activation: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer);
+                // calc_des_drop_state(1, 0, drop_data, &des_drop_data);
+                // snprintf(buffer, sizeof(buffer), "Destination: %.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); Serial.print(buffer);
+                // for (int i = 0; i < 3; i++) {
+                //     broadcastMessage(des_drop_data);
+                // }
 
                 des_drop_data.bottleID = 2; 
                 notDropped = false;

src/SUAS_Link_2024/src/params.h

@@ -19,10 +19,10 @@ 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 10.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
 
@@ -31,8 +31,8 @@ extern HardwareSerial CubeSerial;
 // 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

src/main/encoder_steering.cpp

@@ -93,7 +93,14 @@ double angle_diff(double angle1, double angle2) {
  */
 void servo_control(AngleData data) {
 
-  if (data.desiredForward == -1){
+  // 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;
   }
 
@@ -152,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

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,

src/main/main.ino

@@ -402,9 +402,10 @@ void ComputePID(){
   // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH (and if it was given a coordinate to go to lol)
   // double height = X_Zaxis(0, 0);
   double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset;
-  // Serial.println("\nHeight: " + String(height) + "\n");
+  Serial.println("\nHeight: " + String(height) + "\n");
   if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0 && target_lon != 1000.0 && target_lat != 1000.0){
     // Make the Data packet
+    // Serial.printf("Height matched if condition. Value: %lf\n", height);
     AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0};
     sendSteeringData(data);
   }

src/main/sensors.h

@@ -37,7 +37,7 @@
 #define GPSBAUDRATE 115200
 #define GPS_RX 4
 #define GPS_TX 2
-#define GPS_RATE 5
+#define GPS_RATE 10
 
 // BATTERY MEASUREMENT
 #define BATTERY_PIN 14