mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
776e8c7b3c
* RX task update rate to 22Hz, to improve 25Hz link stability * modified Rx code * add LQ to debug * use max of frameAge or FrameDelta * Require a dropouit of 200ms, not 100ms, before RXLOSS * remove FrameAge, use 150ms timeout 50ms checks * fix tests and tidy up the comments * Handle NULL input as before, log frame time * possible solutions to review comment about names * Remove rxFrameTimeUs - prepare for direct use of lastRcFrameTimeUs - refactor rx_spi callback * remove global currentRxRateHz * simplify updateRcRefreshRate * re-name to recheck interval, return frame time debug * Calculate RxRate only once * use rxReceivingSignal for consistency * use signalReceived not rxDataReceived for consistency * suggestions from review PL * move defines, thanks K * fixes from review, thanks MH * review changes, undo task interval change rename bool rxIsReceivingSignal to isRxReceivingSignal thanks Steve for resolving that tasks changes are not needed thanks PL for your feedback also --------- Co-authored-by: Petr Ledvina <ledvinap@hp124.ekotip.cz>
778 lines
25 KiB
C++
778 lines
25 KiB
C++
/*
|
|
* 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 "platform.h"
|
|
#include "build/debug.h"
|
|
|
|
#include "pg/pg.h"
|
|
#include "pg/pg_ids.h"
|
|
#include "pg/rx.h"
|
|
|
|
#include "common/axis.h"
|
|
#include "common/maths.h"
|
|
#include "common/bitarray.h"
|
|
|
|
#include "fc/runtime_config.h"
|
|
#include "fc/rc_modes.h"
|
|
#include "fc/rc_controls.h"
|
|
#include "fc/core.h"
|
|
|
|
#include "flight/failsafe.h"
|
|
|
|
#include "io/beeper.h"
|
|
|
|
#include "drivers/io.h"
|
|
#include "rx/rx.h"
|
|
|
|
extern boxBitmask_t rcModeActivationMask;
|
|
}
|
|
|
|
#include "unittest_macros.h"
|
|
#include "gtest/gtest.h"
|
|
|
|
uint32_t testFeatureMask = 0;
|
|
uint16_t testMinThrottle = 0;
|
|
throttleStatus_e throttleStatus = THROTTLE_HIGH;
|
|
|
|
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.
|
|
#define TEST_MIN_CHECK 1100;
|
|
#define PERIOD_OF_10_SCONDS 10000
|
|
#define DE_ACTIVATE_ALL_BOXES 0
|
|
|
|
uint32_t sysTickUptime;
|
|
|
|
void configureFailsafe(void)
|
|
{
|
|
rxConfigMutable()->midrc = TEST_MID_RC;
|
|
rxConfigMutable()->mincheck = TEST_MIN_CHECK;
|
|
|
|
failsafeConfigMutable()->failsafe_delay = 10; // 1 second
|
|
failsafeConfigMutable()->failsafe_landing_time = 1; // 1.0 seconds
|
|
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
|
|
failsafeConfigMutable()->failsafe_throttle = 1200;
|
|
failsafeConfigMutable()->failsafe_throttle_low_delay = 100; // 10 seconds
|
|
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
|
|
// NB we don't have failsafe_recovery_delay so use PERIOD_RXDATA_RECOVERY (100ms)
|
|
sysTickUptime = 0;
|
|
}
|
|
|
|
void activateBoxFailsafe()
|
|
{
|
|
boxBitmask_t newMask;
|
|
bitArraySet(&newMask, BOXFAILSAFE);
|
|
rcModeUpdate(&newMask);
|
|
}
|
|
|
|
void deactivateBoxFailsafe()
|
|
{
|
|
boxBitmask_t newMask;
|
|
memset(&newMask, 0, sizeof(newMask));
|
|
rcModeUpdate(&newMask);
|
|
}
|
|
|
|
//
|
|
// Stepwise tests
|
|
//
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeInitialState)
|
|
{
|
|
// given
|
|
configureFailsafe();
|
|
// and
|
|
DISABLE_ARMING_FLAG(ARMED);
|
|
|
|
// when
|
|
failsafeInit();
|
|
failsafeReset();
|
|
|
|
// then
|
|
EXPECT_FALSE(failsafeIsMonitoring());
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
|
|
{
|
|
// when
|
|
failsafeStartMonitoring();
|
|
|
|
// then
|
|
EXPECT_TRUE(failsafeIsMonitoring());
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
|
|
{
|
|
// given
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
|
|
// when
|
|
sysTickUptime ++;
|
|
failsafeOnValidDataFailed(); // set last invalid sample to a non-zero value
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
|
|
failsafeOnValidDataReceived(); // cause a recovered link
|
|
|
|
// and
|
|
failsafeUpdateState();
|
|
|
|
// then
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
|
|
{
|
|
// when
|
|
for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
|
|
failsafeOnValidDataReceived();
|
|
|
|
failsafeUpdateState();
|
|
|
|
// then
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
}
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
//
|
|
// Clean start
|
|
//
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
|
|
{
|
|
// given
|
|
configureFailsafe();
|
|
failsafeInit();
|
|
|
|
DISABLE_ARMING_FLAG(ARMED);
|
|
resetCallCounters();
|
|
failsafeStartMonitoring();
|
|
|
|
// then
|
|
EXPECT_TRUE(failsafeIsMonitoring());
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
|
|
// given
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
|
|
failsafeOnValidDataReceived();
|
|
|
|
sysTickUptime = 0;
|
|
failsafeUpdateState();
|
|
|
|
// then
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
|
|
// simulate an Rx loss for the stage 1 duration
|
|
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
|
|
failsafeOnValidDataFailed();
|
|
failsafeUpdateState();
|
|
|
|
// should be in stage 1
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
sysTickUptime ++; // exceed the stage 1 period by one tick
|
|
failsafeOnValidDataFailed(); // confirm that we still have no valid data
|
|
|
|
failsafeUpdateState();
|
|
|
|
// we should now be in stage 2, landing phase
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
|
|
// note this test follows on from the previous test
|
|
{
|
|
// exceed the stage 2 landing time
|
|
sysTickUptime += (failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND);
|
|
failsafeOnValidDataFailed(); // confirm that we still have no valid data
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// expect to still be in landing phase
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
|
|
|
|
// adjust time to point just past the landing time
|
|
sysTickUptime++;
|
|
failsafeOnValidDataFailed(); // confirm that we still have no valid data
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// expect to be in monitoring mode
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// let's wait 3 seconds and still get no signal
|
|
sysTickUptime += PERIOD_OF_3_SECONDS;
|
|
failsafeOnValidDataFailed(); // confirm that we still have no valid data
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// nothing should change
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// now lets get a signal while in monitoring mode
|
|
failsafeOnValidDataReceived();
|
|
// we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
failsafeUpdateState();
|
|
|
|
// nothing should change
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
failsafeUpdateState();
|
|
|
|
// nothing should change
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// One tick later
|
|
sysTickUptime++;
|
|
failsafeOnValidDataReceived();
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we expect failsafe to finish and to revert to idle
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
|
|
// Just Disarm is when throttle is low for >10s before signal loss
|
|
// we should exit stage 1 directly into failsafe monitoring mode, and not enter landing mode
|
|
{
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
resetCallCounters();
|
|
failsafeStartMonitoring();
|
|
|
|
// set to normal initial state
|
|
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
|
|
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
|
|
|
|
sysTickUptime += PERIOD_OF_3_SECONDS; // 3s of normality
|
|
failsafeOnValidDataReceived(); // we have a valid signal
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// confirm that we are in idle mode
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
|
|
|
|
// run throttle_low for 11s
|
|
throttleStatus = THROTTLE_LOW; // for failsafe 'just-disarm' procedure
|
|
sysTickUptime += 11000;
|
|
failsafeOnValidDataReceived(); // set the last valid signal to now
|
|
|
|
failsafeUpdateState();
|
|
|
|
// check that we are still in idle mode
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
|
|
// simulate an Rx loss for the stage 1 duration
|
|
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
|
|
failsafeOnValidDataFailed();
|
|
failsafeUpdateState();
|
|
|
|
// should remain in stage 1
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
sysTickUptime ++; // now we exceed stage 1
|
|
failsafeOnValidDataFailed(); // we still have no valid data
|
|
failsafeUpdateState();
|
|
|
|
// we should now be in stage 2 via Just Disarm, going direct to monitoring mode.
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// now lets get a signal while in monitoring mode
|
|
failsafeOnValidDataReceived();
|
|
// we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
failsafeUpdateState();
|
|
|
|
// nothing should change
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
failsafeUpdateState();
|
|
|
|
// nothing should change
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// One tick later
|
|
sysTickUptime++;
|
|
failsafeOnValidDataReceived();
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we expect failsafe to finish and to revert to idle
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
|
|
EXPECT_FALSE(isArmingDisabled());}
|
|
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
|
|
{
|
|
// given
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
resetCallCounters();
|
|
failsafeStartMonitoring();
|
|
|
|
// set to normal initial state
|
|
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
|
|
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
|
|
|
|
sysTickUptime += PERIOD_OF_3_SECONDS; // 3s of normality
|
|
failsafeOnValidDataReceived(); // we have a valid signal
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// confirm that we are in idle mode
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
|
|
// given
|
|
activateBoxFailsafe(); // activate the Kill swith
|
|
|
|
// when
|
|
failsafeUpdateState(); // should failsafe immediately the kill switch is hit
|
|
|
|
// then
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
|
|
// given
|
|
failsafeOnValidDataReceived(); // the link is active the whole time
|
|
|
|
// deactivate the kill switch
|
|
deactivateBoxFailsafe(); // receivingRxData is immediately true
|
|
|
|
// we should go to failsafe monitoring mode, via Landing
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
|
|
// since we didn't enter stage 2, we have one rxDataRecoveryPeriod delay to deal with
|
|
// while within rxDataRecoveryPeriod in monitoring mode...
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we should still be in failsafe monitoring mode
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
|
|
// one tick later
|
|
sysTickUptime ++;
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we should now have exited failsafe
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
|
|
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage1OrStage2Drop)
|
|
// should immediately stop motors and go to monitoring mode diretly
|
|
{
|
|
// given a clean start
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
resetCallCounters();
|
|
failsafeStartMonitoring();
|
|
|
|
// and set initial states
|
|
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
|
|
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
|
|
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
|
|
|
|
sysTickUptime = 0; // restart time from 0
|
|
failsafeOnValidDataReceived(); // we have a valid signal
|
|
sysTickUptime += 3000; // 3s of normality
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// confirm that we are in idle mode
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
|
|
// given
|
|
activateBoxFailsafe(); // activate the stage 2 Drop switch
|
|
failsafeOnValidDataFailed(); // immediate stage 2 switch sets failsafeOnValidDataFailed
|
|
|
|
// when
|
|
failsafeUpdateState(); // should activate stage2 immediately, even though signal is good
|
|
|
|
// expect to be in monitoring mode, and to have disarmed
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// deactivate the failsafe switch
|
|
deactivateBoxFailsafe();
|
|
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
|
|
// by next evaluation we should be out of failsafe
|
|
sysTickUptime ++;
|
|
// receivingRxData is immediately true because signal exists
|
|
failsafeOnValidDataReceived();
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we should now have exited failsafe
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
}
|
|
|
|
/****************************************************************************************/
|
|
|
|
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
|
|
{
|
|
// given a clean start
|
|
ENABLE_ARMING_FLAG(ARMED);
|
|
resetCallCounters();
|
|
failsafeStartMonitoring();
|
|
|
|
// and
|
|
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
|
|
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
|
|
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
|
|
|
|
sysTickUptime = 0; // restart time from 0
|
|
failsafeOnValidDataReceived(); // we have a valid signal
|
|
sysTickUptime += 3000; // 3s of normality
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// confirm that we are in idle mode
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
|
|
// given
|
|
activateBoxFailsafe(); // activate the stage 2 Drop switch
|
|
failsafeOnValidDataFailed(); // immediate stage 2 switch sets failsafeOnValidDataFailed
|
|
|
|
// when
|
|
failsafeUpdateState(); // should activate stage2 immediately
|
|
|
|
// expect to immediately be in landing mode, and not disarmed
|
|
EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming
|
|
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// should stay in landing for failsafe_landing_time (stage 2 landing period) of 1s
|
|
sysTickUptime += failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND;
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming
|
|
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// one tick later, landing time has elapsed
|
|
sysTickUptime ++;
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// now should be in monitoring mode, with switch holding rxDataReceived false
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// given
|
|
sysTickUptime += PERIOD_OF_3_SECONDS; // hang around a bit waiting for switch change
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// should still be in monitoring mode because switch is still forcing receivingRxData false
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// deactivate the failsafe switch
|
|
deactivateBoxFailsafe();
|
|
|
|
// receivingRxData is immediately true
|
|
// we go directly to failsafe monitoring mode, via Landing
|
|
// however, because the switch also forces rxFlightChannelsValid false, emulating real failsafe
|
|
// we have two delays to deal with before we can re-arm
|
|
|
|
EXPECT_TRUE(failsafeIsActive());
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
|
|
|
|
|
// one tick later
|
|
sysTickUptime ++;
|
|
failsafeOnValidDataReceived();
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// we should now have exited failsafe
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
}
|
|
|
|
|
|
/****************************************************************************************/
|
|
//
|
|
// Clean start
|
|
//
|
|
/****************************************************************************************/
|
|
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
|
|
{
|
|
// given
|
|
resetCallCounters();
|
|
configureFailsafe();
|
|
|
|
// and
|
|
failsafeInit();
|
|
|
|
// and ** WE ARE DISARMED **
|
|
DISABLE_ARMING_FLAG(ARMED);
|
|
|
|
// when failsafe starts
|
|
failsafeStartMonitoring();
|
|
|
|
// and
|
|
sysTickUptime = 0; // restart time from 0
|
|
failsafeOnValidDataReceived(); // we have a valid signal
|
|
sysTickUptime += 3000; // 3s of normality
|
|
|
|
// when
|
|
failsafeUpdateState();
|
|
|
|
// confirm that we are in idle mode
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// and that arming is not disabled
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
|
|
// simulate an Rx loss for the stage 1 duration
|
|
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
|
|
failsafeOnValidDataFailed();
|
|
failsafeUpdateState();
|
|
|
|
// within stage 1 time from a true failsafe while disarmed, stage 2 should normally not be active
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// arming is disabled due to setArmingDisabled in stage 1 due to failsafeOnValidDataFailed()
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
|
|
// given
|
|
sysTickUptime++; // adjust time to point just past stage 1
|
|
failsafeOnValidDataFailed(); // would normally enter stage 2, but we are disarmed
|
|
failsafeUpdateState();
|
|
|
|
// expect that we do not enter failsafe stage 2, ie no change from above
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// the lock on the aux channels persists at fixed stage 1 values until recovery time
|
|
EXPECT_TRUE(isArmingDisabled());
|
|
|
|
// allow signal received for the recovery time
|
|
sysTickUptime += PERIOD_RXDATA_RECOVERY;
|
|
failsafeOnValidDataReceived();
|
|
failsafeUpdateState();
|
|
|
|
// no change in failsafe state (still idle)
|
|
EXPECT_FALSE(failsafeIsActive());
|
|
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
|
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
|
|
|
// but now arming is possible
|
|
EXPECT_FALSE(isArmingDisabled());
|
|
}
|
|
|
|
// STUBS
|
|
|
|
extern "C" {
|
|
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
|
float rcCommand[4];
|
|
int16_t debug[DEBUG16_VALUE_COUNT];
|
|
uint8_t debugMode = 0;
|
|
bool isUsingSticksToArm = true;
|
|
|
|
PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
|
|
|
|
// Return system uptime in milliseconds (rollover in 49 days)
|
|
uint32_t millis(void)
|
|
{
|
|
return sysTickUptime;
|
|
}
|
|
|
|
uint32_t micros(void)
|
|
{
|
|
return millis() * 1000;
|
|
}
|
|
|
|
throttleStatus_e calculateThrottleStatus()
|
|
{
|
|
return throttleStatus;
|
|
}
|
|
|
|
void delay(uint32_t) {}
|
|
|
|
bool featureIsEnabled(uint32_t mask)
|
|
{
|
|
return (mask & testFeatureMask);
|
|
}
|
|
|
|
void disarm(flightLogDisarmReason_e)
|
|
{
|
|
callCounts[COUNTER_MW_DISARM]++;
|
|
}
|
|
|
|
void beeper(beeperMode_e mode)
|
|
{
|
|
UNUSED(mode);
|
|
}
|
|
|
|
uint16_t getCurrentMinthrottle(void)
|
|
{
|
|
return testMinThrottle;
|
|
}
|
|
|
|
bool isUsingSticksForArming(void)
|
|
{
|
|
return isUsingSticksToArm;
|
|
}
|
|
|
|
bool areSticksActive(uint8_t stickPercentLimit)
|
|
{
|
|
UNUSED(stickPercentLimit);
|
|
return false;
|
|
}
|
|
|
|
void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
|
|
|
|
bool crashRecoveryModeActive(void) { return false; }
|
|
void pinioBoxTaskControl(void) {}
|
|
}
|