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 {
  68     memset(&callCounts, 0, sizeof(callCounts));
  69 }
  70
  71 #define TEST_MID_RC 1495            // something other than the default 1500 will suffice.
  72 #define TEST_MIN_CHECK 1100;
  73 #define PERIOD_OF_10_SCONDS 10000
  74 #define DE_ACTIVATE_ALL_BOXES 0
  75
  76 uint32_t sysTickUptime;
  77
  78 void configureFailsafe(void)
  79 {
  80     rxConfigMutable()->midrc = TEST_MID_RC;
  81     rxConfigMutable()->mincheck = TEST_MIN_CHECK;
  82
  83     failsafeConfigMutable()->failsafe_delay = 10; // 1 second
  84     failsafeConfigMutable()->failsafe_off_delay = 15; // 1.5 seconds
  85     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
  86     failsafeConfigMutable()->failsafe_throttle = 1200;
  87     failsafeConfigMutable()->failsafe_throttle_low_delay = 100; // 10 seconds
  88     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
  89     // NB we don't have failsafe_recovery_delay so use PERIOD_RXDATA_RECOVERY (200ms)
  90     sysTickUptime = 0;
  91 }
  92
  93 void activateBoxFailsafe()
  94 {
  95     boxBitmask_t newMask;
  96     bitArraySet(&newMask, BOXFAILSAFE);
  97     rcModeUpdate(&newMask);
  98 }
  99
 100 void deactivateBoxFailsafe()
 101 {
 102     boxBitmask_t newMask;
 103     memset(&newMask, 0, sizeof(newMask));
 104     rcModeUpdate(&newMask);
 105 }
 106
 107 //
 108 // Stepwise tests
 109 //
 110
 111 /****************************************************************************************/
 112 TEST(FlightFailsafeTest, TestFailsafeInitialState)
 113 {
 114     // given
 115     configureFailsafe();
 116     // and
 117     DISABLE_ARMING_FLAG(ARMED);
 118
 119     // when
 120     failsafeInit();
 121     failsafeReset();
 122
 123     // then
 124     EXPECT_FALSE(failsafeIsMonitoring());
 125     EXPECT_FALSE(failsafeIsActive());
 126     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 127 }
 128
 129 /****************************************************************************************/
 130 TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
 131 {
 132     // when
 133     failsafeStartMonitoring();
 134
 135     // then
 136     EXPECT_TRUE(failsafeIsMonitoring());
 137     EXPECT_FALSE(failsafeIsActive());
 138     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 139 }
 140
 141 /****************************************************************************************/
 142 TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
 143 {
 144     // given
 145     ENABLE_ARMING_FLAG(ARMED);
 146
 147     // when
 148     sysTickUptime ++;
 149     failsafeOnValidDataFailed();                    // set last invalid sample to a non-zero value
 150     sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
 151     failsafeOnValidDataReceived();                  // cause a recovered link
 152
 153     // and
 154     failsafeUpdateState();
 155
 156     // then
 157     EXPECT_FALSE(failsafeIsActive());
 158     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 159 }
 160
 161 /****************************************************************************************/
 162 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
 163 {
 164     // when
 165     for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
 166         failsafeOnValidDataReceived();
 167
 168         failsafeUpdateState();
 169
 170         // then
 171         EXPECT_FALSE(failsafeIsActive());
 172         EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 173     }
 174 }
 175
 176 /****************************************************************************************/
 177 //
 178 // Clean start
 179 //
 180 /****************************************************************************************/
 181 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
 182 {
 183     // given
 184     configureFailsafe();
 185     failsafeInit();
 186
 187     DISABLE_ARMING_FLAG(ARMED);
 188     resetCallCounters();
 189     failsafeStartMonitoring();
 190
 191     // then
 192     EXPECT_TRUE(failsafeIsMonitoring());
 193     EXPECT_FALSE(failsafeIsActive());
 194     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 195
 196     // given
 197     ENABLE_ARMING_FLAG(ARMED);
 198     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 199     failsafeOnValidDataReceived();
 200
 201     sysTickUptime = 0;
 202     failsafeUpdateState();
 203
 204     // then
 205     EXPECT_FALSE(failsafeIsActive());
 206     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 207     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 208     EXPECT_FALSE(isArmingDisabled());
 209
 210     // simulate an Rx loss for the stage 1 duration
 211     sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
 212     failsafeOnValidDataFailed();
 213     failsafeUpdateState();
 214
 215     //  should be in stage 1
 216     EXPECT_FALSE(failsafeIsActive());
 217     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 218     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 219     EXPECT_TRUE(isArmingDisabled());
 220
 221     sysTickUptime ++;                               // exceed the stage 1 period by one tick
 222     failsafeOnValidDataFailed();                    // confirm that we still have no valid data
 223
 224     failsafeUpdateState();
 225
 226     // we should now be in stage 2, landing phase
 227     EXPECT_TRUE(failsafeIsActive());
 228     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 229 }
 230
 231 /****************************************************************************************/
 232 TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
 233 // note this test follows on from the previous test
 234 {
 235     // exceed the stage 2 landing time
 236     sysTickUptime += (failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND);
 237     failsafeOnValidDataFailed();                    // confirm that we still have no valid data
 238
 239     // when
 240     failsafeUpdateState();
 241
 242     // expect to still be in landing phase
 243     EXPECT_TRUE(failsafeIsActive());
 244     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 245
 246     // adjust time to point just past the landing time
 247     sysTickUptime++;
 248     failsafeOnValidDataFailed();                    // confirm that we still have no valid data
 249
 250     // when
 251     failsafeUpdateState();
 252
 253     // expect to be in monitoring mode
 254     EXPECT_TRUE(failsafeIsActive());
 255     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 256     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 257     EXPECT_TRUE(isArmingDisabled());
 258
 259     // let's wait 3 seconds and still get no signal
 260     sysTickUptime += PERIOD_OF_3_SECONDS;
 261     failsafeOnValidDataFailed();                    // confirm that we still have no valid data
 262     // when
 263     failsafeUpdateState();
 264
 265     // nothing should change
 266     EXPECT_TRUE(failsafeIsActive());
 267     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 268     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 269     EXPECT_TRUE(isArmingDisabled());
 270
 271     // now lets get a signal while in monitoring mode
 272     failsafeOnValidDataReceived();
 273     // we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
 274     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 275     failsafeOnValidDataReceived();
 276     failsafeUpdateState();
 277
 278     // nothing should change
 279     EXPECT_TRUE(failsafeIsActive());
 280     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 281     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 282     EXPECT_TRUE(isArmingDisabled());
 283
 284     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 285     failsafeOnValidDataReceived();
 286     failsafeUpdateState();
 287
 288     // nothing should change
 289     EXPECT_TRUE(failsafeIsActive());
 290     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 291     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 292     EXPECT_TRUE(isArmingDisabled());
 293
 294     // One tick later
 295     sysTickUptime++;
 296     failsafeOnValidDataReceived();
 297     // when
 298     failsafeUpdateState();
 299
 300     // we expect failsafe to finish and to revert to idle
 301     EXPECT_FALSE(failsafeIsActive());
 302     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 303     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
 304     EXPECT_FALSE(isArmingDisabled());
 305 }
 306
 307 /****************************************************************************************/
 308 TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
 309 //  Just Disarm is when throttle is low for >10s before signal loss
 310 //  we should exit stage 1 directly into failsafe monitoring mode, and not enter landing mode
 311 {
 312     ENABLE_ARMING_FLAG(ARMED);
 313     resetCallCounters();
 314     failsafeStartMonitoring();
 315
 316     // set to normal initial state
 317     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 318     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
 319
 320     sysTickUptime += PERIOD_OF_3_SECONDS;           // 3s of normality
 321     failsafeOnValidDataReceived();                  // we have a valid signal
 322     // when
 323     failsafeUpdateState();
 324
 325     // confirm that we are in idle mode
 326     EXPECT_FALSE(failsafeIsActive());
 327     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 328     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 329     EXPECT_FALSE(isArmingDisabled());
 330
 331
 332     // run throttle_low for 11s
 333     throttleStatus = THROTTLE_LOW;                  // for failsafe 'just-disarm' procedure
 334     sysTickUptime += 11000;
 335     failsafeOnValidDataReceived();                  // set the last valid signal to now
 336
 337     failsafeUpdateState();
 338
 339     // check that we are still in idle mode
 340     EXPECT_FALSE(failsafeIsActive());
 341     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 342     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 343     EXPECT_FALSE(isArmingDisabled());
 344
 345     // simulate an Rx loss for the stage 1 duration
 346     sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
 347     failsafeOnValidDataFailed();
 348     failsafeUpdateState();
 349
 350     //  should remain in stage 1
 351     EXPECT_FALSE(failsafeIsActive());
 352     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 353     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 354     EXPECT_TRUE(isArmingDisabled());
 355
 356     sysTickUptime ++;                               // now we exceed stage 1
 357     failsafeOnValidDataFailed();                    // we still have no valid data
 358     failsafeUpdateState();
 359
 360     // we should now be in stage 2 via Just Disarm, going direct to monitoring mode.
 361     EXPECT_TRUE(failsafeIsActive());
 362     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 363     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 364     EXPECT_TRUE(isArmingDisabled());
 365
 366     // now lets get a signal while in monitoring mode
 367     failsafeOnValidDataReceived();
 368     // we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
 369     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 370     failsafeOnValidDataReceived();
 371     failsafeUpdateState();
 372
 373     // nothing should change
 374     EXPECT_TRUE(failsafeIsActive());
 375     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 376     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 377     EXPECT_TRUE(isArmingDisabled());
 378
 379     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 380     failsafeOnValidDataReceived();
 381     failsafeUpdateState();
 382
 383     // nothing should change
 384     EXPECT_TRUE(failsafeIsActive());
 385     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 386     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 387     EXPECT_TRUE(isArmingDisabled());
 388
 389     // One tick later
 390     sysTickUptime++;
 391     failsafeOnValidDataReceived();
 392     // when
 393     failsafeUpdateState();
 394
 395     // we expect failsafe to finish and to revert to idle
 396     EXPECT_FALSE(failsafeIsActive());
 397     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 398     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
 399     EXPECT_FALSE(isArmingDisabled());}
 400
 401 /****************************************************************************************/
 402 TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
 403 {
 404     // given
 405     ENABLE_ARMING_FLAG(ARMED);
 406     resetCallCounters();
 407     failsafeStartMonitoring();
 408
 409     // set to normal initial state
 410     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 411     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
 412
 413     sysTickUptime += PERIOD_OF_3_SECONDS;           // 3s of normality
 414     failsafeOnValidDataReceived();                  // we have a valid signal
 415     // when
 416     failsafeUpdateState();
 417
 418     // confirm that we are in idle mode
 419     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 420     EXPECT_FALSE(failsafeIsActive());
 421     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 422
 423     // given
 424     activateBoxFailsafe();                          // activate the Kill swith
 425
 426     // when
 427     failsafeUpdateState();                          // should failsafe immediately the kill switch is hit
 428
 429     // then
 430     EXPECT_TRUE(failsafeIsActive());
 431     EXPECT_TRUE(isArmingDisabled());
 432     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 433     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 434
 435     // given
 436     failsafeOnValidDataReceived();                  // the link is active the whole time
 437
 438     // deactivate the kill switch
 439     deactivateBoxFailsafe();                        // receivingRxData is immediately true
 440
 441     // we should go to failsafe monitoring mode, via Landing
 442     EXPECT_TRUE(failsafeIsActive());
 443     EXPECT_TRUE(isArmingDisabled());
 444     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 445     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 446
 447     // since we didn't enter stage 2, we have one rxDataRecoveryPeriod delay to deal with
 448     // while within rxDataRecoveryPeriod in monitoring mode...
 449     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 450     failsafeOnValidDataReceived();
 451
 452     // when
 453     failsafeUpdateState();
 454
 455     // we should still be in failsafe monitoring mode
 456     EXPECT_TRUE(failsafeIsActive());
 457     EXPECT_TRUE(isArmingDisabled());
 458     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 459     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 460
 461     // one tick later
 462     sysTickUptime ++;
 463
 464     // when
 465     failsafeUpdateState();
 466
 467     // we should now have exited failsafe
 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 TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage1OrStage2Drop)
 477 // should immediately stop motors and go to monitoring mode diretly
 478 {
 479     // given a clean start
 480     ENABLE_ARMING_FLAG(ARMED);
 481     resetCallCounters();
 482     failsafeStartMonitoring();
 483
 484     // and set initial states
 485     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 486     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
 487     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
 488
 489     sysTickUptime = 0;                              // restart time from 0
 490     failsafeOnValidDataReceived();                  // we have a valid signal
 491     sysTickUptime += 3000;                          // 3s of normality
 492
 493     // when
 494     failsafeUpdateState();
 495
 496     // confirm that we are in idle mode
 497     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 498     EXPECT_FALSE(failsafeIsActive());
 499     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 500
 501     // given
 502     activateBoxFailsafe();                          // activate the stage 2 Drop switch
 503     failsafeOnValidDataFailed();                    // immediate stage 2 switch sets failsafeOnValidDataFailed
 504
 505     // when
 506     failsafeUpdateState();                          // should activate stage2 immediately, even though signal is good
 507
 508     // expect to be in monitoring mode, and to have disarmed
 509     EXPECT_TRUE(failsafeIsActive());
 510     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 511     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 512     EXPECT_TRUE(isArmingDisabled());
 513
 514     // deactivate the failsafe switch
 515     deactivateBoxFailsafe();
 516
 517     EXPECT_TRUE(failsafeIsActive());
 518     EXPECT_TRUE(isArmingDisabled());
 519     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 520     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 521
 522     // by next evaluation we should be out of failsafe
 523     sysTickUptime ++;
 524     // receivingRxData is immediately true because signal exists
 525     failsafeOnValidDataReceived();
 526
 527     // when
 528     failsafeUpdateState();
 529
 530     // we should now have exited failsafe
 531     EXPECT_FALSE(failsafeIsActive());
 532     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 533     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 534     EXPECT_FALSE(isArmingDisabled());
 535 }
 536
 537 /****************************************************************************************/
 538
 539 TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
 540 {
 541     // given a clean start
 542     ENABLE_ARMING_FLAG(ARMED);
 543     resetCallCounters();
 544     failsafeStartMonitoring();
 545
 546     // and
 547     throttleStatus = THROTTLE_HIGH;                 // throttle HIGH to go for a failsafe landing procedure
 548     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
 549     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
 550
 551     sysTickUptime = 0;                              // restart time from 0
 552     failsafeOnValidDataReceived();                  // we have a valid signal
 553     sysTickUptime += 3000;                          // 3s of normality
 554
 555     // when
 556     failsafeUpdateState();
 557
 558     // confirm that we are in idle mode
 559     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 560     EXPECT_FALSE(failsafeIsActive());
 561     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 562
 563     // given
 564     activateBoxFailsafe();                          // activate the stage 2 Drop switch
 565     failsafeOnValidDataFailed();                    // immediate stage 2 switch sets failsafeOnValidDataFailed
 566
 567     // when
 568     failsafeUpdateState();                          // should activate stage2 immediately
 569
 570     // expect to immediately be in landing mode, and not disarmed
 571     EXPECT_TRUE(failsafeIsActive());               // stick induced failsafe allows re-arming
 572     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 573     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 574
 575     // should stay in landing for failsafe_off_delay (stage 2 period) of 1s
 576     sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 577
 578     // when
 579     failsafeUpdateState();
 580
 581     EXPECT_TRUE(failsafeIsActive());                    // stick induced failsafe allows re-arming
 582     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 583     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 584
 585     // one tick later, landing time has elapsed
 586     sysTickUptime ++;
 587
 588     // when
 589     failsafeUpdateState();
 590
 591     // now should be in monitoring mode, with switch holding signalReceived false
 592     EXPECT_TRUE(failsafeIsActive());
 593     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 594     EXPECT_TRUE(isArmingDisabled());
 595     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 596
 597     // given
 598     sysTickUptime += PERIOD_OF_3_SECONDS;               // hang around a bit waiting for switch change
 599
 600     // when
 601     failsafeUpdateState();
 602
 603     // should still be in monitoring mode because switch is still forcing receivingRxData false
 604     EXPECT_TRUE(failsafeIsActive());
 605     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 606     EXPECT_TRUE(isArmingDisabled());
 607     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 608
 609     // deactivate the failsafe switch
 610     deactivateBoxFailsafe();
 611
 612     // receivingRxData is immediately true
 613     // we go directly to failsafe monitoring mode, via Landing
 614     // however, because the switch also forces rxFlightChannelsValid false, emulating real failsafe
 615     // we have two delays to deal with before we can re-arm
 616
 617     EXPECT_TRUE(failsafeIsActive());
 618     EXPECT_TRUE(isArmingDisabled());
 619     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
 620     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 621
 622
 623     // one tick later
 624     sysTickUptime ++;
 625     failsafeOnValidDataReceived();
 626
 627     // when
 628     failsafeUpdateState();
 629
 630     // we should now have exited failsafe
 631     EXPECT_FALSE(failsafeIsActive());
 632     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 633     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
 634     EXPECT_FALSE(isArmingDisabled());
 635 }
 636
 637
 638 /****************************************************************************************/
 639 //
 640 // Clean start
 641 //
 642 /****************************************************************************************/
 643 TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
 644 {
 645     // given
 646     resetCallCounters();
 647     configureFailsafe();
 648
 649     // and
 650     failsafeInit();
 651
 652     // and ** WE ARE DISARMED **
 653     DISABLE_ARMING_FLAG(ARMED);
 654
 655     // when failsafe starts
 656     failsafeStartMonitoring();
 657
 658     // and
 659     sysTickUptime = 0;                              // restart time from 0
 660     failsafeOnValidDataReceived();                  // we have a valid signal
 661     sysTickUptime += 3000;                          // 3s of normality
 662
 663     // when
 664     failsafeUpdateState();
 665
 666     // confirm that we are in idle mode
 667     EXPECT_FALSE(failsafeIsActive());
 668     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 669     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 670
 671     // and that arming is not disabled
 672     EXPECT_FALSE(isArmingDisabled());
 673
 674     // simulate an Rx loss for the stage 1 duration
 675     sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);
 676     failsafeOnValidDataFailed();
 677     failsafeUpdateState();
 678
 679     // within stage 1 time from a true failsafe while disarmed, stage 2 should normally not be active
 680     EXPECT_FALSE(failsafeIsActive());
 681     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 682     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 683
 684     // arming is disabled due to setArmingDisabled in stage 1 due to failsafeOnValidDataFailed()
 685     EXPECT_FALSE(failsafeIsActive());
 686
 687    // given
 688     sysTickUptime++;                                // adjust time to point just past stage 1
 689     failsafeOnValidDataFailed();                    // would normally enter stage 2, but we are disarmed
 690     failsafeUpdateState();
 691
 692     // expect that we do not enter failsafe stage 2, ie no change from above
 693     EXPECT_FALSE(failsafeIsActive());
 694     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 695     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 696
 697     // the lock on the aux channels persists at fixed stage 1 values until recovery time
 698     EXPECT_TRUE(isArmingDisabled());
 699
 700     // allow signal received for the recovery time
 701     sysTickUptime += PERIOD_RXDATA_RECOVERY;
 702     failsafeOnValidDataReceived();
 703     failsafeUpdateState();
 704
 705     // no change in failsafe state (still idle)
 706     EXPECT_FALSE(failsafeIsActive());
 707     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
 708     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
 709
 710     // but now arming is possible
 711     EXPECT_FALSE(isArmingDisabled());
 712 }
 713
 714 // STUBS
 715
 716 extern "C" {
 717 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 718 float rcCommand[4];
 719 int16_t debug[DEBUG16_VALUE_COUNT];
 720 uint8_t debugMode = 0;
 721 bool isUsingSticksToArm = true;
 722
 723 PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
 724
 725 // Return system uptime in milliseconds (rollover in 49 days)
 726 uint32_t millis(void)
 727 {
 728     return sysTickUptime;
 729 }
 730
 731 uint32_t micros(void)
 732 {
 733     return millis() * 1000;
 734 }
 735
 736 throttleStatus_e calculateThrottleStatus()
 737 {
 738     return throttleStatus;
 739 }
 740
 741 void delay(uint32_t) {}
 742
 743 bool featureIsEnabled(uint32_t mask)
 744 {
 745     return (mask & testFeatureMask);
 746 }
 747
 748 void disarm(flightLogDisarmReason_e)
 749 {
 750     callCounts[COUNTER_MW_DISARM]++;
 751 }
 752
 753 void beeper(beeperMode_e mode)
 754 {
 755     UNUSED(mode);
 756 }
 757
 758 uint16_t getCurrentMinthrottle(void)
 759 {
 760     return testMinThrottle;
 761 }
 762
 763 bool isUsingSticksForArming(void)
 764 {
 765     return isUsingSticksToArm;
 766 }
 767
 768 bool areSticksActive(uint8_t stickPercentLimit)
 769 {
 770     UNUSED(stickPercentLimit);
 771     return false;
 772 }
 773
 774 void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
 775
 776 bool crashRecoveryModeActive(void) { return false; }
 777 void pinioBoxTaskControl(void) {}
 778 }