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