src/main/flight/pid_init.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
  34 #include "drivers/dshot_command.h"
  35
  36 #include "fc/rc_controls.h"
  37 #include "fc/runtime_config.h"
  38 #include "fc/rc.h"
  39
  40 #include "flight/pid.h"
  41 #include "flight/rpm_filter.h"
  42
  43 #include "pg/motor.h"
  44
  45 #include "rx/rx.h"
  46
  47 #include "sensors/gyro.h"
  48 #include "sensors/sensors.h"
  49
  50 #include "pid_init.h"
  51
  52 #ifdef USE_D_MAX
  53 #define D_MAX_RANGE_HZ 85    // PT2 lowpass input cutoff to peak D around propwash frequencies
  54 #define D_MAX_LOWPASS_HZ 35  // PT2 lowpass cutoff to smooth the boost effect
  55 #define D_MAX_GAIN_FACTOR 0.00008f
  56 #define D_MAX_SETPOINT_GAIN_FACTOR 0.00008f
  57 #endif
  58
  59 #define ATTITUDE_CUTOFF_HZ 50
  60
  61 static void pidSetTargetLooptime(uint32_t pidLooptime)
  62 {
  63     targetPidLooptime = pidLooptime;
  64     pidRuntime.dT = targetPidLooptime * 1e-6f;
  65     pidRuntime.pidFrequency = 1.0f / pidRuntime.dT;
  66 #ifdef USE_DSHOT
  67     dshotSetPidLoopTime(targetPidLooptime);
  68 #endif
  69 }
  70
  71 #ifdef USE_WING
  72 void tpaSpeedBasicInit(const pidProfile_t *pidProfile)
  73 {
  74     // basic model assumes prop pitch speed is inf
  75     const float gravityFactor = pidProfile->tpa_speed_basic_gravity / 100.0f;
  76     const float delaySec = pidProfile->tpa_speed_basic_delay / 1000.0f;
  77
  78     pidRuntime.tpaSpeed.twr = 1.0f / (gravityFactor * gravityFactor);
  79     const float massDragRatio = (2.0f / logf(3.0f)) * (2.0f / logf(3.0f)) * pidRuntime.tpaSpeed.twr * G_ACCELERATION * delaySec * delaySec;
  80     pidRuntime.tpaSpeed.dragMassRatio = 1.0f / massDragRatio;
  81     pidRuntime.tpaSpeed.maxSpeed = sqrtf(massDragRatio * pidRuntime.tpaSpeed.twr * G_ACCELERATION + G_ACCELERATION);
  82     pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f;
  83 }
  84
  85 void tpaSpeedAdvancedInit(const pidProfile_t *pidProfile)
  86 {
  87     // Advanced model uses prop pitch speed, and is quite limited when craft speed is far above prop pitch speed.
  88     pidRuntime.tpaSpeed.twr = (float)pidProfile->tpa_speed_adv_thrust / (float)pidProfile->tpa_speed_adv_mass;
  89     const float mass = pidProfile->tpa_speed_adv_mass / 1000.0f;
  90     const float dragK = pidProfile->tpa_speed_adv_drag_k / 10000.0f;
  91     const float propPitch = pidProfile->tpa_speed_adv_prop_pitch / 100.0f;
  92     pidRuntime.tpaSpeed.dragMassRatio = dragK / mass;
  93     const float propMaxSpeed = (2.54f / 100.0f / 60.0f) * propPitch * motorConfig()->kv * pidRuntime.tpaSpeed.maxVoltage;
  94     if (propMaxSpeed <= 0.0f) { // assuming propMaxSpeed is inf
  95         pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f;
  96     } else {
  97         pidRuntime.tpaSpeed.inversePropMaxSpeed = 1.0f / propMaxSpeed;
  98     }
  99
 100     const float maxFallSpeed = sqrtf(mass * G_ACCELERATION / dragK);
 101
 102     const float a = dragK;
 103     const float b = mass * pidRuntime.tpaSpeed.twr * G_ACCELERATION * pidRuntime.tpaSpeed.inversePropMaxSpeed;
 104     const float c = -mass * (pidRuntime.tpaSpeed.twr + 1) * G_ACCELERATION;
 105
 106     const float maxDiveSpeed = (-b + sqrtf(b*b - 4.0f * a * c)) / (2.0f * a);
 107
 108     pidRuntime.tpaSpeed.maxSpeed = MAX(maxFallSpeed, maxDiveSpeed);
 109     UNUSED(pidProfile);
 110 }
 111
 112 void tpaSpeedInit(const pidProfile_t *pidProfile)
 113 {
 114     pidRuntime.tpaSpeed.speed = 0.0f;
 115     pidRuntime.tpaSpeed.maxVoltage = pidProfile->tpa_speed_max_voltage / 100.0f;
 116     pidRuntime.tpaSpeed.pitchOffset = pidProfile->tpa_speed_pitch_offset * M_PIf / 10.0f / 180.0f;
 117
 118     switch (pidProfile->tpa_speed_type) {
 119     case TPA_SPEED_BASIC:
 120         tpaSpeedBasicInit(pidProfile);
 121         break;
 122     case TPA_SPEED_ADVANCED:
 123         tpaSpeedAdvancedInit(pidProfile);
 124         break;
 125     default:
 126         break;
 127     }
 128 }
 129 #endif // USE_WING
 130
 131 void pidInitFilters(const pidProfile_t *pidProfile)
 132 {
 133     STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
 134
 135     if (targetPidLooptime == 0) {
 136         // no looptime set, so set all the filters to null
 137         pidRuntime.dtermNotchApplyFn = nullFilterApply;
 138         pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 139         pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
 140         pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
 141         return;
 142     }
 143
 144     const uint32_t pidFrequencyNyquist = pidRuntime.pidFrequency / 2; // No rounding needed
 145
 146     uint16_t dTermNotchHz;
 147     if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
 148         dTermNotchHz = pidProfile->dterm_notch_hz;
 149     } else {
 150         if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
 151             dTermNotchHz = pidFrequencyNyquist;
 152         } else {
 153             dTermNotchHz = 0;
 154         }
 155     }
 156
 157     if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
 158         pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
 159         const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
 160         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 161             biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH, 1.0f);
 162         }
 163     } else {
 164         pidRuntime.dtermNotchApplyFn = nullFilterApply;
 165     }
 166
 167     //1st Dterm Lowpass Filter
 168     uint16_t dterm_lpf1_init_hz = pidProfile->dterm_lpf1_static_hz;
 169
 170 #ifdef USE_DYN_LPF
 171     if (pidProfile->dterm_lpf1_dyn_min_hz) {
 172         dterm_lpf1_init_hz = pidProfile->dterm_lpf1_dyn_min_hz;
 173     }
 174 #endif
 175
 176     if (dterm_lpf1_init_hz > 0) {
 177         switch (pidProfile->dterm_lpf1_type) {
 178         case FILTER_PT1:
 179             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 180             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 181                 pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 182             }
 183             break;
 184         case FILTER_BIQUAD:
 185             if (pidProfile->dterm_lpf1_static_hz < pidFrequencyNyquist) {
 186 #ifdef USE_DYN_LPF
 187                 pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
 188 #else
 189                 pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
 190 #endif
 191                 for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 192                     biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lpf1_init_hz, targetPidLooptime);
 193                 }
 194             } else {
 195                 pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 196             }
 197             break;
 198         case FILTER_PT2:
 199             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
 200             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 201                 pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 202             }
 203             break;
 204         case FILTER_PT3:
 205             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
 206             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 207                 pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 208             }
 209             break;
 210         default:
 211             pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 212             break;
 213         }
 214     } else {
 215         pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 216     }
 217
 218     //2nd Dterm Lowpass Filter
 219     if (pidProfile->dterm_lpf2_static_hz > 0) {
 220         switch (pidProfile->dterm_lpf2_type) {
 221         case FILTER_PT1:
 222             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
 223             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 224                 pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 225             }
 226             break;
 227         case FILTER_BIQUAD:
 228             if (pidProfile->dterm_lpf2_static_hz < pidFrequencyNyquist) {
 229                 pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 230                 for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 231                     biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lpf2_static_hz, targetPidLooptime);
 232                 }
 233             } else {
 234                 pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 235             }
 236             break;
 237         case FILTER_PT2:
 238             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
 239             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 240                 pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 241             }
 242             break;
 243         case FILTER_PT3:
 244             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
 245             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 246                 pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 247             }
 248             break;
 249         default:
 250             pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
 251             break;
 252         }
 253     } else {
 254         pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
 255     }
 256
 257     if (pidProfile->yaw_lowpass_hz == 0) {
 258         pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
 259     } else {
 260         pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 261         pt1FilterInit(&pidRuntime.ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, pidRuntime.dT));
 262     }
 263
 264 #if defined(USE_THROTTLE_BOOST)
 265     pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, pidRuntime.dT));
 266 #endif
 267
 268 #if defined(USE_ITERM_RELAX)
 269     if (pidRuntime.itermRelax) {
 270         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 271             pt1FilterInit(&pidRuntime.windupLpf[i], pt1FilterGain(pidRuntime.itermRelaxCutoff, pidRuntime.dT));
 272         }
 273     }
 274 #endif
 275
 276 #if defined(USE_ABSOLUTE_CONTROL)
 277     if (pidRuntime.itermRelax) {
 278         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 279             pt1FilterInit(&pidRuntime.acLpf[i], pt1FilterGain(pidRuntime.acCutoff, pidRuntime.dT));
 280         }
 281     }
 282 #endif
 283
 284 #ifdef USE_D_MAX
 285     // Initialize the filters for all axis even if the d_max[axis] value is 0
 286     // Otherwise if the pidProfile->d_max_xxx parameters are ever added to
 287     // in-flight adjustments and transition from 0 to > 0 in flight the feature
 288     // won't work because the filter wasn't initialized.
 289     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 290         pt2FilterInit(&pidRuntime.dMaxRange[axis], pt2FilterGain(D_MAX_RANGE_HZ, pidRuntime.dT));
 291         pt2FilterInit(&pidRuntime.dMaxLowpass[axis], pt2FilterGain(D_MAX_LOWPASS_HZ, pidRuntime.dT));
 292      }
 293 #endif
 294
 295 #if defined(USE_AIRMODE_LPF)
 296     if (pidProfile->transient_throttle_limit) {
 297         pt1FilterInit(&pidRuntime.airmodeThrottleLpf1, pt1FilterGain(7.0f, pidRuntime.dT));
 298         pt1FilterInit(&pidRuntime.airmodeThrottleLpf2, pt1FilterGain(20.0f, pidRuntime.dT));
 299     }
 300 #endif
 301
 302 #ifdef USE_ACC
 303     const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
 304     const float angleCutoffHz = 1000.0f / (2.0f * M_PIf * pidProfile->angle_feedforward_smoothing_ms); // default of 80ms -> 2.0Hz, 160ms -> 1.0Hz, approximately
 305     const float k2 = pt3FilterGain(angleCutoffHz, pidRuntime.dT);
 306     pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
 307     if (pidRuntime.horizonDelayMs) {
 308         const float horizonSmoothingHz = 1e3f / (2.0f * M_PIf * pidProfile->horizon_delay_ms); // default of 500ms means 0.318Hz
 309         const float kHorizon = pt1FilterGain(horizonSmoothingHz, pidRuntime.dT);
 310         pt1FilterInit(&pidRuntime.horizonSmoothingPt1, kHorizon);
 311     }
 312
 313     for (int axis = 0; axis < 2; axis++) {  // ROLL and PITCH only
 314         pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
 315         pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
 316     }
 317     pidRuntime.angleYawSetpoint = 0.0f;
 318 #endif
 319
 320 #ifdef USE_CHIRP
 321     const float alpha = pidRuntime.chirpLeadFreqHz / pidRuntime.chirpLagFreqHz;
 322     const float centerFreqHz = pidRuntime.chirpLagFreqHz * sqrtf(alpha);
 323     const float centerPhaseDeg = asinf( (1.0f - alpha) / (1.0f + alpha) ) / RAD;
 324     phaseCompInit(&pidRuntime.chirpFilter, centerFreqHz, centerPhaseDeg, targetPidLooptime);
 325     chirpInit(&pidRuntime.chirp, pidRuntime.chirpFrequencyStartHz, pidRuntime.chirpFrequencyEndHz, pidRuntime.chirpTimeSeconds, targetPidLooptime);
 326 #endif
 327
 328     pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
 329 #ifdef USE_WING
 330     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 331         pidRuntime.spa[axis] = 1.0f; // 1.0 = no PID attenuation in runtime. 0 - full attenuation (no PIDs)
 332     }
 333 #endif
 334 }
 335
 336 #ifdef USE_ADVANCED_TPA
 337 float tpaCurveHyperbolicFunction(float x, void *args)
 338 {
 339     const pidProfile_t *pidProfile = (const pidProfile_t*)args;
 340
 341     const float thrStall = pidProfile->tpa_curve_stall_throttle / 100.0f;
 342     const float pidThr0 = pidProfile->tpa_curve_pid_thr0 / 100.0f;
 343
 344     if (x <= thrStall) {
 345         return pidThr0;
 346     }
 347
 348     const float expoDivider = pidProfile->tpa_curve_expo / 10.0f - 1.0f;
 349     const float expo = (fabsf(expoDivider) > 1e-3f) ?  1.0f / expoDivider : 1e3f; // avoiding division by zero for const float base = ...
 350
 351     const float pidThr100 = pidProfile->tpa_curve_pid_thr100 / 100.0f;
 352     const float xShifted = scaleRangef(x, thrStall, 1.0f, 0.0f, 1.0f);
 353     const float base = (1 + (powf(pidThr0 / pidThr100, 1.0f / expo) - 1) * xShifted);
 354     const float divisor = powf(base, expo);
 355
 356     return pidThr0 / divisor;
 357 }
 358
 359 void tpaCurveHyperbolicInit(const pidProfile_t *pidProfile)
 360 {
 361     pwlInitialize(&pidRuntime.tpaCurvePwl, pidRuntime.tpaCurvePwl_yValues, TPA_CURVE_PWL_SIZE, 0.0f, 1.0f);
 362     pwlFill(&pidRuntime.tpaCurvePwl, tpaCurveHyperbolicFunction, (void*)pidProfile);
 363 }
 364
 365 void tpaCurveInit(const pidProfile_t *pidProfile)
 366 {
 367         pidRuntime.tpaCurveType = pidProfile->tpa_curve_type;
 368         switch (pidRuntime.tpaCurveType) {
 369         case TPA_CURVE_HYPERBOLIC:
 370             tpaCurveHyperbolicInit(pidProfile);
 371             return;
 372         case TPA_CURVE_CLASSIC:
 373         default:
 374             return;
 375         }
 376 }
 377 #endif // USE_ADVANCED_TPA
 378
 379 void pidInit(const pidProfile_t *pidProfile)
 380 {
 381     pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
 382     pidInitFilters(pidProfile);
 383     pidInitConfig(pidProfile);
 384 #ifdef USE_RPM_FILTER
 385     rpmFilterInit(rpmFilterConfig(), gyro.targetLooptime);
 386 #endif
 387 #ifdef USE_ADVANCED_TPA
 388     tpaCurveInit(pidProfile);
 389 #endif
 390 }
 391
 392 void pidInitConfig(const pidProfile_t *pidProfile)
 393 {
 394     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 395         pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
 396         pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
 397         pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
 398         pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F * 0.01f);
 399     }
 400 #ifdef USE_INTEGRATED_YAW_CONTROL
 401     if (!pidProfile->use_integrated_yaw)
 402 #endif
 403     {
 404         pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
 405     }
 406     pidRuntime.angleGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
 407     pidRuntime.angleFeedforwardGain = pidProfile->pid[PID_LEVEL].F / 100.0f;
 408 #ifdef USE_ACC
 409     pidRuntime.angleEarthRef = pidProfile->angle_earth_ref / 100.0f;
 410 #endif
 411     pidRuntime.horizonGain = MIN(pidProfile->pid[PID_LEVEL].I / 100.0f, 1.0f);
 412     pidRuntime.horizonIgnoreSticks = (pidProfile->horizon_ignore_sticks) ? 1.0f : 0.0f;
 413
 414     pidRuntime.horizonLimitSticks = pidProfile->pid[PID_LEVEL].D / 100.0f;
 415     pidRuntime.horizonLimitSticksInv = (pidProfile->pid[PID_LEVEL].D) ? 1.0f / pidRuntime.horizonLimitSticks : 1.0f;
 416     pidRuntime.horizonLimitDegrees = (float)pidProfile->horizon_limit_degrees;
 417     pidRuntime.horizonLimitDegreesInv = (pidProfile->horizon_limit_degrees) ? 1.0f / pidRuntime.horizonLimitDegrees : 1.0f;
 418 #ifdef USE_ACC
 419     pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
 420 #endif
 421
 422 #ifdef USE_CHIRP
 423     pidRuntime.chirpLagFreqHz = pidProfile->chirp_lag_freq_hz;
 424     pidRuntime.chirpLeadFreqHz = pidProfile->chirp_lead_freq_hz;
 425     pidRuntime.chirpAmplitude[FD_ROLL] = pidProfile->chirp_amplitude_roll;
 426     pidRuntime.chirpAmplitude[FD_PITCH] = pidProfile->chirp_amplitude_pitch;
 427     pidRuntime.chirpAmplitude[FD_YAW]= pidProfile->chirp_amplitude_yaw;
 428     pidRuntime.chirpFrequencyStartHz = pidProfile->chirp_frequency_start_deci_hz / 10.0f;
 429     pidRuntime.chirpFrequencyEndHz = pidProfile->chirp_frequency_end_deci_hz / 10.0f;
 430     pidRuntime.chirpTimeSeconds = pidProfile->chirp_time_seconds;
 431 #endif
 432
 433     pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
 434     pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
 435     pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain;
 436     pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
 437     pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
 438     pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
 439     pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
 440     pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold; // error in deg/s
 441     pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold * 1000.0f; // gyro delta in deg/s/s * 1000 to match original 2017 intent
 442     pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
 443     pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
 444
 445     pidRuntime.itermLimit = 0.01f * pidProfile->itermWindup * pidProfile->pidSumLimit;
 446     pidRuntime.itermLimitYaw = 0.01f * pidProfile->itermWindup * pidProfile->pidSumLimitYaw;
 447
 448 #if defined(USE_THROTTLE_BOOST)
 449     throttleBoost = pidProfile->throttle_boost * 0.1f;
 450 #endif
 451     pidRuntime.itermRotation = pidProfile->iterm_rotation;
 452
 453     // Calculate the anti-gravity value that will trigger the OSD display when its strength exceeds 25% of max.
 454     // This gives a useful indication of AG activity without excessive display.
 455     pidRuntime.antiGravityOsdCutoff = (pidRuntime.antiGravityGain / 10.0f) * 0.25f;
 456     pidRuntime.antiGravityPGain = ((float)(pidProfile->anti_gravity_p_gain) / 100.0f) * ANTIGRAVITY_KP;
 457
 458 #if defined(USE_ITERM_RELAX)
 459     pidRuntime.itermRelax = pidProfile->iterm_relax;
 460     pidRuntime.itermRelaxType = pidProfile->iterm_relax_type;
 461     pidRuntime.itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
 462 #endif
 463
 464 #ifdef USE_ACRO_TRAINER
 465     pidRuntime.acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
 466     pidRuntime.acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
 467     pidRuntime.acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
 468     pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
 469 #endif // USE_ACRO_TRAINER
 470
 471 #if defined(USE_ABSOLUTE_CONTROL)
 472     pidRuntime.acGain = (float)pidProfile->abs_control_gain;
 473     pidRuntime.acLimit = (float)pidProfile->abs_control_limit;
 474     pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit;
 475     pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff;
 476     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 477         float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp;
 478         pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection);
 479     }
 480 #endif
 481
 482 #ifdef USE_DYN_LPF
 483     if (pidProfile->dterm_lpf1_dyn_min_hz > 0) {
 484         switch (pidProfile->dterm_lpf1_type) {
 485         case FILTER_PT1:
 486             pidRuntime.dynLpfFilter = DYN_LPF_PT1;
 487             break;
 488         case FILTER_BIQUAD:
 489             pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
 490             break;
 491         case FILTER_PT2:
 492             pidRuntime.dynLpfFilter = DYN_LPF_PT2;
 493             break;
 494         case FILTER_PT3:
 495             pidRuntime.dynLpfFilter = DYN_LPF_PT3;
 496             break;
 497         default:
 498             pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 499             break;
 500         }
 501     } else {
 502         pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 503     }
 504     pidRuntime.dynLpfMin = pidProfile->dterm_lpf1_dyn_min_hz;
 505     pidRuntime.dynLpfMax = pidProfile->dterm_lpf1_dyn_max_hz;
 506     pidRuntime.dynLpfCurveExpo = pidProfile->dterm_lpf1_dyn_expo;
 507 #endif
 508
 509 #ifdef USE_LAUNCH_CONTROL
 510     pidRuntime.launchControlMode = pidProfile->launchControlMode;
 511     if (sensors(SENSOR_ACC)) {
 512         pidRuntime.launchControlAngleLimit = pidProfile->launchControlAngleLimit;
 513     } else {
 514         pidRuntime.launchControlAngleLimit = 0;
 515     }
 516     pidRuntime.launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
 517 #endif
 518
 519 #ifdef USE_INTEGRATED_YAW_CONTROL
 520     pidRuntime.useIntegratedYaw = pidProfile->use_integrated_yaw;
 521     pidRuntime.integratedYawRelax = pidProfile->integrated_yaw_relax;
 522 #endif
 523
 524 #ifdef USE_THRUST_LINEARIZATION
 525     pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
 526     pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * sq(pidRuntime.thrustLinearization);
 527 #endif
 528
 529 #ifdef USE_D_MAX
 530     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 531         const uint8_t dMax = pidProfile->d_max[axis];
 532         if ((pidProfile->pid[axis].D > 0) && dMax > pidProfile->pid[axis].D) {
 533             pidRuntime.dMaxPercent[axis] = (float) dMax / pidProfile->pid[axis].D;
 534             // fraction that Dmax is higher than D, eg if D is 8 and Dmax is 10, Dmax is 1.25 times bigger
 535         } else {
 536             pidRuntime.dMaxPercent[axis] = 1.0f;
 537         }
 538     }
 539     pidRuntime.dMaxGyroGain = D_MAX_GAIN_FACTOR * pidProfile->d_max_gain / D_MAX_LOWPASS_HZ;
 540     pidRuntime.dMaxSetpointGain = D_MAX_SETPOINT_GAIN_FACTOR * pidProfile->d_max_gain * pidProfile->d_max_advance / 100.0f / D_MAX_LOWPASS_HZ;
 541     // lowpass included inversely in gain since stronger lowpass decreases peak effect
 542 #endif
 543
 544 #if defined(USE_AIRMODE_LPF)
 545     pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
 546 #endif
 547
 548 #ifdef USE_FEEDFORWARD
 549     pidRuntime.feedforwardTransition = pidProfile->feedforward_transition / 100.0f;
 550     pidRuntime.feedforwardTransitionInv = (pidProfile->feedforward_transition == 0) ? 0.0f : 100.0f / pidProfile->feedforward_transition;
 551     pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
 552     pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor);
 553     pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
 554     pidRuntime.feedforwardJitterFactorInv = 1.0f / (1.0f + pidProfile->feedforward_jitter_factor);
 555     pidRuntime.feedforwardBoostFactor = 0.001f * pidProfile->feedforward_boost;
 556     pidRuntime.feedforwardMaxRateLimit = pidProfile->feedforward_max_rate_limit;
 557     pidRuntime.feedforwardInterpolate = !(rxRuntimeState.serialrxProvider == SERIALRX_CRSF);
 558     pidRuntime.feedforwardYawHoldTime = 0.001f * pidProfile->feedforward_yaw_hold_time; // input time constant in milliseconds, converted to seconds
 559     pidRuntime.feedforwardYawHoldGain = pidProfile->feedforward_yaw_hold_gain;
 560     // normalise/maintain boost when time constant is small, 1.5x at 50ms, 2x at 25ms, almost 3x at 10ms
 561     if (pidProfile->feedforward_yaw_hold_time < 100) {
 562         pidRuntime.feedforwardYawHoldGain *= 150.0f / (float)(pidProfile->feedforward_yaw_hold_time + 50);
 563     }
 564 #endif
 565
 566     pidRuntime.levelRaceMode = pidProfile->level_race_mode;
 567     pidRuntime.tpaBreakpoint = constrainf((pidProfile->tpa_breakpoint - PWM_RANGE_MIN) / 1000.0f, 0.0f, 0.99f);
 568     // default of 1350 returns 0.35. range limited to 0 to 0.99
 569     pidRuntime.tpaMultiplier = (pidProfile->tpa_rate / 100.0f) / (1.0f - pidRuntime.tpaBreakpoint);
 570     // it is assumed that tpaLowBreakpoint is always less than or equal to tpaBreakpoint
 571     pidRuntime.tpaLowBreakpoint = constrainf((pidProfile->tpa_low_breakpoint - PWM_RANGE_MIN) / 1000.0f, 0.01f, 1.0f);
 572     pidRuntime.tpaLowBreakpoint = MIN(pidRuntime.tpaLowBreakpoint, pidRuntime.tpaBreakpoint);
 573     pidRuntime.tpaLowMultiplier = pidProfile->tpa_low_rate / (100.0f * pidRuntime.tpaLowBreakpoint);
 574     pidRuntime.tpaLowAlways = pidProfile->tpa_low_always;
 575
 576     pidRuntime.useEzDisarm = pidProfile->landing_disarm_threshold > 0;
 577     pidRuntime.landingDisarmThreshold = pidProfile->landing_disarm_threshold * 10.0f;
 578
 579 #ifdef USE_WING
 580     tpaSpeedInit(pidProfile);
 581 #endif
 582 }
 583
 584 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 585 {
 586     if (dstPidProfileIndex < PID_PROFILE_COUNT && srcPidProfileIndex < PID_PROFILE_COUNT
 587         && dstPidProfileIndex != srcPidProfileIndex) {
 588         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
 589     }
 590 }