rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4516 from DanNixon/fix_3d_arm_checks 

Improve 3D arming checks
This commit is contained in:
Michael Keller 2017-11-11 11:24:44 +13:00 committed by GitHub
parent 8b334e5674 c97fbd04b8
commit 60bce27e66
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 279 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/main/fc/fc_core.c
View File

@ -214,8 +214,21 @@ 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)
&& !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
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);

9
src/main/fc/rc_controls.c
View File

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

262
src/test/unit/arming_prevention_unittest.cc
View File

@ -65,6 +65,7 @@ extern "C" {
bool cmsInMenu = false;
}
uint32_t simulationFeatureFlags = 0;
uint32_t simulationTime = 0;
bool gyroCalibDone = false;
bool simulationHaveRx = false;
@ -160,6 +161,7 @@ TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch)
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(isArmingDisabled());
}
TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed)
{
// given
@ -346,13 +348,271 @@ TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed)
EXPECT_EQ(0, getArmingDisableFlags());
}
TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband)
{
// 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);
simulationHaveRx = true;
// 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());
}
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);
simulationHaveRx = true;
// 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());
}
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; }
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) {}