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