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)(PERIOD_RXDATA_FAILURE + 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_30_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_1_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());
 439     EXPECT_TRUE(isArmingDisabled());
 440     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 441     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 442
 443
 444     sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
 445
 446     // given
 447     failsafeOnValidDataFailed();                    // set last invalid sample at current time
 448
 449     // when
 450     failsafeUpdateState();
 451
 452     // then
 453     EXPECT_TRUE(failsafeIsActive());
 454     EXPECT_TRUE(isArmingDisabled());
 455     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 456     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 457
 458     // given
 459     sysTickUptime += PERIOD_OF_30_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 460
 461     // and
 462     deactivateBoxFailsafe();
 463     failsafeOnValidDataReceived();                   // inactive box failsafe gives valid data
 464
 465     // when
 466     failsafeUpdateState();
 467
 468     // then
 469     EXPECT_FALSE(failsafeIsActive());
 470     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 471     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 472     EXPECT_FALSE(isArmingDisabled());
 473 }
 474
 475
 476 /****************************************************************************************/
 477 //
 478 // Additional non-stepwise tests
 479 //
 480 /****************************************************************************************/
 481 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
 482 {
 483     // given
 484     resetCallCounters();
 485     configureFailsafe();
 486
 487     // and
 488     failsafeInit();
 489
 490     // and
 491     DISABLE_ARMING_FLAG(ARMED);
 492
 493     // when
 494     failsafeStartMonitoring();
 495
 496     // and
 497     sysTickUptime = 0;                              // restart time from 0
 498     failsafeOnValidDataReceived();                  // set last valid sample at current time
 499
 500     // when
 501     for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
 502         failsafeOnValidDataFailed();
 503
 504         failsafeUpdateState();
 505
 506         // then
 507         EXPECT_FALSE(failsafeIsActive());
 508         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 509     }
 510
 511     // given
 512     sysTickUptime++;                                // adjust time to point just past the failure time to
 513     failsafeOnValidDataFailed();                    // cause a lost link
 514
 515     // when
 516     failsafeUpdateState();
 517
 518     // then
 519     EXPECT_TRUE(failsafeIsMonitoring());
 520     EXPECT_FALSE(failsafeIsActive());
 521     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 522     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 523     EXPECT_TRUE(isArmingDisabled());
 524
 525     // given
 526     // enough valid data is received
 527     uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY;
 528     for (; sysTickUptime < sysTickTarget; sysTickUptime++) {
 529         failsafeOnValidDataReceived();
 530         failsafeUpdateState();
 531
 532         EXPECT_TRUE(isArmingDisabled());
 533     }
 534
 535     // and
 536     sysTickUptime++;                                // adjust time to point just past the failure time to
 537     failsafeOnValidDataReceived();                  // cause link recovery
 538
 539     // then
 540     EXPECT_FALSE(isArmingDisabled());
 541 }
 542
 543 // STUBS
 544
 545 extern "C" {
 546 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 547 float rcCommand[4];
 548 int16_t debug[DEBUG16_VALUE_COUNT];
 549 bool isUsingSticksToArm = true;
 550
 551 PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
 552
 553 // Return system uptime in milliseconds (rollover in 49 days)
 554 uint32_t millis(void)
 555 {
 556     return sysTickUptime;
 557 }
 558
 559 uint32_t micros(void)
 560 {
 561     return millis() * 1000;
 562 }
 563
 564 throttleStatus_e calculateThrottleStatus()
 565 {
 566     return throttleStatus;
 567 }
 568
 569 void delay(uint32_t) {}
 570
 571 bool featureIsEnabled(uint32_t mask) {
 572     return (mask & testFeatureMask);
 573 }
 574
 575 void disarm(flightLogDisarmReason_e) {
 576     callCounts[COUNTER_MW_DISARM]++;
 577 }
 578
 579 void beeper(beeperMode_e mode) {
 580     UNUSED(mode);
 581 }
 582
 583 uint16_t getCurrentMinthrottle(void)
 584 {
 585     return testMinThrottle;
 586 }
 587
 588 bool isUsingSticksForArming(void)
 589 {
 590     return isUsingSticksToArm;
 591 }
 592
 593 bool areSticksActive(uint8_t stickPercentLimit) {
 594     UNUSED(stickPercentLimit);
 595     return false;
 596 }
 597
 598 void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
 599
 600 bool crashRecoveryModeActive(void) { return false; }
 601 void pinioBoxTaskControl(void) {}
 602 }