Fixed sending bug

ubcuas · Jun 25, 2024 · e625b0d · e625b0d
1 parent 9666af9
commit e625b0d
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 7 deletions.
diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp
@@ -89,10 +89,12 @@ void loop() {
 
     // drop_data.lon = -76.5207332; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
     // drop_data.lat = 38.3022803;
-    drop_data.lon = -76.5494557; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
-    drop_data.lat = 38.3140069;
+    // drop_data.lon = -76.5494557; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
+    // drop_data.lat = 38.3140069;
+    drop_data.lon = -76.5216808; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
+    drop_data.lat = 38.3012800;
     drop_data.heading = 0.0;
-    drop_data.bottleID = 4;
+    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);
@@ -116,18 +118,37 @@ void loop() {
     // 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);
+        delay(500);
     }
 
     // REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-    des_drop_data.bottleID = 2;
+    des_drop_data.bottleID = 3;
     for (int i = 0; i < 3; i++) {
         broadcastMessage(des_drop_data);
+        delay(500);
     }
-    des_drop_data.bottleID = 3;
+    des_drop_data.bottleID = 4;
     for (int i = 0; i < 3; i++) {
         broadcastMessage(des_drop_data);
+        delay(500);
     }
 
+    // struct_message drop_data_2;
+    // drop_data_2.lon = test_lon; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
+    // drop_data_2.lat = test_lat;
+    // drop_data_2.heading = 0.0;
+    // drop_data_2.bottleID = 3;
+    // for (int i = 0; i < 3; i++) {
+    //     broadcastMessage(drop_data_2);
+    // }
+    // struct_message drop_data_4;
+    // drop_data_4.lon = test_lon; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , 
+    // drop_data_4.lat = test_lat;
+    // drop_data_4.heading = 0.0;
+    // drop_data_4.bottleID = 4;
+    // for (int i = 0; i < 3; i++) {
+    //     broadcastMessage(drop_data_4);
+    // }
 
     // Wait to get close enough to desired drop point
     digitalWrite(LED_RED, HIGH);

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/main.ino b/src/main/main.ino
@@ -369,6 +369,7 @@ void ComputePID(){
   if (myData.bottleID == configData_inst.BottleID){
     target_lon = myData.lon;
     target_lat = myData.lat;
+    printf("Target received.\n");
   }
   else {
     target_lon = 1000.0; // 1000 deg lat or lon doesn't exist (I hope)