diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index acfe35b99c..a70f72d867 100644 --- a/src/main/flight/failsafe.c +++ b/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; } diff --git a/src/test/Makefile b/src/test/Makefile index fb5f8beb6a..c7154b1fa0 100644 --- a/src/test/Makefile +++ b/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 \ diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc new file mode 100644 index 0000000000..2ba6cf31b3 --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +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]++; +} + +}