Fix 3D arming checks for throttle value

When FEATURE_3D is on the arm switch does not need to be toggled when
the throttle returns to a valid value (i.e. in the 3D deadband) from
either direction.

This allows the previous "arm and slowly raise throttle" arming
behaviour for 3D flight.

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);

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) {}