Dedicated task for GPS Rescue (#11972)

src/main/fc/tasks.c

@@ -53,10 +53,11 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/position.h"
+#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/position.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -249,6 +250,18 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
     schedulerSetNextStateTime(rxStateDurationFractionUs[rxState] >> RX_TASK_DECAY_SHIFT);
 }
 
+#ifdef USE_GPS_RESCUE
+static void taskGpsRescue(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (gpsRescueIsConfigured()) {
+        gpsRescueUpdate();
+    } else {
+        schedulerIgnoreTaskStateTime();
+    }
+}
+#endif
 
 #ifdef USE_BARO
 static void taskUpdateBaro(timeUs_t currentTimeUs)
@@ -278,6 +291,14 @@ static void taskUpdateMag(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_BARO) || defined(USE_GPS)
+static void taskCalculateAltitude(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    calculateEstimatedAltitude();
+}
+#endif // USE_BARO || USE_GPS
+
 #if defined(USE_RANGEFINDER)
 void taskUpdateRangefinder(timeUs_t currentTimeUs)
 {
@@ -293,14 +314,6 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
-#if defined(USE_BARO) || defined(USE_GPS)
-static void taskCalculateAltitude(timeUs_t currentTimeUs)
-{
-    UNUSED(currentTimeUs);
-    calculateEstimatedAltitude();
-}
-#endif // USE_BARO || USE_GPS
-
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)
 {
@@ -337,6 +350,7 @@ task_t tasks[TASK_COUNT];
 
 // Task ID data in .data (initialised data)
 task_attribute_t task_attributes[TASK_COUNT] = {
+
     [TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
     [TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
     [TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
@@ -355,10 +369,12 @@ task_attribute_t task_attributes[TASK_COUNT] = {
     [TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
     [TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
     [TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+
 #ifdef USE_ACC
     [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
     [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
 #endif
+
     [TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
     [TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),
 
@@ -370,6 +386,10 @@ task_attribute_t task_attributes[TASK_COUNT] = {
     [TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(TASK_GPS_RATE), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer)
 #endif
 
+#ifdef USE_GPS_RESCUE
+    [TASK_GPS_RESCUE] = DEFINE_TASK("GPS_RESCUE", NULL, NULL, taskGpsRescue, TASK_PERIOD_HZ(TASK_GPS_RESCUE_RATE_HZ), TASK_PRIORITY_MEDIUM),
+#endif
+
 #ifdef USE_MAG
     [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
 #endif
@@ -516,6 +536,10 @@ void tasksInit(void)
     setTaskEnabled(TASK_GPS, featureIsEnabled(FEATURE_GPS));
 #endif
 
+#ifdef USE_GPS_RESCUE
+    setTaskEnabled(TASK_GPS_RESCUE, featureIsEnabled(FEATURE_GPS));
+#endif
+
 #ifdef USE_MAG
     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 #endif

src/main/flight/gps_rescue.c

@@ -206,7 +206,7 @@ void gpsRescueInit(void)
 /*
  If we have new GPS data, update home heading if possible and applicable.
 */
-void rescueNewGpsData(void)
+void gpsRescueNewGpsData(void)
 {
     newGPSData = true;
 }
@@ -298,7 +298,7 @@ static void rescueAttainPosition(void)
     /**
         Altitude (throttle) controller
     */
-    // currentAltitudeCm is updated at TASK_GPS_RATE since GPS initiates updateGPSRescueState()
+    // currentAltitudeCm is updated at TASK_GPS_RESCUE_RATE_HZ
     const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
     // height above target in metres (negative means too low)
     // at the start, the target starts at current altitude plus one step.  Increases stepwise to intended value.
@@ -709,7 +709,7 @@ void altitudeAchieved(void)
     rescueState.phase = RESCUE_ROTATE;
 }
 
-void updateGPSRescueState(void)
+void gpsRescueUpdate(void)
 // this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
 {
     if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {

src/main/flight/gps_rescue.h

@@ -21,11 +21,13 @@
 
 #include "pg/pg.h"
 
+#define TASK_GPS_RESCUE_RATE_HZ 100 // synced to altitude task rate
+
 typedef struct gpsRescue_s {
-    uint16_t angle; //degrees
+    uint16_t angle; // degrees
-    uint16_t initialAltitudeM; //meters
+    uint16_t initialAltitudeM; // meters
-    uint16_t descentDistanceM; //meters
+    uint16_t descentDistanceM; // meters
-    uint16_t rescueGroundspeed; //centimeters per second
+    uint16_t rescueGroundspeed; // centimeters per second
     uint8_t throttleP, throttleI, throttleD;
     uint8_t yawP;
     uint16_t throttleMin;
@@ -33,25 +35,25 @@ typedef struct gpsRescue_s {
     uint16_t throttleHover;
     uint8_t minSats;
     uint8_t velP, velI, velD;
-    uint16_t minRescueDth; //meters
+    uint16_t minRescueDth; // meters
     uint8_t sanityChecks;
     uint8_t allowArmingWithoutFix;
     uint8_t useMag;
-    uint8_t targetLandingAltitudeM; //meters
+    uint8_t targetLandingAltitudeM; // meters
     uint8_t altitudeMode;
     uint16_t ascendRate;
     uint16_t descendRate;
-    uint16_t rescueAltitudeBufferM; //meters
+    uint16_t rescueAltitudeBufferM; // meters
     uint8_t rollMix;
 } gpsRescueConfig_t;
 
 PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
 
-extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
+extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void gpsRescueInit(void);
-void updateGPSRescueState(void);
+void gpsRescueUpdate(void);
-void rescueNewGpsData(void);
+void gpsRescueNewGpsData(void);
 
 float gpsRescueGetYawRate(void);
 float gpsRescueGetThrottle(void);

src/main/io/gps.c

@@ -877,12 +877,6 @@ void gpsUpdate(timeUs_t currentTimeUs)
         DISABLE_STATE(GPS_FIX_HOME);
     }
 
-#if defined(USE_GPS_RESCUE)
-    if (gpsRescueIsConfigured()) {
-        updateGPSRescueState();
-    }
-#endif
-
     static bool hasBeeped = false;
     if (!ARMING_FLAG(ARMED)) {
         // while disarmed, beep when requirements for a home fix are met
@@ -1914,7 +1908,7 @@ void onGpsNewData(void)
     }
 
 #ifdef USE_GPS_RESCUE
-    rescueNewGpsData();
+    gpsRescueNewGpsData();
 #endif
 }
 

src/main/scheduler/scheduler.h

@@ -116,6 +116,9 @@ typedef enum {
 #ifdef USE_GPS
     TASK_GPS,
 #endif
+#ifdef USE_GPS_RESCUE
+    TASK_GPS_RESCUE,
+#endif
 #ifdef USE_MAG
     TASK_COMPASS,
 #endif
@@ -161,19 +164,15 @@ typedef enum {
 #ifdef USE_CAMERA_CONTROL
     TASK_CAMCTRL,
 #endif
-
 #ifdef USE_RCDEVICE
     TASK_RCDEVICE,
 #endif
-
 #ifdef USE_ADC_INTERNAL
     TASK_ADC_INTERNAL,
 #endif
-
 #ifdef USE_PINIOBOX
     TASK_PINIOBOX,
 #endif
-
 #ifdef USE_CRSF_V3
     TASK_SPEED_NEGOTIATION,
 #endif