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
  39 #include "flight/feedforward.h"
  40 #include "flight/pid.h"
  41 #include "flight/rpm_filter.h"
  42
  43 #include "sensors/gyro.h"
  44 #include "sensors/sensors.h"
  45
  46 #include "pid_init.h"
  47
  48 #if defined(USE_D_MIN)
  49 #define D_MIN_RANGE_HZ 85    // PT2 lowpass input cutoff to peak D around propwash frequencies
  50 #define D_MIN_LOWPASS_HZ 35  // PT2 lowpass cutoff to smooth the boost effect
  51 #define D_MIN_GAIN_FACTOR 0.00008f
  52 #define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f
  53 #endif
  54
  55 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
  56 #define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3  // The anti gravity P smoothing filter cutoff
  57
  58 static void pidSetTargetLooptime(uint32_t pidLooptime)
  59 {
  60     targetPidLooptime = pidLooptime;
  61     pidRuntime.dT = targetPidLooptime * 1e-6f;
  62     pidRuntime.pidFrequency = 1.0f / pidRuntime.dT;
  63 #ifdef USE_DSHOT
  64     dshotSetPidLoopTime(targetPidLooptime);
  65 #endif
  66 }
  67
  68 void pidInitFilters(const pidProfile_t *pidProfile)
  69 {
  70     STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
  71
  72     if (targetPidLooptime == 0) {
  73         // no looptime set, so set all the filters to null
  74         pidRuntime.dtermNotchApplyFn = nullFilterApply;
  75         pidRuntime.dtermLowpassApplyFn = nullFilterApply;
  76         pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
  77         pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
  78         return;
  79     }
  80
  81     const uint32_t pidFrequencyNyquist = pidRuntime.pidFrequency / 2; // No rounding needed
  82
  83     uint16_t dTermNotchHz;
  84     if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
  85         dTermNotchHz = pidProfile->dterm_notch_hz;
  86     } else {
  87         if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
  88             dTermNotchHz = pidFrequencyNyquist;
  89         } else {
  90             dTermNotchHz = 0;
  91         }
  92     }
  93
  94     if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
  95         pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
  96         const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
  97         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
  98             biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH, 1.0f);
  99         }
 100     } else {
 101         pidRuntime.dtermNotchApplyFn = nullFilterApply;
 102     }
 103
 104     //1st Dterm Lowpass Filter
 105     uint16_t dterm_lpf1_init_hz = pidProfile->dterm_lpf1_static_hz;
 106
 107 #ifdef USE_DYN_LPF
 108     if (pidProfile->dterm_lpf1_dyn_min_hz) {
 109         dterm_lpf1_init_hz = pidProfile->dterm_lpf1_dyn_min_hz;
 110     }
 111 #endif
 112
 113     if (dterm_lpf1_init_hz > 0) {
 114         switch (pidProfile->dterm_lpf1_type) {
 115         case FILTER_PT1:
 116             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 117             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 118                 pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 119             }
 120             break;
 121         case FILTER_BIQUAD:
 122             if (pidProfile->dterm_lpf1_static_hz < pidFrequencyNyquist) {
 123 #ifdef USE_DYN_LPF
 124                 pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
 125 #else
 126                 pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
 127 #endif
 128                 for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 129                     biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lpf1_init_hz, targetPidLooptime);
 130                 }
 131             } else {
 132                 pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 133             }
 134             break;
 135         case FILTER_PT2:
 136             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
 137             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 138                 pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 139             }
 140             break;
 141         case FILTER_PT3:
 142             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
 143             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 144                 pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
 145             }
 146             break;
 147         default:
 148             pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 149             break;
 150         }
 151     } else {
 152         pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 153     }
 154
 155     //2nd Dterm Lowpass Filter
 156     if (pidProfile->dterm_lpf2_static_hz > 0) {
 157         switch (pidProfile->dterm_lpf2_type) {
 158         case FILTER_PT1:
 159             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
 160             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 161                 pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 162             }
 163             break;
 164         case FILTER_BIQUAD:
 165             if (pidProfile->dterm_lpf2_static_hz < pidFrequencyNyquist) {
 166                 pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 167                 for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 168                     biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lpf2_static_hz, targetPidLooptime);
 169                 }
 170             } else {
 171                 pidRuntime.dtermLowpassApplyFn = nullFilterApply;
 172             }
 173             break;
 174         case FILTER_PT2:
 175             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
 176             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 177                 pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 178             }
 179             break;
 180         case FILTER_PT3:
 181             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
 182             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 183                 pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
 184             }
 185             break;
 186         default:
 187             pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
 188             break;
 189         }
 190     } else {
 191         pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
 192     }
 193
 194     if (pidProfile->yaw_lowpass_hz == 0) {
 195         pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
 196     } else {
 197         pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
 198         pt1FilterInit(&pidRuntime.ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, pidRuntime.dT));
 199     }
 200
 201 #if defined(USE_THROTTLE_BOOST)
 202     pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, pidRuntime.dT));
 203 #endif
 204
 205 #if defined(USE_ITERM_RELAX)
 206     if (pidRuntime.itermRelax) {
 207         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 208             pt1FilterInit(&pidRuntime.windupLpf[i], pt1FilterGain(pidRuntime.itermRelaxCutoff, pidRuntime.dT));
 209         }
 210     }
 211 #endif
 212
 213 #if defined(USE_ABSOLUTE_CONTROL)
 214     if (pidRuntime.itermRelax) {
 215         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 216             pt1FilterInit(&pidRuntime.acLpf[i], pt1FilterGain(pidRuntime.acCutoff, pidRuntime.dT));
 217         }
 218     }
 219 #endif
 220
 221 #if defined(USE_D_MIN)
 222     // Initialize the filters for all axis even if the d_min[axis] value is 0
 223     // Otherwise if the pidProfile->d_min_xxx parameters are ever added to
 224     // in-flight adjustments and transition from 0 to > 0 in flight the feature
 225     // won't work because the filter wasn't initialized.
 226     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 227         pt2FilterInit(&pidRuntime.dMinRange[axis], pt2FilterGain(D_MIN_RANGE_HZ, pidRuntime.dT));
 228         pt2FilterInit(&pidRuntime.dMinLowpass[axis], pt2FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT));
 229      }
 230 #endif
 231
 232 #if defined(USE_AIRMODE_LPF)
 233     if (pidProfile->transient_throttle_limit) {
 234         pt1FilterInit(&pidRuntime.airmodeThrottleLpf1, pt1FilterGain(7.0f, pidRuntime.dT));
 235         pt1FilterInit(&pidRuntime.airmodeThrottleLpf2, pt1FilterGain(20.0f, pidRuntime.dT));
 236     }
 237 #endif
 238
 239     pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
 240     pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
 241 }
 242
 243 void pidInit(const pidProfile_t *pidProfile)
 244 {
 245     pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
 246     pidInitFilters(pidProfile);
 247     pidInitConfig(pidProfile);
 248 #ifdef USE_RPM_FILTER
 249     rpmFilterInit(rpmFilterConfig());
 250 #endif
 251 }
 252
 253 #ifdef USE_RC_SMOOTHING_FILTER
 254 void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
 255 {
 256     pidRuntime.rcSmoothingDebugAxis = debugAxis;
 257     if (filterCutoff > 0) {
 258         pidRuntime.feedforwardLpfInitialized = true;
 259         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 260             pt3FilterInit(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
 261         }
 262     }
 263 }
 264
 265 void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
 266 {
 267     if (filterCutoff > 0) {
 268         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 269             pt3FilterUpdateCutoff(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
 270         }
 271     }
 272 }
 273 #endif // USE_RC_SMOOTHING_FILTER
 274
 275 void pidInitConfig(const pidProfile_t *pidProfile)
 276 {
 277     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 278         pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
 279         pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
 280         pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
 281         pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
 282     }
 283 #ifdef USE_INTEGRATED_YAW_CONTROL
 284     if (!pidProfile->use_integrated_yaw)
 285 #endif
 286     {
 287         pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
 288     }
 289     pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
 290     pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
 291     pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
 292     pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
 293     pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
 294     pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
 295     pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
 296     pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
 297     pidRuntime.itermWindupPointInv = 1.0f;
 298     if (pidProfile->itermWindupPointPercent < 100) {
 299         const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
 300         pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
 301     }
 302     pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
 303     pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
 304     pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
 305     pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
 306     pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
 307     pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold;
 308     pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold;
 309     pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
 310     pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
 311     pidRuntime.itermLimit = pidProfile->itermLimit;
 312 #if defined(USE_THROTTLE_BOOST)
 313     throttleBoost = pidProfile->throttle_boost * 0.1f;
 314 #endif
 315     pidRuntime.itermRotation = pidProfile->iterm_rotation;
 316     pidRuntime.antiGravityMode = pidProfile->antiGravityMode;
 317
 318     // Calculate the anti-gravity value that will trigger the OSD display.
 319     // For classic AG it's either 1.0 for off and > 1.0 for on.
 320     // For the new AG it's a continuous floating value so we want to trigger the OSD
 321     // display when it exceeds 25% of its possible range. This gives a useful indication
 322     // of AG activity without excessive display.
 323     pidRuntime.antiGravityOsdCutoff = 0.0f;
 324     if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
 325         pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f;
 326     }
 327
 328 #if defined(USE_ITERM_RELAX)
 329     pidRuntime.itermRelax = pidProfile->iterm_relax;
 330     pidRuntime.itermRelaxType = pidProfile->iterm_relax_type;
 331     pidRuntime.itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
 332 #endif
 333
 334 #ifdef USE_ACRO_TRAINER
 335     pidRuntime.acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
 336     pidRuntime.acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
 337     pidRuntime.acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
 338     pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
 339 #endif // USE_ACRO_TRAINER
 340
 341 #if defined(USE_ABSOLUTE_CONTROL)
 342     pidRuntime.acGain = (float)pidProfile->abs_control_gain;
 343     pidRuntime.acLimit = (float)pidProfile->abs_control_limit;
 344     pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit;
 345     pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff;
 346     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 347         float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp;
 348         pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection);
 349     }
 350 #endif
 351
 352 #ifdef USE_DYN_LPF
 353     if (pidProfile->dterm_lpf1_dyn_min_hz > 0) {
 354         switch (pidProfile->dterm_lpf1_type) {
 355         case FILTER_PT1:
 356             pidRuntime.dynLpfFilter = DYN_LPF_PT1;
 357             break;
 358         case FILTER_BIQUAD:
 359             pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
 360             break;
 361         case FILTER_PT2:
 362             pidRuntime.dynLpfFilter = DYN_LPF_PT2;
 363             break;
 364         case FILTER_PT3:
 365             pidRuntime.dynLpfFilter = DYN_LPF_PT3;
 366             break;
 367         default:
 368             pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 369             break;
 370         }
 371     } else {
 372         pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 373     }
 374     pidRuntime.dynLpfMin = pidProfile->dterm_lpf1_dyn_min_hz;
 375     pidRuntime.dynLpfMax = pidProfile->dterm_lpf1_dyn_max_hz;
 376     pidRuntime.dynLpfCurveExpo = pidProfile->dterm_lpf1_dyn_expo;
 377 #endif
 378
 379 #ifdef USE_LAUNCH_CONTROL
 380     pidRuntime.launchControlMode = pidProfile->launchControlMode;
 381     if (sensors(SENSOR_ACC)) {
 382         pidRuntime.launchControlAngleLimit = pidProfile->launchControlAngleLimit;
 383     } else {
 384         pidRuntime.launchControlAngleLimit = 0;
 385     }
 386     pidRuntime.launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
 387 #endif
 388
 389 #ifdef USE_INTEGRATED_YAW_CONTROL
 390     pidRuntime.useIntegratedYaw = pidProfile->use_integrated_yaw;
 391     pidRuntime.integratedYawRelax = pidProfile->integrated_yaw_relax;
 392 #endif
 393
 394 #ifdef USE_THRUST_LINEARIZATION
 395     pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
 396     pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
 397 #endif
 398
 399 #if defined(USE_D_MIN)
 400     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 401         const uint8_t dMin = pidProfile->d_min[axis];
 402         if ((dMin > 0) && (dMin < pidProfile->pid[axis].D)) {
 403             pidRuntime.dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
 404         } else {
 405             pidRuntime.dMinPercent[axis] = 0;
 406         }
 407     }
 408     pidRuntime.dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
 409     pidRuntime.dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidRuntime.pidFrequency / (100 * D_MIN_LOWPASS_HZ);
 410     // lowpass included inversely in gain since stronger lowpass decreases peak effect
 411 #endif
 412
 413 #if defined(USE_AIRMODE_LPF)
 414     pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
 415 #endif
 416
 417 #ifdef USE_FEEDFORWARD
 418     if (pidProfile->feedforward_transition == 0) {
 419         pidRuntime.feedforwardTransitionFactor = 0;
 420     } else {
 421         pidRuntime.feedforwardTransitionFactor = 100.0f / pidProfile->feedforward_transition;
 422     }
 423     pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
 424     pidRuntime.feedforwardSmoothFactor = 1.0f;
 425     if (pidProfile->feedforward_smooth_factor) {
 426         pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
 427     }
 428     pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
 429     pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
 430     feedforwardInit(pidProfile);
 431 #endif
 432
 433     pidRuntime.levelRaceMode = pidProfile->level_race_mode;
 434 }
 435
 436 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 437 {
 438     if (dstPidProfileIndex < PID_PROFILE_COUNT && srcPidProfileIndex < PID_PROFILE_COUNT
 439         && dstPidProfileIndex != srcPidProfileIndex) {
 440         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
 441     }
 442 }
 443