diff --git a/src/main/fc/core.c b/src/main/fc/core.c index c365502a3..e017c49cf 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -282,6 +282,11 @@ void updateArmingStatus(void) } else { setArmingDisabled(ARMING_DISABLED_GPS); } + if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) { + setArmingDisabled(ARMING_DISABLED_RESC); + } else { + unsetArmingDisabled(ARMING_DISABLED_RESC); + } } #endif @@ -828,7 +833,7 @@ bool processRx(timeUs_t currentTimeUs) } #ifdef USE_GPS_RESCUE - if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { + if (ARMING_FLAG(ARMED) && (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE))) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index a3164601c..bd9f2ce2f 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -53,7 +53,8 @@ const char *armingDisableFlagNames[]= { "MSP", "PARALYZE", "GPS", - "ARMSWITCH" + "RESCUE SW", + "ARMSWITCH", }; static armingDisableFlags_e armingDisableFlags = 0; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index fb0f787dc..72b75e2bd 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -57,10 +57,11 @@ typedef enum { ARMING_DISABLED_MSP = (1 << 16), ARMING_DISABLED_PARALYZE = (1 << 17), ARMING_DISABLED_GPS = (1 << 18), - ARMING_DISABLED_ARM_SWITCH = (1 << 19), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_RESC = (1 << 19), + ARMING_DISABLED_ARM_SWITCH = (1 << 20), // Needs to be the last element, since it's always activated if one of the others is active when arming } armingDisableFlags_e; -#define ARMING_DISABLE_FLAGS_COUNT 20 +#define ARMING_DISABLE_FLAGS_COUNT 21 extern const char *armingDisableFlagNames[ARMING_DISABLE_FLAGS_COUNT]; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index c8025414f..91c8043ac 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -427,17 +427,17 @@ void updateGPSRescueState(void) hoverThrottle = gpsRescueConfig()->throttleHover; } - // Minimum distance detection. Disarm regardless of sanity check configuration. Rescue too close is never a good idea. + // Minimum distance detection. if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { - // Never allow rescue mode to engage as a failsafe when too close or when disarmed. - if (rescueState.isFailsafe || !ARMING_FLAG(ARMED)) { - rescueState.failure = RESCUE_TOO_CLOSE; + rescueState.failure = RESCUE_TOO_CLOSE; + + // Never allow rescue mode to engage as a failsafe when too close. + if (rescueState.isFailsafe) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(); - } else { - // Leave it up to the sanity check setting - rescueState.failure = RESCUE_TOO_CLOSE; } + + // When not in failsafe mode: leave it up to the sanity check setting. } rescueState.phase = RESCUE_ATTAIN_ALT; diff --git a/src/test/Makefile b/src/test/Makefile index ec7b62d62..cc6482946 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -39,6 +39,9 @@ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/common/bitarray.c +arming_prevention_unittest_DEFINES := \ + USE_GPS_RESCUE= + atomic_unittest_SRC := \ $(USER_DIR)/build/atomic.c \ $(TEST_DIR)/atomic_unittest_c.c diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 7c45f05af..de430d83e 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -43,6 +43,7 @@ extern "C" { #include "sensors/acceleration.h" #include "sensors/gyro.h" #include "telemetry/telemetry.h" + #include "flight/gps_rescue.h" PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); @@ -54,6 +55,7 @@ extern "C" { PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); + PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -612,6 +614,118 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, Rescue) +{ + // given + simulationFeatureFlags = 0; + simulationTime = 0; + gyroCalibDone = true; + + // 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); + useRcControlsConfig(NULL); + + // and + rxConfigMutable()->mincheck = 1050; + + // given + rcData[THROTTLE] = 1000; + rcData[AUX1] = 1000; + rcData[AUX2] = 1800; // Start out with rescue enabled + ENABLE_STATE(SMALL_ANGLE); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_RESC, getArmingDisableFlags()); + EXPECT_TRUE(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_RESC, getArmingDisableFlags()); + EXPECT_TRUE(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_RESC, getArmingDisableFlags()); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE)); + + // given + // disable Rescue + rcData[AUX2] = 1000; + + // 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, ParalyzeOnAtBoot) { // given