From eadcf426501e20144a795cc333260ad093825f3d Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Wed, 8 Nov 2017 13:50:31 +0000 Subject: [PATCH 1/3] 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 | 11 +++- src/test/unit/arming_prevention_unittest.cc | 69 ++++++++++++++++++++- 2 files changed, 78 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 932db2abf..103de8f2c 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 1ce9075e5..828e31585 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) {} From 0315005445828969535e472365dfafca83c792fc Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Wed, 8 Nov 2017 18:52:00 +0000 Subject: [PATCH 2/3] Fix 3D arming checks for BOX3DDISABLE When FEATURE_3D is on and BOX3DDISABLE is on then the normal throttle arming check is performed. When FEATURE_3D is on and BOX3DDISABLE is off then the reduced 3D throttle arming check is performed. --- src/main/fc/fc_core.c | 7 +- src/main/fc/rc_controls.c | 9 +- src/test/unit/arming_prevention_unittest.cc | 107 +++++++++++++++++++- 3 files changed, 115 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 103de8f2c..cc9cd0297 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -215,10 +215,13 @@ 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)); + 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)); + bool ignoreThrottle = feature(FEATURE_3D) + && !IS_RC_MODE_ACTIVE(BOX3DDISABLE) + && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); // If arming is disabled and the ARM switch is on if (isArmingDisabled() diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index ddfcb04ec..142b1ddc2 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -110,15 +110,14 @@ throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D)) { if (IS_RC_MODE_ACTIVE(BOX3DDISABLE) || isModeActivationConditionPresent(BOX3DONASWITCH)) { - if (rcData[THROTTLE] < rxConfig()->mincheck) + if (rcData[THROTTLE] < rxConfig()->mincheck) { return THROTTLE_LOW; + } } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) { return THROTTLE_LOW; } - } else { - if (rcData[THROTTLE] < rxConfig()->mincheck) { - return THROTTLE_LOW; - } + } else if (rcData[THROTTLE] < rxConfig()->mincheck) { + return THROTTLE_LOW; } return THROTTLE_HIGH; diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 828e31585..787013b49 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -161,6 +161,7 @@ TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch) EXPECT_EQ(0, getArmingDisableFlags()); EXPECT_FALSE(isArmingDisabled()); } + TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed) { // given @@ -347,7 +348,7 @@ TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed) EXPECT_EQ(0, getArmingDisableFlags()); } -TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadbandFromNegativeSide) +TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband) { // given simulationFeatureFlags = FEATURE_3D; // Using 3D mode @@ -413,6 +414,110 @@ TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadbandFromNe EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionApplies) +{ + // 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); + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOX3DDISABLE; + 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; + rxConfigMutable()->midrc = 1500; + flight3DConfigMutable()->deadband3d_throttle = 5; + + // and + // safe throttle value for 3D mode + rcData[THROTTLE] = 1500; + ENABLE_STATE(SMALL_ANGLE); + + // and + // RX has no link to radio + simulationHaveRx = false; + + // and + // arm channel has a safe default value + rcData[4] = 1100; + + // and + // disable 3D mode is off (i.e. 3D mode is on) + rcData[5] = 1100; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + // ok to arm in 3D mode + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + + // given + // disable 3D mode + rcData[5] = 1800; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + // ok to arm in 3D mode + 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 | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags()); + + // given + // throttle moved low + rcData[THROTTLE] = 1000; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags()); + + // given + // arm switch turned off + rcData[4] = 1000; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); +} + // STUBS extern "C" { uint32_t micros(void) { return simulationTime; } From c97fbd04b8de5a073d241ffa70ac1ea08bff7f35 Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Wed, 8 Nov 2017 19:00:52 +0000 Subject: [PATCH 3/3] Fix 3D arming checks when using switched 3D mode When FEATURE_3D is on and BOX3DONASWITCH is configured the "standard" throttle arming condition is used. --- src/main/fc/fc_core.c | 1 + src/test/unit/arming_prevention_unittest.cc | 104 ++++++++++++++++++-- 2 files changed, 97 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index cc9cd0297..2a9e1aec5 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -221,6 +221,7 @@ void updateArmingStatus(void) /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */ bool ignoreThrottle = feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE) + && !isModeActivationConditionPresent(BOX3DONASWITCH) && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); // If arming is disabled and the ARM switch is on diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 787013b49..0814908f9 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -369,10 +369,7 @@ TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband) // and rcData[THROTTLE] = 1400; ENABLE_STATE(SMALL_ANGLE); - - // and - // RX has no link to radio - simulationHaveRx = false; + simulationHaveRx = true; // and // arm channel has a safe default value @@ -441,10 +438,7 @@ TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionAp // safe throttle value for 3D mode rcData[THROTTLE] = 1500; ENABLE_STATE(SMALL_ANGLE); - - // and - // RX has no link to radio - simulationHaveRx = false; + simulationHaveRx = true; // and // arm channel has a safe default value @@ -518,6 +512,100 @@ TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionAp EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingConditionApplies) +{ + // 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); + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOX3DONASWITCH; + 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; + + // and + rcData[THROTTLE] = 1000; + ENABLE_STATE(SMALL_ANGLE); + simulationHaveRx = true; + + // and + // arm channel has a safe default value + rcData[4] = 1100; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + // ok to arm in 3D mode + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + + // given + // raise throttle to unsafe position + rcData[THROTTLE] = 1500; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + // ok to arm in 3D mode + 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 | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags()); + + // given + // throttle moved low + rcData[THROTTLE] = 1000; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags()); + + // given + // arm switch turned off + rcData[4] = 1000; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(isUsingSticksForArming()); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); +} + // STUBS extern "C" { uint32_t micros(void) { return simulationTime; }