diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 932db2abfb..103de8f2cf 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -214,8 +214,17 @@ void updateArmingStatus(void) } if (!isUsingSticksForArming()) { + /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */ + bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING)); + + /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */ + bool ignoreThrottle = feature(FEATURE_3D) && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); + // If arming is disabled and the ARM switch is on - if (isArmingDisabled() && !(armingConfig()->gyro_cal_on_first_arm && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING))) && IS_RC_MODE_ACTIVE(BOXARM)) { + if (isArmingDisabled() + && !ignoreGyro + && !ignoreThrottle + && IS_RC_MODE_ACTIVE(BOXARM)) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); } else if (!IS_RC_MODE_ACTIVE(BOXARM)) { unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 1ce9075e5a..828e31585e 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -65,6 +65,7 @@ extern "C" { bool cmsInMenu = false; } +uint32_t simulationFeatureFlags = 0; uint32_t simulationTime = 0; bool gyroCalibDone = false; bool simulationHaveRx = false; @@ -346,13 +347,79 @@ TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed) EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadbandFromNegativeSide) +{ + // given + simulationFeatureFlags = FEATURE_3D; // Using 3D mode + simulationTime = 30e6; // 30 seconds after boot + 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); + useRcControlsConfig(NULL); + + // and + rxConfigMutable()->midrc = 1500; + flight3DConfigMutable()->deadband3d_throttle = 5; + + // and + rcData[THROTTLE] = 1400; + ENABLE_STATE(SMALL_ANGLE); + + // and + // RX has no link to radio + simulationHaveRx = false; + + // and + // arm channel has a safe default value + rcData[4] = 1100; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags()); + + // given + // attempt to arm + rcData[4] = 1800; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags()); + + // given + // throttle moved to centre + rcData[THROTTLE] = 1496; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); +} + // STUBS extern "C" { uint32_t micros(void) { return simulationTime; } uint32_t millis(void) { return micros() / 1000; } bool rxIsReceivingSignal(void) { return simulationHaveRx; } - bool feature(uint32_t) { return false; } + bool feature(uint32_t f) { return simulationFeatureFlags & f; } void warningLedFlash(void) {} void warningLedDisable(void) {} void warningLedUpdate(void) {}