Don't activate before coordinates sent

ubcuas · Jun 13, 2024 · 06aeaf7 · 06aeaf7
1 parent 8898594
commit 06aeaf7
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp
@@ -81,8 +81,8 @@ 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.lon = -123.072022; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
+    drop_data.lat = 49.244365;
     drop_data.heading = 0.0;
     drop_data.bottleID = 2;
 

diff --git a/src/main/main.ino b/src/main/main.ino
@@ -369,8 +369,8 @@ void ComputePID(){
     target_lat = myData.lat;
   }
   else {
-    target_lon = 0;
-    target_lat = 0;
+    target_lon = 1000.0; // 1000 deg lat or lon doesn't exist (I hope)
+    target_lat = 1000.0;
   }
   //To access kalman filter values for current x,y,&z direction
   MatrixXd X_Zaxis = myKalmanFilter_inst_Z.getState();
@@ -399,11 +399,11 @@ void ComputePID(){
     setpoint = setpoint - 360;
   }
 
-  // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH
+  // 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");
-  if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0){
+  if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0 && target_lon != 1000.0 && target_lat != 1000.0){
     // Make the Data packet
     AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0};
     sendSteeringData(data);