Failsafe testing, detach servo when parachute not enabled

src/SUAS_Link_2024/src/main.cpp

@@ -96,7 +96,7 @@ 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!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    calc_des_drop_state(1, 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));
 
@@ -106,9 +106,9 @@ void loop() {
     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);
-    }
+    // 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,6 +126,17 @@ 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);
+                }
+
                 des_drop_data.bottleID = 2; 
                 notDropped = false;
                 Serial.println("Failsafe mode: Left bottle dropped.");

src/SUAS_Link_2024/src/params.h

@@ -20,7 +20,7 @@ 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 DRIFT_FACTOR 10.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
 

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);
 }
 
 
@@ -91,10 +94,14 @@ double angle_diff(double angle1, double angle2) {
 void servo_control(AngleData data) {
 
   if (data.desiredForward == -1){
-    do_nothing();
     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)};
 

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);