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

Updates and feature additions to failsafe system.

Browse source 
- Added failsafe flightmode and rc control box.

To make failsafe procedure a separate flight mode and make it possible
to trigger failsafe with an AUX switch.

- Failsafe mode is activated when failsafe is active.

RC link lost is simulated with the failsafe AUX switch.
When NOT armed: failsafe switch to failsafe mode is shown in GUI (mode
tab).

- Activate failsafe mode with AUX switch.

- Prevent arming when failsafe via AUX switch is active (safety issue).

- Make failsafe disarm if motors armed and throttle was LOW (2D & 3D)
for `failsafe_throttle_low_delay` time (__JustDisarmEvent__).

Applied code changes to effectively add pull request: Make failsafe
disarm if motors armed and throttle low #717.

- Use failsafeIsMonitoring() to actually start monitoring.

- Added `failsafe_kill_switch` to code.

When set to 1 (0 is default), the failsafe switch will instantly disarm
(__KillswitchEvent__) instead of executing the landings procedure.
Arming is NOT locked after
this, so the craft could be re-armed if needed.
This is intended for racing quads where damage and danger must be
minimized in case of a pilot error.

- Added `failsafe_throttle_low_delay`, adapted documentation.

Used to adjust the time throttle level must have been LOW
to _only disarm_ instead of _full failsafe procedure_
(__JustDisarmEvent__).

- Updated the failsafe documentation.

- Re-enable arming at end of failsafe procedure.

At the end of a handled failsafe event, that means: auto-landing,
__JustDisarmEvent__ or __KillswitchEvent__, the RX link is monitored for
valid data.
Monitoring is a part of the failsafe handling, which means the craft is
still in failsafe mode while this is done.
Arming is re-enabled (allowed) when there is a valid RX link for more
then XX seconds, where XX depends on the handled event like this:
1. XX = 30 seconds after auto landing.
2. XX = 3 seconds after __JustDisarmEvent__.
3. XX = 0 seconds after __KillswitchEvent__.

NOTE: When armed via an AUX switch, you will have to switch to the
disarmed position at the very end to be able to re-arm.
The failsafe mode will not end until you do.

- __KillswitchEvent__ has now priority over __JustDisarmEvent__

- Apply rxfail values instantly when failsafe switch is ON

- Added missing cases to display.c

Show M when failsafe is monitoring for RX recovery (AND disarming when
armed with a switch).

===

Reworked the code from counter-based to time-based.

- AUX failsafe switch now has identical behavior to RX loss.

- Added RX failure and RX recovery timing.

- __KillswitchEvent__ skips RX failure detection delay (direct disarm).

===

[UNIT TESTS]

Adapted failsafe related unittests from counter-based to time-based

- Added failsafeOnValidDataFailed() to some tests

- Removed duplicate test setup from rc_controls_unittest.cc

- Removed magic numbers from rx_ranges_unittest.cc and rx_rx_unittest.cc

- Reworked all test-cases for flight_failsafe_unittest.cc
This commit is contained in:
ProDrone 2015-05-05 16:11:05 +02:00
parent f7530df974
commit f0681de53d
17 changed files with 542 additions and 251 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -23,10 +23,12 @@
#include "common/axis.h"
#include "rx/rx.h"
#include "drivers/system.h"
#include "io/beeper.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "flight/failsafe.h"
@ -47,10 +49,19 @@ static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
static void failsafeReset(void)
{
failsafeState.counter = 0;
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
failsafeState.landingShouldBeFinishedAt = 0;
failsafeState.receivingRxDataPeriod = 0;
failsafeState.receivingRxDataPeriodPreset = 0;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
/*
@ -62,10 +73,11 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig)
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
rxConfig = intialRxConfig;
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;
failsafeState.monitoring = false;
@ -77,13 +89,6 @@ failsafePhase_e failsafePhase()
return failsafeState.phase;
}
#define FAILSAFE_COUNTER_THRESHOLD 20
bool failsafeIsReceivingRxData(void)
{
return failsafeState.counter <= FAILSAFE_COUNTER_THRESHOLD;
}
bool failsafeIsMonitoring(void)
{
return failsafeState.monitoring;
@ -99,25 +104,17 @@ void failsafeStartMonitoring(void)
failsafeState.monitoring = true;
}
static bool failsafeHasTimerElapsed(void)
{
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
static bool failsafeShouldForceLanding(bool armed)
{
return failsafeHasTimerElapsed() && armed;
}
static bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
return (millis() > failsafeState.landingShouldBeFinishedAt);
}
static void failsafeActivate(void)
{
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.events++;
}
@ -130,27 +127,41 @@ static void failsafeApplyControlInput(void)
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
}
bool failsafeIsReceivingRxData(void)
{
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
}
void failsafeOnValidDataReceived(void)
{
if (failsafeState.counter > FAILSAFE_COUNTER_THRESHOLD)
failsafeState.counter -= FAILSAFE_COUNTER_THRESHOLD;
else
failsafeState.counter = 0;
failsafeState.validRxDataReceivedAt = millis();
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
}
}
void failsafeOnValidDataFailed(void)
{
failsafeState.validRxDataFailedAt = millis();
if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
}
void failsafeUpdateState(void)
{
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (receivingRxData) {
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
} else {
beeperMode = BEEPER_RX_LOST;
if (!failsafeIsMonitoring()) {
return;
}
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (!receivingRxData) {
beeperMode = BEEPER_RX_LOST;
}
bool reprocessState;
@ -159,50 +170,100 @@ void failsafeUpdateState(void)
switch (failsafeState.phase) {
case FAILSAFE_IDLE:
if (!receivingRxData && armed) {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
} else {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
}
reprocessState = true;
}
} else {
// When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
if (failsafeSwitchIsOn) {
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
} else {
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
}
// Throttle low period expired (= low long enough for JustDisarm)
failsafeState.throttleLowPeriod = 0;
}
break;
case FAILSAFE_RX_LOSS_DETECTED:
if (failsafeShouldForceLanding(armed)) {
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
// Stabilize, and set Throttle to specified level
failsafeActivate();
reprocessState = true;
}
reprocessState = true;
break;
case FAILSAFE_LANDING:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
if (armed) {
failsafeApplyControlInput();
beeperMode = BEEPER_RX_LOST_LANDING;
}
if (failsafeShouldHaveCausedLandingByNow() || !armed) {
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;
}
break;
case FAILSAFE_LANDED:
if (!armed) {
break;
}
// This will prevent the automatic rearm if failsafe shuts it down and prevents
// to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
ENABLE_ARMING_FLAG(PREVENT_ARMING);
failsafeState.active = false;
ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
mwDisarm();
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
reprocessState = true;
break;
case FAILSAFE_RX_LOSS_MONITORING:
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
if (receivingRxData) {
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);
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
} else {
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
}
break;
case FAILSAFE_RX_LOSS_RECOVERED:
// Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
// This is to prevent that JustDisarm is activated on the next iteration.
// Because that would have the effect of shutting down failsafe handling on intermittent connections.
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
reprocessState = true;
break;
default:
@ -214,11 +275,3 @@ void failsafeUpdateState(void)
beeper(beeperMode);
}
}
/**
* Should be called once when RX data is processed by the system.
*/
void failsafeOnRxCycleStarted(void)
{
failsafeState.counter++;
}