src/test/unit/arming_prevention_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
  20 extern "C" {
  21     #include "blackbox/blackbox.h"
  22     #include "build/debug.h"
  23     #include "common/maths.h"
  24     #include "config/feature.h"
  25     #include "pg/motor.h"
  26     #include "pg/pg.h"
  27     #include "pg/pg_ids.h"
  28     #include "pg/rx.h"
  29     #include "config/config.h"
  30     #include "fc/controlrate_profile.h"
  31     #include "fc/core.h"
  32     #include "fc/rc_controls.h"
  33     #include "fc/rc_modes.h"
  34     #include "fc/runtime_config.h"
  35     #include "flight/failsafe.h"
  36     #include "flight/imu.h"
  37     #include "flight/mixer.h"
  38     #include "flight/pid.h"
  39     #include "flight/servos.h"
  40     #include "io/beeper.h"
  41     #include "io/gps.h"
  42     #include "rx/rx.h"
  43     #include "scheduler/scheduler.h"
  44     #include "sensors/acceleration.h"
  45     #include "sensors/gyro.h"
  46     #include "telemetry/telemetry.h"
  47     #include "flight/gps_rescue.h"
  48
  49     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
  50     PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
  51     PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
  52     PG_REGISTER(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
  53     PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0);
  54     PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
  55     PG_REGISTER(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
  56     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
  57     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
  58     PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
  59     PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
  60
  61     float rcCommand[4];
  62     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  63     uint16_t averageSystemLoadPercent = 0;
  64     uint8_t cliMode = 0;
  65     uint8_t debugMode = 0;
  66     int16_t debug[DEBUG16_VALUE_COUNT];
  67     pidProfile_t *currentPidProfile;
  68     controlRateConfig_t *currentControlRateProfile;
  69     attitudeEulerAngles_t attitude;
  70     gpsSolutionData_t gpsSol;
  71     uint32_t targetPidLooptime;
  72     bool cmsInMenu = false;
  73     float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
  74     rxRuntimeState_t rxRuntimeState = {};
  75     uint16_t GPS_distanceToHome = 0;
  76     int16_t GPS_directionToHome = 0;
  77     acc_t acc = {};
  78     bool mockIsUpright = false;
  79     uint8_t activePidLoopDenom = 1;
  80 }
  81
  82 uint32_t simulationFeatureFlags = 0;
  83 uint32_t simulationTime = 0;
  84 bool gyroCalibDone = false;
  85 bool simulationHaveRx = false;
  86
  87 #include "gtest/gtest.h"
  88
  89 TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch)
  90 {
  91     // given
  92     simulationTime = 0;
  93     gyroCalibDone = false;
  94     sensorsSet(SENSOR_GYRO);
  95
  96     // and
  97     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
  98     modeActivationConditionsMutable(0)->modeId = BOXARM;
  99     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 100     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 101     rcControlsInit();
 102
 103     // and
 104     rxConfigMutable()->mincheck = 1050;
 105
 106     // and
 107     // default channel positions
 108     rcData[THROTTLE] = 1400;
 109     rcData[4] = 1800;
 110
 111     // and
 112     systemConfigMutable()->powerOnArmingGraceTime = 5;
 113     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 114
 115     // when
 116     updateActivatedModes();
 117     updateArmingStatus();
 118
 119     // expect
 120     EXPECT_TRUE(isArmingDisabled());
 121     EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_CALIBRATING | ARMING_DISABLED_ANGLE | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 122
 123     // given
 124     // gyro calibration is done
 125     gyroCalibDone = true;
 126
 127     // when
 128     updateActivatedModes();
 129     updateArmingStatus();
 130
 131     // expect
 132     EXPECT_TRUE(isArmingDisabled());
 133     EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ANGLE | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 134
 135     // given
 136     // quad is level
 137     mockIsUpright = true;
 138
 139     // when
 140     updateArmingStatus();
 141
 142     // expect
 143     EXPECT_TRUE(isArmingDisabled());
 144     EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 145
 146     // given
 147     rcData[THROTTLE] = 1000;
 148
 149     // when
 150     updateArmingStatus();
 151
 152     // expect
 153     EXPECT_TRUE(isArmingDisabled());
 154     EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 155
 156     // given
 157     // arming grace time has elapsed
 158     simulationTime += systemConfig()->powerOnArmingGraceTime * 1e6;
 159
 160     // when
 161     updateArmingStatus();
 162
 163     // expect
 164     EXPECT_TRUE(isArmingDisabled());
 165     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 166
 167     // given
 168     rcData[4] = 1000;
 169
 170     // when
 171     // arm guard time elapses
 172     updateActivatedModes();
 173     updateArmingStatus();
 174
 175     // expect
 176     EXPECT_EQ(0, getArmingDisableFlags());
 177     EXPECT_FALSE(isArmingDisabled());
 178 }
 179
 180 TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed)
 181 {
 182     // given
 183     simulationTime = 0;
 184     gyroCalibDone = false;
 185     sensorsSet(SENSOR_GYRO);
 186
 187     // and
 188     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 189     modeActivationConditionsMutable(0)->modeId = BOXARM;
 190     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 191     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 192     rcControlsInit();
 193
 194     // and
 195     rxConfigMutable()->mincheck = 1050;
 196
 197     // and
 198     rcData[THROTTLE] = 1000;
 199     mockIsUpright = true;
 200
 201     // when
 202     updateActivatedModes();
 203     updateArmingStatus();
 204
 205     // expect
 206     EXPECT_FALSE(isUsingSticksForArming());
 207     EXPECT_TRUE(isArmingDisabled());
 208     EXPECT_EQ(ARMING_DISABLED_CALIBRATING, getArmingDisableFlags());
 209
 210     // given
 211     // arm channel takes a safe default value from the RX after power on
 212     rcData[4] = 1500;
 213
 214     // and
 215     // a short time passes while calibration is in progress
 216     simulationTime += 1e6;
 217
 218     // and
 219     // during calibration RF link is established and ARM switch is on
 220     rcData[4] = 1800;
 221
 222     // when
 223     updateActivatedModes();
 224     updateArmingStatus();
 225
 226     // expect
 227     EXPECT_TRUE(isArmingDisabled());
 228     EXPECT_EQ(ARMING_DISABLED_CALIBRATING | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 229
 230     // given
 231     // calibration is done
 232     gyroCalibDone = true;
 233
 234     // when
 235     updateActivatedModes();
 236     updateArmingStatus();
 237
 238     // expect
 239     EXPECT_TRUE(isArmingDisabled());
 240     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 241
 242     // given
 243     // arm switch is switched off by user
 244     rcData[4] = 1000;
 245
 246     // when
 247     updateActivatedModes();
 248     updateArmingStatus();
 249
 250     // expect
 251     // arming enabled as arm switch has been off for sufficient time
 252     EXPECT_EQ(0, getArmingDisableFlags());
 253     EXPECT_FALSE(isArmingDisabled());
 254 }
 255
 256 TEST(ArmingPreventionTest, Prearm)
 257 {
 258     // given
 259     simulationTime = 0;
 260
 261     // and
 262     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 263     modeActivationConditionsMutable(0)->modeId = BOXARM;
 264     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 265     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 266     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 267     modeActivationConditionsMutable(1)->modeId = BOXPREARM;
 268     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 269     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 270     rcControlsInit();
 271
 272     // and
 273     rxConfigMutable()->mincheck = 1050;
 274
 275     // given
 276     rcData[THROTTLE] = 1000;
 277     mockIsUpright = true;
 278
 279     // when
 280     updateActivatedModes();
 281     updateArmingStatus();
 282
 283     // expect
 284     EXPECT_FALSE(isUsingSticksForArming());
 285     EXPECT_TRUE(isArmingDisabled());
 286     EXPECT_EQ(ARMING_DISABLED_NOPREARM, getArmingDisableFlags());
 287
 288     // given
 289     // prearm is enabled
 290     rcData[5] = 1800;
 291
 292     // when
 293     updateActivatedModes();
 294     updateArmingStatus();
 295
 296     // expect
 297     // arming enabled as arm switch has been off for sufficient time
 298     EXPECT_EQ(0, getArmingDisableFlags());
 299     EXPECT_FALSE(isArmingDisabled());
 300 }
 301
 302 TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed)
 303 {
 304     // given
 305     simulationTime = 30e6; // 30 seconds after boot
 306     gyroCalibDone = true;
 307
 308     // and
 309     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 310     modeActivationConditionsMutable(0)->modeId = BOXARM;
 311     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 312     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 313     rcControlsInit();
 314
 315     // and
 316     rxConfigMutable()->mincheck = 1050;
 317
 318     // and
 319     rcData[THROTTLE] = 1000;
 320     mockIsUpright = true;
 321
 322     // and
 323     // RX has no link to radio
 324     simulationHaveRx = false;
 325
 326     // and
 327     // arm channel has a safe default value
 328     rcData[4] = 1100;
 329
 330     // when
 331     updateActivatedModes();
 332     updateArmingStatus();
 333
 334     // expect
 335     EXPECT_FALSE(isUsingSticksForArming());
 336     EXPECT_FALSE(isArmingDisabled());
 337     EXPECT_EQ(0, getArmingDisableFlags());
 338
 339     // given
 340     // RF link is established and arm switch is turned on on radio
 341     simulationHaveRx = true;
 342     rcData[4] = 1800;
 343
 344     // when
 345     updateActivatedModes();
 346     updateArmingStatus();
 347
 348     // expect
 349     EXPECT_FALSE(isUsingSticksForArming());
 350     EXPECT_TRUE(isArmingDisabled());
 351     EXPECT_EQ(ARMING_DISABLED_BAD_RX_RECOVERY | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 352
 353     // given
 354     // arm switch turned off by user
 355     rcData[4] = 1100;
 356
 357     // when
 358     updateActivatedModes();
 359     updateArmingStatus();
 360
 361     // expect
 362     EXPECT_FALSE(isUsingSticksForArming());
 363     EXPECT_FALSE(isArmingDisabled());
 364     EXPECT_EQ(0, getArmingDisableFlags());
 365 }
 366
 367 TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband)
 368 {
 369     // given
 370     simulationFeatureFlags = FEATURE_3D; // Using 3D mode
 371     simulationTime = 30e6; // 30 seconds after boot
 372     gyroCalibDone = true;
 373
 374     // and
 375     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 376     modeActivationConditionsMutable(0)->modeId = BOXARM;
 377     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 378     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 379     rcControlsInit();
 380
 381     // and
 382     rxConfigMutable()->midrc = 1500;
 383     flight3DConfigMutable()->deadband3d_throttle = 5;
 384
 385     // and
 386     rcData[THROTTLE] = 1400;
 387     mockIsUpright = true;
 388     simulationHaveRx = true;
 389
 390     // and
 391     // arm channel has a safe default value
 392     rcData[4] = 1100;
 393
 394     // when
 395     updateActivatedModes();
 396     updateArmingStatus();
 397
 398     // expect
 399     EXPECT_FALSE(isUsingSticksForArming());
 400     EXPECT_TRUE(isArmingDisabled());
 401     EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 402
 403     // given
 404     // attempt to arm
 405     rcData[4] = 1800;
 406
 407     // when
 408     updateActivatedModes();
 409     updateArmingStatus();
 410
 411     // expect
 412     EXPECT_FALSE(isUsingSticksForArming());
 413     EXPECT_TRUE(isArmingDisabled());
 414     EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 415
 416     // given
 417     // throttle moved to centre
 418     rcData[THROTTLE] = 1496;
 419
 420     // when
 421     updateActivatedModes();
 422     updateArmingStatus();
 423
 424     // expect
 425     EXPECT_FALSE(isUsingSticksForArming());
 426     EXPECT_FALSE(isArmingDisabled());
 427     EXPECT_EQ(0, getArmingDisableFlags());
 428 }
 429
 430 TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionApplies)
 431 {
 432     // given
 433     simulationFeatureFlags = FEATURE_3D; // Using 3D mode
 434     simulationTime = 30e6; // 30 seconds after boot
 435     gyroCalibDone = true;
 436
 437     // and
 438     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 439     modeActivationConditionsMutable(0)->modeId = BOXARM;
 440     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 441     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 442     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 443     modeActivationConditionsMutable(1)->modeId = BOX3D;
 444     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 445     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 446     rcControlsInit();
 447
 448     // and
 449     rxConfigMutable()->mincheck = 1050;
 450     rxConfigMutable()->midrc = 1500;
 451     flight3DConfigMutable()->deadband3d_throttle = 5;
 452
 453     // and
 454     // safe throttle value for 3D mode
 455     rcData[THROTTLE] = 1500;
 456     mockIsUpright = true;
 457     simulationHaveRx = true;
 458
 459     // and
 460     // arm channel has a safe default value
 461     rcData[4] = 1100;
 462
 463     // and
 464     // disable 3D mode is off (i.e. 3D mode is on)
 465     rcData[5] = 1100;
 466
 467     // when
 468     updateActivatedModes();
 469     updateArmingStatus();
 470
 471     // expect
 472     // ok to arm in 3D mode
 473     EXPECT_FALSE(isUsingSticksForArming());
 474     EXPECT_FALSE(isArmingDisabled());
 475     EXPECT_EQ(0, getArmingDisableFlags());
 476
 477     // given
 478     // disable 3D mode
 479     rcData[5] = 1800;
 480
 481     // when
 482     updateActivatedModes();
 483     updateArmingStatus();
 484
 485     // expect
 486     // ok to arm in 3D mode
 487     EXPECT_FALSE(isUsingSticksForArming());
 488     EXPECT_TRUE(isArmingDisabled());
 489     EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 490
 491     // given
 492     // attempt to arm
 493     rcData[4] = 1800;
 494
 495     // when
 496     updateActivatedModes();
 497     updateArmingStatus();
 498
 499     // expect
 500     EXPECT_FALSE(isUsingSticksForArming());
 501     EXPECT_TRUE(isArmingDisabled());
 502     EXPECT_EQ(ARMING_DISABLED_THROTTLE | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 503
 504     // given
 505     // throttle moved low
 506     rcData[THROTTLE] = 1000;
 507
 508     // when
 509     updateActivatedModes();
 510     updateArmingStatus();
 511
 512     // expect
 513     EXPECT_FALSE(isUsingSticksForArming());
 514     EXPECT_TRUE(isArmingDisabled());
 515     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 516
 517     // given
 518     // arm switch turned off
 519     rcData[4] = 1000;
 520
 521     // when
 522     updateActivatedModes();
 523     updateArmingStatus();
 524
 525     // expect
 526     EXPECT_FALSE(isUsingSticksForArming());
 527     EXPECT_FALSE(isArmingDisabled());
 528     EXPECT_EQ(0, getArmingDisableFlags());
 529 }
 530
 531 TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingConditionApplies)
 532 {
 533     // given
 534     simulationFeatureFlags = FEATURE_3D; // Using 3D mode
 535     simulationTime = 30e6; // 30 seconds after boot
 536     gyroCalibDone = true;
 537
 538     // and
 539     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 540     modeActivationConditionsMutable(0)->modeId = BOXARM;
 541     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 542     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 543     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 544     modeActivationConditionsMutable(1)->modeId = BOX3D;
 545     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 546     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 547     rcControlsInit();
 548
 549     // and
 550     rxConfigMutable()->mincheck = 1050;
 551
 552     // and
 553     rcData[THROTTLE] = 1000;
 554     mockIsUpright = true;
 555     simulationHaveRx = true;
 556
 557     // and
 558     // arm channel has a safe default value
 559     rcData[4] = 1100;
 560
 561     // when
 562     updateActivatedModes();
 563     updateArmingStatus();
 564
 565     // expect
 566     // ok to arm in 3D mode
 567     EXPECT_FALSE(isUsingSticksForArming());
 568     EXPECT_FALSE(isArmingDisabled());
 569     EXPECT_EQ(0, getArmingDisableFlags());
 570
 571     // given
 572     // raise throttle to unsafe position
 573     rcData[THROTTLE] = 1500;
 574
 575     // when
 576     updateActivatedModes();
 577     updateArmingStatus();
 578
 579     // expect
 580     // ok to arm in 3D mode
 581     EXPECT_FALSE(isUsingSticksForArming());
 582     EXPECT_TRUE(isArmingDisabled());
 583     EXPECT_EQ(ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
 584
 585     // given
 586     // attempt to arm
 587     rcData[4] = 1800;
 588
 589     // when
 590     updateActivatedModes();
 591     updateArmingStatus();
 592
 593     // expect
 594     EXPECT_FALSE(isUsingSticksForArming());
 595     EXPECT_TRUE(isArmingDisabled());
 596     EXPECT_EQ(ARMING_DISABLED_THROTTLE | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 597
 598     // given
 599     // throttle moved low
 600     rcData[THROTTLE] = 1000;
 601
 602     // when
 603     updateActivatedModes();
 604     updateArmingStatus();
 605
 606     // expect
 607     EXPECT_FALSE(isUsingSticksForArming());
 608     EXPECT_TRUE(isArmingDisabled());
 609     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
 610
 611     // given
 612     // arm switch turned off
 613     rcData[4] = 1000;
 614
 615     // when
 616     updateActivatedModes();
 617     updateArmingStatus();
 618
 619     // expect
 620     EXPECT_FALSE(isUsingSticksForArming());
 621     EXPECT_FALSE(isArmingDisabled());
 622     EXPECT_EQ(0, getArmingDisableFlags());
 623 }
 624
 625 TEST(ArmingPreventionTest, GPSRescueWithoutFixPreventsArm)
 626 {
 627     // given
 628     simulationFeatureFlags = 0;
 629     simulationTime = 0;
 630     gyroCalibDone = true;
 631
 632     // and
 633     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 634     modeActivationConditionsMutable(0)->modeId = BOXARM;
 635     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 636     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 637     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 638     modeActivationConditionsMutable(1)->modeId = BOXGPSRESCUE;
 639     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 640     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 641     rcControlsInit();
 642
 643     // and
 644     rxConfigMutable()->mincheck = 1050;
 645
 646     // given
 647     rcData[THROTTLE] = 1000;
 648     rcData[AUX1] = 1000;
 649     rcData[AUX2] = 1000;
 650     mockIsUpright = true;
 651
 652     // when
 653     updateActivatedModes();
 654     updateArmingStatus();
 655
 656     // expect
 657     EXPECT_FALSE(ARMING_FLAG(ARMED));
 658     EXPECT_TRUE(isArmingDisabled());
 659     EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags());
 660     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 661
 662     // given
 663     // arm
 664     rcData[AUX1] = 1800;
 665
 666     // when
 667     tryArm();
 668     updateActivatedModes();
 669     updateArmingStatus();
 670
 671     // expect
 672     EXPECT_FALSE(ARMING_FLAG(ARMED));
 673     EXPECT_TRUE(isArmingDisabled());
 674     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH|ARMING_DISABLED_GPS, getArmingDisableFlags());
 675     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 676
 677     // given
 678     // disarm
 679     rcData[AUX1] = 1000;
 680
 681     // when
 682     disarm(DISARM_REASON_SYSTEM);
 683     updateActivatedModes();
 684     updateArmingStatus();
 685
 686     // expect
 687     EXPECT_FALSE(ARMING_FLAG(ARMED));
 688     EXPECT_TRUE(isArmingDisabled());
 689     EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags());
 690     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 691
 692     // given
 693     // receive GPS fix
 694     ENABLE_STATE(GPS_FIX);
 695
 696     // when
 697     updateActivatedModes();
 698     updateArmingStatus();
 699
 700     // expect
 701     EXPECT_FALSE(ARMING_FLAG(ARMED));
 702     EXPECT_FALSE(isArmingDisabled());
 703     EXPECT_EQ(0, getArmingDisableFlags());
 704     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 705
 706     // given
 707     // arm
 708     rcData[AUX1] = 1800;
 709
 710     // when
 711     tryArm();
 712     updateActivatedModes();
 713     updateArmingStatus();
 714
 715     // expect
 716     EXPECT_TRUE(ARMING_FLAG(ARMED));
 717     EXPECT_FALSE(isArmingDisabled());
 718     EXPECT_EQ(0, getArmingDisableFlags());
 719     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 720
 721     // given
 722     // disarm
 723     rcData[AUX1] = 1000;
 724
 725     // when
 726     disarm(DISARM_REASON_SYSTEM);
 727     updateActivatedModes();
 728     updateArmingStatus();
 729
 730     // expect
 731     EXPECT_FALSE(ARMING_FLAG(ARMED));
 732     EXPECT_FALSE(isArmingDisabled());
 733     EXPECT_EQ(0, getArmingDisableFlags());
 734     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 735 }
 736
 737 TEST(ArmingPreventionTest, GPSRescueSwitchPreventsArm)
 738 {
 739     // given
 740     simulationFeatureFlags = 0;
 741     simulationTime = 0;
 742     gyroCalibDone = true;
 743     gpsSol.numSat = 5;
 744     ENABLE_STATE(GPS_FIX);
 745
 746     // and
 747     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 748     modeActivationConditionsMutable(0)->modeId = BOXARM;
 749     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 750     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 751     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 752     modeActivationConditionsMutable(1)->modeId = BOXGPSRESCUE;
 753     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 754     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 755     rcControlsInit();
 756
 757     // and
 758     rxConfigMutable()->mincheck = 1050;
 759
 760     // given
 761     rcData[THROTTLE] = 1000;
 762     rcData[AUX1] = 1000;
 763     rcData[AUX2] = 1800; // Start out with rescue enabled
 764     mockIsUpright = true;
 765
 766     // when
 767     updateActivatedModes();
 768     updateArmingStatus();
 769
 770     // expect
 771     EXPECT_FALSE(ARMING_FLAG(ARMED));
 772     EXPECT_TRUE(isArmingDisabled());
 773     EXPECT_EQ(ARMING_DISABLED_RESC, getArmingDisableFlags());
 774     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 775
 776     // given
 777     // arm
 778     rcData[AUX1] = 1800;
 779
 780     // when
 781     tryArm();
 782     updateActivatedModes();
 783     updateArmingStatus();
 784
 785     // expect
 786     EXPECT_FALSE(ARMING_FLAG(ARMED));
 787     EXPECT_TRUE(isArmingDisabled());
 788     EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH|ARMING_DISABLED_RESC, getArmingDisableFlags());
 789     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 790
 791     // given
 792     // disarm
 793     rcData[AUX1] = 1000;
 794
 795     // when
 796     disarm(DISARM_REASON_SYSTEM);
 797     updateActivatedModes();
 798     updateArmingStatus();
 799
 800     // expect
 801     EXPECT_FALSE(ARMING_FLAG(ARMED));
 802     EXPECT_TRUE(isArmingDisabled());
 803     EXPECT_EQ(ARMING_DISABLED_RESC, getArmingDisableFlags());
 804     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 805
 806     // given
 807     // disable Rescue
 808     rcData[AUX2] = 1000;
 809
 810     // when
 811     updateActivatedModes();
 812     updateArmingStatus();
 813
 814     // expect
 815     EXPECT_FALSE(ARMING_FLAG(ARMED));
 816     EXPECT_FALSE(isArmingDisabled());
 817     EXPECT_EQ(0, getArmingDisableFlags());
 818     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 819
 820     // given
 821     // arm
 822     rcData[AUX1] = 1800;
 823
 824     // when
 825     tryArm();
 826     updateActivatedModes();
 827     updateArmingStatus();
 828
 829     // expect
 830     EXPECT_TRUE(ARMING_FLAG(ARMED));
 831     EXPECT_FALSE(isArmingDisabled());
 832     EXPECT_EQ(0, getArmingDisableFlags());
 833     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 834
 835     // given
 836     // disarm
 837     rcData[AUX1] = 1000;
 838
 839     // when
 840     disarm(DISARM_REASON_SYSTEM);
 841     updateActivatedModes();
 842     updateArmingStatus();
 843
 844     // expect
 845     EXPECT_FALSE(ARMING_FLAG(ARMED));
 846     EXPECT_FALSE(isArmingDisabled());
 847     EXPECT_EQ(0, getArmingDisableFlags());
 848     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
 849 }
 850
 851 TEST(ArmingPreventionTest, ParalyzeOnAtBoot)
 852 {
 853     // given
 854     simulationFeatureFlags = 0;
 855     simulationTime = 0;
 856     gyroCalibDone = true;
 857
 858     // and
 859     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 860     modeActivationConditionsMutable(0)->modeId = BOXARM;
 861     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 862     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 863     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 864     modeActivationConditionsMutable(1)->modeId = BOXPARALYZE;
 865     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 866     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 867     rcControlsInit();
 868
 869     // and
 870     rxConfigMutable()->mincheck = 1050;
 871
 872     // given
 873     rcData[THROTTLE] = 1000;
 874     rcData[AUX1] = 1000;
 875     rcData[AUX2] = 1800; // Paralyze on at boot
 876     mockIsUpright = true;
 877
 878     // when
 879     updateActivatedModes();
 880     updateArmingStatus();
 881
 882     // expect
 883     EXPECT_FALSE(ARMING_FLAG(ARMED));
 884     EXPECT_FALSE(isArmingDisabled());
 885     EXPECT_EQ(0, getArmingDisableFlags());
 886     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
 887
 888     // when
 889     updateActivatedModes();
 890
 891     // expect
 892     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
 893 }
 894
 895 TEST(ArmingPreventionTest, Paralyze)
 896 {
 897     // given
 898     simulationFeatureFlags = 0;
 899     simulationTime = 0;
 900     gyroCalibDone = true;
 901
 902     // and
 903     modeActivationConditionsMutable(0)->auxChannelIndex = 0;
 904     modeActivationConditionsMutable(0)->modeId = BOXARM;
 905     modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 906     modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 907     modeActivationConditionsMutable(1)->auxChannelIndex = 1;
 908     modeActivationConditionsMutable(1)->modeId = BOXPARALYZE;
 909     modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 910     modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 911     modeActivationConditionsMutable(2)->auxChannelIndex = 2;
 912     modeActivationConditionsMutable(2)->modeId = BOXBEEPERON;
 913     modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
 914     modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
 915     modeActivationConditionsMutable(3)->modeId = BOXVTXPITMODE;
 916     modeActivationConditionsMutable(3)->linkedTo = BOXPARALYZE;
 917     rcControlsInit();
 918
 919     // and
 920     rxConfigMutable()->mincheck = 1050;
 921
 922     // given
 923     rcData[THROTTLE] = 1000;
 924     rcData[AUX1] = 1000;
 925     rcData[AUX2] = 1800; // Start out with paralyze enabled
 926     rcData[AUX3] = 1000;
 927     mockIsUpright = true;
 928
 929     // when
 930     updateActivatedModes();
 931     updateArmingStatus();
 932
 933     // expect
 934     EXPECT_FALSE(ARMING_FLAG(ARMED));
 935     EXPECT_FALSE(isArmingDisabled());
 936     EXPECT_EQ(0, getArmingDisableFlags());
 937
 938     // given
 939     // arm
 940     rcData[AUX1] = 1800;
 941
 942     // when
 943     tryArm();
 944     updateActivatedModes();
 945     updateArmingStatus();
 946
 947     // expect
 948     EXPECT_TRUE(ARMING_FLAG(ARMED));
 949     EXPECT_FALSE(isArmingDisabled());
 950     EXPECT_EQ(0, getArmingDisableFlags());
 951
 952     // given
 953     // disarm
 954     rcData[AUX1] = 1000;
 955
 956     // when
 957     disarm(DISARM_REASON_SYSTEM);
 958     updateActivatedModes();
 959     updateArmingStatus();
 960
 961     // expect
 962     EXPECT_FALSE(ARMING_FLAG(ARMED));
 963     EXPECT_FALSE(isArmingDisabled());
 964     EXPECT_EQ(0, getArmingDisableFlags());
 965     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
 966
 967     // given
 968     simulationTime = 10e6; // 10 seconds after boot
 969
 970     // when
 971     updateActivatedModes();
 972
 973     // expect
 974     EXPECT_FALSE(ARMING_FLAG(ARMED));
 975     EXPECT_FALSE(isArmingDisabled());
 976     EXPECT_EQ(0, getArmingDisableFlags());
 977     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
 978
 979     // given
 980     // disable paralyze once after the startup timer
 981     rcData[AUX2] = 1000;
 982
 983     // when
 984     updateActivatedModes();
 985
 986     // enable paralyze again
 987     rcData[AUX2] = 1800;
 988
 989     // when
 990     updateActivatedModes();
 991     updateArmingStatus();
 992
 993     // expect
 994     EXPECT_TRUE(isArmingDisabled());
 995     EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags());
 996     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
 997     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE));
 998     EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXBEEPERON));
 999
1000     // given
1001     // enable beeper
1002     rcData[AUX3] = 1800;
1003
1004     // when
1005     updateActivatedModes();
1006
1007     // expect
1008     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE));
1009     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXBEEPERON));
1010
1011     // given
1012     // try exiting paralyze mode and ensure arming and pit mode are still disabled
1013     rcData[AUX2] = 1000;
1014
1015     // when
1016     updateActivatedModes();
1017     updateArmingStatus();
1018
1019     // expect
1020     EXPECT_TRUE(isArmingDisabled());
1021     EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags());
1022     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
1023     EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE));
1024 }
1025
1026 // STUBS
1027 extern "C" {
1028     uint32_t micros(void) { return simulationTime; }
1029     uint32_t millis(void) { return micros() / 1000; }
1030     bool rxIsReceivingSignal(void) { return simulationHaveRx; }
1031
1032     bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; }
1033     void warningLedFlash(void) {}
1034     void warningLedDisable(void) {}
1035     void warningLedUpdate(void) {}
1036     void beeper(beeperMode_e) {}
1037     void beeperConfirmationBeeps(uint8_t) {}
1038     void beeperWarningBeeps(uint8_t) {}
1039     void beeperSilence(void) {}
1040     void systemBeep(bool) {}
1041     void saveConfigAndNotify(void) {}
1042     void blackboxFinish(void) {}
1043     bool accIsCalibrationComplete(void) { return true; }
1044     bool baroIsCalibrationComplete(void) { return true; }
1045     bool gyroIsCalibrationComplete(void) { return gyroCalibDone; }
1046     void gyroStartCalibration(bool) {}
1047     bool isFirstArmingGyroCalibrationRunning(void) { return false; }
1048     void pidController(const pidProfile_t *, timeUs_t) {}
1049     void pidStabilisationState(pidStabilisationState_e) {}
1050     void mixTable(timeUs_t) {};
1051     void writeMotors(void) {};
1052     void writeServos(void) {};
1053     bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
1054     bool isMixerUsingServos(void) { return false; }
1055     void gyroUpdate(void) {}
1056     timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 0; }
1057     void updateRSSI(timeUs_t) {}
1058     bool failsafeIsMonitoring(void) { return false; }
1059     void failsafeStartMonitoring(void) {}
1060     void failsafeUpdateState(void) {}
1061     bool failsafeIsActive(void) { return false; }
1062     void pidResetIterm(void) {}
1063     void updateAdjustmentStates(void) {}
1064     void processRcAdjustments(controlRateConfig_t *) {}
1065     void updateGpsWaypointsAndMode(void) {}
1066     void mspSerialReleaseSharedTelemetryPorts(void) {}
1067     void telemetryCheckState(void) {}
1068     void mspSerialAllocatePorts(void) {}
1069     void gyroReadTemperature(void) {}
1070     void updateRcCommands(void) {}
1071     void applyAltHold(void) {}
1072     void resetYawAxis(void) {}
1073     int16_t calculateThrottleAngleCorrection(uint8_t) { return 0; }
1074     void processRcCommand(void) {}
1075     void updateGpsStateForHomeAndHoldMode(void) {}
1076     void blackboxUpdate(timeUs_t) {}
1077     void transponderUpdate(timeUs_t) {}
1078     void GPS_reset_home_position(void) {}
1079     void accStartCalibration(void) {}
1080     bool accHasBeenCalibrated(void) { return true; }
1081     void baroSetGroundLevel(void) {}
1082     void changePidProfile(uint8_t) {}
1083     void changeControlRateProfile(uint8_t) {}
1084     void dashboardEnablePageCycling(void) {}
1085     void dashboardDisablePageCycling(void) {}
1086     bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
1087     void rescheduleTask(taskId_e, timeDelta_t) {}
1088     bool usbCableIsInserted(void) { return false; }
1089     bool usbVcpIsConnected(void) { return false; }
1090     void pidSetAntiGravityState(bool) {}
1091     void osdSuppressStats(bool) {}
1092     float scaleRangef(float, float, float, float, float) { return 0.0f; }
1093     bool crashRecoveryModeActive(void) { return false; }
1094     int32_t getEstimatedAltitudeCm(void) { return 0; }
1095     bool gpsIsHealthy() { return false; }
1096     bool isAltitudeOffset(void) { return false; }
1097     float getCosTiltAngle(void) { return 0.0f; }
1098     void pidSetItermReset(bool) {}
1099     void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
1100     bool isFixedWing(void) { return false; }
1101     void compassStartCalibration(void) {}
1102     bool compassIsCalibrationComplete(void) { return true; }
1103     bool isUpright(void) { return mockIsUpright; }
1104     void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
1105     void gyroFiltering(timeUs_t) {};
1106     timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
1107     void updateRcRefreshRate(timeUs_t) {};
1108     uint16_t getAverageSystemLoadPercent(void) { return 0; }
1109     bool isMotorProtocolEnabled(void) { return true; }
1110     void pinioBoxTaskControl(void) {}
1111     void schedulerSetNextStateTime(timeDelta_t) {}
1112 }