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

BUGFIX - Warning LED / arm prevention code fixes. 

The old code was using f.ACC_CALIBRATED to mean 'flash the warning led'
and 'prevent arming'
There was another flag called f.OK_TO_ARM which really meant 'prevent
arming after failsafe unless using a switch to arm' - this meant the
comment in the code was incorrect since it did not mention the switch.
There was code which every ~71 minutes would set f.ACC_CALIBRATED to
FALSE and would toggle the LED, even when flying.
There was code that was run during every cycle that did not need to be
run when armed.
There was code that was run during every cycle which did not need to be
run when waiting for calibration to finish.
The warning light was toggled regardless of actual accelerometer
calibration state.

This commit fixes all that and prevents the user from arming after a
failsafe landing until the system is reset, regardless of wether a
switch is used to arm or not.

This commit also flashes the warning led during barometer calibration
and will not let you arm until it has completed.

Add a 5 second delay on startup to failsafe to prevent failsafe from
activating in the case of TX/RX gear with long bind procedures before
they send out valid data.

Failsafe now only prevents re-arming if a landing was forced.

In the case when you power on your RX without your TX on the beeper
sounds and failsafe warning ligts are activated.  When the FC then
detects a signal the lights go off and you can then arm.
This commit is contained in:
Dominic Clifton 2014-05-22 21:31:15 +01:00
parent d1be2ed5e2
commit d6c1ce323d
8 changed files with 121 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
docs/Failsafe.md
View File

@ -14,6 +14,10 @@ It is possible to use both types at the same time and may be desirable. Flight
## Flight controller failsafe system
The failsafe system is not activated until 5 seconds after the flight controller boots up. This is to prevent failsafe from activating in the case of TX/RX gear with long bind procedures before they send out valid data.
After the failsafe has been forced a landing, the flight controller cannot be armed and has to be reset.
The failsafe system attempts to detect when your receiver looses signal. It then attempts to prevent your aircraft from flying away uncontrollably.
The failsafe is activated when:
@ -28,7 +32,6 @@ Failsafe delays are configured in 0.1 second steps.
1 step = 0.1sec
1 second = 10 steps
### `failsafe_delay`
Guard time for failsafe activation after signal lost.

31
src/failsafe.c
View File

@ -17,6 +17,8 @@
*
* failsafeInit() and useFailsafeConfig() can be called in any order.
* failsafeInit() should only be called once.
*
* enable() should be called after system initialisation.
*/
static failsafe_t failsafe;
@ -47,6 +49,7 @@ failsafe_t* failsafeInit(rxConfig_t *intialRxConfig)
failsafe.vTable = failsafeVTable;
failsafe.events = 0;
failsafe.enabled = false;
return &failsafe;
}
@ -56,6 +59,16 @@ bool isIdle(void)
return failsafe.counter == 0;
}
bool isEnabled(void)
{
return failsafe.enabled;
}
void enable(void)
{
failsafe.enabled = true;
}
bool hasTimerElapsed(void)
{
return failsafe.counter > (5 * failsafeConfig->failsafe_delay);
@ -73,8 +86,9 @@ bool shouldHaveCausedLandingByNow(void)
void failsafeAvoidRearm(void)
{
mwDisarm(); // This will prevent the automatic rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
// 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
f.PREVENT_ARMING = 1;
}
void onValidDataReceived(void)
@ -93,7 +107,14 @@ void updateState(void)
return;
}
if (!isEnabled()) {
reset();
return;
}
if (shouldForceLanding(f.ARMED)) { // Stabilize, and set Throttle to specified level
failsafeAvoidRearm();
for (i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec)
}
@ -102,7 +123,7 @@ void updateState(void)
}
if (shouldHaveCausedLandingByNow() || !f.ARMED) {
failsafeAvoidRearm();
mwDisarm();
}
}
@ -138,7 +159,9 @@ const failsafeVTable_t failsafeVTable[] = {
incrementCounter,
updateState,
isIdle,
failsafeCheckPulse
failsafeCheckPulse,
isEnabled,
enable
}
};

5
src/failsafe.h
View File

@ -1,5 +1,7 @@
#pragma once
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
typedef struct failsafeConfig_s {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
@ -17,6 +19,8 @@ typedef struct failsafeVTable_s {
void (*updateState)(void);
bool (*isIdle)(void);
void (*checkPulse)(uint8_t channel, uint16_t pulseDuration);
bool (*isEnabled)(void);
void (*enable)(void);
} failsafeVTable_t;
@ -25,6 +29,7 @@ typedef struct failsafe_s {
int16_t counter;
int16_t events;
bool enabled;
} failsafe_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);

6
src/flight_common.c
View File

@ -28,12 +28,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void mwDisarm(void)
{
if (f.ARMED)
f.ARMED = 0;
}
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
{
rollAndPitchTrims->trims.roll = 0;

1
src/flight_common.h
View File

@ -108,7 +108,6 @@ extern int32_t AltHold;
extern int32_t EstAlt;
extern int32_t vario;
void mwDisarm(void);
void setPIDController(int type);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void resetErrorAngle(void);

1
src/main.c
View File

@ -228,6 +228,7 @@ void init(void)
#endif
f.SMALL_ANGLE = 1;
f.PREVENT_ARMING = 0;
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly

109
src/mw.c
View File

@ -80,9 +80,47 @@ bool AccInflightCalibrationSavetoEEProm = false;
bool AccInflightCalibrationActive = false;
uint16_t InflightcalibratingA = 0;
bool isCalibrating()
{
#ifdef BARO
if (sensors(SENSOR_ACC) && !isBaroCalibrationComplete()) {
return false;
}
#endif
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
static uint32_t warningLedTimer = 0;
void enableWarningLed(uint32_t currentTime)
{
if (warningLedTimer != 0) {
return; // already enabled
}
warningLedTimer = currentTime + 500000;
LED0_ON;
}
void disableWarningLed(void)
{
warningLedTimer = 0;
LED0_OFF;
}
void updateWarningLed(uint32_t currentTime)
{
if (warningLedTimer && (int32_t)(currentTime - warningLedTimer) >= 0) {
LED0_TOGGLE;
warningLedTimer = warningLedTimer + 500000;
}
}
void annexCode(void)
{
static uint32_t calibratedAccTime;
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
@ -160,17 +198,31 @@ void annexCode(void)
buzzer(batteryWarningEnabled); // external buzzer routine that handles buzzer events globally now
if ((!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete())) { // Calibration phasis
LED0_TOGGLE;
if (f.ARMED) {
LED0_ON;
} else {
if (f.ACC_CALIBRATED)
LED0_OFF;
if (f.ARMED)
LED0_ON;
if (isCalibrating()) {
LED0_TOGGLE;
f.OK_TO_ARM = 0;
}
checkTelemetryState();
f.OK_TO_ARM = 1;
if (!f.ARMED && !f.SMALL_ANGLE) {
f.OK_TO_ARM = 0;
}
if (f.OK_TO_ARM) {
disableWarningLed();
} else {
enableWarningLed(currentTime);
}
updateWarningLed(currentTime);
}
checkTelemetryState();
#ifdef LEDRING
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
@ -181,15 +233,6 @@ void annexCode(void)
}
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLE) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
handleSerial();
@ -204,21 +247,32 @@ void annexCode(void)
// TODO MCU temp
}
}
static void mwArm(void)
void mwDisarm(void)
{
if (isGyroCalibrationComplete() && f.ACC_CALIBRATED) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
// TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
if (!f.ARMED) { // arm now!
if (f.ARMED)
f.ARMED = 0;
}
void mwArm(void)
{
if (f.OK_TO_ARM) {
if (f.ARMED) {
return;
}
if (!f.PREVENT_ARMING) {
f.ARMED = 1;
headFreeModeHold = heading;
return;
}
} else if (!f.ARMED) {
}
if (!f.ARMED) {
blinkLedAndSoundBeeper(2, 255, 1);
}
}
static void mwVario(void)
{
@ -256,6 +310,11 @@ void loop(void)
}
if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
failsafe->vTable->enable();
}
failsafe->vTable->updateState();
}
@ -428,8 +487,6 @@ void loop(void)
f.HORIZON_MODE = 0;
}
if ((rcOptions[BOXARM]) == 0)
f.OK_TO_ARM = 1;
if (f.ANGLE_MODE || f.HORIZON_MODE) {
LED1_ON;
} else {

2
src/runtime_config.h
View File

@ -30,8 +30,8 @@ extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT];
// FIXME some of these are flight modes, some of these are general status indicators
typedef struct flags_t {
uint8_t OK_TO_ARM;
uint8_t PREVENT_ARMING;
uint8_t ARMED;
uint8_t ACC_CALIBRATED;
uint8_t ANGLE_MODE;
uint8_t HORIZON_MODE;
uint8_t MAG_MODE;