Remove failsafe vtable usage.

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 failsafeReset(void)
-
-void reset(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;
+    failsafeState.events = 0;
-    failsafe.events = 0;
+    failsafeState.enabled = false;
-    failsafe.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)
+    if (failsafeState.counter > 20)
-        failsafe.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()) {
+    if (!failsafeIsEnabled()) {
-        reset();
+        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
-    }
-};
-
-

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);
+

src/main/io/beeper.c

@@ -54,11 +54,8 @@ typedef enum {
     FAILSAFE_FIND_ME
 } failsafeBeeperWarnings_e;
 
-static failsafe_t* failsafe;
+void beepcodeInit(void)
-
-void beepcodeInit(failsafe_t *initialFailsafe)
 {
-    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
         }
     }

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;
 }
 

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 rxInit(rxConfig_t *rxConfig);
-void beepcodeInit(failsafe_t *initialFailsafe);
+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();

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()) {
+        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
-            failsafe->vTable->enable();
+            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;
 

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()) {

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) { }
+
 }