From 25a499cc9a6c5ff0efcc06a43583ed40cd09b673 Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Thu, 27 Dec 2018 02:49:25 +0100 Subject: [PATCH] Mag heading optionally ignored while GPS Rescue is running If flyaway condition is met and a mag is in use, mag is disabled and countdown is reset Minor cleanup --- src/main/flight/gps_rescue.c | 22 ++++++++++++++++++---- src/main/flight/gps_rescue.h | 4 +++- src/main/flight/imu.c | 16 ++++++++-------- src/main/interface/settings.c | 1 + src/main/io/osd.c | 2 +- src/test/unit/flight_imu_unittest.cc | 1 + 6 files changed, 32 insertions(+), 14 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index b7faf1ae6..890b398a6 100644 --- a/src/main/flight/gps_rescue.c +++ b/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 diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 05b16a105..be0d4db13 100644 --- a/src/main/flight/gps_rescue.h +++ b/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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 5b17b7140..45ef0b698 100644 --- a/src/main/flight/imu.c +++ b/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 - // probably stop re calculating GPS heading data. Other future modes can also use this extern + courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - if (canUseGPSHeading) { - courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - - useCOG = true; - } + useCOG = true; } if (useCOG && shouldInitializeGPSHeading()) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 087a160ac..31b6f5041 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -851,6 +851,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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d7a24c9ec..3a49c48f0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -996,7 +996,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; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 401757beb..f97581ef7 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/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; } }