src/test/unit/flight_failsafe_unittest.cc

   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 "platform.h"
  25     #include "build/debug.h"
  26
  27     #include "pg/pg.h"
  28     #include "pg/pg_ids.h"
  29     #include "pg/rx.h"
  30
  31     #include "common/axis.h"
  32     #include "common/maths.h"
  33     #include "common/bitarray.h"
  34
  35     #include "fc/runtime_config.h"
  36     #include "fc/rc_modes.h"
  37     #include "fc/rc_controls.h"
  38     #include "fc/core.h"
  39
  40     #include "flight/failsafe.h"
  41
  42     #include "io/beeper.h"
  43
  44     #include "drivers/io.h"
  45     #include "rx/rx.h"
  46
  47     extern boxBitmask_t rcModeActivationMask;
  48 }
  49
  50 #include "unittest_macros.h"
  51 #include "gtest/gtest.h"
  52
  53 uint32_t testFeatureMask = 0;
  54 uint16_t testMinThrottle = 0;
  55 throttleStatus_e throttleStatus = THROTTLE_HIGH;
  56
  57 enum {
  58     COUNTER_MW_DISARM = 0,
  59 };
  60 #define CALL_COUNT_ITEM_COUNT 1
  61
  62 static int callCounts[CALL_COUNT_ITEM_COUNT];
  63
  64 #define CALL_COUNTER(item) (callCounts[item])
  65
  66 void resetCallCounters(void) {
  67     memset(&callCounts, 0, sizeof(callCounts));
  68 }
  69
  70 #define TEST_MID_RC 1495            // something other than the default 1500 will suffice.
  71 #define TEST_MIN_CHECK 1100;
  72 #define PERIOD_OF_10_SCONDS 10000
  73 #define DE_ACTIVATE_ALL_BOXES 0
  74
  75 uint32_t sysTickUptime;
  76
  77 void configureFailsafe(void)
  78 {
  79     rxConfigMutable()->midrc = TEST_MID_RC;
  80     rxConfigMutable()->mincheck = TEST_MIN_CHECK;
  81
  82     failsafeConfigMutable()->failsafe_delay = 10; // 1 second
  83     failsafeConfigMutable()->failsafe_off_delay = 50; // 5 seconds
  84     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
  85     failsafeConfigMutable()->failsafe_throttle = 1200;
  86     failsafeConfigMutable()->failsafe_throttle_low_delay = 50; // 5 seconds
  87     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
  88     sysTickUptime = 0;
  89 }
  90
  91 void activateBoxFailsafe()
  92 {
  93     boxBitmask_t newMask;
  94     bitArraySet(&newMask, BOXFAILSAFE);
  95     rcModeUpdate(&newMask);
  96 }
  97
  98 void deactivateBoxFailsafe()
  99 {
 100     boxBitmask_t newMask;
 101     memset(&newMask, 0, sizeof(newMask));
 102     rcModeUpdate(&newMask);
 103 }
 104
 105 //
 106 // Stepwise tests
 107 //
 108
 109 /****************************************************************************************/
 110 TEST(FlightFailsafeTest, TestFailsafeInitialState)
 111 {
 112     // given
 113     configureFailsafe();
 114     // and
 115     DISABLE_ARMING_FLAG(ARMED);
 116
 117     // when
 118     failsafeInit();
 119     failsafeReset();
 120
 121     // then
 122     EXPECT_FALSE(failsafeIsMonitoring());
 123     EXPECT_FALSE(failsafeIsActive());
 124     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 125 }
 126
 127 /****************************************************************************************/
 128 TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
 129 {
 130     // when
 131     failsafeStartMonitoring();
 132
 133     // then
 134     EXPECT_TRUE(failsafeIsMonitoring());
 135     EXPECT_FALSE(failsafeIsActive());
 136     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 137 }
 138
 139 /****************************************************************************************/
 140 TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
 141 {
 142     // given
 143     ENABLE_ARMING_FLAG(ARMED);
 144
 145     // when
 146     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 147     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 148     failsafeOnValidDataReceived();                  // cause a recovered link
 149
 150     // and
 151     failsafeUpdateState();
 152
 153     // then
 154     EXPECT_FALSE(failsafeIsActive());
 155     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 156 }
 157
 158 /****************************************************************************************/
 159 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
 160 {
 161     // when
 162     for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
 163         failsafeOnValidDataReceived();
 164
 165         failsafeUpdateState();
 166
 167         // then
 168         EXPECT_FALSE(failsafeIsActive());
 169         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 170     }
 171 }
 172
 173 /****************************************************************************************/
 174 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
 175 {
 176     // given
 177     ENABLE_ARMING_FLAG(ARMED);
 178
 179     // and
 180     failsafeStartMonitoring();
 181     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 182     sysTickUptime = 0;                              // restart time from 0
 183     failsafeOnValidDataReceived();                  // set last valid sample at current time
 184
 185     // when
 186     for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
 187         failsafeOnValidDataFailed();
 188
 189         failsafeUpdateState();
 190
 191         // then
 192         EXPECT_FALSE(failsafeIsActive());
 193         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 194     }
 195
 196     // given
 197     sysTickUptime++;                                // adjust time to point just past the failure time to
 198     failsafeOnValidDataFailed();                    // cause a lost link
 199
 200     // when
 201     failsafeUpdateState();
 202
 203     // then
 204     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 205     EXPECT_TRUE(failsafeIsActive());
 206 }
 207
 208 /****************************************************************************************/
 209 TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
 210 {
 211     // given
 212     sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 213     sysTickUptime++;
 214
 215     // when
 216     // no call to failsafeOnValidDataReceived();
 217     failsafeUpdateState();
 218
 219     // then
 220     EXPECT_TRUE(failsafeIsActive());
 221     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 222     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 223     EXPECT_TRUE(isArmingDisabled());
 224
 225     // given
 226     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 227     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 228     failsafeOnValidDataReceived();                  // cause a recovered link
 229
 230     // when
 231     failsafeUpdateState();
 232
 233     // then
 234     EXPECT_TRUE(failsafeIsActive());
 235     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 236     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 237     EXPECT_TRUE(isArmingDisabled());
 238
 239     // given
 240     sysTickUptime += PERIOD_OF_3_SECONDS + 1;      // adjust time to point just past the required additional recovery time
 241     failsafeOnValidDataReceived();
 242
 243     // when
 244     failsafeUpdateState();
 245
 246     // then
 247     EXPECT_FALSE(failsafeIsActive());
 248     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 249     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
 250     EXPECT_FALSE(isArmingDisabled());
 251 }
 252
 253 /****************************************************************************************/
 254 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
 255 {
 256     // given
 257     DISABLE_ARMING_FLAG(ARMED);
 258     resetCallCounters();
 259
 260     // and
 261     failsafeStartMonitoring();
 262     throttleStatus = THROTTLE_LOW;                  // throttle LOW to go for a failsafe just-disarm procedure
 263     sysTickUptime = 0;                              // restart time from 0
 264     failsafeOnValidDataReceived();                  // set last valid sample at current time
 265
 266     // when
 267     for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
 268         failsafeOnValidDataFailed();
 269
 270         failsafeUpdateState();
 271
 272         // then
 273         EXPECT_FALSE(failsafeIsActive());
 274         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 275     }
 276
 277     // given
 278     sysTickUptime++;                                // adjust time to point just past the failure time to
 279     failsafeOnValidDataFailed();                    // cause a lost link
 280     ENABLE_ARMING_FLAG(ARMED);                      // armed from here (disarmed state has cleared throttleLowPeriod).
 281
 282     // when
 283     failsafeUpdateState();
 284
 285     // then
 286     EXPECT_TRUE(failsafeIsActive());
 287     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 288     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 289     EXPECT_TRUE(isArmingDisabled());
 290
 291     // given
 292     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 293     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 294     failsafeOnValidDataReceived();                  // cause a recovered link
 295
 296     // when
 297     failsafeUpdateState();
 298
 299     // then
 300     EXPECT_TRUE(failsafeIsActive());
 301     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 302     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 303     EXPECT_TRUE(isArmingDisabled());
 304
 305     // given
 306     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 307     failsafeOnValidDataReceived();
 308
 309     // when
 310     failsafeUpdateState();
 311
 312     // then
 313     EXPECT_FALSE(failsafeIsActive());
 314     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 315     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 316     EXPECT_FALSE(isArmingDisabled());
 317 }
 318
 319 /****************************************************************************************/
 320 TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
 321 {
 322     // given
 323     ENABLE_ARMING_FLAG(ARMED);
 324     resetCallCounters();
 325     failsafeStartMonitoring();
 326
 327     // and
 328     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 329     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
 330
 331     activateBoxFailsafe();
 332
 333     sysTickUptime = 0;                              // restart time from 0
 334     failsafeOnValidDataReceived();                  // set last valid sample at current time
 335     sysTickUptime = PERIOD_RXDATA_FAILURE + 1;      // adjust time to point just past the failure time to
 336     failsafeOnValidDataFailed();                    // cause a lost link
 337
 338     // when
 339     failsafeUpdateState();                          // kill switch handling should come first
 340
 341     // then
 342     EXPECT_TRUE(failsafeIsActive());
 343     EXPECT_TRUE(isArmingDisabled());
 344     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 345     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 346
 347     // given
 348     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 349     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 350     failsafeOnValidDataReceived();                  // cause a recovered link
 351
 352     deactivateBoxFailsafe();
 353
 354     // when
 355     failsafeUpdateState();
 356
 357     // then
 358     EXPECT_TRUE(failsafeIsActive());
 359     EXPECT_TRUE(isArmingDisabled());
 360     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 361     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 362
 363     // given
 364     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 365     failsafeOnValidDataReceived();
 366
 367     // when
 368     failsafeUpdateState();
 369
 370     // then
 371     EXPECT_FALSE(failsafeIsActive());
 372     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 373     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 374     EXPECT_FALSE(isArmingDisabled());
 375 }
 376
 377 TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Drop)
 378 {
 379     // given
 380     ENABLE_ARMING_FLAG(ARMED);
 381     resetCallCounters();
 382
 383     // and
 384     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 385     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
 386     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
 387
 388
 389
 390     sysTickUptime = 0;                              // restart time from 0
 391     activateBoxFailsafe();
 392     failsafeOnValidDataFailed();                    // box failsafe causes data to be invalid
 393
 394     // when
 395     failsafeUpdateState();                          // should activate stage2 immediately
 396
 397     // then
 398     EXPECT_TRUE(failsafeIsActive());
 399     EXPECT_TRUE(isArmingDisabled());
 400     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 401     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 402
 403     // given
 404     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 405     deactivateBoxFailsafe();
 406     failsafeOnValidDataReceived();                  // inactive box failsafe gives valid data
 407
 408     // when
 409     failsafeUpdateState();
 410
 411     // then
 412     EXPECT_FALSE(failsafeIsActive());
 413     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 414     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 415     EXPECT_FALSE(isArmingDisabled());
 416 }
 417
 418 TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
 419 {
 420     // given
 421     ENABLE_ARMING_FLAG(ARMED);
 422     resetCallCounters();
 423
 424     // and
 425     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 426     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
 427     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
 428
 429
 430     sysTickUptime = 0;                              // restart time from 0
 431     activateBoxFailsafe();
 432     failsafeOnValidDataFailed();                    // box failsafe causes data to be invalid
 433
 434     // when
 435     failsafeUpdateState();                          // should activate stage2 immediately
 436
 437     // then
 438     EXPECT_TRUE(failsafeIsActive());               // stick induced failsafe allows re-arming
 439     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 440     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 441
 442
 443     sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
 444
 445     // given
 446     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 447
 448     // when
 449     failsafeUpdateState();
 450
 451     // then
 452     EXPECT_TRUE(failsafeIsActive());
 453     EXPECT_TRUE(isArmingDisabled());
 454     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 455     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 456
 457     // given
 458     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 459
 460     // and
 461     deactivateBoxFailsafe();
 462     failsafeOnValidDataReceived();                   // inactive box failsafe gives valid data
 463
 464     // when
 465     failsafeUpdateState();
 466
 467     // then
 468     EXPECT_FALSE(failsafeIsActive());
 469     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 470     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 471     EXPECT_FALSE(isArmingDisabled());
 472 }
 473
 474
 475 /****************************************************************************************/
 476 //
 477 // Additional non-stepwise tests
 478 //
 479 /****************************************************************************************/
 480 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
 481 {
 482     // given
 483     resetCallCounters();
 484     configureFailsafe();
 485
 486     // and
 487     failsafeInit();
 488
 489     // and
 490     DISABLE_ARMING_FLAG(ARMED);
 491
 492     // when
 493     failsafeStartMonitoring();
 494
 495     // and
 496     sysTickUptime = 0;                              // restart time from 0
 497     failsafeOnValidDataReceived();                  // set last valid sample at current time
 498
 499     // enter stage 1 failsafe
 500     for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
 501         failsafeOnValidDataFailed();
 502
 503         failsafeUpdateState();
 504
 505         // then
 506         EXPECT_FALSE(failsafeIsActive());
 507         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 508     }
 509
 510     // given
 511     sysTickUptime++;                                // adjust time to point just past the failure time to
 512     failsafeOnValidDataFailed();                    // cause a lost link
 513
 514     // when
 515     failsafeUpdateState();
 516
 517     // then
 518     EXPECT_TRUE(failsafeIsMonitoring());
 519     EXPECT_FALSE(failsafeIsActive());
 520     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 521     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 522     EXPECT_FALSE(isArmingDisabled());               // arming not blocked in stage 1
 523
 524     // add enough time to enter stage 2
 525     sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
 526     failsafeOnValidDataFailed();
 527
 528     // when
 529     failsafeUpdateState();
 530
 531     // then
 532     EXPECT_TRUE(isArmingDisabled());                // arming blocked until recovery time
 533     EXPECT_FALSE(failsafeIsActive());               // failsafe is still not active
 534
 535     // given valid data is received for the recovery time, allow re-arming
 536     uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY;
 537     for (; sysTickUptime < sysTickTarget; sysTickUptime++) {
 538         failsafeOnValidDataReceived();
 539         failsafeUpdateState();
 540
 541         EXPECT_TRUE(isArmingDisabled());
 542     }
 543
 544     // and
 545     sysTickUptime++;                                // adjust time to point just past the failure time to
 546     failsafeOnValidDataReceived();                  // cause link recovery
 547
 548     // then
 549     EXPECT_FALSE(isArmingDisabled());
 550 }
 551
 552 // STUBS
 553
 554 extern "C" {
 555 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 556 float rcCommand[4];
 557 int16_t debug[DEBUG16_VALUE_COUNT];
 558 uint8_t debugMode = 0;
 559 bool isUsingSticksToArm = true;
 560
 561 PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
 562
 563 // Return system uptime in milliseconds (rollover in 49 days)
 564 uint32_t millis(void)
 565 {
 566     return sysTickUptime;
 567 }
 568
 569 uint32_t micros(void)
 570 {
 571     return millis() * 1000;
 572 }
 573
 574 throttleStatus_e calculateThrottleStatus()
 575 {
 576     return throttleStatus;
 577 }
 578
 579 void delay(uint32_t) {}
 580
 581 bool featureIsEnabled(uint32_t mask) {
 582     return (mask & testFeatureMask);
 583 }
 584
 585 void disarm(flightLogDisarmReason_e) {
 586     callCounts[COUNTER_MW_DISARM]++;
 587 }
 588
 589 void beeper(beeperMode_e mode) {
 590     UNUSED(mode);
 591 }
 592
 593 uint16_t getCurrentMinthrottle(void)
 594 {
 595     return testMinThrottle;
 596 }
 597
 598 bool isUsingSticksForArming(void)
 599 {
 600     return isUsingSticksToArm;
 601 }
 602
 603 bool areSticksActive(uint8_t stickPercentLimit) {
 604     UNUSED(stickPercentLimit);
 605     return false;
 606 }
 607
 608 void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
 609
 610 bool crashRecoveryModeActive(void) { return false; }
 611 void pinioBoxTaskControl(void) {}
 612 }