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

Remove failsafe vtable usage.

This commit is contained in:
Dominic Clifton 2015-03-09 22:19:09 +00:00
parent 60a95f1d22
commit e40a3663d2
8 changed files with 88 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
src/main/flight/failsafe.c
View File

@ -38,17 +38,15 @@
* enable() should be called after system initialisation.
*/
static failsafe_t failsafe;
static failsafeState_t failsafeState;
static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
const failsafeVTable_t failsafeVTable[];
void reset(void)
void failsafeReset(void)
{
failsafe.counter = 0;
failsafeState.counter = 0;
}
/*
@ -57,129 +55,118 @@ void reset(void)
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
{
failsafeConfig = failsafeConfigToUse;
reset();
failsafeReset();
}
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig)
failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
{
rxConfig = intialRxConfig;
failsafe.vTable = failsafeVTable;
failsafe.events = 0;
failsafe.enabled = false;
failsafeState.events = 0;
failsafeState.enabled = false;
return &failsafe;
return &failsafeState;
}
bool isIdle(void)
bool failsafeIsIdle(void)
{
return failsafe.counter == 0;
return failsafeState.counter == 0;
}
bool isEnabled(void)
bool failsafeIsEnabled(void)
{
return failsafe.enabled;
return failsafeState.enabled;
}
void enable(void)
void failsafeEnable(void)
{
failsafe.enabled = true;
failsafeState.enabled = true;
}
bool hasTimerElapsed(void)
bool failsafeHasTimerElapsed(void)
{
return failsafe.counter > (5 * failsafeConfig->failsafe_delay);
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
bool shouldForceLanding(bool armed)
bool failsafeShouldForceLanding(bool armed)
{
return hasTimerElapsed() && armed;
return failsafeHasTimerElapsed() && armed;
}
bool shouldHaveCausedLandingByNow(void)
bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafe.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
}
void failsafeAvoidRearm(void)
static void failsafeAvoidRearm(void)
{
// 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);
}
void onValidDataReceived(void)
static void failsafeOnValidDataReceived(void)
{
if (failsafe.counter > 20)
failsafe.counter -= 20;
if (failsafeState.counter > 20)
failsafeState.counter -= 20;
else
failsafe.counter = 0;
failsafeState.counter = 0;
}
void updateState(void)
void failsafeUpdateState(void)
{
uint8_t i;
if (!hasTimerElapsed()) {
if (!failsafeHasTimerElapsed()) {
return;
}
if (!isEnabled()) {
reset();
if (!failsafeIsEnabled()) {
failsafeReset();
return;
}
if (shouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
if (failsafeShouldForceLanding(ARMING_FLAG(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)
}
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
failsafe.events++;
failsafeState.events++;
}
if (shouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
mwDisarm();
}
}
void incrementCounter(void)
/**
* Should be called once each time RX data is processed by the system.
*/
void failsafeOnRxCycle(void)
{
failsafe.counter++;
failsafeState.counter++;
}
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
// pulse duration is in micro secons (usec)
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
{
static uint8_t goodChannelMask;
static uint8_t goodChannelMask = 0;
if (channel < 4 &&
pulseDuration > failsafeConfig->failsafe_min_usec &&
pulseDuration < failsafeConfig->failsafe_max_usec
)
goodChannelMask |= (1 << channel); // if signal is valid - mark channel as OK
) {
// if signal is valid - mark channel as OK
goodChannelMask |= (1 << channel);
}
if (goodChannelMask == 0x0F) { // If first four channels have good pulses, clear FailSafe counter
if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
goodChannelMask = 0;
onValidDataReceived();
failsafeOnValidDataReceived();
}
}
const failsafeVTable_t failsafeVTable[] = {
{
reset,
shouldForceLanding,
hasTimerElapsed,
shouldHaveCausedLandingByNow,
incrementCounter,
updateState,
isIdle,
failsafeCheckPulse,
isEnabled,
enable
}
};

33
src/main/flight/failsafe.h
View File

@ -27,27 +27,26 @@ typedef struct failsafeConfig_s {
uint16_t failsafe_max_usec;
} failsafeConfig_t;
typedef struct failsafeVTable_s {
void (*reset)(void);
bool (*shouldForceLanding)(bool armed);
bool (*hasTimerElapsed)(void);
bool (*shouldHaveCausedLandingByNow)(void);
void (*incrementCounter)(void);
void (*updateState)(void);
bool (*isIdle)(void);
void (*checkPulse)(uint8_t channel, uint16_t pulseDuration);
bool (*isEnabled)(void);
void (*enable)(void);
} failsafeVTable_t;
typedef struct failsafe_s {
const failsafeVTable_t *vTable;
typedef struct failsafeState_s {
int16_t counter;
int16_t events;
bool enabled;
} failsafe_t;
} failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
void failsafeEnable(void);
void failsafeOnRxCycle(void);
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration);
void failsafeUpdateState(void);
void failsafeReset(void);
bool failsafeIsEnabled(void);
bool failsafeIsIdle(void);
bool failsafeHasTimerElapsed(void);
bool failsafeShouldForceLanding(bool armed);
bool failsafeShouldHaveCausedLandingByNow(void);

13
src/main/io/beeper.c
View File

@ -54,11 +54,8 @@ typedef enum {
FAILSAFE_FIND_ME
} failsafeBeeperWarnings_e;
static failsafe_t* failsafe;
void beepcodeInit(failsafe_t *initialFailsafe)
void beepcodeInit(void)
{
failsafe = initialFailsafe;
}
void beepcodeUpdateState(batteryState_e batteryState)
@ -77,19 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
if (failsafe->vTable->shouldForceLanding(ARMING_FLAG(ARMED))) {
if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) {
warn_failsafe = FAILSAFE_LANDING;
if (failsafe->vTable->shouldHaveCausedLandingByNow()) {
if (failsafeShouldHaveCausedLandingByNow()) {
warn_failsafe = FAILSAFE_FIND_ME;
}
}
if (failsafe->vTable->hasTimerElapsed() && !ARMING_FLAG(ARMED)) {
if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) {
warn_failsafe = FAILSAFE_FIND_ME;
}
if (failsafe->vTable->isIdle()) {
if (failsafeIsIdle()) {
warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay
}
}

7
src/main/io/ledstrip.c
View File

@ -52,8 +52,6 @@
static bool ledStripInitialised = false;
static bool ledStripEnabled = true;
static failsafe_t* failsafe;
static void ledStripDisable(void);
//#define USE_LED_ANIMATION
@ -663,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow)
if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
warningFlags |= WARNING_FLAG_LOW_BATTERY;
}
if (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed()) {
if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) {
warningFlags |= WARNING_FLAG_FAILSAFE;
}
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
@ -1031,11 +1029,10 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
reevalulateLedConfig();
}
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse)
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse)
{
ledConfigs = ledConfigsToUse;
colors = colorsToUse;
failsafe = failsafeToUse;
ledStripInitialised = false;
}

18
src/main/main.c
View File

@ -97,26 +97,24 @@ extern uint32_t previousTime;
serialPort_t *loopbackPort;
#endif
failsafe_t *failsafe;
void printfSupportInit(void);
void timerInit(void);
void telemetryInit(void);
void serialInit(serialConfig_t *initialSerialConfig);
void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
void failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void beepcodeInit(failsafe_t *initialFailsafe);
void rxInit(rxConfig_t *rxConfig);
void beepcodeInit(void);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void loop(void);
void spektrumBind(rxConfig_t *rxConfig);
@ -347,11 +345,11 @@ void init(void)
mspInit(&masterConfig.serialConfig);
cliInit(&masterConfig.serialConfig);
failsafe = failsafeInit(&masterConfig.rxConfig);
failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe);
beepcodeInit();
rxInit(&masterConfig.rxConfig, failsafe);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
@ -373,7 +371,7 @@ void init(void)
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe);
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();

9
src/main/mw.c
View File

@ -102,7 +102,6 @@ int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
extern failsafe_t *failsafe;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -511,11 +510,11 @@ void processRx(void)
if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
failsafe->vTable->enable();
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
failsafeEnable();
}
failsafe->vTable->updateState();
failsafeUpdateState();
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
@ -555,7 +554,7 @@ void processRx(void)
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;

14
src/main/rx/rx.c
View File

@ -79,8 +79,6 @@ static rxConfig_t *rxConfig;
void serialRxInit(rxConfig_t *rxConfig);
static failsafe_t *failsafe;
void useRxConfig(rxConfig_t *rxConfigToUse)
{
rxConfig = rxConfigToUse;
@ -88,7 +86,7 @@ void useRxConfig(rxConfig_t *rxConfigToUse)
#define STICK_CHANNEL_COUNT 4
void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
void rxInit(rxConfig_t *rxConfig)
{
uint8_t i;
@ -98,8 +96,6 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
rcData[i] = rxConfig->midrc;
}
failsafe = initialFailsafe;
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
serialRxInit(rxConfig);
@ -205,7 +201,7 @@ void updateRx(void)
if (rcDataReceived) {
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->reset();
failsafeReset();
}
}
}
@ -277,7 +273,7 @@ void processRxChannels(void)
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
failsafe->vTable->checkPulse(chan, sample);
failsafeCheckPulse(chan, sample);
}
// validate the range
@ -295,7 +291,7 @@ void processRxChannels(void)
void processDataDrivenRx(void)
{
if (rcDataReceived) {
failsafe->vTable->reset();
failsafeReset();
}
processRxChannels();
@ -315,7 +311,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
rxUpdateAt = currentTime + DELAY_50_HZ;
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->incrementCounter();
failsafeOnRxCycle();
}
if (isRxDataDriven()) {

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

@ -418,4 +418,6 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return 0;
}
bool failsafeHasTimerElapsed(void) { }
}