diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 0964acca9b..fc8eb1249e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -574,7 +574,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) return; cmsInMenu = true; currentCtx = (cmsCtx_t){ &menuMain, 0, 0 }; - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_CMS_MENU); } else { // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); @@ -642,7 +642,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) systemReset(); } - ENABLE_ARMING_FLAG(OK_TO_ARM); + unsetArmingDisabled(ARMING_DISABLED_CMS_MENU); return 0; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68fb5f4042..ea06814106 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3494,7 +3494,7 @@ void cliEnter(serialPort_t *serialPort) #endif cliPrompt(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); + setArmingDisabled(ARMING_DISABLED_CLI); } void cliInit(const serialConfig_t *serialConfig) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 50e126a398..3aa78b71dd 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveConfigAndNotify(); } -bool isCalibrating() +static bool isCalibrating() { #ifdef BARO if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { @@ -141,35 +141,52 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -void updateLEDs(void) +void updateArmingStatus(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { - ENABLE_ARMING_FLAG(OK_TO_ARM); + if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { + setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); + } else { + unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); + } + + if (calculateThrottleStatus() != THROTTLE_LOW) { + setArmingDisabled(ARMING_DISABLED_THROTTLE); + } else { + unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } if (!STATE(SMALL_ANGLE)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_ANGLE); + } else { + unsetArmingDisabled(ARMING_DISABLED_ANGLE); } - if (isCalibrating() || (averageSystemLoadPercent > 100)) { - warningLedFlash(); - DISABLE_ARMING_FLAG(OK_TO_ARM); + if (averageSystemLoadPercent > 100) { + setArmingDisabled(ARMING_DISABLED_LOAD); } else { - if (ARMING_FLAG(OK_TO_ARM)) { - warningLedDisable(); - } else { - warningLedFlash(); - } + unsetArmingDisabled(ARMING_DISABLED_LOAD); + } + + if (isCalibrating()) { + setArmingDisabled(ARMING_DISABLED_CALIBRATING); + } else { + unsetArmingDisabled(ARMING_DISABLED_CALIBRATING); + } + + if (isArmingDisabled()) { + warningLedFlash(); + } else { + warningLedDisable(); } warningLedUpdate(); } } -void mwDisarm(void) +void disarm(void) { armingCalibrationWasInitialised = false; @@ -186,7 +203,7 @@ void mwDisarm(void) } } -void mwArm(void) +void tryArm(void) { static bool firstArmingCalibrationWasCompleted; @@ -196,51 +213,47 @@ void mwArm(void) firstArmingCalibrationWasCompleted = true; } - if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated + updateArmingStatus(); - if (ARMING_FLAG(OK_TO_ARM)) { + if (!isArmingDisabled()) { if (ARMING_FLAG(ARMED)) { return; } - if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { - return; - } - if (!ARMING_FLAG(PREVENT_ARMING)) { #ifdef USE_DSHOT - if (!feature(FEATURE_3D)) { - //TODO: Use BOXDSHOTREVERSE here - if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - reverseMotors = false; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); - } - } else { - reverseMotors = true; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); - } + if (!feature(FEATURE_3D)) { + //TODO: Use BOXDSHOTREVERSE here + if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); + } + } else { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); } } + } #endif - ENABLE_ARMING_FLAG(ARMED); - ENABLE_ARMING_FLAG(WAS_EVER_ARMED); - headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + ENABLE_ARMING_FLAG(ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero - //beep to indicate arming + //beep to indicate arming #ifdef GPS - if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) - beeper(BEEPER_ARMING_GPS_FIX); - else - beeper(BEEPER_ARMING); -#else + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { + beeper(BEEPER_ARMING_GPS_FIX); + } else { beeper(BEEPER_ARMING); + } +#else + beeper(BEEPER_ARMING); #endif - return; - } + return; } if (!ARMING_FLAG(ARMED)) { @@ -315,7 +328,7 @@ void processRx(timeUs_t currentTimeUs) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) - mwDisarm(); + disarm(); } updateRSSI(currentTimeUs); @@ -364,7 +377,7 @@ void processRx(timeUs_t currentTimeUs) && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over - mwDisarm(); + disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index baba693d5c..8bc8b42531 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -40,12 +40,12 @@ union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); -void mwDisarm(void); -void mwArm(void); +void disarm(void); +void tryArm(void); void processRx(timeUs_t currentTimeUs); -void updateLEDs(void); +void updateArmingStatus(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -bool isMotorsReversed(void); \ No newline at end of file +bool isMotorsReversed(void); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 783c0f956d..780ba0e413 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -663,7 +663,6 @@ void init(void) timerStart(); ENABLE_STATE(SMALL_ANGLE); - DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0131892928..209a746650 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -151,7 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif - updateLEDs(); + updateArmingStatus(); #ifdef BARO if (sensors(SENSOR_BARO)) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 259cab256f..f904457039 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -144,11 +144,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX - if (throttleStatus == THROTTLE_LOW) { - if (ARMING_FLAG(OK_TO_ARM)) { - mwArm(); - } - } + tryArm(); } else { // Disarming via ARM BOX @@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus) rcDisarmTicks++; if (rcDisarmTicks > 3) { if (armingConfig()->disarm_kill_switch) { - mwDisarm(); + disarm(); } else if (throttleStatus == THROTTLE_LOW) { - mwDisarm(); + disarm(); } } } @@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (ARMING_FLAG(ARMED)) - mwDisarm(); + disarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat @@ -233,7 +229,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW - mwArm(); + tryArm(); + return; } } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 889f612e15..40a92eb034 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -29,6 +29,23 @@ uint16_t flightModeFlags = 0; static uint32_t enabledSensors = 0; +static armingDisableFlags_e armingDisableFlags = 0; + +void setArmingDisabled(armingDisableFlags_e flag) +{ + armingDisableFlags = armingDisableFlags | flag; +} + +void unsetArmingDisabled(armingDisableFlags_e flag) +{ + armingDisableFlags = armingDisableFlags & ~flag; +} + +bool isArmingDisabled() +{ + return armingDisableFlags; +} + /** * Enables the given flight mode. A beep is sounded if the flight mode * has changed. Returns the new 'flightModeFlags' value. diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 6311050c92..ffa3604ffd 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -19,10 +19,8 @@ // FIXME some of these are flight modes, some of these are general status indicators typedef enum { - OK_TO_ARM = (1 << 0), - PREVENT_ARMING = (1 << 1), - ARMED = (1 << 2), - WAS_EVER_ARMED = (1 << 3) + ARMED = (1 << 0), + WAS_EVER_ARMED = (1 << 1) } armingFlag_e; extern uint8_t armingFlags; @@ -31,6 +29,23 @@ extern uint8_t armingFlags; #define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask)) #define ARMING_FLAG(mask) (armingFlags & (mask)) +typedef enum { + ARMING_DISABLED_FAILSAFE = (1 << 0), + ARMING_DISABLED_BOXFAILSAFE = (1 << 1), + ARMING_DISABLED_THROTTLE = (1 << 2), + ARMING_DISABLED_ANGLE = (1 << 3), + ARMING_DISABLED_LOAD = (1 << 4), + ARMING_DISABLED_CALIBRATING = (1 << 5), + ARMING_DISABLED_CLI = (1 << 6), + ARMING_DISABLED_CMS_MENU = (1 << 7), + ARMING_DISABLED_OSD_MENU = (1 << 8), + ARMING_DISABLED_BST = (1 << 9), +} armingDisableFlags_e; + +void setArmingDisabled(armingDisableFlags_e flag); +void unsetArmingDisabled(armingDisableFlags_e flag); +bool isArmingDisabled(void); + typedef enum { ANGLE_MODE = (1 << 0), HORIZON_MODE = (1 << 1), diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a9c9e9c877..445d260c49 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -30,6 +30,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -261,8 +262,8 @@ void failsafeUpdateState(void) break; case FAILSAFE_LANDED: - ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link - mwDisarm(); + setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link + disarm(); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; reprocessState = true; @@ -274,7 +275,7 @@ void failsafeUpdateState(void) if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { - DISABLE_ARMING_FLAG(PREVENT_ARMING); + unsetArmingDisabled(ARMING_DISABLED_FAILSAFE); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 6baa47362d..4bd3b4eb6c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -541,7 +541,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) warningFlags |= 1 << WARNING_LOW_BATTERY; if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) warningFlags |= 1 << WARNING_FAILSAFE; - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + if (!ARMING_FLAG(ARMED) && isArmingDisabled()) warningFlags |= 1 << WARNING_ARMING_DISABLED; } *timer += HZ_TO_US(10); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 60c572b4c9..afb3ea9d7d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1143,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs) #ifdef CMS // do not allow ARM if we are in menu if (displayIsGrabbed(osdDisplayPort)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_OSD_MENU); + } else { + unsetArmingDisabled(ARMING_DISABLED_OSD_MENU); } #endif } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index ca870492eb..9c93858bd8 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -596,12 +596,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) isRebootScheduled = true; break; case BST_DISARM: - if (ARMING_FLAG(ARMED)) - mwDisarm(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); + if (ARMING_FLAG(ARMED)) { + disarm(); + } + setArmingDisabled(ARMING_DISABLED_BST); break; case BST_ENABLE_ARM: - DISABLE_ARMING_FLAG(PREVENT_ARMING); + unsetArmingDisabled(ARMING_DISABLED_BST); break; case BST_SET_DEADBAND: rcControlsConfigMutable()->alt_hold_deadband = bstRead8(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 08f85752c2..3f06e00397 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -698,10 +698,11 @@ void handleSmartPortTelemetry(void) // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer - if (ARMING_FLAG(OK_TO_ARM)) + if (!isArmingDisabled()) { tmpi += 1; - if (ARMING_FLAG(PREVENT_ARMING)) + } else { tmpi += 2; + } if (ARMING_FLAG(ARMED)) tmpi += 4; diff --git a/src/test/Makefile b/src/test/Makefile index 4776ac9dfd..a30ab5cfda 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -70,6 +70,7 @@ encoding_unittest_SRC := \ flight_failsafe_unittest_SRC := \ $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/flight/failsafe.c diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index a61f2a4dfd..c133cde63c 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -30,6 +30,7 @@ extern "C" { #include "target.h" #include "cms/cms.h" #include "cms/cms_types.h" + #include "fc/runtime_config.h" void cmsMenuOpen(void); long cmsMenuBack(displayPort_t *pDisplay); uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key); @@ -141,4 +142,6 @@ void saveConfigAndNotify(void) {} void stopMotors(void) {} void stopPwmAllMotors(void) {} void systemReset(void) {} +void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } +void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5edb8c45de..219db6cbc1 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -49,7 +49,6 @@ extern "C" { #include "gtest/gtest.h" uint32_t testFeatureMask = 0; -uint16_t flightModeFlags = 0; uint16_t testMinThrottle = 0; throttleStatus_e throttleStatus = THROTTLE_HIGH; @@ -203,7 +202,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given failsafeOnValidDataFailed(); // set last invalid sample at current time @@ -217,7 +216,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time @@ -230,7 +229,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -269,7 +268,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given failsafeOnValidDataFailed(); // set last invalid sample at current time @@ -283,7 +282,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time @@ -296,7 +295,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -325,7 +324,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // then EXPECT_EQ(true, failsafeIsActive()); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); @@ -342,7 +341,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // then EXPECT_EQ(true, failsafeIsActive()); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); @@ -357,7 +356,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -406,14 +405,13 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } // STUBS extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -uint8_t armingFlags; float rcCommand[4]; int16_t debug[DEBUG16_VALUE_COUNT]; bool isUsingSticksToArm = true; @@ -437,7 +435,7 @@ bool feature(uint32_t mask) { return (mask & testFeatureMask); } -void mwDisarm(void) { +void disarm(void) { callCounts[COUNTER_MW_DISARM]++; } @@ -445,18 +443,6 @@ void beeper(beeperMode_e mode) { UNUSED(mode); } -uint16_t enableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags |= (mask); - return flightModeFlags; -} - -uint16_t disableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags &= ~(mask); - return flightModeFlags; -} - uint16_t getCurrentMinthrottle(void) { return testMinThrottle; @@ -467,4 +453,5 @@ bool isUsingSticksForArming(void) return isUsingSticksToArm; } +void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); } } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 058cf2131e..7bb59caaf4 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -382,4 +382,6 @@ bool sensors(uint32_t mask) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {}; +bool isArmingDisabled(void) { return false; } + } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 57c94cab66..de1524b111 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -574,4 +574,7 @@ extern "C" { UNUSED(pDisplay); return false; } + + void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } + void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 5cf1c57e6b..75f8e8358d 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -679,8 +679,8 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} bool feature(uint32_t) { return false;} bool sensors(uint32_t) { return false;} -void mwArm(void) {} -void mwDisarm(void) {} +void tryArm(void) {} +void disarm(void) {} void dashboardDisablePageCycling() {} void dashboardEnablePageCycling() {}