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