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