diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a70f72d867..add9b5c087 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -145,42 +145,59 @@ void failsafeUpdateState(void) failsafeState.active = false; } - switch (failsafeState.phase) { - case FAILSAFE_IDLE: - if (!receivingRxData && armed) { - failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; - } - break; - case FAILSAFE_RX_LOSS_DETECTED: - if (failsafeShouldForceLanding(armed)) { - // Stabilize, and set Throttle to specified level - failsafeActivate(); - } - break; + bool reprocessState; - case FAILSAFE_LANDING: - if (armed) { - failsafeApplyControlInput(); - } + do { + reprocessState = false; - if (failsafeShouldHaveCausedLandingByNow() || !armed) { + switch (failsafeState.phase) { + case FAILSAFE_IDLE: + if (!receivingRxData && armed) { + failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; + + reprocessState = true; + } + break; + + case FAILSAFE_RX_LOSS_DETECTED: + if (failsafeShouldForceLanding(armed)) { + // Stabilize, and set Throttle to specified level + failsafeActivate(); + + reprocessState = true; + } + break; + + case FAILSAFE_LANDING: + if (armed) { + failsafeApplyControlInput(); + } + + if (failsafeShouldHaveCausedLandingByNow() || !armed) { + + failsafeState.phase = FAILSAFE_LANDED; + + reprocessState = true; + + } + break; + + case FAILSAFE_LANDED: + + // 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); - failsafeState.phase = FAILSAFE_LANDED; failsafeState.active = false; mwDisarm(); - } - break; - case FAILSAFE_LANDED: - // 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); - break; + break; - default: - break; - } + default: + break; + } + } while (reprocessState); } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 2ba6cf31b3..c9034b06e4 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -162,19 +162,23 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData) 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()); + // + // currently one cycle must occur (above) so that the next cycle (below) can detect the lack of an update. + // // when for (int i = 0; i < FAILSAFE_UPDATE_HZ - 1; i++) { @@ -190,7 +194,10 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding) } - // FIXME one more cycle currently needed + // + // one more cycle currently needed before the counter is re-checked. + // + // when failsafeOnRxCycleStarted(); // no call to failsafeOnValidDataReceived(); @@ -220,7 +227,6 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) } - // FIXME one more cycle currently needed // when failsafeOnRxCycleStarted(); // no call to failsafeOnValidDataReceived(); @@ -230,15 +236,6 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) 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)); }