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 <stdlib.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/filter.h"
  34
  35 #include "config/config.h"
  36 #include "config/config_reset.h"
  37 #include "config/simplified_tuning.h"
  38
  39 #include "drivers/pwm_output.h"
  40 #include "drivers/sound_beeper.h"
  41 #include "drivers/time.h"
  42
  43 #include "fc/controlrate_profile.h"
  44 #include "fc/core.h"
  45 #include "fc/rc.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/runtime_config.h"
  48
  49 #include "flight/gps_rescue.h"
  50 #include "flight/imu.h"
  51 #include "flight/mixer.h"
  52 #include "flight/rpm_filter.h"
  53 #include "flight/feedforward.h"
  54
  55 #include "io/gps.h"
  56
  57 #include "pg/pg.h"
  58 #include "pg/pg_ids.h"
  59
  60 #include "sensors/acceleration.h"
  61 #include "sensors/battery.h"
  62 #include "sensors/gyro.h"
  63
  64 #include "pid.h"
  65
  66 typedef enum {
  67     LEVEL_MODE_OFF = 0,
  68     LEVEL_MODE_R,
  69     LEVEL_MODE_RP,
  70 } levelMode_e;
  71
  72 const char pidNames[] =
  73     "ROLL;"
  74     "PITCH;"
  75     "YAW;"
  76     "LEVEL;"
  77     "MAG;";
  78
  79 FAST_DATA_ZERO_INIT uint32_t targetPidLooptime;
  80 FAST_DATA_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT];
  81 FAST_DATA_ZERO_INIT pidRuntime_t pidRuntime;
  82
  83 #if defined(USE_ABSOLUTE_CONTROL)
  84 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
  85 #endif
  86
  87 #if defined(USE_THROTTLE_BOOST)
  88 FAST_DATA_ZERO_INIT float throttleBoost;
  89 pt1Filter_t throttleLpf;
  90 #endif
  91
  92 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
  93
  94 #if defined(STM32F411xE)
  95 #define PID_PROCESS_DENOM_DEFAULT       2
  96 #else
  97 #define PID_PROCESS_DENOM_DEFAULT       1
  98 #endif
  99
 100 #ifdef USE_RUNAWAY_TAKEOFF
 101 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 102     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
 103     .runaway_takeoff_prevention = true,
 104     .runaway_takeoff_deactivate_throttle = 20,  // throttle level % needed to accumulate deactivation time
 105     .runaway_takeoff_deactivate_delay = 500,    // Accumulated time (in milliseconds) before deactivation in successful takeoff
 106 );
 107 #else
 108 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 109     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
 110 );
 111 #endif
 112
 113 #ifdef USE_ACRO_TRAINER
 114 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f  // Max gyro rate for lookahead time scaling
 115 #define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 116 #endif // USE_ACRO_TRAINER
 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, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
 123
 124 void resetPidProfile(pidProfile_t *pidProfile)
 125 {
 126     RESET_CONFIG(pidProfile_t, pidProfile,
 127         .pid = {
 128             [PID_ROLL] =  PID_ROLL_DEFAULT,
 129             [PID_PITCH] = PID_PITCH_DEFAULT,
 130             [PID_YAW] =   PID_YAW_DEFAULT,
 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 = 100,
 137         .dterm_notch_hz = 0,
 138         .dterm_notch_cutoff = 0,
 139         .itermWindupPointPercent = 85,
 140         .pidAtMinThrottle = PID_STABILISATION_ON,
 141         .levelAngleLimit = 55,
 142         .feedforward_transition = 0,
 143         .yawRateAccelLimit = 0,
 144         .rateAccelLimit = 0,
 145         .anti_gravity_gain = 80,
 146         .crash_time = 500,          // ms
 147         .crash_delay = 0,           // ms
 148         .crash_recovery_angle = 10, // degrees
 149         .crash_recovery_rate = 100, // degrees/second
 150         .crash_dthreshold = 50,     // degrees/second/second
 151         .crash_gthreshold = 400,    // degrees/second
 152         .crash_setpoint_threshold = 350, // degrees/second
 153         .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
 154         .horizon_tilt_effect = 75,
 155         .horizon_tilt_expert_mode = false,
 156         .crash_limit_yaw = 200,
 157         .itermLimit = 400,
 158         .throttle_boost = 5,
 159         .throttle_boost_cutoff = 15,
 160         .iterm_rotation = false,
 161         .iterm_relax = ITERM_RELAX_RP,
 162         .iterm_relax_cutoff = ITERM_RELAX_CUTOFF_DEFAULT,
 163         .iterm_relax_type = ITERM_RELAX_SETPOINT,
 164         .acro_trainer_angle_limit = 20,
 165         .acro_trainer_lookahead_ms = 50,
 166         .acro_trainer_debug_axis = FD_ROLL,
 167         .acro_trainer_gain = 75,
 168         .abs_control_gain = 0,
 169         .abs_control_limit = 90,
 170         .abs_control_error_limit = 20,
 171         .abs_control_cutoff = 11,
 172         .dterm_lpf1_static_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT,
 173             // NOTE: dynamic lpf is enabled by default so this setting is actually
 174             // overridden and the static lowpass 1 is disabled. We can't set this
 175             // value to 0 otherwise Configurator versions 10.4 and earlier will also
 176             // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
 177         .dterm_lpf2_static_hz = DTERM_LPF2_HZ_DEFAULT,   // second Dterm LPF ON by default
 178         .dterm_lpf1_type = FILTER_PT1,
 179         .dterm_lpf2_type = FILTER_PT1,
 180         .dterm_lpf1_dyn_min_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT,
 181         .dterm_lpf1_dyn_max_hz = DTERM_LPF1_DYN_MAX_HZ_DEFAULT,
 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         .d_min = D_MIN_DEFAULT,
 191         .d_min_gain = 37,
 192         .d_min_advance = 20,
 193         .motor_output_limit = 100,
 194         .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
 195         .transient_throttle_limit = 0,
 196         .profileName = { 0 },
 197         .dyn_idle_min_rpm = 0,
 198         .dyn_idle_p_gain = 50,
 199         .dyn_idle_i_gain = 50,
 200         .dyn_idle_d_gain = 50,
 201         .dyn_idle_max_increase = 150,
 202         .feedforward_averaging = FEEDFORWARD_AVERAGING_OFF,
 203         .feedforward_max_rate_limit = 90,
 204         .feedforward_smooth_factor = 25,
 205         .feedforward_jitter_factor = 7,
 206         .feedforward_boost = 15,
 207         .dterm_lpf1_dyn_expo = 5,
 208         .level_race_mode = false,
 209         .vbat_sag_compensation = 0,
 210         .simplified_pids_mode = PID_SIMPLIFIED_TUNING_RPY,
 211         .simplified_master_multiplier = SIMPLIFIED_TUNING_DEFAULT,
 212         .simplified_roll_pitch_ratio = SIMPLIFIED_TUNING_DEFAULT,
 213         .simplified_i_gain = SIMPLIFIED_TUNING_DEFAULT,
 214         .simplified_d_gain = SIMPLIFIED_TUNING_D_DEFAULT,
 215         .simplified_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
 216         .simplified_dmin_ratio = SIMPLIFIED_TUNING_D_DEFAULT,
 217         .simplified_feedforward_gain = SIMPLIFIED_TUNING_DEFAULT,
 218         .simplified_pitch_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
 219         .simplified_dterm_filter = true,
 220         .simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
 221         .anti_gravity_cutoff_hz = 5,
 222         .anti_gravity_p_gain = 100,
 223         .tpa_mode = TPA_MODE_D,
 224         .tpa_rate = 65,
 225         .tpa_breakpoint = 1350,
 226     );
 227
 228 #ifndef USE_D_MIN
 229     pidProfile->pid[PID_ROLL].D = 30;
 230     pidProfile->pid[PID_PITCH].D = 32;
 231 #endif
 232 }
 233
 234 void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
 235 {
 236     for (int i = 0; i < PID_PROFILE_COUNT; i++) {
 237         resetPidProfile(&pidProfiles[i]);
 238     }
 239 }
 240
 241 // Scale factors to make best use of range with D_LPF debugging, aiming for max +/-16K as debug values are 16 bit
 242 #define D_LPF_RAW_SCALE 25
 243 #define D_LPF_FILT_SCALE 22
 244
 245
 246 void pidSetItermAccelerator(float newItermAccelerator)
 247 {
 248     pidRuntime.itermAccelerator = newItermAccelerator;
 249 }
 250
 251 bool pidOsdAntiGravityActive(void)
 252 {
 253     return (pidRuntime.itermAccelerator > pidRuntime.antiGravityOsdCutoff);
 254 }
 255
 256 void pidStabilisationState(pidStabilisationState_e pidControllerState)
 257 {
 258     pidRuntime.pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
 259 }
 260
 261 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 262
 263 #ifdef USE_FEEDFORWARD
 264 float pidGetFeedforwardTransitionFactor(void)
 265 {
 266     return pidRuntime.feedforwardTransitionFactor;
 267 }
 268
 269 float pidGetFeedforwardSmoothFactor(void)
 270 {
 271     return pidRuntime.feedforwardSmoothFactor;
 272 }
 273
 274 float pidGetFeedforwardJitterFactor(void)
 275 {
 276     return pidRuntime.feedforwardJitterFactor;
 277 }
 278
 279 float pidGetFeedforwardBoostFactor(void)
 280 {
 281     return pidRuntime.feedforwardBoostFactor;
 282 }
 283 #endif
 284
 285 void pidResetIterm(void)
 286 {
 287     for (int axis = 0; axis < 3; axis++) {
 288         pidData[axis].I = 0.0f;
 289 #if defined(USE_ABSOLUTE_CONTROL)
 290         axisError[axis] = 0.0f;
 291 #endif
 292     }
 293 }
 294
 295 void pidUpdateTpaFactor(float throttle)
 296 {
 297     pidProfile_t *currentPidProfile;
 298
 299     currentPidProfile = pidProfilesMutable(systemConfig()->pidProfileIndex);
 300     const float tpaBreakpoint = (currentPidProfile->tpa_breakpoint - 1000) / 1000.0f;
 301     float tpaRate = currentPidProfile->tpa_rate / 100.0f;
 302
 303     if (throttle > tpaBreakpoint) {
 304         if (throttle < 1.0f) {
 305             tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint);
 306         }
 307     } else {
 308         tpaRate = 0.0f;
 309     }
 310     pidRuntime.tpaFactor = 1.0f - tpaRate;
 311 }
 312
 313 void pidUpdateAntiGravityThrottleFilter(float throttle)
 314 {
 315     static float previousThrottle = 0.0f;
 316     const float throttleInv = 1.0f - throttle;
 317     float throttleDerivative = fabsf(throttle - previousThrottle) * pidRuntime.pidFrequency;
 318     DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
 319     throttleDerivative *= throttleInv * throttleInv;
 320     // generally focus on the low throttle period
 321     if (throttle > previousThrottle) {
 322         throttleDerivative *= throttleInv * 0.5f;
 323         // when increasing throttle, focus even more on the low throttle range
 324     }
 325     previousThrottle = throttle;
 326     throttleDerivative = pt2FilterApply(&pidRuntime.antiGravityLpf, throttleDerivative);
 327     // lower cutoff suppresses peaks relative to troughs and prolongs the effects
 328     // PT2 smoothing of throttle derivative.
 329     // 6 is a typical value for the peak boost factor with default cutoff of 6Hz
 330     DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
 331     pidRuntime.antiGravityThrottleD = throttleDerivative;
 332 }
 333
 334 #ifdef USE_ACRO_TRAINER
 335 void pidAcroTrainerInit(void)
 336 {
 337     pidRuntime.acroTrainerAxisState[FD_ROLL] = 0;
 338     pidRuntime.acroTrainerAxisState[FD_PITCH] = 0;
 339 }
 340 #endif // USE_ACRO_TRAINER
 341
 342 #ifdef USE_THRUST_LINEARIZATION
 343 float pidCompensateThrustLinearization(float throttle)
 344 {
 345     if (pidRuntime.thrustLinearization != 0.0f) {
 346         // for whoops where a lot of TL is needed, allow more throttle boost
 347         const float throttleReversed = (1.0f - throttle);
 348         throttle /= 1.0f + pidRuntime.throttleCompensateAmount * sq(throttleReversed);
 349     }
 350     return throttle;
 351 }
 352
 353 float pidApplyThrustLinearization(float motorOutput)
 354 {
 355     if (pidRuntime.thrustLinearization != 0.0f) {
 356         if (motorOutput > 0.0f) {
 357             const float motorOutputReversed = (1.0f - motorOutput);
 358             motorOutput *= 1.0f + sq(motorOutputReversed) * pidRuntime.thrustLinearization;
 359         }
 360     }
 361     return motorOutput;
 362 }
 363 #endif
 364
 365 #if defined(USE_ACC)
 366 // calculate the stick deflection while applying level mode expo
 367 static float getLevelModeRcDeflection(uint8_t axis)
 368 {
 369     const float stickDeflection = getRcDeflection(axis);
 370     if (axis < FD_YAW) {
 371         const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f;
 372         return power3(stickDeflection) * expof + stickDeflection * (1 - expof);
 373     } else {
 374         return stickDeflection;
 375     }
 376 }
 377
 378 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
 379 STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength(void)
 380 {
 381     // start with 1.0 at center stick, 0.0 at max stick deflection:
 382     float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
 383
 384     // 0 at level, 90 at vertical, 180 at inverted (degrees):
 385     const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) / 10.0f;
 386
 387     // horizonTiltExpertMode:  0 = leveling always active when sticks centered,
 388     //                         1 = leveling can be totally off when inverted
 389     if (pidRuntime.horizonTiltExpertMode) {
 390         if (pidRuntime.horizonTransition > 0 && pidRuntime.horizonCutoffDegrees > 0) {
 391                     // if d_level > 0 and horizonTiltEffect < 175
 392             // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
 393             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
 394             //  for larger inclinations; 0.0 at horizonCutoffDegrees value:
 395             const float inclinationLevelRatio = constrainf((pidRuntime.horizonCutoffDegrees-currentInclination) / pidRuntime.horizonCutoffDegrees, 0, 1);
 396             // apply configured horizon sensitivity:
 397                 // when stick is near center (horizonLevelStrength ~= 1.0)
 398                 //  H_sensitivity value has little effect,
 399                 // when stick is deflected (horizonLevelStrength near 0.0)
 400                 //  H_sensitivity value has more effect:
 401             horizonLevelStrength = (horizonLevelStrength - 1) * 100 / pidRuntime.horizonTransition + 1;
 402             // apply inclination ratio, which may lower leveling
 403             //  to zero regardless of stick position:
 404             horizonLevelStrength *= inclinationLevelRatio;
 405         } else  { // d_level=0 or horizon_tilt_effect>=175 means no leveling
 406           horizonLevelStrength = 0;
 407         }
 408     } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
 409         float sensitFact;
 410         if (pidRuntime.horizonFactorRatio < 1.0f) { // if horizonTiltEffect > 0
 411             // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
 412             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
 413             //  for larger inclinations, goes to 1.0 at inclination==level:
 414             const float inclinationLevelRatio = (180 - currentInclination) / 180 * (1.0f - pidRuntime.horizonFactorRatio) + pidRuntime.horizonFactorRatio;
 415             // apply ratio to configured horizon sensitivity:
 416             sensitFact = pidRuntime.horizonTransition * inclinationLevelRatio;
 417         } else { // horizonTiltEffect=0 for "old" functionality
 418             sensitFact = pidRuntime.horizonTransition;
 419         }
 420
 421         if (sensitFact <= 0) {           // zero means no leveling
 422             horizonLevelStrength = 0;
 423         } else {
 424             // when stick is near center (horizonLevelStrength ~= 1.0)
 425             //  sensitFact value has little effect,
 426             // when stick is deflected (horizonLevelStrength near 0.0)
 427             //  sensitFact value has more effect:
 428             horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
 429         }
 430     }
 431
 432     return constrainf(horizonLevelStrength, 0, 1);
 433 }
 434
 435 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
 436 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
 437 // processing power that it should be a non-issue.
 438 STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
 439                                                         float currentPidSetpoint, float horizonLevelStrength)
 440 {
 441     const float levelAngleLimit = pidProfile->levelAngleLimit;
 442     // calculate error angle and limit the angle to the max inclination
 443     // rcDeflection is in range [-1.0, 1.0]
 444     float angle = levelAngleLimit * getLevelModeRcDeflection(axis);
 445 #ifdef USE_GPS_RESCUE
 446     angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
 447 #endif
 448     angle = constrainf(angle, -levelAngleLimit, levelAngleLimit);
 449     const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
 450     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
 451         // ANGLE mode - control is angle based
 452         const float setpointCorrection = errorAngle * pidRuntime.levelGain;
 453         currentPidSetpoint = pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
 454     } else {
 455         // HORIZON mode - mix of ANGLE and ACRO modes
 456         // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
 457         const float setpointCorrection = errorAngle * pidRuntime.horizonGain * horizonLevelStrength;
 458         currentPidSetpoint += pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
 459     }
 460     return currentPidSetpoint;
 461 }
 462
 463 static void handleCrashRecovery(
 464     const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
 465     const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
 466 {
 467     if (pidRuntime.inCrashRecoveryMode && cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) > pidRuntime.crashTimeDelayUs) {
 468         if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
 469             BEEP_ON;
 470         }
 471         if (axis == FD_YAW) {
 472             *errorRate = constrainf(*errorRate, -pidRuntime.crashLimitYaw, pidRuntime.crashLimitYaw);
 473         } else {
 474             // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
 475             if (sensors(SENSOR_ACC)) {
 476                 // errorAngle is deviation from horizontal
 477                 const float errorAngle =  -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
 478                 *currentPidSetpoint = errorAngle * pidRuntime.levelGain;
 479                 *errorRate = *currentPidSetpoint - gyroRate;
 480             }
 481         }
 482         // reset iterm, since accumulated error before crash is now meaningless
 483         // and iterm windup during crash recovery can be extreme, especially on yaw axis
 484         pidData[axis].I = 0.0f;
 485         if (cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) > pidRuntime.crashTimeLimitUs
 486             || (getMotorMixRange() < 1.0f
 487                    && fabsf(gyro.gyroADCf[FD_ROLL]) < pidRuntime.crashRecoveryRate
 488                    && fabsf(gyro.gyroADCf[FD_PITCH]) < pidRuntime.crashRecoveryRate
 489                    && fabsf(gyro.gyroADCf[FD_YAW]) < pidRuntime.crashRecoveryRate)) {
 490             if (sensors(SENSOR_ACC)) {
 491                 // check aircraft nearly level
 492                 if (abs(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < pidRuntime.crashRecoveryAngleDeciDegrees
 493                    && abs(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < pidRuntime.crashRecoveryAngleDeciDegrees) {
 494                     pidRuntime.inCrashRecoveryMode = false;
 495                     BEEP_OFF;
 496                 }
 497             } else {
 498                 pidRuntime.inCrashRecoveryMode = false;
 499                 BEEP_OFF;
 500             }
 501         }
 502     }
 503 }
 504
 505 static void detectAndSetCrashRecovery(
 506     const pidCrashRecovery_e crash_recovery, const int axis,
 507     const timeUs_t currentTimeUs, const float delta, const float errorRate)
 508 {
 509     // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
 510     // no point in trying to recover if the crash is so severe that the gyro overflows
 511     if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
 512         if (ARMING_FLAG(ARMED)) {
 513             if (getMotorMixRange() >= 1.0f && !pidRuntime.inCrashRecoveryMode
 514                 && fabsf(delta) > pidRuntime.crashDtermThreshold
 515                 && fabsf(errorRate) > pidRuntime.crashGyroThreshold
 516                 && fabsf(getSetpointRate(axis)) < pidRuntime.crashSetpointThreshold) {
 517                 if (crash_recovery == PID_CRASH_RECOVERY_DISARM) {
 518                     setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 519                     disarm(DISARM_REASON_CRASH_PROTECTION);
 520                 } else {
 521                     pidRuntime.inCrashRecoveryMode = true;
 522                     pidRuntime.crashDetectedAtUs = currentTimeUs;
 523                 }
 524             }
 525             if (pidRuntime.inCrashRecoveryMode && cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) < pidRuntime.crashTimeDelayUs && (fabsf(errorRate) < pidRuntime.crashGyroThreshold
 526                 || fabsf(getSetpointRate(axis)) > pidRuntime.crashSetpointThreshold)) {
 527                 pidRuntime.inCrashRecoveryMode = false;
 528                 BEEP_OFF;
 529             }
 530         } else if (pidRuntime.inCrashRecoveryMode) {
 531             pidRuntime.inCrashRecoveryMode = false;
 532             BEEP_OFF;
 533         }
 534     }
 535 }
 536 #endif // USE_ACC
 537
 538 #ifdef USE_ACRO_TRAINER
 539
 540 int acroTrainerSign(float x)
 541 {
 542     return x > 0 ? 1 : -1;
 543 }
 544
 545 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
 546 // There are three states:
 547 // 1. Current angle has exceeded limit
 548 //    Apply correction to return to limit (similar to pidLevel)
 549 // 2. Future overflow has been projected based on current angle and gyro rate
 550 //    Manage the setPoint to control the gyro rate as the actual angle  approaches the limit (try to prevent overshoot)
 551 // 3. If no potential overflow is detected, then return the original setPoint
 552
 553 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
 554 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
 555 // expecting ultimate flight performance at very high loop rates when in this mode.
 556 static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint)
 557 {
 558     float ret = setPoint;
 559
 560     if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
 561         bool resetIterm = false;
 562         float projectedAngle = 0;
 563         const int setpointSign = acroTrainerSign(setPoint);
 564         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
 565         const int angleSign = acroTrainerSign(currentAngle);
 566
 567         if ((pidRuntime.acroTrainerAxisState[axis] != 0) && (pidRuntime.acroTrainerAxisState[axis] != setpointSign)) {  // stick has reversed - stop limiting
 568             pidRuntime.acroTrainerAxisState[axis] = 0;
 569         }
 570
 571         // Limit and correct the angle when it exceeds the limit
 572         if ((fabsf(currentAngle) > pidRuntime.acroTrainerAngleLimit) && (pidRuntime.acroTrainerAxisState[axis] == 0)) {
 573             if (angleSign == setpointSign) {
 574                 pidRuntime.acroTrainerAxisState[axis] = angleSign;
 575                 resetIterm = true;
 576             }
 577         }
 578
 579         if (pidRuntime.acroTrainerAxisState[axis] != 0) {
 580             ret = constrainf(((pidRuntime.acroTrainerAngleLimit * angleSign) - currentAngle) * pidRuntime.acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
 581         } else {
 582
 583         // Not currently over the limit so project the angle based on current angle and
 584         // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
 585         // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
 586             // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
 587             float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * pidRuntime.acroTrainerLookaheadTime;
 588             projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
 589             const int projectedAngleSign = acroTrainerSign(projectedAngle);
 590             if ((fabsf(projectedAngle) > pidRuntime.acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
 591                 ret = ((pidRuntime.acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * pidRuntime.acroTrainerGain;
 592                 resetIterm = true;
 593             }
 594         }
 595
 596         if (resetIterm) {
 597             pidData[axis].I = 0;
 598         }
 599
 600         if (axis == pidRuntime.acroTrainerDebugAxis) {
 601             DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
 602             DEBUG_SET(DEBUG_ACRO_TRAINER, 1, pidRuntime.acroTrainerAxisState[axis]);
 603             DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
 604             DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
 605         }
 606     }
 607
 608     return ret;
 609 }
 610 #endif // USE_ACRO_TRAINER
 611
 612 static float accelerationLimit(int axis, float currentPidSetpoint)
 613 {
 614     static float previousSetpoint[XYZ_AXIS_COUNT];
 615     const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
 616
 617     if (fabsf(currentVelocity) > pidRuntime.maxVelocity[axis]) {
 618         currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + pidRuntime.maxVelocity[axis] : previousSetpoint[axis] - pidRuntime.maxVelocity[axis];
 619     }
 620
 621     previousSetpoint[axis] = currentPidSetpoint;
 622     return currentPidSetpoint;
 623 }
 624
 625 static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
 626 {
 627     // rotate v around rotation vector rotation
 628     // rotation in radians, all elements must be small
 629     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 630         int i_1 = (i + 1) % 3;
 631         int i_2 = (i + 2) % 3;
 632         float newV = v[i_1] + v[i_2] * rotation[i];
 633         v[i_2] -= v[i_1] * rotation[i];
 634         v[i_1] = newV;
 635     }
 636 }
 637
 638 STATIC_UNIT_TESTED void rotateItermAndAxisError(void)
 639 {
 640     if (pidRuntime.itermRotation
 641 #if defined(USE_ABSOLUTE_CONTROL)
 642         || pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR
 643 #endif
 644         ) {
 645         const float gyroToAngle = pidRuntime.dT * RAD;
 646         float rotationRads[XYZ_AXIS_COUNT];
 647         for (int i = FD_ROLL; i <= FD_YAW; i++) {
 648             rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
 649         }
 650 #if defined(USE_ABSOLUTE_CONTROL)
 651         if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) {
 652             rotateVector(axisError, rotationRads);
 653         }
 654 #endif
 655         if (pidRuntime.itermRotation) {
 656             float v[XYZ_AXIS_COUNT];
 657             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 658                 v[i] = pidData[i].I;
 659             }
 660             rotateVector(v, rotationRads );
 661             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 662                 pidData[i].I = v[i];
 663             }
 664         }
 665     }
 666 }
 667
 668 #ifdef USE_RC_SMOOTHING_FILTER
 669 float FAST_CODE applyRcSmoothingFeedforwardFilter(int axis, float pidSetpointDelta)
 670 {
 671     float ret = pidSetpointDelta;
 672     if (axis == pidRuntime.rcSmoothingDebugAxis) {
 673         DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
 674     }
 675     if (pidRuntime.feedforwardLpfInitialized) {
 676         ret = pt3FilterApply(&pidRuntime.feedforwardPt3[axis], pidSetpointDelta);
 677         if (axis == pidRuntime.rcSmoothingDebugAxis) {
 678             DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
 679         }
 680     }
 681     return ret;
 682 }
 683 #endif // USE_RC_SMOOTHING_FILTER
 684
 685 #if defined(USE_ITERM_RELAX)
 686 #if defined(USE_ABSOLUTE_CONTROL)
 687 STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
 688 {
 689     if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) {
 690         const float setpointLpf = pt1FilterApply(&pidRuntime.acLpf[axis], *currentPidSetpoint);
 691         const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
 692         float acErrorRate = 0;
 693         const float gmaxac = setpointLpf + 2 * setpointHpf;
 694         const float gminac = setpointLpf - 2 * setpointHpf;
 695         if (gyroRate >= gminac && gyroRate <= gmaxac) {
 696             const float acErrorRate1 = gmaxac - gyroRate;
 697             const float acErrorRate2 = gminac - gyroRate;
 698             if (acErrorRate1 * axisError[axis] < 0) {
 699                 acErrorRate = acErrorRate1;
 700             } else {
 701                 acErrorRate = acErrorRate2;
 702             }
 703             if (fabsf(acErrorRate * pidRuntime.dT) > fabsf(axisError[axis]) ) {
 704                 acErrorRate = -axisError[axis] * pidRuntime.pidFrequency;
 705             }
 706         } else {
 707             acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
 708         }
 709
 710         if (isAirmodeActivated()) {
 711             axisError[axis] = constrainf(axisError[axis] + acErrorRate * pidRuntime.dT,
 712                 -pidRuntime.acErrorLimit, pidRuntime.acErrorLimit);
 713             const float acCorrection = constrainf(axisError[axis] * pidRuntime.acGain, -pidRuntime.acLimit, pidRuntime.acLimit);
 714             *currentPidSetpoint += acCorrection;
 715             *itermErrorRate += acCorrection;
 716             DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10));
 717             if (axis == FD_ROLL) {
 718                 DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10));
 719             }
 720         }
 721         DEBUG_SET(DEBUG_AC_ERROR, axis, lrintf(axisError[axis] * 10));
 722     }
 723 }
 724 #endif
 725
 726 STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
 727     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
 728 {
 729     const float setpointLpf = pt1FilterApply(&pidRuntime.windupLpf[axis], *currentPidSetpoint);
 730     const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
 731
 732     if (pidRuntime.itermRelax) {
 733         if (axis < FD_YAW || pidRuntime.itermRelax == ITERM_RELAX_RPY || pidRuntime.itermRelax == ITERM_RELAX_RPY_INC) {
 734             const float itermRelaxFactor = MAX(0, 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD);
 735             const bool isDecreasingI =
 736                 ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
 737             if ((pidRuntime.itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
 738                 // Do Nothing, use the precalculed itermErrorRate
 739             } else if (pidRuntime.itermRelaxType == ITERM_RELAX_SETPOINT) {
 740                 *itermErrorRate *= itermRelaxFactor;
 741             } else if (pidRuntime.itermRelaxType == ITERM_RELAX_GYRO ) {
 742                 *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
 743             } else {
 744                 *itermErrorRate = 0.0f;
 745             }
 746
 747             if (axis == FD_ROLL) {
 748                 DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
 749                 DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
 750                 DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
 751             }
 752         }
 753
 754 #if defined(USE_ABSOLUTE_CONTROL)
 755         applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate);
 756 #endif
 757     }
 758 }
 759 #endif
 760
 761 #ifdef USE_AIRMODE_LPF
 762 void pidUpdateAirmodeLpf(float currentOffset)
 763 {
 764     if (pidRuntime.airmodeThrottleOffsetLimit == 0.0f) {
 765         return;
 766     }
 767
 768     float offsetHpf = currentOffset * 2.5f;
 769     offsetHpf = offsetHpf - pt1FilterApply(&pidRuntime.airmodeThrottleLpf2, offsetHpf);
 770
 771     // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
 772     pt1FilterApply(&pidRuntime.airmodeThrottleLpf1, offsetHpf);
 773     // Bring offset up immediately so the filter only applies to the decline
 774     if (currentOffset * pidRuntime.airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > pidRuntime.airmodeThrottleLpf1.state) {
 775         pidRuntime.airmodeThrottleLpf1.state = currentOffset;
 776     }
 777     pidRuntime.airmodeThrottleLpf1.state = constrainf(pidRuntime.airmodeThrottleLpf1.state, -pidRuntime.airmodeThrottleOffsetLimit, pidRuntime.airmodeThrottleOffsetLimit);
 778 }
 779
 780 float pidGetAirmodeThrottleOffset(void)
 781 {
 782     return pidRuntime.airmodeThrottleLpf1.state;
 783 }
 784 #endif
 785
 786 #ifdef USE_LAUNCH_CONTROL
 787 #define LAUNCH_CONTROL_MAX_RATE 100.0f
 788 #define LAUNCH_CONTROL_MIN_RATE 5.0f
 789 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f  // The remaining angle degrees where rate dampening starts
 790
 791 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
 792 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
 793 // processing power that it should be a non-issue.
 794 static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
 795 {
 796     float ret = 0.0f;
 797
 798     // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
 799     // reached at 50% stick deflection. This keeps the launch control positioning consistent
 800     // regardless of the user's rates.
 801     if ((axis == FD_PITCH) || (pidRuntime.launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
 802         const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
 803         ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
 804     }
 805
 806 #if defined(USE_ACC)
 807     // If ACC is enabled and a limit angle is set, then try to limit forward tilt
 808     // to that angle and slow down the rate as the limit is approached to reduce overshoot
 809     if ((axis == FD_PITCH) && (pidRuntime.launchControlAngleLimit > 0) && (ret > 0)) {
 810         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
 811         if (currentAngle >= pidRuntime.launchControlAngleLimit) {
 812             ret = 0.0f;
 813         } else {
 814             //for the last 10 degrees scale the rate from the current input to 5 dps
 815             const float angleDelta = pidRuntime.launchControlAngleLimit - currentAngle;
 816             if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
 817                 ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
 818             }
 819         }
 820     }
 821 #else
 822     UNUSED(angleTrim);
 823 #endif
 824
 825     return ret;
 826 }
 827 #endif
 828
 829 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 830 // Based on 2DOF reference design (matlab)
 831 void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
 832 {
 833     static float previousGyroRateDterm[XYZ_AXIS_COUNT];
 834     static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
 835
 836 #ifdef USE_TPA_MODE
 837     const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
 838 #else
 839     const float tpaFactorKp = pidRuntime.tpaFactor;
 840 #endif
 841
 842 #ifdef USE_YAW_SPIN_RECOVERY
 843     const bool yawSpinActive = gyroYawSpinDetected();
 844 #endif
 845
 846     const bool launchControlActive = isLaunchControlActive();
 847
 848 #if defined(USE_ACC)
 849     static timeUs_t levelModeStartTimeUs = 0;
 850     static bool gpsRescuePreviousState = false;
 851     const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
 852     float horizonLevelStrength = 0.0f;
 853
 854     const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
 855     levelMode_e levelMode;
 856     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive) {
 857         if (pidRuntime.levelRaceMode && !gpsRescueIsActive) {
 858             levelMode = LEVEL_MODE_R;
 859         } else {
 860             levelMode = LEVEL_MODE_RP;
 861         }
 862
 863         // Keep track of when we entered a self-level mode so that we can
 864         // add a guard time before crash recovery can activate.
 865         // Also reset the guard time whenever GPS Rescue is activated.
 866         if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) {
 867             levelModeStartTimeUs = currentTimeUs;
 868         }
 869
 870         // Calc horizonLevelStrength if needed
 871         if (FLIGHT_MODE(HORIZON_MODE)) {
 872             horizonLevelStrength = calcHorizonLevelStrength();
 873         }
 874     } else {
 875         levelMode = LEVEL_MODE_OFF;
 876         levelModeStartTimeUs = 0;
 877     }
 878
 879     gpsRescuePreviousState = gpsRescueIsActive;
 880 #else
 881     UNUSED(pidProfile);
 882     UNUSED(currentTimeUs);
 883 #endif
 884
 885     // Anti Gravity
 886     if (pidRuntime.antiGravityEnabled) {
 887         pidRuntime.antiGravityThrottleD *= pidRuntime.antiGravityGain;
 888         // used later to increase pTerm
 889         pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleD * ANTIGRAVITY_KI;
 890     } else {
 891         pidRuntime.antiGravityThrottleD = 0.0f;
 892         pidRuntime.itermAccelerator = 0.0f;
 893     }
 894     DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000));
 895     // amount of antigravity added relative to user's pitch iTerm coefficient
 896     // used later to increase iTerm
 897
 898     // iTerm windup (attenuation of iTerm if motorMix range is large)
 899     float dynCi = 1.0;
 900     if (pidRuntime.itermWindupPointInv > 1.0f) {
 901         dynCi = constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
 902     }
 903
 904     // Precalculate gyro delta for D-term here, this allows loop unrolling
 905     float gyroRateDterm[XYZ_AXIS_COUNT];
 906     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 907         gyroRateDterm[axis] = gyro.gyroADCf[axis];
 908         // -----calculate raw, unfiltered D component
 909
 910         // Divide rate change by dT to get differential (ie dr/dt).
 911         // dT is fixed and calculated from the target PID loop time
 912         // This is done to avoid DTerm spikes that occur with dynamically
 913         // calculated deltaT whenever another task causes the PID
 914         // loop execution to be delayed.
 915
 916         // Log the unfiltered D for ROLL and PITCH
 917         if (axis != FD_YAW) {
 918             const float delta = (previousRawGyroRateDterm[axis] - gyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
 919             previousRawGyroRateDterm[axis] = gyroRateDterm[axis];
 920             DEBUG_SET(DEBUG_D_LPF, axis, lrintf(delta));
 921         }
 922
 923         gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
 924         gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *) &pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]);
 925         gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]);
 926     }
 927
 928     rotateItermAndAxisError();
 929
 930 #ifdef USE_RPM_FILTER
 931     rpmFilterUpdate();
 932 #endif
 933
 934 #ifdef USE_FEEDFORWARD
 935     const bool newRcFrame = getShouldUpdateFeedforward();
 936 #endif
 937
 938     // ----------PID controller----------
 939     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 940
 941         float currentPidSetpoint = getSetpointRate(axis);
 942         if (pidRuntime.maxVelocity[axis]) {
 943             currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
 944         }
 945         // Yaw control is GYRO based, direct sticks control is applied to rate PID
 946         // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
 947 #if defined(USE_ACC)
 948         if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL)
 949             || (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis ==  FD_PITCH)) ) {
 950             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
 951             DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint);
 952         }
 953 #endif
 954
 955 #ifdef USE_ACRO_TRAINER
 956         if ((axis != FD_YAW) && pidRuntime.acroTrainerActive && !pidRuntime.inCrashRecoveryMode && !launchControlActive) {
 957             currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
 958         }
 959 #endif // USE_ACRO_TRAINER
 960
 961 #ifdef USE_LAUNCH_CONTROL
 962         if (launchControlActive) {
 963 #if defined(USE_ACC)
 964             currentPidSetpoint = applyLaunchControl(axis, angleTrim);
 965 #else
 966             currentPidSetpoint = applyLaunchControl(axis, NULL);
 967 #endif
 968         }
 969 #endif
 970
 971         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
 972         // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
 973 #ifdef USE_YAW_SPIN_RECOVERY
 974         if ((axis == FD_YAW) && yawSpinActive) {
 975             currentPidSetpoint = 0.0f;
 976         }
 977 #endif // USE_YAW_SPIN_RECOVERY
 978
 979         // -----calculate error rate
 980         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
 981         float errorRate = currentPidSetpoint - gyroRate; // r - y
 982 #if defined(USE_ACC)
 983         handleCrashRecovery(
 984             pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
 985             &currentPidSetpoint, &errorRate);
 986 #endif
 987
 988         const float previousIterm = pidData[axis].I;
 989         float itermErrorRate = errorRate;
 990 #ifdef USE_ABSOLUTE_CONTROL
 991         const float uncorrectedSetpoint = currentPidSetpoint;
 992 #endif
 993
 994 #if defined(USE_ITERM_RELAX)
 995         if (!launchControlActive && !pidRuntime.inCrashRecoveryMode) {
 996             applyItermRelax(axis, previousIterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
 997             errorRate = currentPidSetpoint - gyroRate;
 998         }
 999 #endif
1000 #ifdef USE_ABSOLUTE_CONTROL
1001         const float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
1002 #endif
1003
1004         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1005         // 2-DOF PID controller with optional filter on derivative term.
1006         // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
1007
1008         // -----calculate P component
1009         pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
1010         if (axis == FD_YAW) {
1011             pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
1012         }
1013
1014         // -----calculate I component
1015         float Ki = pidRuntime.pidCoefficient[axis].Ki;
1016 #ifdef USE_LAUNCH_CONTROL
1017         // if launch control is active override the iterm gains and apply iterm windup protection to all axes
1018         if (launchControlActive) {
1019             Ki = pidRuntime.launchControlKi;
1020         } else
1021 #endif
1022         {
1023             if (axis == FD_YAW) {
1024                 pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
1025             }
1026         }
1027         const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
1028         pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
1029
1030         // -----calculate pidSetpointDelta
1031         float pidSetpointDelta = 0;
1032 #ifdef USE_FEEDFORWARD
1033         pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging);
1034 #endif
1035         pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
1036
1037         // -----calculate D component
1038         // disable D if launch control is active
1039         if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) {
1040
1041             // Divide rate change by dT to get differential (ie dr/dt).
1042             // dT is fixed and calculated from the target PID loop time
1043             // This is done to avoid DTerm spikes that occur with dynamically
1044             // calculated deltaT whenever another task causes the PID
1045             // loop execution to be delayed.
1046             const float delta =
1047                 - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
1048             float preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta;
1049
1050 #if defined(USE_ACC)
1051             if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
1052                 detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
1053             }
1054 #endif
1055
1056 #if defined(USE_D_MIN)
1057             float dMinFactor = 1.0f;
1058             if (pidRuntime.dMinPercent[axis] > 0) {
1059                 float dMinGyroFactor = pt2FilterApply(&pidRuntime.dMinRange[axis], delta);
1060                 dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain;
1061                 const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain;
1062                 dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
1063                 dMinFactor = pidRuntime.dMinPercent[axis] + (1.0f - pidRuntime.dMinPercent[axis]) * dMinFactor;
1064                 dMinFactor = pt2FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor);
1065                 dMinFactor = MIN(dMinFactor, 1.0f);
1066                 if (axis == FD_ROLL) {
1067                     DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100));
1068                     DEBUG_SET(DEBUG_D_MIN, 1, lrintf(dMinSetpointFactor * 100));
1069                     DEBUG_SET(DEBUG_D_MIN, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
1070                 } else if (axis == FD_PITCH) {
1071                     DEBUG_SET(DEBUG_D_MIN, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
1072                 }
1073             }
1074
1075             // Apply the dMinFactor
1076             preTpaD *= dMinFactor;
1077 #endif
1078             pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
1079
1080             // Log the value of D pre application of TPA
1081             preTpaD *= D_LPF_FILT_SCALE;
1082
1083             if (axis != FD_YAW) {
1084                 DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, lrintf(preTpaD));
1085             }
1086         } else {
1087             pidData[axis].D = 0;
1088             if (axis != FD_YAW) {
1089                 DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, 0);
1090             }
1091         }
1092
1093         previousGyroRateDterm[axis] = gyroRateDterm[axis];
1094
1095         // -----calculate feedforward component
1096 #ifdef USE_ABSOLUTE_CONTROL
1097         // include abs control correction in feedforward
1098         pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
1099         pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
1100 #endif
1101
1102         // no feedforward in launch control
1103         float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
1104         if (feedforwardGain > 0) {
1105             // halve feedforward in Level mode since stick sensitivity is weaker by about half
1106             feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
1107             // transition now calculated in feedforward.c when new RC data arrives
1108             float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
1109
1110 #ifdef USE_FEEDFORWARD
1111             pidData[axis].F = shouldApplyFeedforwardLimits(axis) ?
1112                 applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
1113 #else
1114             pidData[axis].F = feedForward;
1115 #endif
1116 #ifdef USE_RC_SMOOTHING_FILTER
1117             pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
1118 #endif // USE_RC_SMOOTHING_FILTER
1119         } else {
1120             pidData[axis].F = 0;
1121         }
1122
1123 #ifdef USE_YAW_SPIN_RECOVERY
1124         if (yawSpinActive) {
1125             pidData[axis].I = 0;  // in yaw spin always disable I
1126             if (axis <= FD_PITCH)  {
1127                 // zero PIDs on pitch and roll leaving yaw P to correct spin
1128                 pidData[axis].P = 0;
1129                 pidData[axis].D = 0;
1130                 pidData[axis].F = 0;
1131             }
1132         }
1133 #endif // USE_YAW_SPIN_RECOVERY
1134
1135 #ifdef USE_LAUNCH_CONTROL
1136         // Disable P/I appropriately based on the launch control mode
1137         if (launchControlActive) {
1138             // if not using FULL mode then disable I accumulation on yaw as
1139             // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1140             const int launchControlYawItermLimit = (pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
1141             pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit);
1142
1143             // for pitch-only mode we disable everything except pitch P/I
1144             if (pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
1145                 pidData[FD_ROLL].P = 0;
1146                 pidData[FD_ROLL].I = 0;
1147                 pidData[FD_YAW].P = 0;
1148                 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1149                 pidData[FD_PITCH].I = MAX(0.0f, pidData[FD_PITCH].I);
1150             }
1151         }
1152 #endif
1153
1154         // Add P boost from antiGravity when sticks are close to zero
1155         if (axis != FD_YAW) {
1156             float agSetpointAttenuator = fabsf(currentPidSetpoint) / 50.0f;
1157             agSetpointAttenuator = MAX(agSetpointAttenuator, 1.0f);
1158             // attenuate effect if turning more than 50 deg/s, half at 100 deg/s
1159             const float antiGravityPBoost = 1.0f + (pidRuntime.antiGravityThrottleD / agSetpointAttenuator) * pidRuntime.antiGravityPGain;
1160             pidData[axis].P *= antiGravityPBoost;
1161             if (axis == FD_PITCH) {
1162                 DEBUG_SET(DEBUG_ANTI_GRAVITY, 3, lrintf(antiGravityPBoost * 1000));
1163             }
1164         }
1165
1166         // calculating the PID sum
1167         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
1168 #ifdef USE_INTEGRATED_YAW_CONTROL
1169         if (axis == FD_YAW && pidRuntime.useIntegratedYaw) {
1170             pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
1171             pidData[axis].Sum -= pidData[axis].Sum * pidRuntime.integratedYawRelax / 100000.0f * pidRuntime.dT / 0.000125f;
1172         } else
1173 #endif
1174         {
1175             pidData[axis].Sum = pidSum;
1176         }
1177     }
1178
1179     // Disable PID control if at zero throttle or if gyro overflow detected
1180     // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1181     if (!pidRuntime.pidStabilisationEnabled || gyroOverflowDetected()) {
1182         for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
1183             pidData[axis].P = 0;
1184             pidData[axis].I = 0;
1185             pidData[axis].D = 0;
1186             pidData[axis].F = 0;
1187
1188             pidData[axis].Sum = 0;
1189         }
1190     } else if (pidRuntime.zeroThrottleItermReset) {
1191         pidResetIterm();
1192     }
1193 }
1194
1195 bool crashRecoveryModeActive(void)
1196 {
1197     return pidRuntime.inCrashRecoveryMode;
1198 }
1199
1200 #ifdef USE_ACRO_TRAINER
1201 void pidSetAcroTrainerState(bool newState)
1202 {
1203     if (pidRuntime.acroTrainerActive != newState) {
1204         if (newState) {
1205             pidAcroTrainerInit();
1206         }
1207         pidRuntime.acroTrainerActive = newState;
1208     }
1209 }
1210 #endif // USE_ACRO_TRAINER
1211
1212 void pidSetAntiGravityState(bool newState)
1213 {
1214     if (newState != pidRuntime.antiGravityEnabled) {
1215         // reset the accelerator on state changes
1216         pidRuntime.itermAccelerator = 0.0f;
1217     }
1218     pidRuntime.antiGravityEnabled = newState;
1219 }
1220
1221 bool pidAntiGravityEnabled(void)
1222 {
1223     return pidRuntime.antiGravityEnabled;
1224 }
1225
1226 #ifdef USE_DYN_LPF
1227 void dynLpfDTermUpdate(float throttle)
1228 {
1229     if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) {
1230         float cutoffFreq;
1231         if (pidRuntime.dynLpfCurveExpo > 0) {
1232             cutoffFreq = dynLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo);
1233         } else {
1234             cutoffFreq = fmaxf(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
1235         }
1236
1237         switch (pidRuntime.dynLpfFilter) {
1238         case DYN_LPF_PT1:
1239             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1240                 pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT));
1241             }
1242             break;
1243         case DYN_LPF_BIQUAD:
1244             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1245                 biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
1246             }
1247             break;
1248         case DYN_LPF_PT2:
1249             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1250                 pt2FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(cutoffFreq, pidRuntime.dT));
1251             }
1252             break;
1253         case DYN_LPF_PT3:
1254             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1255                 pt3FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(cutoffFreq, pidRuntime.dT));
1256             }
1257             break;
1258         }
1259     }
1260 }
1261 #endif
1262
1263 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo)
1264 {
1265     const float expof = expo / 10.0f;
1266     const float curve = throttle * (1 - throttle) * expof + throttle;
1267     return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
1268 }
1269
1270 void pidSetItermReset(bool enabled)
1271 {
1272     pidRuntime.zeroThrottleItermReset = enabled;
1273 }
1274
1275 float pidGetPreviousSetpoint(int axis)
1276 {
1277     return pidRuntime.previousPidSetpoint[axis];
1278 }
1279
1280 float pidGetDT(void)
1281 {
1282     return pidRuntime.dT;
1283 }
1284
1285 float pidGetPidFrequency(void)
1286 {
1287     return pidRuntime.pidFrequency;
1288 }