src/test/unit/flight_failsafe_unittest.cc.txt

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <stdbool.h>
  20
  21 #include <limits.h>
  22
  23 extern "C" {
  24     #include "debug.h"
  25
  26     #include "platform.h"
  27
  28     #include "common/axis.h"
  29     #include "common/maths.h"
  30
  31     #include "config/runtime_config.h"
  32
  33     #include "io/beeper.h"
  34     #include "io/rc_controls.h"
  35
  36     #include "rx/rx.h"
  37     #include "flight/failsafe.h"
  38
  39     failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig);
  40 }
  41
  42 #include "unittest_macros.h"
  43 #include "gtest/gtest.h"
  44
  45 uint32_t testFeatureMask = 0;
  46 uint16_t flightModeFlags = 0;
  47 uint16_t testMinThrottle = 0;
  48 throttleStatus_e throttleStatus = THROTTLE_HIGH;
  49
  50 enum {
  51     COUNTER_MW_DISARM = 0,
  52 };
  53 #define CALL_COUNT_ITEM_COUNT 1
  54
  55 static int callCounts[CALL_COUNT_ITEM_COUNT];
  56
  57 #define CALL_COUNTER(item) (callCounts[item])
  58
  59 void resetCallCounters(void) {
  60     memset(&callCounts, 0, sizeof(callCounts));
  61 }
  62
  63 #define TEST_MID_RC 1495            // something other than the default 1500 will suffice.
  64 #define TEST_MIN_CHECK 1100;
  65 #define PERIOD_OF_10_SCONDS 10000
  66 #define DE_ACTIVATE_ALL_BOXES 0
  67
  68 rxConfig_t rxConfig;
  69 failsafeConfig_t failsafeConfig;
  70 uint32_t sysTickUptime;
  71
  72 void configureFailsafe(void)
  73 {
  74     memset(&rxConfig, 0, sizeof(rxConfig));
  75     rxConfig.midrc = TEST_MID_RC;
  76     rxConfig.mincheck = TEST_MIN_CHECK;
  77
  78     memset(&failsafeConfig, 0, sizeof(failsafeConfig));
  79     failsafeConfig.failsafe_delay = 10; // 1 second
  80     failsafeConfig.failsafe_off_delay = 50; // 5 seconds
  81     failsafeConfig.failsafe_kill_switch = false;
  82     failsafeConfig.failsafe_throttle = 1200;
  83     failsafeConfig.failsafe_throttle_low_delay = 50; // 5 seconds
  84     sysTickUptime = 0;
  85 }
  86 //
  87 // Stepwise tests
  88 //
  89
  90 /****************************************************************************************/
  91 TEST(FlightFailsafeTest, TestFailsafeInitialState)
  92 {
  93     // given
  94     configureFailsafe();
  95     // and
  96     DISABLE_ARMING_FLAG(ARMED);
  97
  98     // when
  99     useFailsafeConfig(&failsafeConfig);
 100     failsafeInit(&rxConfig);
 101
 102     // then
 103     EXPECT_EQ(false, failsafeIsMonitoring());
 104     EXPECT_EQ(false, failsafeIsActive());
 105     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 106 }
 107
 108 /****************************************************************************************/
 109 TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
 110 {
 111     // when
 112     failsafeStartMonitoring();
 113
 114     // then
 115     EXPECT_EQ(true, failsafeIsMonitoring());
 116     EXPECT_EQ(false, failsafeIsActive());
 117     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 118 }
 119
 120 /****************************************************************************************/
 121 TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
 122 {
 123     // given
 124     ENABLE_ARMING_FLAG(ARMED);
 125
 126     // when
 127     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 128     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 129     failsafeOnValidDataReceived();                  // cause a recovered link
 130
 131     // and
 132     failsafeUpdateState();
 133
 134     // then
 135     EXPECT_EQ(false, failsafeIsActive());
 136     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 137 }
 138
 139 /****************************************************************************************/
 140 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
 141 {
 142     // when
 143     for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
 144         failsafeOnValidDataReceived();
 145
 146         failsafeUpdateState();
 147
 148         // then
 149         EXPECT_EQ(false, failsafeIsActive());
 150         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 151     }
 152 }
 153
 154 /****************************************************************************************/
 155 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
 156 {
 157     // given
 158     ENABLE_ARMING_FLAG(ARMED);
 159
 160     // and
 161     failsafeStartMonitoring();
 162     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 163     sysTickUptime = 0;                              // restart time from 0
 164     failsafeOnValidDataReceived();                  // set last valid sample at current time
 165
 166     // when
 167     for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
 168         failsafeOnValidDataFailed();
 169
 170         failsafeUpdateState();
 171
 172         // then
 173         EXPECT_EQ(false, failsafeIsActive());
 174         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 175     }
 176
 177     // given
 178     sysTickUptime++;                                // adjust time to point just past the failure time to
 179     failsafeOnValidDataFailed();                    // cause a lost link
 180
 181     // when
 182     failsafeUpdateState();
 183
 184     // then
 185     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 186     EXPECT_EQ(true, failsafeIsActive());
 187 }
 188
 189 /****************************************************************************************/
 190 TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
 191 {
 192     // given
 193     sysTickUptime += failsafeConfig.failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 194     sysTickUptime++;
 195
 196     // when
 197     // no call to failsafeOnValidDataReceived();
 198     failsafeUpdateState();
 199
 200     // then
 201     EXPECT_EQ(true, failsafeIsActive());
 202     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 203     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 204     EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
 205
 206     // given
 207     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 208     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 209     failsafeOnValidDataReceived();                  // cause a recovered link
 210
 211     // when
 212     failsafeUpdateState();
 213
 214     // then
 215     EXPECT_EQ(true, failsafeIsActive());
 216     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 217     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 218     EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
 219
 220     // given
 221     sysTickUptime += PERIOD_OF_30_SECONDS + 1;      // adjust time to point just past the required additional recovery time
 222     failsafeOnValidDataReceived();
 223
 224     // when
 225     failsafeUpdateState();
 226
 227     // then
 228     EXPECT_EQ(false, failsafeIsActive());
 229     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 230     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
 231     EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
 232 }
 233
 234 /****************************************************************************************/
 235 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
 236 {
 237     // given
 238     DISABLE_ARMING_FLAG(ARMED);
 239     resetCallCounters();
 240
 241     // and
 242     failsafeStartMonitoring();
 243     throttleStatus = THROTTLE_LOW;                  // throttle LOW to go for a failsafe just-disarm procedure
 244     sysTickUptime = 0;                              // restart time from 0
 245     failsafeOnValidDataReceived();                  // set last valid sample at current time
 246
 247     // when
 248     for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
 249         failsafeOnValidDataFailed();
 250
 251         failsafeUpdateState();
 252
 253         // then
 254         EXPECT_EQ(false, failsafeIsActive());
 255         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 256     }
 257
 258     // given
 259     sysTickUptime++;                                // adjust time to point just past the failure time to
 260     failsafeOnValidDataFailed();                    // cause a lost link
 261     ENABLE_ARMING_FLAG(ARMED);                      // armed from here (disarmed state has cleared throttleLowPeriod).
 262
 263     // when
 264     failsafeUpdateState();
 265
 266     // then
 267     EXPECT_EQ(true, failsafeIsActive());
 268     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 269     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 270     EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
 271
 272     // given
 273     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 274     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 275     failsafeOnValidDataReceived();                  // cause a recovered link
 276
 277     // when
 278     failsafeUpdateState();
 279
 280     // then
 281     EXPECT_EQ(true, failsafeIsActive());
 282     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 283     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 284     EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
 285
 286     // given
 287     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 288     failsafeOnValidDataReceived();
 289
 290     // when
 291     failsafeUpdateState();
 292
 293     // then
 294     EXPECT_EQ(false, failsafeIsActive());
 295     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 296     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 297     EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
 298 }
 299
 300 /****************************************************************************************/
 301 //
 302 // Additional non-stepwise tests
 303 //
 304 /****************************************************************************************/
 305 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
 306 {
 307     // given
 308     configureFailsafe();
 309
 310     // and
 311     useFailsafeConfig(&failsafeConfig);
 312     failsafeInit(&rxConfig);
 313
 314     // and
 315     DISABLE_ARMING_FLAG(ARMED);
 316
 317     // when
 318     failsafeStartMonitoring();
 319
 320     // and
 321     sysTickUptime = 0;                              // restart time from 0
 322     failsafeOnValidDataReceived();                  // set last valid sample at current time
 323
 324     // when
 325     for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
 326         failsafeOnValidDataFailed();
 327
 328         failsafeUpdateState();
 329
 330         // then
 331         EXPECT_EQ(false, failsafeIsActive());
 332         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 333     }
 334
 335     // given
 336     sysTickUptime++;                                // adjust time to point just past the failure time to
 337     failsafeOnValidDataFailed();                    // cause a lost link
 338
 339     // when
 340     failsafeUpdateState();
 341
 342     // then
 343     EXPECT_EQ(true, failsafeIsMonitoring());
 344     EXPECT_EQ(false, failsafeIsActive());
 345     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 346     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 347     EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
 348 }
 349
 350 // STUBS
 351
 352 extern "C" {
 353 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 354 uint8_t armingFlags;
 355 int16_t rcCommand[4];
 356 uint32_t rcModeActivationMask;
 357 int32_t debug[DEBUG32_VALUE_COUNT];
 358 bool isUsingSticksToArm = true;
 359
 360
 361 // Return system uptime in milliseconds (rollover in 49 days)
 362 uint32_t millis(void)
 363 {
 364     return sysTickUptime;
 365 }
 366
 367 throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband)
 368 {
 369     UNUSED(rxConfig);
 370     UNUSED(mid_throttle_deadband);
 371     return throttleStatus;
 372 }
 373
 374 void delay(uint32_t) {}
 375
 376 bool feature(uint32_t mask) {
 377     return (mask & testFeatureMask);
 378 }
 379
 380 void mwDisarm(void) {
 381     callCounts[COUNTER_MW_DISARM]++;
 382 }
 383
 384 void beeper(beeperMode_e mode) {
 385     UNUSED(mode);
 386 }
 387
 388 uint16_t enableFlightMode(flightModeFlags_e mask)
 389 {
 390     flightModeFlags |= (mask);
 391     return flightModeFlags;
 392 }
 393
 394 uint16_t disableFlightMode(flightModeFlags_e mask)
 395 {
 396     flightModeFlags &= ~(mask);
 397     return flightModeFlags;
 398 }
 399
 400 uint16_t getCurrentMinthrottle(void)
 401 {
 402     return testMinThrottle;
 403 }
 404
 405 }