Merge pull request #7294 from TonyBlit/gps_rescue_no_mag

Mag heading ignored while GPS Rescue is running

src/main/flight/gps_rescue.c

@@ -140,6 +140,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .minSats = 8,
     .minRescueDth = 100,
     .allowArmingWithoutFix = false,
+    .useMag = true
 );
 
 static uint16_t rescueThrottle;
@@ -150,6 +151,7 @@ uint16_t      hoverThrottle = 0;
 float         averageThrottle = 0.0;
 float         altitudeError = 0.0;
 uint32_t      throttleSamples = 0;
+bool          magForceDisable = false;
 
 static bool newGPSData = false;
 
@@ -368,7 +370,14 @@ static void performSanityChecks()
         lastDistanceToHomeM = rescueState.sensor.distanceToHomeM;
 
         if (secondsFlyingAway == 10) {
-            rescueState.failure = RESCUE_FLYAWAY;
+            //If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c
+            if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) {
+                //Try again with mag disabled
+                magForceDisable = true;
+                secondsFlyingAway = 0;
+            } else {
+                rescueState.failure = RESCUE_FLYAWAY;
+            }
         }
     }
 
@@ -414,7 +423,7 @@ static void sensorUpdate()
 // 3. GPS number of satellites is less than the minimum configured for GPS rescue.
 // Note: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth) and
 // is also independent of the gps_rescue_sanity_checks configuration
-static bool gpsRescueIsAvailable(void)
+static bool checkGPSRescueIsAvailable(void)
 {
     static uint32_t previousTimeUs = 0; // Last time LowSat was checked
     const uint32_t currentTimeUs = micros();
@@ -473,7 +482,7 @@ void updateGPSRescueState(void)
 
     sensorUpdate();
 
-    rescueState.isAvailable = gpsRescueIsAvailable();
+    rescueState.isAvailable = checkGPSRescueIsAvailable();
 
     switch (rescueState.phase) {
     case RESCUE_IDLE:
@@ -604,9 +613,14 @@ bool gpsRescueIsConfigured(void)
     return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);
 }
 
-bool isGPSRescueAvailable(void)
+bool gpsRescueIsAvailable(void)
 {
     return rescueState.isAvailable;
 }
+
+bool gpsRescueDisableMag(void)
+{
+    return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING));
+}
 #endif
 

src/main/flight/gps_rescue.h

@@ -34,6 +34,7 @@ typedef struct gpsRescue_s {
     uint16_t minRescueDth; //meters
     uint8_t sanityChecks;
     uint8_t allowArmingWithoutFix;
+    uint8_t useMag;
 } gpsRescueConfig_t;
 
 PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
@@ -46,4 +47,5 @@ void rescueNewGpsData(void);
 float gpsRescueGetYawRate(void);
 float gpsRescueGetThrottle(void);
 bool gpsRescueIsConfigured(void);
-bool isGPSRescueAvailable(void);
+bool gpsRescueIsAvailable(void);
+bool gpsRescueDisableMag(void);

src/main/flight/imu.c

@@ -38,6 +38,7 @@
 
 #include "fc/runtime_config.h"
 
+#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -441,7 +442,11 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     previousIMUUpdateTime = currentTimeUs;
 
 #ifdef USE_MAG
-    if (sensors(SENSOR_MAG) && compassIsHealthy()) {
+    if (sensors(SENSOR_MAG) && compassIsHealthy()
+#ifdef USE_GPS_RESCUE
+        && !gpsRescueDisableMag()
+#endif
+        ) {
         useMag = true;
     }
 #endif
@@ -452,14 +457,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
             courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
             useCOG = true;
         } else {
-            // If GPS rescue mode is active and we can use it, go for it.  When we're close to home we will
+            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-            // probably stop re calculating GPS heading data.  Other future modes can also use this extern
 
-            if (canUseGPSHeading) {
+            useCOG = true;
-                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-
-                useCOG = true;
-            }
         }
 
         if (useCOG && shouldInitializeGPSHeading()) {

src/main/interface/settings.c

@@ -861,6 +861,7 @@ const clivalue_t valueTable[] = {
     { "gps_rescue_min_sats",        VAR_UINT8  | MASTER_VALUE, .config.minmax = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
     { "gps_rescue_min_dth",         VAR_UINT16  | MASTER_VALUE, .config.minmax = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
     { "gps_rescue_allow_arming_without_fix", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
+    { "gps_rescue_use_mag",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
 #endif
 #endif
 

src/main/io/osd.c

@@ -1015,7 +1015,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
                ARMING_FLAG(ARMED) &&
                gpsRescueIsConfigured() &&
-               !isGPSRescueAvailable()) {
+               !gpsRescueIsAvailable()) {
                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC");
                 SET_BLINK(OSD_WARNINGS);
                 break;

src/test/unit/flight_imu_unittest.cc

@@ -245,4 +245,5 @@ int32_t baroCalculateAltitude(void) { return 0; }
 bool gyroGetAccumulationAverage(float *) { return false; }
 bool accGetAccumulationAverage(float *) { return false; }
 void mixerSetThrottleAngleCorrection(int) {};
+bool gpsRescueIsRunning(void) { return false; }
 }