Merge pull request #13 from ubcuas/failsafe

Failsafe

src/SUAS_Link_2024/src/Sender.cpp

@@ -51,7 +51,10 @@ bool broadcastMessage(struct_message myData){
   //timer for timeout
   unsigned long start = millis();
   esp_err_t result2 = esp_now_send(0, (uint8_t *) &myData, sizeof(myData)); // peer_addr 0 means send to all
-  if (result2 == ESP_OK) {  
+  if (result2 == ESP_OK) { 
+
+    return true; // I AM BYPASSING RETURN MESSAGE LOL
+
     //Wait for confirmation from reciever
     while(!success){
       if (millis() - start > TIMEOUT_MILLIS){

src/SUAS_Link_2024/src/autopilot_interface.cpp

@@ -235,6 +235,14 @@ Mavlink_Messages Autopilot_Interface::read_messages()
                     break;
                 }
 
+                case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
+                {
+                    mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
+                    current_messages.time_stamps.servo_output_raw = get_time_usec();
+                    this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
+                    break;
+                }
+
 				default:
 				{
 					// printf("Warning, did not handle message id %i\n",message.msgid);
@@ -265,7 +273,8 @@ Mavlink_Messages Autopilot_Interface::read_messages()
                 // this_timestamps.gps_raw                    &&
                 // this_timestamps.gps_status                 &&
                 // this_timestamps.wind                       &&
-                this_timestamps.high_latency2                                  
+                this_timestamps.high_latency2                 &&
+                this_timestamps.servo_output_raw                            
 				// this_timestamps.sys_status
 				;
     }

src/SUAS_Link_2024/src/autopilot_interface.h

@@ -41,6 +41,7 @@ struct Time_Stamps
     uint64_t gps_status;
     uint64_t wind;
     uint64_t high_latency2;
+    uint64_t servo_output_raw;
 
 	void
 	reset_timestamps()
@@ -66,6 +67,7 @@ struct Time_Stamps
         gps_status = 0;
         wind = 0;
         high_latency2 = 0;
+        servo_output_raw = 0;
 	}
 
 };
@@ -132,6 +134,9 @@ struct Mavlink_Messages {
     // High Latency 2 (gives a variety of data including wind speed and direction)
     mavlink_high_latency2_t high_latency2;
 
+    // Servo outputs
+    mavlink_servo_output_raw_t servo_output_raw;
+
 	// Time Stamps
 	Time_Stamps time_stamps;
 

src/SUAS_Link_2024/src/main.cpp

@@ -52,6 +52,7 @@ void setup() {
     peerInfo.encrypt = false;
     // Register parachute boards
     memcpy(peerInfo.peer_addr, ADDRESS_1, 6);
+    memcpy(peerInfo.peer_addr, ADDRESS_2, 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
@@ -63,6 +64,8 @@ void setup() {
         // linker.request_msg(MAVLINK_MSG_ID_WIND_COV, 1);
         linker.request_msg(MAVLINK_MSG_ID_HIGH_LATENCY2, 5);
         delay(250);
+        linker.request_msg(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+        delay(250);
     }
     digitalWrite(LED_BLUE, LOW); // Blue LED off indicates everything ready
     delay(1000);
@@ -78,10 +81,10 @@ void loop() {
 
     // Read drop point and bottle number from Pi (blocking)
     // drop_data = recieveData();
-    drop_data.lon =  -123.247933; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
-    drop_data.lat = 49.262253;
+    drop_data.lon = -123.0734507; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
+    drop_data.lat = 49.2444132;
     drop_data.heading = 0.0;
-    drop_data.bottleID = 1;
+    drop_data.bottleID = 2;
 
     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);
@@ -94,16 +97,18 @@ void loop() {
     // 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));
+    // 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);
 
-    msg = pixhawk.read_messages();
-    Serial.println(msg.heartbeat.system_status);
+    // 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);
+    }
 
     // Wait to get close enough to desired drop point
     digitalWrite(LED_RED, HIGH);
@@ -114,27 +119,40 @@ void loop() {
         double lon = (double) msg.global_position_int.lon / 10000000.0;
         // Serial.println("Coords: " + String(lat, 8) + ", " + String(lon, 8));
 
-        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 (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");
+            }
         }
     }
     digitalWrite(LED_RED, LOW);
 
-    // // Send message to parachutes
-    // broadcastMessage(des_drop_data);
-
     if (des_drop_data.bottleID == 1 || des_drop_data.bottleID == 3 || des_drop_data.bottleID == 5) {
         servo_front_R.write(160);
-        servo_back_R.write(160);
+        servo_back_R.write(170);
     }
     else if (des_drop_data.bottleID == 2 || des_drop_data.bottleID == 4) {
-        servo_front_L.write(160);
+        servo_front_L.write(150);
         servo_back_L.write(160);
     }
 
     PiSerial.println(String(des_drop_data.bottleID)); // Tell software bottle has dropped
 
+    delay(1000);
+
 }

src/SUAS_Link_2024/src/params.h

@@ -20,15 +20,17 @@ 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.1 // TODO: find this from testing, it's arbitrary rn!!
+#define DRIFT_FACTOR 0.0 // TODO: find this from testing, it's arbitrary rn!!
 #define RELEASE_DELAY 0.0 // s // TODO!!!
-#define RELEASE_MARGIN 3.0 // Tolerance margin from desired drop point that we will trigger release
+#define RELEASE_MARGIN 6.0 // Tolerance margin from desired drop point that we will trigger release
 
 #define WINDOW_SIZE 10
 
+#define FAILSAFE_MODE true
+
 // MAC Address of responder - edit as required
-static const uint8_t ADDRESS_1[] = {0xD8, 0xBC, 0x38, 0xE4, 0x9E, 0x5C};
-// static const uint8_t ADDRESS_2[] = {0x, 0x, 0x, 0x, 0x, 0x};
+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};