Steering activates only within altitude range

src/main/ConfigParse.h

@@ -33,8 +33,8 @@ typedef struct{
     double KP;
     double KI;
     double KD;
-    double PID_ControlRate
-}PIDConfig_t ;
+    double PID_ControlRate;
+} PIDConfig_t ;
 
 // Output data settings for each sensor
 typedef struct{

src/main/encoder_steering.cpp

@@ -89,6 +89,12 @@ double angle_diff(double angle1, double angle2) {
  * @param data: AngleData struct containing desired and current yaw values
  */
 void servo_control(AngleData data) {
+
+  if (data.desiredForward == -1){
+    do_nothing();
+    return;
+  }
+
   SteeringData des_lengths;
   SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)};
 
@@ -165,3 +171,11 @@ void sendSteeringData(AngleData data) {
     }
 }
 
+/*
+ * Don't move the servos
+ */
+void do_nothing() {
+  servo_1.write(90);
+  servo_2.write(90);
+}
+

src/main/encoder_steering.h

@@ -44,5 +44,6 @@ double angle_diff(double angle1, double angle2);
 void  servo_control(AngleData data);
 void servoControlTask(void *pvParameters);
 void sendSteeringData(AngleData data);
+void do_nothing();
 
 #endif

src/main/main.ino

@@ -379,26 +379,30 @@ void ComputePID(){
 
   double lon_now = sensorData_inst.gpsData.refLongitude + delta_long;
   double lat_now = sensorData_inst.gpsData.refLatitude + delta_lat;
-  //Height = current GPS_Position & use kinematics to get new height? To be done
+  // Height = current GPS_Position & use kinematics to get new height? To be done
 
-  //Get process variable - pv is the error between (lot & lat_fast direction)-(target) / current heading-target heading
+  // Get process variable - pv is the error between (lot & lat_fast direction)-(target) / current heading-target heading
   double setpoint = GPS.courseTo(lat_now, lon_now, target_lat, target_lon); //Get the heading to the target
-  //convert heading from 0-360 to -180-180
+  // Convert heading from 0-360 to -180-180
   if(setpoint > 180){
     setpoint = setpoint - 360;
   }
-
-  // make the Data packet
-  AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0};
-  // send the data packet to the queue
-  if (sensorData_inst.barometerData.Altitude <= configData_inst.HEIGHT_THRESH){
+
+  // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH
+  // 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 >= 0.0){
+    // Make the Data packet
+    AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0};
     sendSteeringData(data);
   }
   else {
     count_1 = 0; // Keep resetting encoders to zero until steering activated
     count_2 = 0;
+    AngleData data = {0, 0, -1}; // I'm using the desiredForward as a flag to turn off the servos (-1 = off)
+    sendSteeringData(data);
   }
-
 
   double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat);