src/test/unit/pid_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 #include <limits.h>
  21 #include <cmath>
  22
  23 #include "unittest_macros.h"
  24 #include "gtest/gtest.h"
  25 #include "build/debug.h"
  26
  27 bool simulatedAirmodeEnabled = true;
  28 float simulatedSetpointRate[3] = { 0,0,0 };
  29 float simulatedRcDeflection[3] = { 0,0,0 };
  30 float simulatedThrottlePIDAttenuation = 1.0f;
  31 float simulatedMotorMixRange = 0.0f;
  32
  33 int16_t debug[DEBUG16_VALUE_COUNT];
  34 uint8_t debugMode;
  35
  36 extern "C" {
  37     #include "build/debug.h"
  38     #include "common/axis.h"
  39     #include "common/maths.h"
  40     #include "common/filter.h"
  41
  42     #include "config/config_reset.h"
  43     #include "pg/pg.h"
  44     #include "pg/pg_ids.h"
  45
  46     #include "drivers/sound_beeper.h"
  47     #include "drivers/time.h"
  48
  49     #include "fc/core.h"
  50     #include "fc/rc.h"
  51
  52     #include "fc/rc_controls.h"
  53     #include "fc/runtime_config.h"
  54
  55     #include "flight/pid.h"
  56     #include "flight/imu.h"
  57     #include "flight/mixer.h"
  58
  59     #include "io/gps.h"
  60
  61     #include "sensors/gyro.h"
  62     #include "sensors/acceleration.h"
  63
  64     gyro_t gyro;
  65     attitudeEulerAngles_t attitude;
  66
  67     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
  68
  69     bool unitLaunchControlActive = false;
  70     launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
  71
  72     float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
  73     float getMotorMixRange(void) { return simulatedMotorMixRange; }
  74     float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
  75     bool isAirmodeActivated() { return simulatedAirmodeEnabled; }
  76     float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); }
  77     void systemBeep(bool) { }
  78     bool gyroOverflowDetected(void) { return false; }
  79     float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
  80     void beeperConfirmationBeeps(uint8_t) { }
  81     bool isLaunchControlActive(void) {return unitLaunchControlActive; }
  82     void disarm(void) { }
  83     float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) {
  84         UNUSED(axis);
  85         UNUSED(Kp);
  86         UNUSED(currentPidSetpoint);
  87         return value;
  88     }
  89 }
  90
  91 pidProfile_t *pidProfile;
  92
  93 int loopIter = 0;
  94
  95 // Always use same defaults for testing in future releases even when defaults change
  96 void setDefaultTestSettings(void) {
  97     pgResetAll();
  98     pidProfile = pidProfilesMutable(1);
  99     pidProfile->pid[PID_ROLL]  =  { 40, 40, 30, 65 };
 100     pidProfile->pid[PID_PITCH] =  { 58, 50, 35, 60 };
 101     pidProfile->pid[PID_YAW]   =  { 70, 45, 20, 60 };
 102     pidProfile->pid[PID_LEVEL] =  { 50, 50, 75, 0 };
 103
 104     // Compensate for the upscaling done without 'use_integrated_yaw'
 105     pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f;
 106
 107     pidProfile->pidSumLimit = PIDSUM_LIMIT;
 108     pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
 109     pidProfile->yaw_lowpass_hz = 0;
 110     pidProfile->dterm_lowpass_hz = 100;
 111     pidProfile->dterm_lowpass2_hz = 0;
 112     pidProfile->dterm_notch_hz = 260;
 113     pidProfile->dterm_notch_cutoff = 160;
 114     pidProfile->dterm_filter_type = FILTER_BIQUAD;
 115     pidProfile->itermWindupPointPercent = 50;
 116     pidProfile->vbatPidCompensation = 0;
 117     pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
 118     pidProfile->levelAngleLimit = 55;
 119     pidProfile->feedForwardTransition = 100;
 120     pidProfile->yawRateAccelLimit = 100;
 121     pidProfile->rateAccelLimit = 0;
 122     pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
 123     pidProfile->itermThrottleThreshold = 250;
 124     pidProfile->itermAcceleratorGain = 1000;
 125     pidProfile->crash_time = 500;
 126     pidProfile->crash_delay = 0;
 127     pidProfile->crash_recovery_angle = 10;
 128     pidProfile->crash_recovery_rate = 100;
 129     pidProfile->crash_dthreshold = 50;
 130     pidProfile->crash_gthreshold = 400;
 131     pidProfile->crash_setpoint_threshold = 350;
 132     pidProfile->crash_recovery = PID_CRASH_RECOVERY_OFF;
 133     pidProfile->horizon_tilt_effect = 75;
 134     pidProfile->horizon_tilt_expert_mode = false;
 135     pidProfile->crash_limit_yaw = 200;
 136     pidProfile->itermLimit = 150;
 137     pidProfile->throttle_boost = 0;
 138     pidProfile->throttle_boost_cutoff = 15;
 139     pidProfile->iterm_rotation = false;
 140     pidProfile->iterm_relax = ITERM_RELAX_OFF,
 141     pidProfile->iterm_relax_cutoff = 11,
 142     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT,
 143     pidProfile->abs_control_gain = 0,
 144     pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
 145     pidProfile->launchControlGain = 40,
 146
 147     gyro.targetLooptime = 4000;
 148 }
 149
 150 timeUs_t currentTestTime(void) {
 151     return targetPidLooptime * loopIter++;
 152 }
 153
 154 void resetTest(void) {
 155     loopIter = 0;
 156     simulatedThrottlePIDAttenuation = 1.0f;
 157     simulatedMotorMixRange = 0.0f;
 158
 159     pidStabilisationState(PID_STABILISATION_OFF);
 160     DISABLE_ARMING_FLAG(ARMED);
 161
 162     setDefaultTestSettings();
 163     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 164         pidData[axis].P = 0;
 165         pidData[axis].I = 0;
 166         pidData[axis].D = 0;
 167         pidData[axis].F = 0;
 168         pidData[axis].Sum = 0;
 169         simulatedSetpointRate[axis] = 0;
 170         simulatedRcDeflection[axis] = 0;
 171         gyro.gyroADCf[axis] = 0;
 172     }
 173     attitude.values.roll = 0;
 174     attitude.values.pitch = 0;
 175     attitude.values.yaw = 0;
 176
 177     flightModeFlags = 0;
 178     unitLaunchControlActive = false;
 179     pidProfile->launchControlMode = unitLaunchControlMode;
 180     pidInit(pidProfile);
 181
 182     // Run pidloop for a while after reset
 183     for (int loop = 0; loop < 20; loop++) {
 184         pidController(pidProfile, currentTestTime());
 185     }
 186 }
 187
 188 void setStickPosition(int axis, float stickRatio) {
 189     simulatedSetpointRate[axis] = 1998.0f * stickRatio;
 190     simulatedRcDeflection[axis] = stickRatio;
 191 }
 192
 193 // All calculations will have 10% tolerance
 194 float calculateTolerance(float input) {
 195     return fabs(input * 0.1f);
 196 }
 197
 198 TEST(pidControllerTest, testInitialisation)
 199 {
 200     resetTest();
 201
 202     // In initial state PIDsums should be 0
 203     for (int axis = 0; axis <= FD_YAW; axis++) {
 204         EXPECT_FLOAT_EQ(0, pidData[axis].P);
 205         EXPECT_FLOAT_EQ(0, pidData[axis].I);
 206         EXPECT_FLOAT_EQ(0, pidData[axis].D);
 207     }
 208 }
 209
 210 TEST(pidControllerTest, testStabilisationDisabled) {
 211     ENABLE_ARMING_FLAG(ARMED);
 212     // Run few loops to make sure there is no error building up when stabilisation disabled
 213
 214     for (int loop = 0; loop < 10; loop++) {
 215         pidController(pidProfile, currentTestTime());
 216
 217         // PID controller should not do anything, while stabilisation disabled
 218         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 219         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 220         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 221         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 222         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 223         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 224         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 225         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 226         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 227     }
 228 }
 229
 230 TEST(pidControllerTest, testPidLoop) {
 231     // Make sure to start with fresh values
 232     resetTest();
 233     ENABLE_ARMING_FLAG(ARMED);
 234     pidStabilisationState(PID_STABILISATION_ON);
 235
 236     pidController(pidProfile, currentTestTime());
 237
 238     // Loop 1 - Expecting zero since there is no error
 239     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 240     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 241     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 242     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 243     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 244     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 245     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 246     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 247     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 248
 249     // Add some rotation on ROLL to generate error
 250     gyro.gyroADCf[FD_ROLL] = 100;
 251     pidController(pidProfile, currentTestTime());
 252
 253     // Loop 2 - Expect PID loop reaction to ROLL error
 254     ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 255     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 256     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 257     ASSERT_NEAR(-7.8, pidData[FD_ROLL].I, calculateTolerance(-7.8));
 258     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 259     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 260     ASSERT_NEAR(-198.4, pidData[FD_ROLL].D, calculateTolerance(-198.4));
 261     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 262     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 263
 264     // Add some rotation on PITCH to generate error
 265     gyro.gyroADCf[FD_PITCH] = -100;
 266     pidController(pidProfile, currentTestTime());
 267
 268     // Loop 3 - Expect PID loop reaction to PITCH error, ROLL is still in error
 269     ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 270     ASSERT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
 271     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 272     ASSERT_NEAR(-15.6, pidData[FD_ROLL].I, calculateTolerance(-15.6));
 273     ASSERT_NEAR(9.8, pidData[FD_PITCH].I, calculateTolerance(9.8));
 274     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 275     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 276     ASSERT_NEAR(231.4, pidData[FD_PITCH].D, calculateTolerance(231.4));
 277     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 278
 279     // Add some rotation on YAW to generate error
 280     gyro.gyroADCf[FD_YAW] = 100;
 281     pidController(pidProfile, currentTestTime());
 282
 283     // Loop 4 - Expect PID loop reaction to PITCH error, ROLL and PITCH are still in error
 284     ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 285     ASSERT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
 286     ASSERT_NEAR(-224.2, pidData[FD_YAW].P, calculateTolerance(-224.2));
 287     ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
 288     ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
 289     ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
 290     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 291     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 292     ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25));
 293
 294     // Simulate Iterm behaviour during mixer saturation
 295     simulatedMotorMixRange = 1.2f;
 296     pidController(pidProfile, currentTestTime());
 297     ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
 298     ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
 299     ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
 300     simulatedMotorMixRange = 0;
 301
 302     // Match the stick to gyro to stop error
 303     simulatedSetpointRate[FD_ROLL] = 100;
 304     simulatedSetpointRate[FD_PITCH] = -100;
 305     simulatedSetpointRate[FD_YAW] = 100;
 306
 307     for(int loop = 0; loop < 5; loop++) {
 308         pidController(pidProfile, currentTestTime());
 309     }
 310     // Iterm is stalled as it is not accumulating anymore
 311     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 312     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 313     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 314     ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
 315     ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
 316     ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
 317     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 318     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 319     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 320
 321     // Now disable Stabilisation
 322     pidStabilisationState(PID_STABILISATION_OFF);
 323     pidController(pidProfile, currentTestTime());
 324
 325     // Should all be zero again
 326     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 327     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 328     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 329     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 330     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 331     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 332     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 333     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 334     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 335 }
 336
 337 TEST(pidControllerTest, testPidLevel) {
 338     // Make sure to start with fresh values
 339     resetTest();
 340     ENABLE_ARMING_FLAG(ARMED);
 341     pidStabilisationState(PID_STABILISATION_ON);
 342
 343     // Test Angle mode response
 344     enableFlightMode(ANGLE_MODE);
 345     float currentPidSetpoint = 30;
 346     rollAndPitchTrims_t angleTrim = { { 0, 0 } };
 347
 348     currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
 349     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 350     currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
 351     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 352
 353     // Test attitude response
 354     setStickPosition(FD_ROLL, 1.0f);
 355     setStickPosition(FD_PITCH, -1.0f);
 356     currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
 357     EXPECT_FLOAT_EQ(275, currentPidSetpoint);
 358     currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
 359     EXPECT_FLOAT_EQ(-275, currentPidSetpoint);
 360
 361     setStickPosition(FD_ROLL, -0.5f);
 362     setStickPosition(FD_PITCH, 0.5f);
 363     currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
 364     EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint);
 365     currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
 366     EXPECT_FLOAT_EQ(137.5, currentPidSetpoint);
 367
 368     attitude.values.roll = -275;
 369     attitude.values.pitch = 275;
 370     currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
 371     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 372     currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
 373     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 374
 375     // Disable ANGLE_MODE
 376     disableFlightMode(ANGLE_MODE);
 377     currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
 378     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 379     currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
 380     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 381 }
 382
 383
 384 TEST(pidControllerTest, testPidHorizon) {
 385     resetTest();
 386     ENABLE_ARMING_FLAG(ARMED);
 387     pidStabilisationState(PID_STABILISATION_ON);
 388     enableFlightMode(HORIZON_MODE);
 389
 390     // Test full stick response
 391     setStickPosition(FD_ROLL, 1.0f);
 392     setStickPosition(FD_PITCH, -1.0f);
 393     EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength());
 394
 395     // Expect full rate output on full stick
 396     // Test zero stick response
 397     setStickPosition(FD_ROLL, 0);
 398     setStickPosition(FD_PITCH, 0);
 399     EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength());
 400
 401     // Test small stick response
 402     setStickPosition(FD_ROLL, 0.1f);
 403     setStickPosition(FD_PITCH, -0.1f);
 404     ASSERT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82));
 405 }
 406
 407 TEST(pidControllerTest, testMixerSaturation) {
 408     resetTest();
 409     ENABLE_ARMING_FLAG(ARMED);
 410     pidStabilisationState(PID_STABILISATION_ON);
 411
 412     // Test full stick response
 413     setStickPosition(FD_ROLL, 1.0f);
 414     setStickPosition(FD_PITCH, -1.0f);
 415     setStickPosition(FD_YAW, 1.0f);
 416     simulatedMotorMixRange = 2.0f;
 417     pidController(pidProfile, currentTestTime());
 418
 419     // Expect no iterm accumulation
 420     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 421     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 422     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 423
 424     // Test itermWindup limit
 425     // First store values without exceeding iterm windup limit
 426     resetTest();
 427     ENABLE_ARMING_FLAG(ARMED);
 428     pidStabilisationState(PID_STABILISATION_ON);
 429     setStickPosition(FD_ROLL, 0.1f);
 430     setStickPosition(FD_PITCH, -0.1f);
 431     setStickPosition(FD_YAW, 0.1f);
 432     simulatedMotorMixRange = 0.0f;
 433     pidController(pidProfile, currentTestTime());
 434     float rollTestIterm = pidData[FD_ROLL].I;
 435     float pitchTestIterm = pidData[FD_PITCH].I;
 436     float yawTestIterm = pidData[FD_YAW].I;
 437
 438     // Now compare values when exceeding the limit
 439     resetTest();
 440     ENABLE_ARMING_FLAG(ARMED);
 441     pidStabilisationState(PID_STABILISATION_ON);
 442     setStickPosition(FD_ROLL, 0.1f);
 443     setStickPosition(FD_PITCH, -0.1f);
 444     setStickPosition(FD_YAW, 0.1f);
 445     simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f;
 446     pidController(pidProfile, currentTestTime());
 447     ASSERT_LT(pidData[FD_ROLL].I, rollTestIterm);
 448     ASSERT_GE(pidData[FD_PITCH].I, pitchTestIterm);
 449     ASSERT_LT(pidData[FD_YAW].I, yawTestIterm);
 450 }
 451
 452 // TODO - Add more scenarios
 453 TEST(pidControllerTest, testCrashRecoveryMode) {
 454     resetTest();
 455     pidProfile->crash_recovery = PID_CRASH_RECOVERY_ON;
 456     pidInit(pidProfile);
 457     ENABLE_ARMING_FLAG(ARMED);
 458     pidStabilisationState(PID_STABILISATION_ON);
 459     sensorsSet(SENSOR_ACC);
 460
 461     EXPECT_FALSE(crashRecoveryModeActive());
 462
 463     int loopsToCrashTime = (int)((pidProfile->crash_time * 1000) / targetPidLooptime) + 1;
 464
 465     // generate crash detection for roll axis
 466     gyro.gyroADCf[FD_ROLL]  = 800;
 467     simulatedMotorMixRange = 1.2f;
 468     for (int loop =0; loop <= loopsToCrashTime; loop++) {
 469         gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
 470         // advance the time to avoid initialized state prevention of crash recovery
 471         pidController(pidProfile, currentTestTime() + 2000000);
 472     }
 473
 474     EXPECT_TRUE(crashRecoveryModeActive());
 475     // Add additional verifications
 476 }
 477
 478 TEST(pidControllerTest, testFeedForward) {
 479     resetTest();
 480     ENABLE_ARMING_FLAG(ARMED);
 481     pidStabilisationState(PID_STABILISATION_ON);
 482
 483     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 484     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 485     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 486
 487     // Match the stick to gyro to stop error
 488     setStickPosition(FD_ROLL, 1.0f);
 489     setStickPosition(FD_PITCH, -1.0f);
 490     setStickPosition(FD_YAW, -1.0f);
 491
 492     pidController(pidProfile, currentTestTime());
 493
 494     ASSERT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
 495     ASSERT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03));
 496     ASSERT_NEAR(-82.52, pidData[FD_YAW].F, calculateTolerance(-82.5));
 497
 498     // Match the stick to gyro to stop error
 499     setStickPosition(FD_ROLL, 0.5f);
 500     setStickPosition(FD_PITCH, -0.5f);
 501     setStickPosition(FD_YAW, -0.5f);
 502
 503     pidController(pidProfile, currentTestTime());
 504
 505     ASSERT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20));
 506     ASSERT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
 507     ASSERT_NEAR(-41.26, pidData[FD_YAW].F, calculateTolerance(-41.26));
 508
 509     for (int loop =0; loop <= 15; loop++) {
 510         gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
 511         pidController(pidProfile, currentTestTime());
 512     }
 513
 514     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 515     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 516     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 517
 518 }
 519
 520 TEST(pidControllerTest, testItermRelax) {
 521     resetTest();
 522     pidProfile->iterm_relax = ITERM_RELAX_RP;
 523     ENABLE_ARMING_FLAG(ARMED);
 524     pidStabilisationState(PID_STABILISATION_ON);
 525
 526     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT;
 527     pidInit(pidProfile);
 528
 529     float itermErrorRate = 0;
 530     float currentPidSetpoint = 0;
 531     float gyroRate = 0;
 532
 533     applyItermRelax(FD_PITCH, 0, gyroRate, &itermErrorRate, &currentPidSetpoint);
 534     EXPECT_FLOAT_EQ(itermErrorRate, 0);
 535     itermErrorRate = -10;
 536     currentPidSetpoint = 10;
 537     pidData[FD_PITCH].I = 10;
 538
 539     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 540
 541     ASSERT_NEAR(-8.16, itermErrorRate, calculateTolerance(-8.16));
 542     currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD;
 543     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 544     ASSERT_NEAR(-2.69, itermErrorRate, calculateTolerance(-2.69));
 545     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 546     ASSERT_NEAR(-0.84, itermErrorRate, calculateTolerance(-0.84));
 547
 548     pidProfile->iterm_relax_type = ITERM_RELAX_GYRO;
 549     pidInit(pidProfile);
 550
 551     currentPidSetpoint = 100;
 552     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 553     EXPECT_FLOAT_EQ(itermErrorRate, 0);
 554     gyroRate = 10;
 555     itermErrorRate = -10;
 556     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 557     ASSERT_NEAR(7, itermErrorRate, calculateTolerance(7));
 558     gyroRate += 100;
 559     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 560     ASSERT_NEAR(-10, itermErrorRate, calculateTolerance(-10));
 561
 562     pidProfile->iterm_relax = ITERM_RELAX_RP_INC;
 563     pidInit(pidProfile);
 564
 565     itermErrorRate = -10;
 566     pidData[FD_PITCH].I = 10;
 567     currentPidSetpoint = 10;
 568     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 569     EXPECT_FLOAT_EQ(itermErrorRate, -10);
 570     itermErrorRate = 10;
 571     pidData[FD_PITCH].I = -10;
 572     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 573     EXPECT_FLOAT_EQ(itermErrorRate, 10);
 574     itermErrorRate = -10;
 575     currentPidSetpoint = 10;
 576     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 577     EXPECT_FLOAT_EQ(itermErrorRate, -100);
 578
 579     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT;
 580     pidInit(pidProfile);
 581
 582     itermErrorRate = -10;
 583     currentPidSetpoint = ITERM_RELAX_SETPOINT_THRESHOLD;
 584     applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 585     EXPECT_FLOAT_EQ(itermErrorRate, -10);
 586
 587     pidProfile->iterm_relax = ITERM_RELAX_RPY;
 588     pidInit(pidProfile);
 589     applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 590     ASSERT_NEAR(-6.46, itermErrorRate, calculateTolerance(-3.6));
 591 }
 592
 593 // TODO - Add more tests
 594 TEST(pidControllerTest, testAbsoluteControl) {
 595     resetTest();
 596     pidProfile->abs_control_gain = 10;
 597     pidInit(pidProfile);
 598     ENABLE_ARMING_FLAG(ARMED);
 599     pidStabilisationState(PID_STABILISATION_ON);
 600
 601     float gyroRate = 0;
 602
 603     float itermErrorRate = 10;
 604     float currentPidSetpoint = 10;
 605
 606     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 607
 608     ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
 609     ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
 610
 611     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 612     ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
 613     ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
 614
 615     gyroRate = -53;
 616     axisError[FD_PITCH] = -60;
 617     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 618     ASSERT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2));
 619     ASSERT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2));
 620 }
 621
 622 TEST(pidControllerTest, testDtermFiltering) {
 623 // TODO
 624 }
 625
 626 TEST(pidControllerTest, testItermRotationHandling) {
 627     resetTest();
 628     pidInit(pidProfile);
 629
 630     rotateItermAndAxisError();
 631     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0);
 632     EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0);
 633     EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0);
 634
 635     pidProfile->iterm_rotation = true;
 636     pidInit(pidProfile);
 637
 638     rotateItermAndAxisError();
 639     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0);
 640     EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0);
 641     EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0);
 642
 643     pidData[FD_ROLL].I = 10;
 644     pidData[FD_PITCH].I = 1000;
 645     pidData[FD_YAW].I = 1000;
 646     gyro.gyroADCf[FD_ROLL] = -1000;
 647     rotateItermAndAxisError();
 648     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
 649     ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
 650     ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
 651
 652     pidProfile->abs_control_gain = 10;
 653     pidInit(pidProfile);
 654     pidData[FD_ROLL].I = 10;
 655     pidData[FD_PITCH].I = 1000;
 656     pidData[FD_YAW].I = 1000;
 657
 658     gyro.gyroADCf[FD_ROLL] = -1000;
 659     // FIXME - axisError changes don't affect the system. This is a potential bug or intendend behaviour?
 660     axisError[FD_PITCH] = 1000;
 661     axisError[FD_YAW] = 1000;
 662     rotateItermAndAxisError();
 663     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
 664     ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
 665     ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
 666 }
 667
 668 TEST(pidControllerTest, testLaunchControl) {
 669     // The launchControlGain is indirectly tested since when launch control is active the
 670     // the gain overrides the PID settings. If the logic to use launchControlGain wasn't
 671     // working then any I calculations would be incorrect.
 672
 673     resetTest();
 674     unitLaunchControlActive = true;
 675     ENABLE_ARMING_FLAG(ARMED);
 676     pidStabilisationState(PID_STABILISATION_ON);
 677
 678     // test that feedforward and D are disabled (always zero) when launch control is active
 679     // set initial state
 680     pidController(pidProfile, currentTestTime());
 681
 682     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 683     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 684     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 685     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 686     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 687     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 688
 689     // Move the sticks to induce feedforward
 690     setStickPosition(FD_ROLL, 0.5f);
 691     setStickPosition(FD_PITCH, -0.5f);
 692     setStickPosition(FD_YAW, -0.5f);
 693
 694     // add gyro activity to induce D
 695     gyro.gyroADCf[FD_ROLL] = -1000;
 696     gyro.gyroADCf[FD_PITCH] = 1000;
 697     gyro.gyroADCf[FD_YAW] = -1000;
 698
 699     pidController(pidProfile, currentTestTime());
 700
 701     // validate that feedforwad is still 0
 702     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 703     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 704     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 705
 706     // validate that D is still 0
 707     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 708     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 709     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 710
 711     // test NORMAL mode - expect P/I on roll and pitch, P on yaw but I == 0
 712     unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
 713     resetTest();
 714     unitLaunchControlActive = true;
 715     ENABLE_ARMING_FLAG(ARMED);
 716     pidStabilisationState(PID_STABILISATION_ON);
 717
 718     pidController(pidProfile, currentTestTime());
 719
 720     gyro.gyroADCf[FD_ROLL] = -20;
 721     gyro.gyroADCf[FD_PITCH] = 20;
 722     gyro.gyroADCf[FD_YAW] = -20;
 723     pidController(pidProfile, currentTestTime());
 724
 725     ASSERT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
 726     ASSERT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
 727     ASSERT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
 728     ASSERT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
 729     ASSERT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
 730     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 731
 732     // test PITCHONLY mode - expect P/I only on pitch; I cannot go negative
 733     unitLaunchControlMode = LAUNCH_CONTROL_MODE_PITCHONLY;
 734     resetTest();
 735     unitLaunchControlActive = true;
 736     ENABLE_ARMING_FLAG(ARMED);
 737     pidStabilisationState(PID_STABILISATION_ON);
 738
 739     pidController(pidProfile, currentTestTime());
 740
 741     // first test that pitch I is prevented from going negative
 742     gyro.gyroADCf[FD_ROLL] = 0;
 743     gyro.gyroADCf[FD_PITCH] = 20;
 744     gyro.gyroADCf[FD_YAW] = 0;
 745     pidController(pidProfile, currentTestTime());
 746
 747     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 748
 749     gyro.gyroADCf[FD_ROLL] = 20;
 750     gyro.gyroADCf[FD_PITCH] = -20;
 751     gyro.gyroADCf[FD_YAW] = 20;
 752     pidController(pidProfile, currentTestTime());
 753
 754     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 755     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 756     ASSERT_NEAR(37.15, pidData[FD_PITCH].P, calculateTolerance(37.15));
 757     ASSERT_NEAR(1.56,  pidData[FD_PITCH].I, calculateTolerance(1.56));
 758     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 759     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 760
 761     // test FULL mode - expect P/I on all axes
 762     unitLaunchControlMode = LAUNCH_CONTROL_MODE_FULL;
 763     resetTest();
 764     unitLaunchControlActive = true;
 765     ENABLE_ARMING_FLAG(ARMED);
 766     pidStabilisationState(PID_STABILISATION_ON);
 767
 768     pidController(pidProfile, currentTestTime());
 769
 770     gyro.gyroADCf[FD_ROLL] = -20;
 771     gyro.gyroADCf[FD_PITCH] = 20;
 772     gyro.gyroADCf[FD_YAW] = -20;
 773     pidController(pidProfile, currentTestTime());
 774
 775     ASSERT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
 776     ASSERT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
 777     ASSERT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
 778     ASSERT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
 779     ASSERT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
 780     ASSERT_NEAR(1.56,   pidData[FD_YAW].I,  calculateTolerance(1.56));
 781 }