src/main/flight/pid.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #include "build/build_config.h"
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/maths.h"
  33 #include "common/filter.h"
  34
  35 #include "config/config_reset.h"
  36 #include "pg/pg.h"
  37 #include "pg/pg_ids.h"
  38
  39 #include "drivers/sound_beeper.h"
  40 #include "drivers/time.h"
  41
  42 #include "fc/controlrate_profile.h"
  43 #include "fc/core.h"
  44 #include "fc/rc.h"
  45
  46 #include "fc/rc_controls.h"
  47 #include "fc/runtime_config.h"
  48
  49 #include "flight/pid.h"
  50 #include "flight/imu.h"
  51 #include "flight/gps_rescue.h"
  52 #include "flight/mixer.h"
  53
  54 #include "io/gps.h"
  55
  56 #include "sensors/gyro.h"
  57 #include "sensors/acceleration.h"
  58
  59 #include "sensors/rpm_filter.h"
  60
  61 const char pidNames[] =
  62     "ROLL;"
  63     "PITCH;"
  64     "YAW;"
  65     "LEVEL;"
  66     "MAG;";
  67
  68 FAST_RAM_ZERO_INIT uint32_t targetPidLooptime;
  69 FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT];
  70
  71 static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
  72
  73 static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
  74
  75 static FAST_RAM_ZERO_INIT float dT;
  76 static FAST_RAM_ZERO_INIT float pidFrequency;
  77
  78 static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
  79 static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
  80 static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
  81 static FAST_RAM float antiGravityOsdCutoff = 1.0f;
  82 static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
  83 static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
  84
  85 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
  86
  87 #ifdef STM32F10X
  88 #define PID_PROCESS_DENOM_DEFAULT       1
  89 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500)  || defined(USE_GYRO_SPI_ICM20689)
  90 #define PID_PROCESS_DENOM_DEFAULT       4
  91 #else
  92 #define PID_PROCESS_DENOM_DEFAULT       2
  93 #endif
  94 #if defined(USE_D_CUT)
  95 #define D_CUT_GAIN_FACTOR 0.00005f
  96 #endif
  97
  98 #ifdef USE_RUNAWAY_TAKEOFF
  99 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 100     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
 101     .runaway_takeoff_prevention = true,
 102     .runaway_takeoff_deactivate_throttle = 20,  // throttle level % needed to accumulate deactivation time
 103     .runaway_takeoff_deactivate_delay = 500     // Accumulated time (in milliseconds) before deactivation in successful takeoff
 104 );
 105 #else
 106 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 107     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT
 108 );
 109 #endif
 110
 111 #ifdef USE_ACRO_TRAINER
 112 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f  // Max gyro rate for lookahead time scaling
 113 #define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 114 #endif // USE_ACRO_TRAINER
 115
 116 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 117
 118 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000  // 1 second delay before crash recovery detection is active after entering a self-level mode
 119
 120 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 121
 122 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 8);
 123
 124 void resetPidProfile(pidProfile_t *pidProfile)
 125 {
 126     RESET_CONFIG(pidProfile_t, pidProfile,
 127         .pid = {
 128             [PID_ROLL] =  { 46, 65, 40, 60 },
 129             [PID_PITCH] = { 50, 75, 44, 60 },
 130             [PID_YAW] =   { 45, 100, 0, 100 },
 131             [PID_LEVEL] = { 50, 50, 75, 0 },
 132             [PID_MAG] =   { 40, 0, 0, 0 },
 133         },
 134         .pidSumLimit = PIDSUM_LIMIT,
 135         .pidSumLimitYaw = PIDSUM_LIMIT_YAW,
 136         .yaw_lowpass_hz = 0,
 137         .dterm_notch_hz = 0,
 138         .dterm_notch_cutoff = 0,
 139         .itermWindupPointPercent = 100,
 140         .vbatPidCompensation = 0,
 141         .pidAtMinThrottle = PID_STABILISATION_ON,
 142         .levelAngleLimit = 55,
 143         .feedForwardTransition = 0,
 144         .yawRateAccelLimit = 0,
 145         .rateAccelLimit = 0,
 146         .itermThrottleThreshold = 250,
 147         .itermAcceleratorGain = 5000,
 148         .crash_time = 500,          // ms
 149         .crash_delay = 0,           // ms
 150         .crash_recovery_angle = 10, // degrees
 151         .crash_recovery_rate = 100, // degrees/second
 152         .crash_dthreshold = 50,     // degrees/second/second
 153         .crash_gthreshold = 400,    // degrees/second
 154         .crash_setpoint_threshold = 350, // degrees/second
 155         .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
 156         .horizon_tilt_effect = 75,
 157         .horizon_tilt_expert_mode = false,
 158         .crash_limit_yaw = 200,
 159         .itermLimit = 400,
 160         .throttle_boost = 5,
 161         .throttle_boost_cutoff = 15,
 162         .iterm_rotation = true,
 163         .smart_feedforward = false,
 164         .iterm_relax = ITERM_RELAX_RP,
 165         .iterm_relax_cutoff = 20,
 166         .iterm_relax_type = ITERM_RELAX_SETPOINT,
 167         .acro_trainer_angle_limit = 20,
 168         .acro_trainer_lookahead_ms = 50,
 169         .acro_trainer_debug_axis = FD_ROLL,
 170         .acro_trainer_gain = 75,
 171         .abs_control_gain = 0,
 172         .abs_control_limit = 90,
 173         .abs_control_error_limit = 20,
 174         .abs_control_cutoff = 11,
 175         .antiGravityMode = ANTI_GRAVITY_SMOOTH,
 176         .dterm_lowpass_hz = 100,    // dual PT1 filtering ON by default
 177         .dterm_lowpass2_hz = 200,   // second Dterm LPF ON by default
 178         .dterm_filter_type = FILTER_PT1,
 179         .dterm_filter2_type = FILTER_PT1,
 180         .dyn_lpf_dterm_min_hz = 150,
 181         .dyn_lpf_dterm_max_hz = 250,
 182         .launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
 183         .launchControlThrottlePercent = 20,
 184         .launchControlAngleLimit = 0,
 185         .launchControlGain = 40,
 186         .launchControlAllowTriggerReset = true,
 187         .use_integrated_yaw = false,
 188         .integrated_yaw_relax = 200,
 189         .thrustLinearization = 0,
 190         .dterm_cut_percent = 65,
 191         .dterm_cut_gain = 15,
 192         .dterm_cut_range_hz = 40,
 193         .dterm_cut_lowpass_hz = 7,
 194         .motor_output_limit = 100,
 195     );
 196 #ifdef USE_DYN_LPF
 197     pidProfile->dterm_lowpass_hz = 150;
 198     pidProfile->dterm_lowpass2_hz = 150;
 199     pidProfile->dterm_filter_type = FILTER_BIQUAD;
 200     pidProfile->dterm_filter2_type = FILTER_BIQUAD;
 201 #endif
 202 #ifndef USE_D_CUT
 203     pidProfile->pid[PID_ROLL].D = 30;
 204     pidProfile->pid[PID_PITCH].D = 32;
 205 #endif
 206 }
 207
 208 void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
 209 {
 210     for (int i = 0; i < MAX_PROFILE_COUNT; i++) {
 211         resetPidProfile(&pidProfiles[i]);
 212     }
 213 }
 214
 215 static void pidSetTargetLooptime(uint32_t pidLooptime)
 216 {
 217     targetPidLooptime = pidLooptime;
 218     dT = targetPidLooptime * 1e-6f;
 219     pidFrequency = 1.0f / dT;
 220 }
 221
 222 static FAST_RAM float itermAccelerator = 1.0f;
 223
 224 void pidSetItermAccelerator(float newItermAccelerator)
 225 {
 226     itermAccelerator = newItermAccelerator;
 227 }
 228
 229 bool pidOsdAntiGravityActive(void)
 230 {
 231     return (itermAccelerator > antiGravityOsdCutoff);
 232 }
 233
 234 void pidStabilisationState(pidStabilisationState_e pidControllerState)
 235 {
 236     pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
 237 }
 238
 239 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 240
 241 typedef union dtermLowpass_u {
 242     pt1Filter_t pt1Filter;
 243     biquadFilter_t biquadFilter;
 244 } dtermLowpass_t;
 245
 246 static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT];
 247
 248 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
 249 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
 250 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
 251 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
 252 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
 253 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
 254 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
 255 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
 256
 257 #if defined(USE_ITERM_RELAX)
 258 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 259 static FAST_RAM_ZERO_INIT uint8_t itermRelax;
 260 static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
 261 static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
 262 #endif
 263
 264 #if defined(USE_ABSOLUTE_CONTROL)
 265 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
 266 static FAST_RAM_ZERO_INIT float acGain;
 267 static FAST_RAM_ZERO_INIT float acLimit;
 268 static FAST_RAM_ZERO_INIT float acErrorLimit;
 269 static FAST_RAM_ZERO_INIT float acCutoff;
 270 static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
 271 #endif
 272
 273 #if defined(USE_D_CUT)
 274 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutRangeApplyFn;
 275 static FAST_RAM_ZERO_INIT biquadFilter_t dtermCutRange[XYZ_AXIS_COUNT];
 276 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutLowpassApplyFn;
 277 static FAST_RAM_ZERO_INIT pt1Filter_t dtermCutLowpass[XYZ_AXIS_COUNT];
 278 #endif
 279
 280 #ifdef USE_RC_SMOOTHING_FILTER
 281 static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
 282 static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
 283 static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
 284 static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
 285 static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
 286 #endif // USE_RC_SMOOTHING_FILTER
 287
 288 static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
 289
 290 void pidInitFilters(const pidProfile_t *pidProfile)
 291 {
 292     STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
 293
 294     if (targetPidLooptime == 0) {
 295         // no looptime set, so set all the filters to null
 296         dtermNotchApplyFn = nullFilterApply;
 297         dtermLowpassApplyFn = nullFilterApply;
 298         ptermYawLowpassApplyFn = nullFilterApply;
 299         return;
 300     }
 301
 302     const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed
 303
 304     uint16_t dTermNotchHz;
 305     if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
 306         dTermNotchHz = pidProfile->dterm_notch_hz;
 307     } else {
 308         if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
 309             dTermNotchHz = pidFrequencyNyquist;
 310         } else {
 311             dTermNotchHz = 0;
 312         }
 313     }
 314
 315     if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
 316         dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
 317         const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
 318         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 319             biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
 320         }
 321     } else {
 322         dtermNotchApplyFn = nullFilterApply;
 323     }
 324
 325     //1st Dterm Lowpass Filter
 326     uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
 327
 328 #ifdef USE_DYN_LPF
 329     dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz);
 330 #endif
 331
 332     if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
 333         switch (pidProfile->dterm_filter_type) {
 334         case FILTER_PT1:
 335             dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 336             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 337                 pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT));
 338             }
 339             break;
 340         case FILTER_BIQUAD:
 341 #ifdef USE_DYN_LPF
 342             dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
 343 #else
 344             dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
 345 #endif
 346             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 347                 biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
 348             }
 349             break;
 350         default:
 351             dtermLowpassApplyFn = nullFilterApply;
 352             break;
 353         }
 354     } else {
 355         dtermLowpassApplyFn = nullFilterApply;
 356     }
 357
 358     //2nd Dterm Lowpass Filter
 359     if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
 360         dtermLowpass2ApplyFn = nullFilterApply;
 361     } else {
 362         switch (pidProfile->dterm_filter2_type) {
 363         case FILTER_PT1:
 364             dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
 365             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 366                 pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
 367             }
 368             break;
 369         case FILTER_BIQUAD:
 370             dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 371             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 372                 biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
 373             }
 374             break;
 375         default:
 376             dtermLowpass2ApplyFn = nullFilterApply;
 377             break;
 378         }
 379     }
 380
 381     if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
 382         ptermYawLowpassApplyFn = nullFilterApply;
 383     } else {
 384         ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 385         pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
 386     }
 387
 388 #if defined(USE_THROTTLE_BOOST)
 389     pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
 390 #endif
 391 #if defined(USE_ITERM_RELAX)
 392     if (itermRelax) {
 393         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 394             pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
 395         }
 396     }
 397 #endif
 398 #if defined(USE_ABSOLUTE_CONTROL)
 399     if (itermRelax) {
 400         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 401             pt1FilterInit(&acLpf[i], pt1FilterGain(acCutoff, dT));
 402         }
 403     }
 404 #endif
 405 #if defined(USE_D_CUT)
 406     if (pidProfile->dterm_cut_percent == 0) {
 407         dtermCutRangeApplyFn = nullFilterApply;
 408         dtermCutLowpassApplyFn = nullFilterApply;
 409     } else {
 410         dtermCutRangeApplyFn = (filterApplyFnPtr)biquadFilterApply;
 411         dtermCutLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 412         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 413             biquadFilterInitLPF(&dtermCutRange[axis], pidProfile->dterm_cut_range_hz, targetPidLooptime);
 414             pt1FilterInit(&dtermCutLowpass[axis], pt1FilterGain(pidProfile->dterm_cut_lowpass_hz, dT));
 415         }
 416     }
 417 #endif
 418
 419     pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
 420 }
 421
 422 #ifdef USE_RC_SMOOTHING_FILTER
 423 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
 424 {
 425     rcSmoothingDebugAxis = debugAxis;
 426     rcSmoothingFilterType = filterType;
 427     if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
 428         setpointDerivativeLpfInitialized = true;
 429         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 430             switch (rcSmoothingFilterType) {
 431                 case RC_SMOOTHING_DERIVATIVE_PT1:
 432                     pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
 433                     break;
 434                 case RC_SMOOTHING_DERIVATIVE_BIQUAD:
 435                     biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
 436                     break;
 437             }
 438         }
 439     }
 440 }
 441
 442 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
 443 {
 444     if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
 445         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 446             switch (rcSmoothingFilterType) {
 447                 case RC_SMOOTHING_DERIVATIVE_PT1:
 448                     pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
 449                     break;
 450                 case RC_SMOOTHING_DERIVATIVE_BIQUAD:
 451                     biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
 452                     break;
 453             }
 454         }
 455     }
 456 }
 457 #endif // USE_RC_SMOOTHING_FILTER
 458
 459 typedef struct pidCoefficient_s {
 460     float Kp;
 461     float Ki;
 462     float Kd;
 463     float Kf;
 464 } pidCoefficient_t;
 465
 466 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
 467 static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
 468 static FAST_RAM_ZERO_INIT float feedForwardTransition;
 469 static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
 470 static FAST_RAM_ZERO_INIT float itermWindupPointInv;
 471 static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
 472 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
 473 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs;
 474 static FAST_RAM_ZERO_INIT int32_t crashRecoveryAngleDeciDegrees;
 475 static FAST_RAM_ZERO_INIT float crashRecoveryRate;
 476 static FAST_RAM_ZERO_INIT float crashDtermThreshold;
 477 static FAST_RAM_ZERO_INIT float crashGyroThreshold;
 478 static FAST_RAM_ZERO_INIT float crashSetpointThreshold;
 479 static FAST_RAM_ZERO_INIT float crashLimitYaw;
 480 static FAST_RAM_ZERO_INIT float itermLimit;
 481 #if defined(USE_THROTTLE_BOOST)
 482 FAST_RAM_ZERO_INIT float throttleBoost;
 483 pt1Filter_t throttleLpf;
 484 #endif
 485 static FAST_RAM_ZERO_INIT bool itermRotation;
 486
 487 #if defined(USE_SMART_FEEDFORWARD)
 488 static FAST_RAM_ZERO_INIT bool smartFeedforward;
 489 #endif
 490
 491 #ifdef USE_LAUNCH_CONTROL
 492 static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
 493 static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
 494 static FAST_RAM_ZERO_INIT float launchControlKi;
 495 #endif
 496
 497 #ifdef USE_INTEGRATED_YAW_CONTROL
 498 static FAST_RAM_ZERO_INIT bool useIntegratedYaw;
 499 static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax;
 500 #endif
 501
 502 void pidResetIterm(void)
 503 {
 504     for (int axis = 0; axis < 3; axis++) {
 505         pidData[axis].I = 0.0f;
 506 #if defined(USE_ABSOLUTE_CONTROL)
 507         axisError[axis] = 0.0f;
 508 #endif
 509     }
 510 }
 511
 512 #ifdef USE_ACRO_TRAINER
 513 static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit;
 514 static FAST_RAM_ZERO_INIT float acroTrainerLookaheadTime;
 515 static FAST_RAM_ZERO_INIT uint8_t acroTrainerDebugAxis;
 516 static FAST_RAM_ZERO_INIT bool acroTrainerActive;
 517 static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2];  // only need roll and pitch
 518 static FAST_RAM_ZERO_INIT float acroTrainerGain;
 519 #endif // USE_ACRO_TRAINER
 520
 521 #ifdef USE_THRUST_LINEARIZATION
 522 FAST_RAM_ZERO_INIT float thrustLinearization;
 523 FAST_RAM_ZERO_INIT float thrustLinearizationReciprocal;
 524 FAST_RAM_ZERO_INIT float thrustLinearizationB;
 525 #endif
 526
 527 void pidUpdateAntiGravityThrottleFilter(float throttle)
 528 {
 529     if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
 530         antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
 531     }
 532 }
 533
 534 #ifdef USE_DYN_LPF
 535 static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
 536 static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
 537 static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
 538 #endif
 539
 540 #ifdef USE_D_CUT
 541 static FAST_RAM_ZERO_INIT float dtermCutPercent;
 542 static FAST_RAM_ZERO_INIT float dtermCutPercentInv;
 543 static FAST_RAM_ZERO_INIT float dtermCutGain;
 544 static FAST_RAM_ZERO_INIT float dtermCutRangeHz;
 545 static FAST_RAM_ZERO_INIT float dtermCutLowpassHz;
 546 #endif
 547 static FAST_RAM_ZERO_INIT float dtermCutFactor;
 548
 549 void pidInitConfig(const pidProfile_t *pidProfile)
 550 {
 551     if (pidProfile->feedForwardTransition == 0) {
 552         feedForwardTransition = 0;
 553     } else {
 554         feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
 555     }
 556     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 557         pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
 558         pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
 559         pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
 560         pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
 561     }
 562
 563     levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
 564     horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
 565     horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
 566     horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
 567     horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
 568     horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
 569     maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
 570     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
 571     itermWindupPointInv = 1.0f;
 572     if (pidProfile->itermWindupPointPercent < 100) {
 573         const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
 574         itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
 575     }
 576     itermAcceleratorGain = pidProfile->itermAcceleratorGain;
 577     crashTimeLimitUs = pidProfile->crash_time * 1000;
 578     crashTimeDelayUs = pidProfile->crash_delay * 1000;
 579     crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
 580     crashRecoveryRate = pidProfile->crash_recovery_rate;
 581     crashGyroThreshold = pidProfile->crash_gthreshold;
 582     crashDtermThreshold = pidProfile->crash_dthreshold;
 583     crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
 584     crashLimitYaw = pidProfile->crash_limit_yaw;
 585     itermLimit = pidProfile->itermLimit;
 586 #if defined(USE_THROTTLE_BOOST)
 587     throttleBoost = pidProfile->throttle_boost * 0.1f;
 588 #endif
 589     itermRotation = pidProfile->iterm_rotation;
 590     antiGravityMode = pidProfile->antiGravityMode;
 591
 592     // Calculate the anti-gravity value that will trigger the OSD display.
 593     // For classic AG it's either 1.0 for off and > 1.0 for on.
 594     // For the new AG it's a continuous floating value so we want to trigger the OSD
 595     // display when it exceeds 25% of its possible range. This gives a useful indication
 596     // of AG activity without excessive display.
 597     antiGravityOsdCutoff = 1.0f;
 598     if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
 599         antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
 600     }
 601
 602 #if defined(USE_SMART_FEEDFORWARD)
 603     smartFeedforward = pidProfile->smart_feedforward;
 604 #endif
 605 #if defined(USE_ITERM_RELAX)
 606     itermRelax = pidProfile->iterm_relax;
 607     itermRelaxType = pidProfile->iterm_relax_type;
 608     itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
 609 #endif
 610
 611 #ifdef USE_ACRO_TRAINER
 612     acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
 613     acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
 614     acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
 615     acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
 616 #endif // USE_ACRO_TRAINER
 617
 618 #if defined(USE_ABSOLUTE_CONTROL)
 619     acGain = (float)pidProfile->abs_control_gain;
 620     acLimit = (float)pidProfile->abs_control_limit;
 621     acErrorLimit = (float)pidProfile->abs_control_error_limit;
 622     acCutoff = (float)pidProfile->abs_control_cutoff;
 623 #endif
 624
 625 #ifdef USE_DYN_LPF
 626     if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
 627         switch (pidProfile->dterm_filter_type) {
 628         case FILTER_PT1:
 629             dynLpfFilter = DYN_LPF_PT1;
 630             break;
 631         case FILTER_BIQUAD:
 632             dynLpfFilter = DYN_LPF_BIQUAD;
 633             break;
 634         default:
 635             dynLpfFilter = DYN_LPF_NONE;
 636             break;
 637         }
 638     } else {
 639         dynLpfFilter = DYN_LPF_NONE;
 640     }
 641     dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
 642     dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
 643 #endif
 644
 645 #ifdef USE_LAUNCH_CONTROL
 646     launchControlMode = pidProfile->launchControlMode;
 647     if (sensors(SENSOR_ACC)) {
 648         launchControlAngleLimit = pidProfile->launchControlAngleLimit;
 649     } else {
 650         launchControlAngleLimit = 0;
 651     }
 652     launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
 653 #endif
 654
 655 #ifdef USE_INTEGRATED_YAW_CONTROL
 656     useIntegratedYaw = pidProfile->use_integrated_yaw;
 657     integratedYawRelax = pidProfile->integrated_yaw_relax;
 658 #endif
 659
 660 #ifdef USE_THRUST_LINEARIZATION
 661     thrustLinearization = pidProfile->thrustLinearization / 100.0f;
 662     if (thrustLinearization != 0.0f) {
 663         thrustLinearizationReciprocal = 1.0f / thrustLinearization;
 664         thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization);
 665     }
 666 #endif
 667 #if defined(USE_D_CUT)
 668     dtermCutPercent = pidProfile->dterm_cut_percent / 100.0f;
 669     dtermCutPercentInv = 1.0f - dtermCutPercent;
 670     dtermCutRangeHz = pidProfile->dterm_cut_range_hz;
 671     dtermCutLowpassHz = pidProfile->dterm_cut_lowpass_hz;
 672     dtermCutGain = pidProfile->dterm_cut_gain * dtermCutPercent * D_CUT_GAIN_FACTOR / dtermCutLowpassHz;
 673     // lowpass included inversely in gain since stronger lowpass decreases peak effect
 674 #endif
 675     dtermCutFactor = 1.0f;
 676 }
 677
 678 void pidInit(const pidProfile_t *pidProfile)
 679 {
 680     pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
 681     pidInitFilters(pidProfile);
 682     pidInitConfig(pidProfile);
 683 #ifdef USE_RPM_FILTER
 684     rpmFilterInit(rpmFilterConfig());
 685 #endif
 686 }
 687
 688 #ifdef USE_ACRO_TRAINER
 689 void pidAcroTrainerInit(void)
 690 {
 691     acroTrainerAxisState[FD_ROLL] = 0;
 692     acroTrainerAxisState[FD_PITCH] = 0;
 693 }
 694 #endif // USE_ACRO_TRAINER
 695
 696 #ifdef USE_THRUST_LINEARIZATION
 697 float pidCompensateThrustLinearization(float throttle)
 698 {
 699     if (thrustLinearization != 0.0f) {
 700         throttle = throttle * (throttle * thrustLinearization + 1.0f - thrustLinearization);
 701     }
 702     return throttle;
 703 }
 704
 705 float pidApplyThrustLinearization(float motorOutput)
 706 {
 707     if (thrustLinearization != 0.0f) {
 708         if (motorOutput > 0.0f) {
 709             motorOutput = sqrtf(motorOutput * thrustLinearizationReciprocal +
 710                                 thrustLinearizationB * thrustLinearizationB) - thrustLinearizationB;
 711         }
 712     }
 713     return motorOutput;
 714 }
 715 #endif
 716
 717 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 718 {
 719     if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
 720         && dstPidProfileIndex != srcPidProfileIndex
 721     ) {
 722         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
 723     }
 724 }
 725
 726 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
 727 STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
 728 {
 729     // start with 1.0 at center stick, 0.0 at max stick deflection:
 730     float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
 731
 732     // 0 at level, 90 at vertical, 180 at inverted (degrees):
 733     const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f;
 734
 735     // horizonTiltExpertMode:  0 = leveling always active when sticks centered,
 736     //                         1 = leveling can be totally off when inverted
 737     if (horizonTiltExpertMode) {
 738         if (horizonTransition > 0 && horizonCutoffDegrees > 0) {
 739                     // if d_level > 0 and horizonTiltEffect < 175
 740             // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
 741             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
 742             //  for larger inclinations; 0.0 at horizonCutoffDegrees value:
 743             const float inclinationLevelRatio = constrainf((horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1);
 744             // apply configured horizon sensitivity:
 745                 // when stick is near center (horizonLevelStrength ~= 1.0)
 746                 //  H_sensitivity value has little effect,
 747                 // when stick is deflected (horizonLevelStrength near 0.0)
 748                 //  H_sensitivity value has more effect:
 749             horizonLevelStrength = (horizonLevelStrength - 1) * 100 / horizonTransition + 1;
 750             // apply inclination ratio, which may lower leveling
 751             //  to zero regardless of stick position:
 752             horizonLevelStrength *= inclinationLevelRatio;
 753         } else  { // d_level=0 or horizon_tilt_effect>=175 means no leveling
 754           horizonLevelStrength = 0;
 755         }
 756     } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
 757         float sensitFact;
 758         if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0
 759             // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
 760             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
 761             //  for larger inclinations, goes to 1.0 at inclination==level:
 762             const float inclinationLevelRatio = (180-currentInclination)/180 * (1.0f-horizonFactorRatio) + horizonFactorRatio;
 763             // apply ratio to configured horizon sensitivity:
 764             sensitFact = horizonTransition * inclinationLevelRatio;
 765         } else { // horizonTiltEffect=0 for "old" functionality
 766             sensitFact = horizonTransition;
 767         }
 768
 769         if (sensitFact <= 0) {           // zero means no leveling
 770             horizonLevelStrength = 0;
 771         } else {
 772             // when stick is near center (horizonLevelStrength ~= 1.0)
 773             //  sensitFact value has little effect,
 774             // when stick is deflected (horizonLevelStrength near 0.0)
 775             //  sensitFact value has more effect:
 776             horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
 777         }
 778     }
 779     return constrainf(horizonLevelStrength, 0, 1);
 780 }
 781
 782 STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
 783     // calculate error angle and limit the angle to the max inclination
 784     // rcDeflection is in range [-1.0, 1.0]
 785     float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
 786 #ifdef USE_GPS_RESCUE
 787     angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
 788 #endif
 789     angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
 790     const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
 791     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
 792         // ANGLE mode - control is angle based
 793         currentPidSetpoint = errorAngle * levelGain;
 794     } else {
 795         // HORIZON mode - mix of ANGLE and ACRO modes
 796         // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
 797         const float horizonLevelStrength = calcHorizonLevelStrength();
 798         currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
 799     }
 800     return currentPidSetpoint;
 801 }
 802
 803 static float accelerationLimit(int axis, float currentPidSetpoint)
 804 {
 805     static float previousSetpoint[XYZ_AXIS_COUNT];
 806     const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
 807
 808     if (fabsf(currentVelocity) > maxVelocity[axis]) {
 809         currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
 810     }
 811
 812     previousSetpoint[axis] = currentPidSetpoint;
 813     return currentPidSetpoint;
 814 }
 815
 816 static timeUs_t crashDetectedAtUs;
 817
 818 static void handleCrashRecovery(
 819     const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
 820     const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
 821 {
 822     if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
 823         if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
 824             BEEP_ON;
 825         }
 826         if (axis == FD_YAW) {
 827             *errorRate = constrainf(*errorRate, -crashLimitYaw, crashLimitYaw);
 828         } else {
 829             // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
 830             if (sensors(SENSOR_ACC)) {
 831                 // errorAngle is deviation from horizontal
 832                 const float errorAngle =  -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
 833                 *currentPidSetpoint = errorAngle * levelGain;
 834                 *errorRate = *currentPidSetpoint - gyroRate;
 835             }
 836         }
 837         // reset iterm, since accumulated error before crash is now meaningless
 838         // and iterm windup during crash recovery can be extreme, especially on yaw axis
 839         pidData[axis].I = 0.0f;
 840         if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
 841             || (getMotorMixRange() < 1.0f
 842                    && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
 843                    && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
 844                    && fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) {
 845             if (sensors(SENSOR_ACC)) {
 846                 // check aircraft nearly level
 847                 if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
 848                    && ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees) {
 849                     inCrashRecoveryMode = false;
 850                     BEEP_OFF;
 851                 }
 852             } else {
 853                 inCrashRecoveryMode = false;
 854                 BEEP_OFF;
 855             }
 856         }
 857     }
 858 }
 859
 860 static void detectAndSetCrashRecovery(
 861     const pidCrashRecovery_e crash_recovery, const int axis,
 862     const timeUs_t currentTimeUs, const float delta, const float errorRate)
 863 {
 864     // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
 865     // no point in trying to recover if the crash is so severe that the gyro overflows
 866     if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
 867         if (ARMING_FLAG(ARMED)) {
 868             if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode
 869                 && fabsf(delta) > crashDtermThreshold
 870                 && fabsf(errorRate) > crashGyroThreshold
 871                 && fabsf(getSetpointRate(axis)) < crashSetpointThreshold) {
 872                 inCrashRecoveryMode = true;
 873                 crashDetectedAtUs = currentTimeUs;
 874             }
 875             if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (fabsf(errorRate) < crashGyroThreshold
 876                 || fabsf(getSetpointRate(axis)) > crashSetpointThreshold)) {
 877                 inCrashRecoveryMode = false;
 878                 BEEP_OFF;
 879             }
 880         } else if (inCrashRecoveryMode) {
 881             inCrashRecoveryMode = false;
 882             BEEP_OFF;
 883         }
 884     }
 885 }
 886
 887 static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
 888 {
 889     // rotate v around rotation vector rotation
 890     // rotation in radians, all elements must be small
 891     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 892         int i_1 = (i + 1) % 3;
 893         int i_2 = (i + 2) % 3;
 894         float newV = v[i_1] + v[i_2] * rotation[i];
 895         v[i_2] -= v[i_1] * rotation[i];
 896         v[i_1] = newV;
 897     }
 898 }
 899
 900 STATIC_UNIT_TESTED void rotateItermAndAxisError()
 901 {
 902     if (itermRotation
 903 #if defined(USE_ABSOLUTE_CONTROL)
 904         || acGain > 0
 905 #endif
 906         ) {
 907         const float gyroToAngle = dT * RAD;
 908         float rotationRads[XYZ_AXIS_COUNT];
 909         for (int i = FD_ROLL; i <= FD_YAW; i++) {
 910             rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
 911         }
 912 #if defined(USE_ABSOLUTE_CONTROL)
 913         if (acGain > 0) {
 914             rotateVector(axisError, rotationRads);
 915         }
 916 #endif
 917         if (itermRotation) {
 918             float v[XYZ_AXIS_COUNT];
 919             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 920                 v[i] = pidData[i].I;
 921             }
 922             rotateVector(v, rotationRads );
 923             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 924                 pidData[i].I = v[i];
 925             }
 926         }
 927     }
 928 }
 929
 930 #ifdef USE_ACRO_TRAINER
 931
 932 int acroTrainerSign(float x)
 933 {
 934     return x > 0 ? 1 : -1;
 935 }
 936
 937 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
 938 // There are three states:
 939 // 1. Current angle has exceeded limit
 940 //    Apply correction to return to limit (similar to pidLevel)
 941 // 2. Future overflow has been projected based on current angle and gyro rate
 942 //    Manage the setPoint to control the gyro rate as the actual angle  approaches the limit (try to prevent overshoot)
 943 // 3. If no potential overflow is detected, then return the original setPoint
 944
 945 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
 946 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
 947 // expecting ultimate flight performance at very high loop rates when in this mode.
 948 static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint)
 949 {
 950     float ret = setPoint;
 951
 952     if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
 953         bool resetIterm = false;
 954         float projectedAngle = 0;
 955         const int setpointSign = acroTrainerSign(setPoint);
 956         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
 957         const int angleSign = acroTrainerSign(currentAngle);
 958
 959         if ((acroTrainerAxisState[axis] != 0) && (acroTrainerAxisState[axis] != setpointSign)) {  // stick has reversed - stop limiting
 960             acroTrainerAxisState[axis] = 0;
 961         }
 962
 963         // Limit and correct the angle when it exceeds the limit
 964         if ((fabsf(currentAngle) > acroTrainerAngleLimit) && (acroTrainerAxisState[axis] == 0)) {
 965             if (angleSign == setpointSign) {
 966                 acroTrainerAxisState[axis] = angleSign;
 967                 resetIterm = true;
 968             }
 969         }
 970
 971         if (acroTrainerAxisState[axis] != 0) {
 972             ret = constrainf(((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
 973         } else {
 974
 975         // Not currently over the limit so project the angle based on current angle and
 976         // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
 977         // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
 978             // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
 979             float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime;
 980             projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
 981             const int projectedAngleSign = acroTrainerSign(projectedAngle);
 982             if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
 983                 ret = ((acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * acroTrainerGain;
 984                 resetIterm = true;
 985             }
 986         }
 987
 988         if (resetIterm) {
 989             pidData[axis].I = 0;
 990         }
 991
 992         if (axis == acroTrainerDebugAxis) {
 993             DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
 994             DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]);
 995             DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
 996             DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
 997         }
 998     }
 999
1000     return ret;
1001 }
1002 #endif // USE_ACRO_TRAINER
1003
1004 #ifdef USE_RC_SMOOTHING_FILTER
1005 float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
1006 {
1007     float ret = pidSetpointDelta;
1008     if (axis == rcSmoothingDebugAxis) {
1009         DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
1010     }
1011     if (setpointDerivativeLpfInitialized) {
1012         switch (rcSmoothingFilterType) {
1013             case RC_SMOOTHING_DERIVATIVE_PT1:
1014                 ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
1015                 break;
1016             case RC_SMOOTHING_DERIVATIVE_BIQUAD:
1017                 ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
1018                 break;
1019         }
1020         if (axis == rcSmoothingDebugAxis) {
1021             DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
1022         }
1023     }
1024     return ret;
1025 }
1026 #endif // USE_RC_SMOOTHING_FILTER
1027
1028 #ifdef USE_SMART_FEEDFORWARD
1029 void FAST_CODE applySmartFeedforward(int axis)
1030 {
1031     if (smartFeedforward) {
1032         if (pidData[axis].P * pidData[axis].F > 0) {
1033             if (fabsf(pidData[axis].F) > fabsf(pidData[axis].P)) {
1034                 pidData[axis].P = 0;
1035             } else {
1036                 pidData[axis].F = 0;
1037             }
1038         }
1039     }
1040 }
1041 #endif // USE_SMART_FEEDFORWARD
1042
1043 #if defined(USE_ITERM_RELAX)
1044 #if defined(USE_ABSOLUTE_CONTROL)
1045 STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
1046 {
1047     if (acGain > 0) {
1048         const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint);
1049         const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
1050         float acErrorRate = 0;
1051         const float gmaxac = setpointLpf + 2 * setpointHpf;
1052         const float gminac = setpointLpf - 2 * setpointHpf;
1053         if (gyroRate >= gminac && gyroRate <= gmaxac) {
1054             const float acErrorRate1 = gmaxac - gyroRate;
1055             const float acErrorRate2 = gminac - gyroRate;
1056             if (acErrorRate1 * axisError[axis] < 0) {
1057                 acErrorRate = acErrorRate1;
1058             } else {
1059                 acErrorRate = acErrorRate2;
1060             }
1061             if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
1062                 acErrorRate = -axisError[axis] * pidFrequency;
1063             }
1064         } else {
1065             acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
1066         }
1067
1068         if (isAirmodeActivated()) {
1069             axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT,
1070                 -acErrorLimit, acErrorLimit);
1071             const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
1072             *currentPidSetpoint += acCorrection;
1073             *itermErrorRate += acCorrection;
1074             if (axis == FD_ROLL) {
1075                 DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
1076             }
1077         }
1078     }
1079 }
1080 #endif
1081
1082 STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
1083     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
1084 {
1085     const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
1086     const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
1087
1088     if (itermRelax &&
1089        (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY ||
1090        itermRelax == ITERM_RELAX_RPY_INC)) {
1091         const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
1092
1093         const bool isDecreasingI =
1094             ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
1095         if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
1096             // Do Nothing, use the precalculed itermErrorRate
1097         } else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) {
1098             *itermErrorRate *= itermRelaxFactor;
1099         } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
1100             *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
1101         } else {
1102             *itermErrorRate = 0.0f;
1103         }
1104
1105         if (axis == FD_ROLL) {
1106             DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
1107             DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
1108             DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
1109         }
1110 #if defined(USE_ABSOLUTE_CONTROL)
1111         applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate);
1112 #endif
1113     }
1114 }
1115 #endif
1116
1117 #ifdef USE_LAUNCH_CONTROL
1118 #define LAUNCH_CONTROL_MAX_RATE 100.0f
1119 #define LAUNCH_CONTROL_MIN_RATE 5.0f
1120 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f  // The remaining angle degrees where rate dampening starts
1121
1122 static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
1123 {
1124     float ret = 0.0f;
1125
1126     // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
1127     // reached at 50% stick deflection. This keeps the launch control positioning consistent
1128     // regardless of the user's rates.
1129     if ((axis == FD_PITCH) || (launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
1130         const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
1131         ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
1132     }
1133
1134     // If ACC is enabled and a limit angle is set, then try to limit forward tilt
1135     // to that angle and slow down the rate as the limit is approached to reduce overshoot
1136     if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0)) {
1137         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
1138         if (currentAngle >= launchControlAngleLimit) {
1139             ret = 0.0f;
1140         } else {
1141             //for the last 10 degrees scale the rate from the current input to 5 dps
1142             const float angleDelta = launchControlAngleLimit - currentAngle;
1143             if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
1144                 ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
1145             }
1146         }
1147     }
1148     return ret;
1149 }
1150 #endif
1151
1152 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
1153 // Based on 2DOF reference design (matlab)
1154 void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
1155 {
1156     static float previousGyroRateDterm[XYZ_AXIS_COUNT];
1157     static timeUs_t levelModeStartTimeUs = 0;
1158     static bool gpsRescuePreviousState = false;
1159
1160     const float tpaFactor = getThrottlePIDAttenuation();
1161
1162 #ifdef USE_TPA_MODE
1163     const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f;
1164 #else
1165     const float tpaFactorKp = tpaFactor;
1166 #endif
1167
1168 #ifdef USE_YAW_SPIN_RECOVERY
1169     const bool yawSpinActive = gyroYawSpinDetected();
1170 #endif
1171
1172     const bool launchControlActive = isLaunchControlActive();
1173
1174     const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
1175     const bool levelModeActive = FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive;
1176
1177     // Keep track of when we entered a self-level mode so that we can
1178     // add a guard time before crash recovery can activate.
1179     // Also reset the guard time whenever GPS Rescue is activated.
1180     if (levelModeActive) {
1181         if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) {
1182             levelModeStartTimeUs = currentTimeUs;
1183         }
1184     } else {
1185         levelModeStartTimeUs = 0;
1186     }
1187     gpsRescuePreviousState = gpsRescueIsActive;
1188
1189     // Dynamic i component,
1190     if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
1191         itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
1192         DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
1193     }
1194     DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
1195
1196     // gradually scale back integration when above windup point
1197     float dynCi = dT * itermAccelerator;
1198     if (itermWindupPointInv > 1.0f) {
1199         dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
1200     }
1201
1202     // Precalculate gyro deta for D-term here, this allows loop unrolling
1203     float gyroRateDterm[XYZ_AXIS_COUNT];
1204     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
1205         gyroRateDterm[axis] = gyro.gyroADCf[axis];
1206 #ifdef USE_RPM_FILTER
1207         gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
1208 #endif
1209         gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
1210         gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
1211         gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
1212     }
1213
1214     rotateItermAndAxisError();
1215 #ifdef USE_RPM_FILTER
1216     rpmFilterUpdate();
1217 #endif
1218
1219
1220     // ----------PID controller----------
1221     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
1222
1223         float currentPidSetpoint = getSetpointRate(axis);
1224         if (maxVelocity[axis]) {
1225             currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
1226         }
1227         // Yaw control is GYRO based, direct sticks control is applied to rate PID
1228         if (levelModeActive && (axis != FD_YAW)) {
1229             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
1230         }
1231
1232 #ifdef USE_ACRO_TRAINER
1233         if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode && !launchControlActive) {
1234             currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
1235         }
1236 #endif // USE_ACRO_TRAINER
1237
1238 #ifdef USE_LAUNCH_CONTROL
1239         if (launchControlActive) {
1240             currentPidSetpoint = applyLaunchControl(axis, angleTrim);
1241         }
1242 #endif
1243
1244         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
1245         // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
1246 #ifdef USE_YAW_SPIN_RECOVERY
1247         if ((axis == FD_YAW) && yawSpinActive) {
1248             currentPidSetpoint = 0.0f;
1249         }
1250 #endif // USE_YAW_SPIN_RECOVERY
1251
1252         // -----calculate error rate
1253         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
1254         float errorRate = currentPidSetpoint - gyroRate; // r - y
1255         handleCrashRecovery(
1256             pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
1257             &currentPidSetpoint, &errorRate);
1258
1259         const float iterm = pidData[axis].I;
1260         float itermErrorRate = errorRate;
1261
1262 #if defined(USE_ITERM_RELAX)
1263         if (!launchControlActive) {
1264             applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
1265         }
1266 #endif
1267
1268         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1269         // 2-DOF PID controller with optional filter on derivative term.
1270         // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
1271
1272         // -----calculate P component
1273         pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
1274         if (axis == FD_YAW) {
1275             pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
1276         }
1277
1278         // -----calculate I component
1279 #ifdef USE_LAUNCH_CONTROL
1280         // if launch control is active override the iterm gains
1281         const float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
1282 #else
1283         const float Ki = pidCoefficient[axis].Ki;
1284 #endif
1285         pidData[axis].I = constrainf(iterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
1286
1287         // -----calculate D component
1288         // disable D if launch control is active
1289         if ((pidCoefficient[axis].Kd > 0) && !launchControlActive){
1290
1291             // Divide rate change by dT to get differential (ie dr/dt).
1292             // dT is fixed and calculated from the target PID loop time
1293             // This is done to avoid DTerm spikes that occur with dynamically
1294             // calculated deltaT whenever another task causes the PID
1295             // loop execution to be delayed.
1296             const float delta =
1297                 - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
1298
1299             if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
1300                 detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
1301             }
1302 #if defined(USE_D_CUT)
1303             if (dtermCutPercent){
1304                 dtermCutFactor = dtermCutRangeApplyFn((filter_t *) &dtermCutRange[axis], delta);
1305                 dtermCutFactor = fabsf(dtermCutFactor) * dtermCutGain;
1306                 dtermCutFactor = dtermCutLowpassApplyFn((filter_t *) &dtermCutLowpass[axis], dtermCutFactor);
1307                 dtermCutFactor = MIN(dtermCutFactor, 1.0f);
1308                 dtermCutFactor = dtermCutPercentInv + (dtermCutFactor * dtermCutPercent);
1309             }
1310             if (axis == FD_ROLL) {
1311                 DEBUG_SET(DEBUG_D_CUT, 2, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
1312             } else if (axis == FD_PITCH) {
1313                 DEBUG_SET(DEBUG_D_CUT, 3, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
1314             }
1315 #endif
1316
1317             pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dtermCutFactor;
1318
1319         } else {
1320             pidData[axis].D = 0;
1321         }
1322         previousGyroRateDterm[axis] = gyroRateDterm[axis];
1323
1324         // -----calculate feedforward component
1325
1326         // Only enable feedforward for rate mode and if launch control is inactive
1327         const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
1328
1329         if (feedforwardGain > 0) {
1330
1331             // no transition if feedForwardTransition == 0
1332             float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
1333
1334             float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
1335
1336 #ifdef USE_RC_SMOOTHING_FILTER
1337             pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
1338 #endif // USE_RC_SMOOTHING_FILTER
1339
1340             pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
1341
1342 #if defined(USE_SMART_FEEDFORWARD)
1343             applySmartFeedforward(axis);
1344 #endif
1345         } else {
1346             pidData[axis].F = 0;
1347         }
1348         previousPidSetpoint[axis] = currentPidSetpoint;
1349
1350 #ifdef USE_YAW_SPIN_RECOVERY
1351         if (yawSpinActive) {
1352             pidData[axis].I = 0;  // in yaw spin always disable I
1353             if (axis <= FD_PITCH)  {
1354                 // zero PIDs on pitch and roll leaving yaw P to correct spin
1355                 pidData[axis].P = 0;
1356                 pidData[axis].D = 0;
1357                 pidData[axis].F = 0;
1358             }
1359         }
1360 #endif // USE_YAW_SPIN_RECOVERY
1361
1362 #ifdef USE_LAUNCH_CONTROL
1363         // Disable P/I appropriately based on the launch control mode
1364         if (launchControlActive) {
1365             // if not using FULL mode then disable I accumulation on yaw as
1366             // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1367             const int launchControlYawItermLimit = (launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
1368             pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit);
1369
1370             // for pitch-only mode we disable everything except pitch P/I
1371             if (launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
1372                 pidData[FD_ROLL].P = 0;
1373                 pidData[FD_ROLL].I = 0;
1374                 pidData[FD_YAW].P = 0;
1375                 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1376                 pidData[FD_PITCH].I = MAX(0.0f, pidData[FD_PITCH].I);
1377             }
1378         }
1379 #endif
1380         // calculating the PID sum
1381         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
1382 #ifdef USE_INTEGRATED_YAW_CONTROL
1383         if (axis == FD_YAW && useIntegratedYaw) {
1384             pidData[axis].Sum += pidSum * dT * 100.0f;
1385             pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f;
1386         } else
1387 #endif
1388         {
1389             pidData[axis].Sum = pidSum;
1390         }
1391     }
1392
1393     // Disable PID control if at zero throttle or if gyro overflow detected
1394     // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1395     if (!pidStabilisationEnabled || gyroOverflowDetected()) {
1396         for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
1397             pidData[axis].P = 0;
1398             pidData[axis].I = 0;
1399             pidData[axis].D = 0;
1400             pidData[axis].F = 0;
1401
1402             pidData[axis].Sum = 0;
1403         }
1404     } else if (zeroThrottleItermReset) {
1405         pidResetIterm();
1406     }
1407 }
1408
1409 bool crashRecoveryModeActive(void)
1410 {
1411     return inCrashRecoveryMode;
1412 }
1413
1414 #ifdef USE_ACRO_TRAINER
1415 void pidSetAcroTrainerState(bool newState)
1416 {
1417     if (acroTrainerActive != newState) {
1418         if (newState) {
1419             pidAcroTrainerInit();
1420         }
1421         acroTrainerActive = newState;
1422     }
1423 }
1424 #endif // USE_ACRO_TRAINER
1425
1426 void pidSetAntiGravityState(bool newState)
1427 {
1428     if (newState != antiGravityEnabled) {
1429         // reset the accelerator on state changes
1430         itermAccelerator = 1.0f;
1431     }
1432     antiGravityEnabled = newState;
1433 }
1434
1435 bool pidAntiGravityEnabled(void)
1436 {
1437     return antiGravityEnabled;
1438 }
1439
1440 #ifdef USE_DYN_LPF
1441 void dynLpfDTermUpdate(float throttle)
1442 {
1443     if (dynLpfFilter != DYN_LPF_NONE) {
1444         const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
1445
1446          if (dynLpfFilter == DYN_LPF_PT1) {
1447             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1448                 pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
1449             }
1450         } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
1451             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1452                 biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
1453             }
1454         }
1455     }
1456 }
1457 #endif
1458
1459 void pidSetItermReset(bool enabled)
1460 {
1461     zeroThrottleItermReset = enabled;
1462 }
1463
1464 float pidGetPreviousSetpoint(int axis)
1465 {
1466     return previousPidSetpoint[axis];
1467 }