diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 93dec31d4e..650a21b658 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -279,7 +279,7 @@ void updateArmingStatus(void) #ifdef USE_GPS_RESCUE if (gpsRescueIsConfigured()) { - if (!gpsRescueConfig()->minSats || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) { + if (gpsRescueConfig()->allowArmingWithoutFix || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) { unsetArmingDisabled(ARMING_DISABLED_GPS); } else { setArmingDisabled(ARMING_DISABLED_GPS); @@ -447,8 +447,10 @@ void tryArm(void) lastArmingDisabledReason = 0; - //beep to indicate arming #ifdef USE_GPS + GPS_reset_home_position(); + + //beep to indicate arming if (featureIsEnabled(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { @@ -1119,7 +1121,6 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) } } - bool isFlipOverAfterCrashActive(void) { return flipOverAfterCrashActive; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 120234c74c..b7faf1ae66 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -138,7 +138,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .throttleHover = 1280, .sanityChecks = RESCUE_SANITY_ON, .minSats = 8, - .minRescueDth = 100 + .minRescueDth = 100, + .allowArmingWithoutFix = false, ); static uint16_t rescueThrottle; @@ -422,7 +423,7 @@ static bool gpsRescueIsAvailable(void) static bool noGPSfix = false; bool result = true; - if (!gpsIsHealthy() ) { + if (!gpsIsHealthy() || !STATE(GPS_FIX_HOME)) { return false; } @@ -483,6 +484,11 @@ void updateGPSRescueState(void) hoverThrottle = gpsRescueConfig()->throttleHover; } + if (!STATE(GPS_FIX_HOME)) { + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); + disarm(); + } + // Minimum distance detection. if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { rescueState.failure = RESCUE_TOO_CLOSE; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 7dad69fddd..05b16a105f 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -33,6 +33,7 @@ typedef struct gpsRescue_s { uint8_t minSats; uint16_t minRescueDth; //meters uint8_t sanityChecks; + uint8_t allowArmingWithoutFix; } gpsRescueConfig_t; PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 90d39c1828..a0fb4c4883 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -843,8 +843,9 @@ const clivalue_t valueTable[] = { { "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) }, { "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) }, { "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, - { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, + { "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) }, #endif #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index ab9676211f..2ee30534f5 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1271,6 +1271,39 @@ void GPS_calc_longitude_scaling(int32_t lat) GPS_scaleLonDown = cos_approx(rads); } +//////////////////////////////////////////////////////////////////////////////////// +// Calculate the distance flown and vertical speed from gps position data +// +static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) +{ + static int32_t lastCoord[2] = { 0, 0 }; + static int16_t lastAlt; + static int32_t lastMillis; + + int currentMillis = millis(); + + if (initialize) { + GPS_distanceFlownInCm = 0; + GPS_verticalSpeedInCmS = 0; + } else { + if (STATE(GPS_FIX_HOME)) { + // Only add up movement when speed is faster than minimum threshold + if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) { + uint32_t dist; + int32_t dir; + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir); + GPS_distanceFlownInCm += dist; + } + } + + GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis); + GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500.0f, 1500.0f); + } + lastCoord[LON] = gpsSol.llh.lon; + lastCoord[LAT] = gpsSol.llh.lat; + lastAlt = gpsSol.llh.altCm; + lastMillis = currentMillis; +} void GPS_reset_home_position(void) { @@ -1281,6 +1314,7 @@ void GPS_reset_home_position(void) // Set ground altitude ENABLE_STATE(GPS_FIX_HOME); } + GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize } //////////////////////////////////////////////////////////////////////////////////// @@ -1313,49 +1347,12 @@ void GPS_calculateDistanceAndDirectionToHome(void) } } -//////////////////////////////////////////////////////////////////////////////////// -// Calculate the distance flown from gps position data -// -static void GPS_calculateDistanceFlown(bool initialize) -{ - static int32_t lastCoord[2] = { 0, 0 }; - static int16_t lastAlt; - static int32_t lastMillis; - - int currentMillis = millis(); - - if (initialize) { - GPS_distanceFlownInCm = 0; - GPS_verticalSpeedInCmS = 0; - } else { - // Only add up movement when speed is faster than minimum threshold - if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) { - uint32_t dist; - int32_t dir; - GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir); - GPS_distanceFlownInCm += dist; - } - - GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis); - GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500.0f, 1500.0f); - } - lastCoord[LON] = gpsSol.llh.lon; - lastCoord[LAT] = gpsSol.llh.lat; - lastAlt = gpsSol.llh.altCm; - lastMillis = currentMillis; -} - void onGpsNewData(void) { if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { return; } - if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) { - GPS_reset_home_position(); - GPS_calculateDistanceFlown(true); - } - // Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; @@ -1395,7 +1392,7 @@ void onGpsNewData(void) GPS_calculateDistanceAndDirectionToHome(); if (ARMING_FLAG(ARMED)) { - GPS_calculateDistanceFlown(false); + GPS_calculateDistanceFlownVerticalSpeed(false); } #ifdef USE_GPS_RESCUE diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index ae16e74acc..fee5772f3a 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -616,7 +616,7 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit EXPECT_EQ(0, getArmingDisableFlags()); } -TEST(ArmingPreventionTest, Rescue) +TEST(ArmingPreventionTest, GPSRescueWithoutFixPreventsArm) { // given simulationFeatureFlags = 0; @@ -637,6 +637,120 @@ TEST(ArmingPreventionTest, Rescue) // and rxConfigMutable()->mincheck = 1050; + // given + rcData[THROTTLE] = 1000; + rcData[AUX1] = 1000; + rcData[AUX2] = 1000; + ENABLE_STATE(SMALL_ANGLE); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // arm + rcData[AUX1] = 1800; + + // when + tryArm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH|ARMING_DISABLED_GPS, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // disarm + rcData[AUX1] = 1000; + + // when + disarm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // receive GPS fix + ENABLE_STATE(GPS_FIX); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // arm + rcData[AUX1] = 1800; + + // when + tryArm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_TRUE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // disarm + rcData[AUX1] = 1000; + + // when + disarm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); +} + +TEST(ArmingPreventionTest, GPSRescueSwitchPreventsArm) +{ + // given + simulationFeatureFlags = 0; + simulationTime = 0; + gyroCalibDone = true; + gpsSol.numSat = 5; + ENABLE_STATE(GPS_FIX); + + // and + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXARM; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXGPSRESCUE; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + rcControlsInit(); + + // and + rxConfigMutable()->mincheck = 1050; + // given rcData[THROTTLE] = 1000; rcData[AUX1] = 1000;