Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Respect USE_MAG in GPS rescue code (#8136)

Browse source 
Respect USE_MAG in GPS rescue code
This commit is contained in:
Michael Keller 2019-05-02 09:15:28 +12:00 • committed by GitHub
commit 9898dd4378
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 15 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -905,8 +905,10 @@ const clivalue_t valueTable[] = {
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
{ "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, { "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 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_allow_arming_without_fix", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
#ifdef USE_MAG
{ "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) }, { "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif #endif
#endif
#endif #endif
{ "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) }, { "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },

15
src/main/flight/gps_rescue.c
View file

@ -120,6 +120,12 @@ typedef struct {
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle #define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
#define GPS_RESCUE_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed #define GPS_RESCUE_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed
#ifdef USE_MAG
#define GPS_RESCUE_USE_MAG true
#else
#define GPS_RESCUE_USE_MAG false
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1); PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
@ -141,7 +147,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minSats = 8, .minSats = 8,
.minRescueDth = 100, .minRescueDth = 100,
.allowArmingWithoutFix = false, .allowArmingWithoutFix = false,
.useMag = true, .useMag = GPS_RESCUE_USE_MAG,
.targetLandingAltitudeM = 5, .targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10, .targetLandingDistanceM = 10,
); );
@ -373,12 +379,15 @@ static void performSanityChecks()
lastDistanceToHomeM = rescueState.sensor.distanceToHomeM; lastDistanceToHomeM = rescueState.sensor.distanceToHomeM;
if (secondsFlyingAway == 10) { if (secondsFlyingAway == 10) {
#ifdef USE_MAG
//If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c //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) { if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) {
//Try again with mag disabled //Try again with mag disabled
magForceDisable = true; magForceDisable = true;
secondsFlyingAway = 0; secondsFlyingAway = 0;
} else { } else
#endif
{
rescueState.failure = RESCUE_FLYAWAY; rescueState.failure = RESCUE_FLYAWAY;
} }
} }
@ -647,9 +656,11 @@ bool gpsRescueIsDisabled(void)
return (!STATE(GPS_FIX_HOME)); return (!STATE(GPS_FIX_HOME));
} }
#ifdef USE_MAG
bool gpsRescueDisableMag(void) bool gpsRescueDisableMag(void)
{ {
return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING)); return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING));
} }
#endif #endif
#endif