Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Reworked arming conditions.

Browse source
This commit is contained in:
mikeller 2017-06-19 00:40:59 +12:00
parent fb0429597f
commit 31df82db2d
20 changed files with 149 additions and 107 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/cms/cms.c
View file

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

2
src/main/fc/cli.c
View file

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

63
src/main/fc/fc_core.c
View file

@ -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,16 +213,12 @@ 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
@ -231,17 +244,17 @@ void mwArm(void)
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
beeper(BEEPER_ARMING_GPS_FIX);
else
} else {
beeper(BEEPER_ARMING);
}
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
@ -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

6
src/main/fc/fc_core.h
View file

@ -40,11 +40,11 @@ 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);

1
src/main/fc/fc_init.c
View file

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

2
src/main/fc/fc_tasks.c
View file

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

15
src/main/fc/rc_controls.c
View file

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

17
src/main/fc/runtime_config.c
View file

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

23
src/main/fc/runtime_config.h
View file

@ -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),

7
src/main/flight/failsafe.c
View file

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

2
src/main/io/ledstrip.c
View file

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

4
src/main/io/osd.c
View file

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

9
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

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

5
src/main/telemetry/smartport.c
View file

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

1
src/test/Makefile
View file

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

3
src/test/unit/cms_unittest.cc
View file

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

37
src/test/unit/flight_failsafe_unittest.cc
View file

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

2
src/test/unit/ledstrip_unittest.cc
View file

@ -382,4 +382,6 @@ bool sensors(uint32_t mask)
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
bool isArmingDisabled(void) { return false; }
}

3
src/test/unit/osd_unittest.cc
View file

@ -574,4 +574,7 @@ extern "C" {
UNUSED(pDisplay);
return false;
}
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
}

4
src/test/unit/rc_controls_unittest.cc
View file

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