From d6c1ce323d5f185ee4585f5f7c00b92c603446e5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 May 2014 21:31:15 +0100 Subject: [PATCH] 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. --- docs/Failsafe.md | 5 +- src/failsafe.c | 31 ++++++++++-- src/failsafe.h | 5 ++ src/flight_common.c | 6 --- src/flight_common.h | 1 - src/main.c | 1 + src/mw.c | 109 ++++++++++++++++++++++++++++++++----------- src/runtime_config.h | 2 +- 8 files changed, 121 insertions(+), 39 deletions(-) diff --git a/docs/Failsafe.md b/docs/Failsafe.md index 65332638cc..56a7181352 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -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. diff --git a/src/failsafe.c b/src/failsafe.c index 3422e27ccb..c6fcdde7c5 100644 --- a/src/failsafe.c +++ b/src/failsafe.c @@ -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 } }; diff --git a/src/failsafe.h b/src/failsafe.h index 579ae83474..6a1849c621 100644 --- a/src/failsafe.h +++ b/src/failsafe.h @@ -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); diff --git a/src/flight_common.c b/src/flight_common.c index b1320a4c63..408cba1a4c 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -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; diff --git a/src/flight_common.h b/src/flight_common.h index 2f1c4fc161..61e481b180 100644 --- a/src/flight_common.h +++ b/src/flight_common.h @@ -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); diff --git a/src/main.c b/src/main.c index 9ff37f71f7..69d515cd78 100755 --- a/src/main.c +++ b/src/main.c @@ -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 diff --git a/src/mw.c b/src/mw.c index cfd0136b36..19a8317fb2 100755 --- a/src/mw.c +++ b/src/mw.c @@ -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 { diff --git a/src/runtime_config.h b/src/runtime_config.h index 8e42ec91df..2d8b37aeb8 100644 --- a/src/runtime_config.h +++ b/src/runtime_config.h @@ -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;