First cut of unit tests for reworked failsafe.

In writing these tests a bug was discovered with RX_SERIAL and RX_MSP
failsafe delay timing - the configured delays are not honoured.

src/main/flight/failsafe.c

@@ -59,14 +59,14 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
     failsafeReset();
 }
 
-failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
+void failsafeInit(rxConfig_t *intialRxConfig)
 {
     rxConfig = intialRxConfig;
 
     failsafeState.events = 0;
     failsafeState.monitoring = false;
 
-    return &failsafeState;
+    return;
 }
 
 failsafePhase_e failsafePhase()
@@ -101,7 +101,7 @@ static bool failsafeHasTimerElapsed(void)
     return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
 }
 
-bool failsafeShouldForceLanding(bool armed)
+static bool failsafeShouldForceLanding(bool armed)
 {
     return failsafeHasTimerElapsed() && armed;
 }

src/test/Makefile

@@ -47,6 +47,7 @@ TESTS = \
 	battery_unittest \
 	flight_imu_unittest \
 	flight_mixer_unittest \
+	flight_failsafe_unittest \
 	altitude_hold_unittest \
 	maths_unittest \
 	gps_conversion_unittest \
@@ -390,6 +391,30 @@ flight_mixer_unittest : \
 
 	$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
 
+$(OBJECT_DIR)/flight/failsafe.o : \
+	$(USER_DIR)/flight/failsafe.c \
+	$(USER_DIR)/flight/failsafe.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/failsafe.c -o $@
+
+$(OBJECT_DIR)/flight_failsafe_unittest.o : \
+	$(TEST_DIR)/flight_failsafe_unittest.cc \
+	$(USER_DIR)/flight/failsafe.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_failsafe_unittest.cc -o $@
+
+flight_failsafe_unittest : \
+	$(OBJECT_DIR)/flight/failsafe.o \
+	$(OBJECT_DIR)/flight_failsafe_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
+
 $(OBJECT_DIR)/io/serial.o : \
 	$(USER_DIR)/io/serial.c \
 	$(USER_DIR)/io/serial.h \

src/test/unit/flight_failsafe_unittest.cc

@@ -0,0 +1,262 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <limits.h>
+
+extern "C" {
+    #include "debug.h"
+
+    #include "platform.h"
+
+    #include "common/axis.h"
+    #include "common/maths.h"
+
+    #include "config/runtime_config.h"
+
+    #include "rx/rx.h"
+    #include "flight/failsafe.h"
+
+    failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+uint32_t testFeatureMask = 0;
+
+enum {
+    COUNTER_MW_DISARM = 0,
+};
+#define CALL_COUNT_ITEM_COUNT 1
+
+static int callCounts[CALL_COUNT_ITEM_COUNT];
+
+#define CALL_COUNTER(item) (callCounts[item])
+
+void resetCallCounters(void) {
+    memset(&callCounts, 0, sizeof(callCounts));
+}
+
+#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
+
+rxConfig_t rxConfig;
+failsafeConfig_t failsafeConfig;
+
+//
+// Stepwise tests
+//
+
+TEST(FlightFailsafeTest, TestFailsafeInitialState)
+{
+    // given
+    memset(&rxConfig, 0, sizeof(rxConfig));
+    rxConfig.midrc = TEST_MID_RC;
+
+    // and
+    memset(&failsafeConfig, 0, sizeof(failsafeConfig));
+    failsafeConfig.failsafe_delay = 10; // 1 second
+    failsafeConfig.failsafe_off_delay = 50; // 5 seconds
+
+    // when
+    useFailsafeConfig(&failsafeConfig);
+    failsafeInit(&rxConfig);
+
+    // then
+    EXPECT_EQ(false, failsafeIsMonitoring());
+    EXPECT_EQ(false, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+}
+
+TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
+{
+    // when
+    failsafeStartMonitoring();
+
+    // then
+    EXPECT_EQ(true, failsafeIsMonitoring());
+    EXPECT_EQ(false, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+}
+
+TEST(FlightFailsafeTest, TestFailsafeFirstCycle)
+{
+    // when
+    failsafeOnRxCycleStarted();
+    failsafeOnValidDataReceived();
+
+    // and
+    failsafeUpdateState();
+
+    // then
+    EXPECT_EQ(false, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+}
+
+/*
+ * FIXME failsafe assumes that calls to failsafeUpdateState() happen at a set frequency (50hz)
+ * but that is NOT the case when using a RX_SERIAL or RX_MSP as in that case the rx data is processed as soon
+ * as it arrives which may be more or less frequent.
+ *
+ * Since the failsafe uses a counter the counter would not be updated at the same frequency that the maths
+ * in the failsafe code is expecting the failsafe will either be triggered to early or too late when using
+ * RX_SERIAL or RX_MSP.
+ *
+ *  uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
+ *
+ *  static bool failsafeHasTimerElapsed(void)
+ *  {
+ *    return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
+ *  }
+ *
+ *  static bool failsafeShouldHaveCausedLandingByNow(void)
+ *  {
+ *    return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
+ *  }
+ *
+ *  void failsafeOnValidDataReceived(void)
+ *  {
+ *    if (failsafeState.counter > 20)
+ *      failsafeState.counter -= 20;
+ *    else
+ *      failsafeState.counter = 0;
+ *  }
+ *
+ *  1000ms / 50hz = 20
+ */
+
+#define FAILSAFE_UPDATE_HZ 50
+
+TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
+{
+    // when
+    int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
+
+    for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
+        failsafeOnRxCycleStarted();
+        failsafeOnValidDataReceived();
+
+        failsafeUpdateState();
+
+        // then
+        EXPECT_EQ(false, failsafeIsActive());
+        EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+    }
+}
+
+TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
+{
+    // given
+    ENABLE_ARMING_FLAG(ARMED);
+
+    // FIXME one more cycle currently needed
+    // when
+    failsafeOnRxCycleStarted();
+    // no call to failsafeOnValidDataReceived();
+    failsafeUpdateState();
+
+    // then
+    EXPECT_EQ(false, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+
+
+    // when
+    for (int i = 0; i < FAILSAFE_UPDATE_HZ - 1; i++) {
+
+        failsafeOnRxCycleStarted();
+        // no call to failsafeOnValidDataReceived();
+
+        failsafeUpdateState();
+
+        // then
+        EXPECT_EQ(FAILSAFE_RX_LOSS_DETECTED, failsafePhase());
+        EXPECT_EQ(false, failsafeIsActive());
+
+    }
+
+    // FIXME one more cycle currently needed
+    // when
+    failsafeOnRxCycleStarted();
+    // no call to failsafeOnValidDataReceived();
+    failsafeUpdateState();
+
+    // then
+    EXPECT_EQ(true, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
+}
+
+TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
+{
+    // given
+    int callsToMakeToSimulateFiveSeconds = FAILSAFE_UPDATE_HZ * 5;
+
+    // when
+    for (int i = 0; i < callsToMakeToSimulateFiveSeconds - 1; i++) {
+
+        failsafeOnRxCycleStarted();
+        // no call to failsafeOnValidDataReceived();
+
+        failsafeUpdateState();
+
+        // then
+        EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
+        EXPECT_EQ(true, failsafeIsActive());
+
+    }
+
+    // FIXME one more cycle currently needed
+    // when
+    failsafeOnRxCycleStarted();
+    // no call to failsafeOnValidDataReceived();
+    failsafeUpdateState();
+
+    // then
+    EXPECT_EQ(false, failsafeIsActive());
+    EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
+    EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
+    // FIXME one more cycle currently needed
+    EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
+
+    // when
+    failsafeOnRxCycleStarted();
+    // no call to failsafeOnValidDataReceived();
+    failsafeUpdateState();
+
+    // then
+    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+
+}
+
+// STUBS
+
+extern "C" {
+int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+uint8_t armingFlags;
+
+void delay(uint32_t) {}
+
+bool feature(uint32_t mask) {
+    return (mask & testFeatureMask);
+}
+
+void mwDisarm(void) {
+    callCounts[COUNTER_MW_DISARM]++;
+}
+
+}