Cleanup project structure. Update unit test Makefile to place object

files in obj/test

src/main/flight/failsafe.c

@@ -0,0 +1,168 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "common/axis.h"
+
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+#include "rx/rx.h"
+#include "config/runtime_config.h"
+
+#include "flight/failsafe.h"
+
+/*
+ * Usage:
+ *
+ * failsafeInit() and useFailsafeConfig() must be called before the other methods are used.
+ *
+ * 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;
+
+static failsafeConfig_t *failsafeConfig;
+
+static rxConfig_t *rxConfig;
+
+const failsafeVTable_t failsafeVTable[];
+
+void reset(void)
+{
+    failsafe.counter = 0;
+}
+
+/*
+ * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
+ */
+void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
+{
+    failsafeConfig = failsafeConfigToUse;
+    reset();
+}
+
+failsafe_t* failsafeInit(rxConfig_t *intialRxConfig)
+{
+    rxConfig = intialRxConfig;
+
+    failsafe.vTable = failsafeVTable;
+    failsafe.events = 0;
+    failsafe.enabled = false;
+
+    return &failsafe;
+}
+
+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);
+}
+
+bool shouldForceLanding(bool armed)
+{
+    return hasTimerElapsed() && armed;
+}
+
+bool shouldHaveCausedLandingByNow(void)
+{
+    return failsafe.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
+}
+
+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
+    f.PREVENT_ARMING = 1;
+}
+
+void onValidDataReceived(void)
+{
+    if (failsafe.counter > 20)
+        failsafe.counter -= 20;
+    else
+        failsafe.counter = 0;
+}
+
+void updateState(void)
+{
+    uint8_t i;
+
+    if (!hasTimerElapsed()) {
+        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)
+        }
+        rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
+        failsafe.events++;
+    }
+
+    if (shouldHaveCausedLandingByNow() || !f.ARMED) {
+        mwDisarm();
+    }
+}
+
+void incrementCounter(void)
+{
+    failsafe.counter++;
+}
+
+// pulse duration is in micro secons (usec)
+void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
+{
+    static uint8_t goodChannelMask;
+
+    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 (goodChannelMask == 0x0F) {               // If first four channels have good pulses, clear FailSafe counter
+        goodChannelMask = 0;
+        onValidDataReceived();
+    }
+}
+
+
+const failsafeVTable_t failsafeVTable[] = {
+    {
+        reset,
+        shouldForceLanding,
+        hasTimerElapsed,
+        shouldHaveCausedLandingByNow,
+        incrementCounter,
+        updateState,
+        isIdle,
+        failsafeCheckPulse,
+        isEnabled,
+        enable
+    }
+};
+
+