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 simulatedThrottleRaised = true;
  28 float simulatedSetpointRate[3] = { 0,0,0 };
  29 float simulatedPrevSetpointRate[3] = { 0,0,0 };
  30 float simulatedRcDeflection[3] = { 0,0,0 };
  31 float simulatedMaxRcDeflectionAbs = 0;
  32 float simulatedMixerGetRcThrottle = 0;
  33 float simulatedRcCommandDelta[3] = { 1,1,1 };
  34 float simulatedRawSetpoint[3] = { 0,0,0 };
  35 float simulatedMaxRate[3] = { 670,670,670 };
  36 float simulatedFeedforward[3] = { 0,0,0 };
  37 float simulatedMotorMixRange = 0.0f;
  38
  39 int16_t debug[DEBUG16_VALUE_COUNT];
  40 uint8_t debugMode;
  41
  42 extern "C" {
  43     #include "platform.h"
  44
  45     #include "build/debug.h"
  46
  47     #include "common/axis.h"
  48     #include "common/maths.h"
  49     #include "common/filter.h"
  50
  51     #include "config/config.h"
  52     #include "config/config_reset.h"
  53
  54     #include "drivers/sound_beeper.h"
  55     #include "drivers/time.h"
  56
  57     #include "fc/controlrate_profile.h"
  58     #include "fc/core.h"
  59     #include "fc/rc.h"
  60
  61     #include "fc/rc_controls.h"
  62     #include "fc/runtime_config.h"
  63
  64     #include "flight/imu.h"
  65     #include "flight/mixer.h"
  66     #include "flight/pid.h"
  67     #include "flight/pid_init.h"
  68     #include "flight/position.h"
  69
  70     #include "io/gps.h"
  71
  72     #include "pg/pg.h"
  73     #include "pg/pg_ids.h"
  74
  75     #include "pg/rx.h"
  76     #include "rx/rx.h"
  77
  78     #include "sensors/gyro.h"
  79     #include "sensors/acceleration.h"
  80
  81     acc_t acc;
  82     gyro_t gyro;
  83     attitudeEulerAngles_t attitude;
  84
  85     rxRuntimeState_t rxRuntimeState = {};
  86
  87     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
  88     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
  89     PG_REGISTER(positionConfig_t, positionConfig, PG_SYSTEM_CONFIG, 4);
  90
  91     bool unitLaunchControlActive = false;
  92     launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
  93
  94     float getMotorMixRange(void) { return simulatedMotorMixRange; }
  95     float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
  96     bool wasThrottleRaised(void) { return simulatedThrottleRaised; }
  97     float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); }
  98
  99     // used by auto-disarm code
 100     float getMaxRcDeflectionAbs() { return fabsf(simulatedMaxRcDeflectionAbs); }
 101     float mixerGetRcThrottle() { return fabsf(simulatedMixerGetRcThrottle); }
 102
 103
 104     bool isBelowLandingAltitude(void) { return false; }
 105
 106     void systemBeep(bool) { }
 107     bool gyroOverflowDetected(void) { return false; }
 108     float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
 109     float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; }
 110     float getRawSetpoint(int axis) { return simulatedRawSetpoint[axis]; }
 111     float getFeedforward(int axis) {
 112         return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
 113     }
 114     void beeperConfirmationBeeps(uint8_t) { }
 115     bool isLaunchControlActive(void) {return unitLaunchControlActive; }
 116     void disarm(flightLogDisarmReason_e) { }
 117     float getMaxRcRate(int axis)
 118     {
 119         UNUSED(axis);
 120         float maxRate = simulatedMaxRate[axis];
 121         return maxRate;
 122     }
 123     void initRcProcessing(void) { }
 124 }
 125
 126 pidProfile_t *pidProfile;
 127
 128 int loopIter = 0;
 129
 130 // Always use same defaults for testing in future releases even when defaults change
 131 void setDefaultTestSettings(void)
 132 {
 133     pgResetAll();
 134     pidProfile = pidProfilesMutable(1);
 135     pidProfile->pid[PID_ROLL]  =  { 40, 40, 30, 65, 0 };
 136     pidProfile->pid[PID_PITCH] =  { 58, 50, 35, 60, 0 };
 137     pidProfile->pid[PID_YAW]   =  { 70, 45, 20, 60, 0 };
 138     pidProfile->pid[PID_LEVEL] =  { 50, 50, 75, 50, 0 };
 139
 140     // Compensate for the upscaling done without 'use_integrated_yaw'
 141     pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f;
 142
 143     pidProfile->pidSumLimit = PIDSUM_LIMIT;        // 500
 144     pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; // 400
 145     pidProfile->yaw_lowpass_hz = 0;
 146     pidProfile->dterm_lpf1_static_hz = 100;
 147     pidProfile->dterm_lpf2_static_hz = 0;
 148     pidProfile->dterm_notch_hz = 260;
 149     pidProfile->dterm_notch_cutoff = 160;
 150     pidProfile->dterm_lpf1_type = FILTER_BIQUAD;
 151     pidProfile->itermWindup = 80;
 152     pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
 153     pidProfile->angle_limit = 60;
 154     pidProfile->feedforward_transition = 100;
 155     pidProfile->yawRateAccelLimit = 100;
 156     pidProfile->rateAccelLimit = 0;
 157     pidProfile->anti_gravity_gain = 10;
 158     pidProfile->crash_time = 500;
 159     pidProfile->crash_delay = 0;
 160     pidProfile->crash_recovery_angle = 10;
 161     pidProfile->crash_recovery_rate = 100;
 162     pidProfile->crash_dthreshold = 50;
 163     pidProfile->crash_gthreshold = 400;
 164     pidProfile->crash_setpoint_threshold = 350;
 165     pidProfile->crash_recovery = PID_CRASH_RECOVERY_OFF;
 166     pidProfile->horizon_limit_degrees = 135;
 167     pidProfile->horizon_ignore_sticks = false;
 168     pidProfile->crash_limit_yaw = 200;
 169     pidProfile->itermLimit = 150;
 170     pidProfile->throttle_boost = 0;
 171     pidProfile->throttle_boost_cutoff = 15;
 172     pidProfile->iterm_rotation = false;
 173     pidProfile->iterm_relax = ITERM_RELAX_OFF,
 174     pidProfile->iterm_relax_cutoff = 11,
 175     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT,
 176     pidProfile->abs_control_gain = 0,
 177     pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
 178     pidProfile->launchControlGain = 40,
 179     pidProfile->level_race_mode = false,
 180
 181     gyro.targetLooptime = 8000;
 182 }
 183
 184 timeUs_t currentTestTime(void)
 185 {
 186     return targetPidLooptime * loopIter++;
 187 }
 188
 189 void resetTest(void)
 190 {
 191     loopIter = 0;
 192     pidRuntime.tpaFactor = 1.0f;
 193     simulatedMotorMixRange = 0.0f;
 194
 195     pidStabilisationState(PID_STABILISATION_OFF);
 196     DISABLE_ARMING_FLAG(ARMED);
 197
 198     setDefaultTestSettings();
 199     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 200         pidData[axis].P = 0;
 201         pidData[axis].I = 0;
 202         pidData[axis].D = 0;
 203         pidData[axis].F = 0;
 204         pidData[axis].S = 0;
 205         pidData[axis].Sum = 0;
 206         simulatedSetpointRate[axis] = 0;
 207         simulatedRcDeflection[axis] = 0;
 208         simulatedRawSetpoint[axis] = 0;
 209         gyro.gyroADCf[axis] = 0;
 210     }
 211     attitude.values.roll = 0;
 212     attitude.values.pitch = 0;
 213     attitude.values.yaw = 0;
 214
 215     flightModeFlags = 0;
 216     unitLaunchControlActive = false;
 217     pidProfile->launchControlMode = unitLaunchControlMode;
 218     pidInit(pidProfile);
 219     loadControlRateProfile();
 220
 221     // Run pidloop for a while after reset
 222     for (int loop = 0; loop < 20; loop++) {
 223         pidController(pidProfile, currentTestTime());
 224     }
 225 }
 226
 227 void setStickPosition(int axis, float stickRatio)
 228 {
 229     simulatedPrevSetpointRate[axis] = simulatedSetpointRate[axis];
 230     simulatedSetpointRate[axis] = 1998.0f * stickRatio;
 231     simulatedRcDeflection[axis] = stickRatio;
 232 }
 233
 234 // All calculations will have 10% tolerance
 235 float calculateTolerance(float input)
 236 {
 237     return fabsf(input * 0.1f);
 238 }
 239
 240 TEST(pidControllerTest, testInitialisation)
 241 {
 242     resetTest();
 243
 244     // In initial state PIDsums should be 0
 245     for (int axis = 0; axis <= FD_YAW; axis++) {
 246         EXPECT_FLOAT_EQ(0, pidData[axis].P);
 247         EXPECT_FLOAT_EQ(0, pidData[axis].I);
 248         EXPECT_FLOAT_EQ(0, pidData[axis].D);
 249     }
 250 }
 251
 252 TEST(pidControllerTest, testStabilisationDisabled)
 253 {
 254     ENABLE_ARMING_FLAG(ARMED);
 255     // Run few loops to make sure there is no error building up when stabilisation disabled
 256
 257     for (int loop = 0; loop < 10; loop++) {
 258         pidController(pidProfile, currentTestTime());
 259
 260         // PID controller should not do anything, while stabilisation disabled
 261         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 262         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 263         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 264         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 265         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 266         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 267         EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 268         EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 269         EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 270     }
 271 }
 272
 273 TEST(pidControllerTest, testPidLoop)
 274 {
 275     // Make sure to start with fresh values
 276     resetTest();
 277     ENABLE_ARMING_FLAG(ARMED);
 278     pidStabilisationState(PID_STABILISATION_ON);
 279
 280     pidController(pidProfile, currentTestTime());
 281
 282     // Loop 1 - Expecting zero since there is no error
 283     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 284     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 285     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 286     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 287     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 288     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 289     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 290     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 291     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 292
 293     // Add some rotation on ROLL to generate error
 294     gyro.gyroADCf[FD_ROLL] = 100;
 295     pidController(pidProfile, currentTestTime());
 296
 297     // Loop 2 - Expect PID loop reaction to ROLL error
 298     EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 299     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 300     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 301     EXPECT_NEAR(-7.8, pidData[FD_ROLL].I, calculateTolerance(-7.8));
 302     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 303     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 304     EXPECT_NEAR(-198.4, pidData[FD_ROLL].D, calculateTolerance(-198.4));
 305     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 306     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 307
 308     // Add some rotation on PITCH to generate error
 309     gyro.gyroADCf[FD_PITCH] = -100;
 310     pidController(pidProfile, currentTestTime());
 311
 312     // Loop 3 - Expect PID loop reaction to PITCH error, ROLL is still in error
 313     EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 314     EXPECT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
 315     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 316     EXPECT_NEAR(-15.6, pidData[FD_ROLL].I, calculateTolerance(-15.6));
 317     EXPECT_NEAR(9.8, pidData[FD_PITCH].I, calculateTolerance(9.8));
 318     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 319     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 320     EXPECT_NEAR(231.4, pidData[FD_PITCH].D, calculateTolerance(231.4));
 321     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 322
 323     // Add some rotation on YAW to generate error, but not enough to trigger pidSumLimitYaw
 324     gyro.gyroADCf[FD_YAW] = 10;
 325     pidController(pidProfile, currentTestTime());
 326
 327     // Loop 4 - Expect PID loop reaction to PITCH error, ROLL and PITCH are still in error
 328     EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
 329     EXPECT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
 330     EXPECT_NEAR(-22.4, pidData[FD_YAW].P, calculateTolerance(-22.4));
 331     EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
 332     EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
 333     EXPECT_NEAR(-0.87, pidData[FD_YAW].I, calculateTolerance(-0.87));
 334     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 335     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 336
 337     // Simulate Iterm growth if not saturated
 338     pidController(pidProfile, currentTestTime());
 339     EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
 340     EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
 341     EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
 342         EXPECT_NEAR(-24.2, pidData[FD_YAW].Sum, calculateTolerance(-24.2));
 343
 344     // Match the stick to gyro to stop error
 345     simulatedSetpointRate[FD_ROLL] = 100;
 346     simulatedSetpointRate[FD_PITCH] = -100;
 347     simulatedSetpointRate[FD_YAW] = 10; // error
 348
 349     pidController(pidProfile, currentTestTime());
 350     // Iterm is stalled as it is not accumulating anymore
 351     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 352     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 353     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 354     EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
 355     EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
 356     EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
 357     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 358     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 359     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 360
 361
 362     for(int loop = 0; loop < 5; loop++) {
 363         pidController(pidProfile, currentTestTime());
 364     }
 365     // Iterm is stalled as it is not accumulating anymore
 366     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 367     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 368     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 369     EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
 370     EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
 371     EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
 372     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 373     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 374     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 375
 376     // Now disable Stabilisation
 377     pidStabilisationState(PID_STABILISATION_OFF);
 378     pidController(pidProfile, currentTestTime());
 379
 380     // Should all be zero again
 381     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
 382     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
 383     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
 384     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
 385     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
 386     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
 387     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 388     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 389     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 390 }
 391
 392 TEST(pidControllerTest, testPidLevel)
 393 {
 394     // Make sure to start with fresh values
 395     resetTest();
 396     ENABLE_ARMING_FLAG(ARMED);
 397     pidStabilisationState(PID_STABILISATION_ON);
 398
 399     // Test Angle mode response
 400     enableFlightMode(ANGLE_MODE);
 401     float currentPidSetpointRoll = 0;
 402     float currentPidSetpointPitch = 0;
 403     float calculatedAngleSetpoint = 0;
 404     rollAndPitchTrims_t angleTrim = { { 0, 0 } };
 405
 406     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 407     EXPECT_FLOAT_EQ(0, calculatedAngleSetpoint);
 408     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 409     EXPECT_FLOAT_EQ(0, calculatedAngleSetpoint);
 410
 411     currentPidSetpointRoll = 200;
 412     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 413     EXPECT_FLOAT_EQ(51.456356, calculatedAngleSetpoint);
 414     currentPidSetpointPitch = -200;
 415     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 416     EXPECT_FLOAT_EQ(-51.456356, calculatedAngleSetpoint);
 417
 418     currentPidSetpointRoll = 400;
 419     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 420     EXPECT_FLOAT_EQ(128.94597, calculatedAngleSetpoint);
 421     currentPidSetpointPitch = -400;
 422     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 423     EXPECT_FLOAT_EQ(-128.94597, calculatedAngleSetpoint);
 424
 425     // Test attitude response
 426     attitude.values.roll = -275;
 427     attitude.values.pitch = 275;
 428     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 429     EXPECT_FLOAT_EQ(242.76686, calculatedAngleSetpoint);
 430     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 431     EXPECT_FLOAT_EQ(-242.76686, calculatedAngleSetpoint);
 432
 433     // Disable ANGLE_MODE
 434     disableFlightMode(ANGLE_MODE);
 435     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 436     EXPECT_FLOAT_EQ(393.44571, calculatedAngleSetpoint);
 437     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 438     EXPECT_FLOAT_EQ(-392.88422, calculatedAngleSetpoint);
 439
 440     // Test level mode expo
 441     enableFlightMode(ANGLE_MODE);
 442     attitude.values.roll = 0;
 443     attitude.values.pitch = 0;
 444     currentPidSetpointRoll = 400;
 445     currentPidSetpointPitch = -400;
 446     // need to set some rates type and some expo here ??? HELP !!
 447     calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
 448     EXPECT_FLOAT_EQ(231.55479, calculatedAngleSetpoint);
 449     calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
 450     EXPECT_FLOAT_EQ(-231.55479, calculatedAngleSetpoint);
 451 }
 452
 453
 454 TEST(pidControllerTest, testPidHorizon)
 455 {
 456     resetTest();
 457     ENABLE_ARMING_FLAG(ARMED);
 458     pidStabilisationState(PID_STABILISATION_ON);
 459     enableFlightMode(HORIZON_MODE);
 460
 461     // Test stick response greater than default limit of 0.75
 462     setStickPosition(FD_ROLL, 0.76f);
 463     setStickPosition(FD_PITCH, -0.76f);
 464     EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
 465
 466     // Return sticks to center, should expect some levelling, but will be delayed
 467     setStickPosition(FD_ROLL, 0);
 468     setStickPosition(FD_PITCH, 0);
 469     EXPECT_FLOAT_EQ(0.0078740157, calcHorizonLevelStrength());
 470
 471     // Test small stick response when flat, considering delay
 472     setStickPosition(FD_ROLL, 0.1f);
 473     setStickPosition(FD_PITCH, -0.1f);
 474     EXPECT_NEAR(0.01457f, calcHorizonLevelStrength(), calculateTolerance(0.01457));
 475
 476     // Test larger stick response when flat
 477     setStickPosition(FD_ROLL, 0.5f);
 478     setStickPosition(FD_PITCH, -0.5f);
 479     EXPECT_NEAR(0.0166, calcHorizonLevelStrength(), calculateTolerance(0.0166));
 480
 481     // set attitude of craft to 90 degrees
 482     attitude.values.roll = 900;
 483     attitude.values.pitch = 900;
 484
 485     // Test centered sticks at 90 degrees
 486     setStickPosition(FD_ROLL, 0);
 487     setStickPosition(FD_PITCH, 0);
 488     // with gain of 50, and max angle of 135 deg, strength = 0.5 * (135-90) / 90 ie 0.5 * 45/136 or 0.5 * 0.333 = 0.166
 489     EXPECT_NEAR(0.0193f, calcHorizonLevelStrength(), calculateTolerance(0.0193));
 490
 491     // Test small stick response at 90 degrees
 492     setStickPosition(FD_ROLL, 0.1f);
 493     setStickPosition(FD_PITCH, -0.1f);
 494     EXPECT_NEAR(0.0213f, calcHorizonLevelStrength(), calculateTolerance(0.0213));
 495
 496     // Test larger stick response at 90 degrees
 497     setStickPosition(FD_ROLL, 0.5f);
 498     setStickPosition(FD_PITCH, -0.5f);
 499     EXPECT_NEAR(0.0218f, calcHorizonLevelStrength(), calculateTolerance(0.0218));
 500
 501     // set attitude of craft to 120 degrees, inside limit of 135
 502     attitude.values.roll = 1200;
 503     attitude.values.pitch = 1200;
 504
 505     // Test centered sticks at 120 degrees
 506     setStickPosition(FD_ROLL, 0);
 507     setStickPosition(FD_PITCH, 0);
 508     EXPECT_NEAR(0.0224f, calcHorizonLevelStrength(), calculateTolerance(0.0224));
 509
 510     // Test small stick response at 120 degrees
 511     setStickPosition(FD_ROLL, 0.1f);
 512     setStickPosition(FD_PITCH, -0.1f);
 513     EXPECT_NEAR(0.0228f, calcHorizonLevelStrength(), calculateTolerance(0.0228));
 514
 515     // Test larger stick response at 120 degrees
 516     setStickPosition(FD_ROLL, 0.5f);
 517     setStickPosition(FD_PITCH, -0.5f);
 518     EXPECT_NEAR(0.018f, calcHorizonLevelStrength(), calculateTolerance(0.018));
 519
 520     // set attitude of craft to 1500 degrees, outside limit of 135
 521     attitude.values.roll = 1500;
 522     attitude.values.pitch = 1500;
 523
 524     // Test centered sticks at 150 degrees - should be zero at any stick angle
 525     setStickPosition(FD_ROLL, 0);
 526     setStickPosition(FD_PITCH, 0);
 527     EXPECT_NEAR(0.0f, calcHorizonLevelStrength(), calculateTolerance(0.0));
 528
 529     setStickPosition(FD_ROLL, 0.5f);
 530     setStickPosition(FD_PITCH, -0.5f);
 531     EXPECT_NEAR(0.0f, calcHorizonLevelStrength(), calculateTolerance(0.0));
 532
 533 }
 534
 535 // trying to fix
 536
 537
 538
 539 TEST(pidControllerTest, testMixerSaturation)
 540 {
 541     resetTest();
 542
 543     ENABLE_ARMING_FLAG(ARMED);
 544     pidStabilisationState(PID_STABILISATION_ON);
 545
 546     pidRuntime.itermLimit = 400;
 547     pidRuntime.itermLimitYaw = 320;
 548
 549     // Test full stick response
 550     setStickPosition(FD_ROLL, 1.0f);
 551     setStickPosition(FD_PITCH, -1.0f);
 552     setStickPosition(FD_YAW, 1.0f);
 553     pidController(pidProfile, currentTestTime());
 554
 555     // Expect iterm accumulation for all axes because at this point, pidSum is not at limit
 556     EXPECT_NEAR(156.2f, pidData[FD_ROLL].I, calculateTolerance(156.2f));
 557     EXPECT_NEAR(-195.3f, pidData[FD_PITCH].I, calculateTolerance(-195.3f));
 558     EXPECT_NEAR(7.0f, pidData[FD_YAW].I, calculateTolerance(7.0f));
 559
 560      // ????? why such slow yaw iTerm growth ?? this is not what I see in the real logs == strange
 561
 562     // Check for iterm growth, should not reach limits yet
 563     pidController(pidProfile, currentTestTime());
 564     EXPECT_NEAR(312.4f, pidData[FD_ROLL].I, calculateTolerance(312.4f));
 565     EXPECT_NEAR(-390.6f, pidData[FD_PITCH].I, calculateTolerance(-390.6));
 566     EXPECT_NEAR(21.1f, pidData[FD_YAW].I, calculateTolerance(21.1));
 567
 568     // Expect iterm  roll + pitch to stop at limit of 400
 569     // yaw is still growing,
 570     pidController(pidProfile, currentTestTime());
 571     EXPECT_NEAR(400.0f, pidData[FD_ROLL].I, calculateTolerance(400.0f));
 572     EXPECT_NEAR(-400.0f, pidData[FD_PITCH].I, calculateTolerance(-400.0f));
 573     EXPECT_NEAR(42.2f, pidData[FD_YAW].I, calculateTolerance(42.2f));
 574
 575     // run some more loops, check all iTerm values are at their limit
 576     for (int loop = 0; loop < 7; loop++) {
 577         pidController(pidProfile, currentTestTime());
 578     }
 579     EXPECT_NEAR(400, pidData[FD_ROLL].I, calculateTolerance(400));
 580     EXPECT_NEAR(-400, pidData[FD_PITCH].I, calculateTolerance(-400));
 581     EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
 582
 583     // Test that the added i term gain from Anti Gravity
 584     // is also limited
 585     resetTest();
 586     ENABLE_ARMING_FLAG(ARMED);
 587     pidStabilisationState(PID_STABILISATION_ON);
 588     pidRuntime.itermLimit = 400;
 589     pidRuntime.itermLimitYaw = 320;
 590
 591     setStickPosition(FD_ROLL, 1.0f);
 592     setStickPosition(FD_PITCH, -1.0f);
 593     setStickPosition(FD_YAW, 1.0f);
 594     const bool prevAgState = pidRuntime.antiGravityEnabled;
 595     const float prevAgTrhottleD = pidRuntime.antiGravityThrottleD;
 596     pidRuntime.antiGravityEnabled = true;
 597     pidRuntime.antiGravityThrottleD = 1.0;
 598
 599     pidController(pidProfile, currentTestTime());
 600
 601     // Expect more iterm accumulation than before on pitch and roll, no change on yaw
 602     // without antigravity values were 156, 195, 7
 603     EXPECT_NEAR(210.6, pidData[FD_ROLL].I, calculateTolerance(210.6f));
 604     EXPECT_NEAR(-249.6f, pidData[FD_PITCH].I, calculateTolerance(-249.6f));
 605     EXPECT_NEAR(7.0f, pidData[FD_YAW].I, calculateTolerance(7.0f));
 606
 607     // run again and should expect to hit the limit on pitch and roll but yaw unaffected
 608     pidController(pidProfile, currentTestTime());
 609     EXPECT_NEAR(400, pidData[FD_ROLL].I, calculateTolerance(400));
 610     EXPECT_NEAR(-400, pidData[FD_PITCH].I, calculateTolerance(-400));
 611     EXPECT_NEAR(21.0f, pidData[FD_YAW].I, calculateTolerance(21.0f));
 612
 613     pidRuntime.antiGravityEnabled = prevAgState;
 614     pidRuntime.antiGravityThrottleD = prevAgTrhottleD;
 615
 616     // Test that i term is limited on yaw at 320  when only yaw is saturated
 617     resetTest();
 618     ENABLE_ARMING_FLAG(ARMED);
 619     pidRuntime.itermLimit = 400;
 620     pidRuntime.itermLimitYaw = 320;
 621
 622     pidStabilisationState(PID_STABILISATION_ON);
 623     setStickPosition(FD_ROLL, 0.0f);
 624     setStickPosition(FD_PITCH, 0.0f);
 625     setStickPosition(FD_YAW, 0.5f);
 626
 627     for (int loop = 0; loop < 7; loop++) {
 628         pidController(pidProfile, currentTestTime());
 629     }
 630
 631     EXPECT_NEAR(0, pidData[FD_ROLL].I, calculateTolerance(0));
 632     EXPECT_NEAR(0, pidData[FD_PITCH].I, calculateTolerance(0));
 633     EXPECT_NEAR(197, pidData[FD_YAW].I, calculateTolerance(197));
 634
 635     pidController(pidProfile, currentTestTime());
 636     EXPECT_NEAR(253, pidData[FD_YAW].I, calculateTolerance(320));
 637
 638     pidController(pidProfile, currentTestTime());
 639     EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
 640
 641     pidController(pidProfile, currentTestTime());
 642     EXPECT_NEAR(0, pidData[FD_ROLL].I, calculateTolerance(0));
 643     EXPECT_NEAR(0, pidData[FD_PITCH].I, calculateTolerance(0));
 644     EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
 645 }
 646
 647 TEST(pidControllerTest, testiTermWindup)
 648 {
 649     resetTest();
 650     ENABLE_ARMING_FLAG(ARMED);
 651     // simulate the outcome with iterm_windup of 50
 652     pidRuntime.itermLimit = 200;
 653     pidRuntime.itermLimitYaw = 160;
 654
 655     pidStabilisationState(PID_STABILISATION_ON);
 656     setStickPosition(FD_ROLL, 0.12f);
 657     setStickPosition(FD_PITCH, 0.12f);
 658     setStickPosition(FD_YAW, 0.12f);
 659
 660     for (int loop = 0; loop < 7; loop++) {
 661         pidController(pidProfile, currentTestTime());
 662     }
 663
 664     EXPECT_NEAR(131, pidData[FD_ROLL].I, calculateTolerance(131));
 665     EXPECT_NEAR(164, pidData[FD_PITCH].I, calculateTolerance(164));
 666     EXPECT_NEAR(126, pidData[FD_YAW].I, calculateTolerance(126));
 667
 668     pidController(pidProfile, currentTestTime());
 669     EXPECT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150));
 670     EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
 671     EXPECT_NEAR(137, pidData[FD_YAW].I, calculateTolerance(137));
 672
 673     pidController(pidProfile, currentTestTime());
 674     EXPECT_NEAR(169, pidData[FD_ROLL].I, calculateTolerance(200));
 675     EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
 676     EXPECT_NEAR(160, pidData[FD_YAW].I, calculateTolerance(160));
 677
 678     pidController(pidProfile, currentTestTime());
 679     EXPECT_NEAR(200, pidData[FD_ROLL].I, calculateTolerance(200));
 680     EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
 681     EXPECT_NEAR(160, pidData[FD_YAW].I, calculateTolerance(320));
 682 }
 683
 684 // TODO - Add more scenarios
 685 TEST(pidControllerTest, testCrashRecoveryMode)
 686 {
 687     resetTest();
 688     pidProfile->crash_recovery = PID_CRASH_RECOVERY_ON;
 689     pidInit(pidProfile);
 690     ENABLE_ARMING_FLAG(ARMED);
 691     pidStabilisationState(PID_STABILISATION_ON);
 692     sensorsSet(SENSOR_ACC);
 693
 694     EXPECT_FALSE(crashRecoveryModeActive());
 695
 696     int loopsToCrashTime = (int)((pidProfile->crash_time * 1000) / targetPidLooptime) + 1;
 697
 698     // generate crash detection for roll axis
 699     gyro.gyroADCf[FD_ROLL]  = 800;
 700     simulatedMotorMixRange = 1.2f;
 701     for (int loop =0; loop <= loopsToCrashTime; loop++) {
 702         gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
 703         // advance the time to avoid initialized state prevention of crash recovery
 704         pidController(pidProfile, currentTestTime() + 2000000);
 705     }
 706
 707     EXPECT_TRUE(crashRecoveryModeActive());
 708     // Add additional verifications
 709 }
 710
 711 TEST(pidControllerTest, testFeedForward)
 712 // NOTE: THIS DOES NOT TEST THE FEEDFORWARD CALCULATIONS, which are now in rc.c, and return setpointDelta
 713 // This test only validates the feedforward value calculated in pid.c for a given simulated setpointDelta
 714 {
 715     resetTest();
 716     ENABLE_ARMING_FLAG(ARMED);
 717     pidStabilisationState(PID_STABILISATION_ON);
 718
 719     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 720     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 721     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 722
 723     // Move the sticks fully
 724     setStickPosition(FD_ROLL, 1.0f);
 725     setStickPosition(FD_PITCH, -1.0f);
 726     setStickPosition(FD_YAW, -1.0f);
 727
 728     pidController(pidProfile, currentTestTime());
 729
 730     EXPECT_NEAR(17.86, pidData[FD_ROLL].F, calculateTolerance(17.86));
 731     EXPECT_NEAR(-16.49, pidData[FD_PITCH].F, calculateTolerance(-16.49));
 732     EXPECT_NEAR(-16.49, pidData[FD_YAW].F, calculateTolerance(-16.49));
 733
 734     // Bring sticks back to half way
 735     setStickPosition(FD_ROLL, 0.5f);
 736     setStickPosition(FD_PITCH, -0.5f);
 737     setStickPosition(FD_YAW, -0.5f);
 738
 739     pidController(pidProfile, currentTestTime());
 740
 741     EXPECT_NEAR(-8.93, pidData[FD_ROLL].F, calculateTolerance(-8.93));
 742     EXPECT_NEAR(8.24, pidData[FD_PITCH].F, calculateTolerance(8.24));
 743     EXPECT_NEAR(8.24, pidData[FD_YAW].F, calculateTolerance(8.24));
 744
 745     // Bring sticks back to two tenths out
 746     setStickPosition(FD_ROLL, 0.2f);
 747     setStickPosition(FD_PITCH, -0.2f);
 748     setStickPosition(FD_YAW, -0.2f);
 749
 750     pidController(pidProfile, currentTestTime());
 751
 752     EXPECT_NEAR(-5.36, pidData[FD_ROLL].F, calculateTolerance(-5.36));
 753     EXPECT_NEAR(4.95, pidData[FD_PITCH].F, calculateTolerance(4.95));
 754     EXPECT_NEAR(4.95, pidData[FD_YAW].F, calculateTolerance(4.95));
 755
 756     // Bring sticks back to one tenth out, to give a difference of 0.1
 757     setStickPosition(FD_ROLL, 0.1f);
 758     setStickPosition(FD_PITCH, -0.1f);
 759     setStickPosition(FD_YAW, -0.1f);
 760
 761     pidController(pidProfile, currentTestTime());
 762
 763     EXPECT_NEAR(-1.79, pidData[FD_ROLL].F, calculateTolerance(-1.79));
 764     EXPECT_NEAR(1.65, pidData[FD_PITCH].F, calculateTolerance(1.65));
 765     EXPECT_NEAR(1.65, pidData[FD_YAW].F, calculateTolerance(1.65));
 766
 767     // Center the sticks, giving the same delta value as before, should return the same feedforward
 768     setStickPosition(FD_ROLL, 0.0f);
 769     setStickPosition(FD_PITCH, 0.0f);
 770     setStickPosition(FD_YAW, 0.0f);
 771
 772     pidController(pidProfile, currentTestTime());
 773
 774     EXPECT_NEAR(-1.79, pidData[FD_ROLL].F, calculateTolerance(-1.79));
 775     EXPECT_NEAR(1.65, pidData[FD_PITCH].F, calculateTolerance(1.65));
 776     EXPECT_NEAR(1.65, pidData[FD_YAW].F, calculateTolerance(1.65));
 777
 778     // Keep centered
 779     setStickPosition(FD_ROLL, 0.0f);
 780     setStickPosition(FD_PITCH, 0.0f);
 781     setStickPosition(FD_YAW, 0.0f);
 782
 783     pidController(pidProfile, currentTestTime());
 784
 785     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 786     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 787     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 788 }
 789
 790 TEST(pidControllerTest, testItermRelax)
 791 {
 792     resetTest();
 793     pidProfile->iterm_relax = ITERM_RELAX_RP;
 794     ENABLE_ARMING_FLAG(ARMED);
 795     pidStabilisationState(PID_STABILISATION_ON);
 796
 797     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT;
 798     pidInit(pidProfile);
 799
 800     float itermErrorRate = 0;
 801     float currentPidSetpoint = 0;
 802     float gyroRate = 0;
 803
 804     applyItermRelax(FD_PITCH, 0, gyroRate, &itermErrorRate, &currentPidSetpoint);
 805     EXPECT_FLOAT_EQ(itermErrorRate, 0);
 806     itermErrorRate = -10;
 807     currentPidSetpoint = 10;
 808     pidData[FD_PITCH].I = 10;
 809
 810     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 811
 812     EXPECT_NEAR(-8.16, itermErrorRate, calculateTolerance(-8.16));
 813     currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD;
 814     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 815     EXPECT_NEAR(0, itermErrorRate, calculateTolerance(0));
 816     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 817     EXPECT_NEAR(0, itermErrorRate, calculateTolerance(0));
 818
 819     pidProfile->iterm_relax_type = ITERM_RELAX_GYRO;
 820     pidInit(pidProfile);
 821
 822     currentPidSetpoint = 100;
 823     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 824     EXPECT_FLOAT_EQ(itermErrorRate, 0);
 825     gyroRate = 10;
 826     itermErrorRate = -10;
 827     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 828     EXPECT_NEAR(7, itermErrorRate, calculateTolerance(7));
 829     gyroRate += 100;
 830     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 831     EXPECT_NEAR(-10, itermErrorRate, calculateTolerance(-10));
 832
 833     pidProfile->iterm_relax = ITERM_RELAX_RP_INC;
 834     pidInit(pidProfile);
 835
 836     itermErrorRate = -10;
 837     pidData[FD_PITCH].I = 10;
 838     currentPidSetpoint = 10;
 839     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 840     EXPECT_FLOAT_EQ(itermErrorRate, -10);
 841     itermErrorRate = 10;
 842     pidData[FD_PITCH].I = -10;
 843     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 844     EXPECT_FLOAT_EQ(itermErrorRate, 10);
 845     itermErrorRate = -10;
 846     currentPidSetpoint = 10;
 847     applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 848     EXPECT_FLOAT_EQ(itermErrorRate, -100);
 849
 850     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT;
 851     pidInit(pidProfile);
 852
 853     itermErrorRate = -10;
 854     currentPidSetpoint = ITERM_RELAX_SETPOINT_THRESHOLD;
 855     applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 856     EXPECT_FLOAT_EQ(itermErrorRate, -10);
 857
 858     pidProfile->iterm_relax = ITERM_RELAX_RPY;
 859     pidInit(pidProfile);
 860     applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
 861     EXPECT_NEAR(-3.6, itermErrorRate, calculateTolerance(-3.6));
 862 }
 863
 864 // TODO - Add more tests
 865 TEST(pidControllerTest, testAbsoluteControl)
 866 {
 867     resetTest();
 868     pidProfile->abs_control_gain = 10;
 869     pidInit(pidProfile);
 870     ENABLE_ARMING_FLAG(ARMED);
 871     pidStabilisationState(PID_STABILISATION_ON);
 872
 873     float gyroRate = 0;
 874
 875     float itermErrorRate = 10;
 876     float currentPidSetpoint = 10;
 877
 878     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 879
 880     EXPECT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
 881     EXPECT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
 882
 883     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 884     EXPECT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
 885     EXPECT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
 886
 887     gyroRate = -53;
 888     axisError[FD_PITCH] = -60;
 889     applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
 890     EXPECT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2));
 891     EXPECT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2));
 892 }
 893
 894 TEST(pidControllerTest, testDtermFiltering)
 895 {
 896 // TODO
 897 }
 898
 899 TEST(pidControllerTest, testItermRotationHandling)
 900 {
 901     resetTest();
 902     pidInit(pidProfile);
 903
 904     rotateItermAndAxisError();
 905     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0);
 906     EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0);
 907     EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0);
 908
 909     pidProfile->iterm_rotation = true;
 910     pidInit(pidProfile);
 911
 912     rotateItermAndAxisError();
 913     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0);
 914     EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0);
 915     EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0);
 916
 917     pidData[FD_ROLL].I = 10;
 918     pidData[FD_PITCH].I = 1000;
 919     pidData[FD_YAW].I = 1000;
 920     gyro.gyroADCf[FD_ROLL] = -1000;
 921     rotateItermAndAxisError();
 922     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
 923     EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
 924     EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
 925
 926     pidProfile->abs_control_gain = 10;
 927     pidInit(pidProfile);
 928     pidData[FD_ROLL].I = 10;
 929     pidData[FD_PITCH].I = 1000;
 930     pidData[FD_YAW].I = 1000;
 931
 932     gyro.gyroADCf[FD_ROLL] = -1000;
 933     // FIXME - axisError changes don't affect the system. This is a potential bug or intendend behaviour?
 934     axisError[FD_PITCH] = 1000;
 935     axisError[FD_YAW] = 1000;
 936     rotateItermAndAxisError();
 937     EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
 938     EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
 939     EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
 940 }
 941
 942 TEST(pidControllerTest, testLaunchControl)
 943 {
 944     // The launchControlGain is indirectly tested since when launch control is active the
 945     // the gain overrides the PID settings. If the logic to use launchControlGain wasn't
 946     // working then any I calculations would be incorrect.
 947
 948     resetTest();
 949     unitLaunchControlActive = true;
 950     ENABLE_ARMING_FLAG(ARMED);
 951     pidStabilisationState(PID_STABILISATION_ON);
 952
 953     // test that feedforward and D are disabled (always zero) when launch control is active
 954     // set initial state
 955     pidController(pidProfile, currentTestTime());
 956
 957     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 958     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 959     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 960     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 961     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 962     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 963
 964     // Move the sticks to induce feedforward
 965     setStickPosition(FD_ROLL, 0.5f);
 966     setStickPosition(FD_PITCH, -0.5f);
 967     setStickPosition(FD_YAW, -0.5f);
 968
 969     // add gyro activity to induce D
 970     gyro.gyroADCf[FD_ROLL] = -1000;
 971     gyro.gyroADCf[FD_PITCH] = 1000;
 972     gyro.gyroADCf[FD_YAW] = -1000;
 973
 974     pidController(pidProfile, currentTestTime());
 975
 976     // validate that feedforwad is still 0
 977     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
 978     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
 979     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
 980
 981     // validate that D is still 0
 982     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
 983     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
 984     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
 985
 986     // test NORMAL mode - expect P/I on roll and pitch, P on yaw but I == 0
 987     unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
 988     resetTest();
 989     unitLaunchControlActive = true;
 990     ENABLE_ARMING_FLAG(ARMED);
 991     pidStabilisationState(PID_STABILISATION_ON);
 992
 993     pidController(pidProfile, currentTestTime());
 994
 995     gyro.gyroADCf[FD_ROLL] = -20;
 996     gyro.gyroADCf[FD_PITCH] = 20;
 997     gyro.gyroADCf[FD_YAW] = -20;
 998     pidController(pidProfile, currentTestTime());
 999
1000     EXPECT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
1001     EXPECT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
1002     EXPECT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
1003     EXPECT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
1004     EXPECT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
1005     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
1006
1007     // test PITCHONLY mode - expect P/I only on pitch; I cannot go negative
1008     unitLaunchControlMode = LAUNCH_CONTROL_MODE_PITCHONLY;
1009     resetTest();
1010     unitLaunchControlActive = true;
1011     ENABLE_ARMING_FLAG(ARMED);
1012     pidStabilisationState(PID_STABILISATION_ON);
1013
1014     pidController(pidProfile, currentTestTime());
1015
1016     // first test that pitch I is prevented from going negative
1017     gyro.gyroADCf[FD_ROLL] = 0;
1018     gyro.gyroADCf[FD_PITCH] = 20;
1019     gyro.gyroADCf[FD_YAW] = 0;
1020     pidController(pidProfile, currentTestTime());
1021
1022     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
1023
1024     gyro.gyroADCf[FD_ROLL] = 20;
1025     gyro.gyroADCf[FD_PITCH] = -20;
1026     gyro.gyroADCf[FD_YAW] = 20;
1027     pidController(pidProfile, currentTestTime());
1028
1029     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
1030     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
1031     EXPECT_NEAR(37.15, pidData[FD_PITCH].P, calculateTolerance(37.15));
1032     EXPECT_NEAR(1.56,  pidData[FD_PITCH].I, calculateTolerance(1.56));
1033     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
1034     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
1035
1036     // test FULL mode - expect P/I on all axes
1037     unitLaunchControlMode = LAUNCH_CONTROL_MODE_FULL;
1038     resetTest();
1039     unitLaunchControlActive = true;
1040     ENABLE_ARMING_FLAG(ARMED);
1041     pidStabilisationState(PID_STABILISATION_ON);
1042
1043     pidController(pidProfile, currentTestTime());
1044
1045     gyro.gyroADCf[FD_ROLL] = -20;
1046     gyro.gyroADCf[FD_PITCH] = 20;
1047     gyro.gyroADCf[FD_YAW] = -20;
1048     pidController(pidProfile, currentTestTime());
1049
1050     EXPECT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
1051     EXPECT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
1052     EXPECT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
1053     EXPECT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
1054     EXPECT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
1055     EXPECT_NEAR(1.56,   pidData[FD_YAW].I,  calculateTolerance(1.56));
1056 }
1057
1058 TEST(pidControllerTest, testTpaClassic)
1059 {
1060     resetTest();
1061
1062     pidProfile->tpa_curve_type = TPA_CURVE_CLASSIC;
1063     pidProfile->tpa_rate = 30;
1064     pidProfile->tpa_breakpoint = 1600;
1065     pidProfile->tpa_low_rate = -50;
1066     pidProfile->tpa_low_breakpoint = 1200;
1067     pidProfile->tpa_low_always = 1;
1068
1069     pidInit(pidProfile);
1070
1071     pidUpdateTpaFactor(0.0f);
1072     EXPECT_FLOAT_EQ(1.5f, pidRuntime.tpaFactor);
1073
1074     pidUpdateTpaFactor(0.1f);
1075     EXPECT_FLOAT_EQ(1.25f, pidRuntime.tpaFactor);
1076
1077     pidUpdateTpaFactor(0.2f);
1078     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1079
1080     pidUpdateTpaFactor(0.6f);
1081     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1082
1083     pidUpdateTpaFactor(0.8f);
1084     EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
1085
1086     pidUpdateTpaFactor(1.0f);
1087     EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
1088
1089
1090     pidProfile->tpa_curve_type = TPA_CURVE_CLASSIC;
1091     pidProfile->tpa_rate = 30;
1092     pidProfile->tpa_breakpoint = 1600;
1093     pidProfile->tpa_low_rate = -50;
1094     pidProfile->tpa_low_breakpoint = 1000;
1095     pidProfile->tpa_low_always = 1;
1096
1097     pidInit(pidProfile);
1098
1099     pidUpdateTpaFactor(0.0f);
1100     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1101
1102     pidUpdateTpaFactor(0.1f);
1103     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1104
1105     pidUpdateTpaFactor(0.2f);
1106     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1107
1108     pidUpdateTpaFactor(0.6f);
1109     EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
1110
1111     pidUpdateTpaFactor(0.8f);
1112     EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
1113
1114     pidUpdateTpaFactor(1.0f);
1115     EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
1116 }
1117
1118 TEST(pidControllerTest, testTpaHyperbolic)
1119 {
1120     resetTest();
1121
1122     // curve sligly down - edge case where internal expo -> inf
1123     pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
1124     pidProfile->tpa_curve_pid_thr100 = 50;
1125     pidProfile->tpa_curve_pid_thr0 = 500;
1126     pidProfile->tpa_curve_expo = 10;
1127     pidProfile->tpa_curve_stall_throttle = 30;
1128
1129     pidInit(pidProfile);
1130
1131     pidUpdateTpaFactor(0.0f);
1132     EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
1133
1134     pidUpdateTpaFactor(0.15f);
1135     EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
1136
1137     pidUpdateTpaFactor(0.5);
1138     EXPECT_NEAR(2.588f, pidRuntime.tpaFactor, 0.01f);
1139
1140     pidUpdateTpaFactor(0.9);
1141     EXPECT_NEAR(0.693f, pidRuntime.tpaFactor, 0.01f);
1142
1143     pidUpdateTpaFactor(1.0);
1144     EXPECT_NEAR(0.5f, pidRuntime.tpaFactor, 0.01f);
1145
1146     // linear curve
1147     pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
1148     pidProfile->tpa_curve_pid_thr100 = 10;
1149     pidProfile->tpa_curve_pid_thr0 = 300;
1150     pidProfile->tpa_curve_expo = 0;
1151     pidProfile->tpa_curve_stall_throttle = 0;
1152
1153     pidInit(pidProfile);
1154
1155     pidUpdateTpaFactor(0.0f);
1156     EXPECT_FLOAT_EQ(3.0f, pidRuntime.tpaFactor);
1157
1158     pidUpdateTpaFactor(0.15f);
1159     EXPECT_NEAR(2.565f, pidRuntime.tpaFactor, 0.01f);
1160
1161     pidUpdateTpaFactor(0.5);
1162     EXPECT_NEAR(1.550f, pidRuntime.tpaFactor, 0.01f);
1163
1164     pidUpdateTpaFactor(0.9);
1165     EXPECT_NEAR(0.390f, pidRuntime.tpaFactor, 0.01f);
1166
1167     pidUpdateTpaFactor(1.0);
1168     EXPECT_NEAR(0.1f, pidRuntime.tpaFactor, 0.01f);
1169
1170     // curve bends up
1171     pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
1172     pidProfile->tpa_curve_pid_thr100 = 60;
1173     pidProfile->tpa_curve_pid_thr0 = 1000;
1174     pidProfile->tpa_curve_expo = -50;
1175     pidProfile->tpa_curve_stall_throttle = 40;
1176
1177     pidInit(pidProfile);
1178
1179     pidUpdateTpaFactor(0.0f);
1180     EXPECT_FLOAT_EQ(10.0f, pidRuntime.tpaFactor);
1181
1182     pidUpdateTpaFactor(0.15f);
1183     EXPECT_NEAR(10.0f, pidRuntime.tpaFactor, 0.01f);
1184
1185     pidUpdateTpaFactor(0.5);
1186     EXPECT_NEAR(9.700f, pidRuntime.tpaFactor, 0.01f);
1187
1188     pidUpdateTpaFactor(0.9);
1189     EXPECT_NEAR(7.364f, pidRuntime.tpaFactor, 0.01f);
1190
1191     pidUpdateTpaFactor(1.0);
1192     EXPECT_NEAR(0.625f, pidRuntime.tpaFactor, 0.01f);
1193
1194     // curve bends down
1195     pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
1196     pidProfile->tpa_curve_pid_thr100 = 90;
1197     pidProfile->tpa_curve_pid_thr0 = 250;
1198     pidProfile->tpa_curve_expo = 60;
1199     pidProfile->tpa_curve_stall_throttle = 60;
1200
1201     pidInit(pidProfile);
1202
1203     pidUpdateTpaFactor(0.0f);
1204     EXPECT_FLOAT_EQ(2.5f, pidRuntime.tpaFactor);
1205
1206     pidUpdateTpaFactor(0.15f);
1207     EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
1208
1209     pidUpdateTpaFactor(0.5);
1210     EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
1211
1212     pidUpdateTpaFactor(0.9);
1213     EXPECT_NEAR(0.954f, pidRuntime.tpaFactor, 0.01f);
1214
1215     pidUpdateTpaFactor(1.0);
1216     EXPECT_NEAR(0.9f, pidRuntime.tpaFactor, 0.01f);
1217 }