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