From e40a3663d2178851b3d31f78309127fe78f539a0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 22:19:09 +0000 Subject: [PATCH] Remove failsafe vtable usage. --- src/main/flight/failsafe.c | 105 +++++++++++++---------------- src/main/flight/failsafe.h | 33 +++++---- src/main/io/beeper.c | 13 ++-- src/main/io/ledstrip.c | 7 +- src/main/main.c | 18 +++-- src/main/mw.c | 9 ++- src/main/rx/rx.c | 14 ++-- src/test/unit/ledstrip_unittest.cc | 2 + 8 files changed, 88 insertions(+), 113 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 97521c6f84..01edaf28fd 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -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 - } -}; - - diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 1f2fe6ca26..35a94e0a34 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -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); + diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 9d4f33c548..93af618a43 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -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 } } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 5d40671a5c..bd8cb20029 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -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; } diff --git a/src/main/main.c b/src/main/main.c index 8fbe3e6638..fa060ce5e2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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(); diff --git a/src/main/mw.c b/src/main/mw.c index e678848ecd..0437c58b58 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4fb62bd9e1..1af7a24b92 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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()) { diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index bf955b3646..b650efebcd 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -418,4 +418,6 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return 0; } +bool failsafeHasTimerElapsed(void) { } + }