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 80    // Biquad lowpass input cutoff to peak D around propwash frequencies
  50 #define D_MIN_LOWPASS_HZ 10  // PT1 lowpass cutoff to smooth the boost effect
  51 #define D_MIN_GAIN_FACTOR 0.00005f
  52 #define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
  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_lowpass_hz = pidProfile->dterm_lowpass_hz;
 106
 107 #ifdef USE_DYN_LPF
 108     if (pidProfile->dyn_lpf_dterm_min_hz) {
 109         dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
 110     }
 111 #endif
 112
 113     if (dterm_lowpass_hz > 0) {
 114         switch (pidProfile->dterm_filter_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_lowpass_hz, pidRuntime.dT));
 119             }
 120             break;
 121         case FILTER_BIQUAD:
 122             if (pidProfile->dterm_lowpass_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_lowpass_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_lowpass_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_lowpass_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_lowpass2_hz > 0) {
 157         switch (pidProfile->dterm_filter2_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_lowpass2_hz, pidRuntime.dT));
 162             }
 163             break;
 164         case FILTER_BIQUAD:
 165             if (pidProfile->dterm_lowpass2_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_lowpass2_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_lowpass2_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_lowpass2_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         biquadFilterInitLPF(&pidRuntime.dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
 228         pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(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     pidRuntime.ffBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
 243 }
 244
 245 void pidInit(const pidProfile_t *pidProfile)
 246 {
 247     pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
 248     pidInitFilters(pidProfile);
 249     pidInitConfig(pidProfile);
 250 #ifdef USE_RPM_FILTER
 251     rpmFilterInit(rpmFilterConfig());
 252 #endif
 253 }
 254
 255 #ifdef USE_RC_SMOOTHING_FILTER
 256 void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
 257 {
 258     pidRuntime.rcSmoothingDebugAxis = debugAxis;
 259     if (filterCutoff > 0) {
 260         pidRuntime.feedforwardLpfInitialized = true;
 261         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 262             pt3FilterInit(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
 263         }
 264     }
 265 }
 266
 267 void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
 268 {
 269     if (filterCutoff > 0) {
 270         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 271             pt3FilterUpdateCutoff(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
 272         }
 273     }
 274 }
 275 #endif // USE_RC_SMOOTHING_FILTER
 276
 277 void pidInitConfig(const pidProfile_t *pidProfile)
 278 {
 279     if (pidProfile->feedforwardTransition == 0) {
 280         pidRuntime.feedforwardTransition = 0;
 281     } else {
 282         pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition;
 283     }
 284     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 285         pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
 286         pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
 287         pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
 288         pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
 289     }
 290 #ifdef USE_INTEGRATED_YAW_CONTROL
 291     if (!pidProfile->use_integrated_yaw)
 292 #endif
 293     {
 294         pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
 295     }
 296     pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
 297     pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
 298     pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
 299     pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
 300     pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
 301     pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
 302     pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
 303     pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
 304     pidRuntime.itermWindupPointInv = 1.0f;
 305     if (pidProfile->itermWindupPointPercent < 100) {
 306         const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
 307         pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
 308     }
 309     pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
 310     pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
 311     pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
 312     pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
 313     pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
 314     pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold;
 315     pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold;
 316     pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
 317     pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
 318     pidRuntime.itermLimit = pidProfile->itermLimit;
 319 #if defined(USE_THROTTLE_BOOST)
 320     throttleBoost = pidProfile->throttle_boost * 0.1f;
 321 #endif
 322     pidRuntime.itermRotation = pidProfile->iterm_rotation;
 323     pidRuntime.antiGravityMode = pidProfile->antiGravityMode;
 324
 325     // Calculate the anti-gravity value that will trigger the OSD display.
 326     // For classic AG it's either 1.0 for off and > 1.0 for on.
 327     // For the new AG it's a continuous floating value so we want to trigger the OSD
 328     // display when it exceeds 25% of its possible range. This gives a useful indication
 329     // of AG activity without excessive display.
 330     pidRuntime.antiGravityOsdCutoff = 0.0f;
 331     if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
 332         pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f;
 333     }
 334
 335 #if defined(USE_ITERM_RELAX)
 336     pidRuntime.itermRelax = pidProfile->iterm_relax;
 337     pidRuntime.itermRelaxType = pidProfile->iterm_relax_type;
 338     pidRuntime.itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
 339 #endif
 340
 341 #ifdef USE_ACRO_TRAINER
 342     pidRuntime.acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
 343     pidRuntime.acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
 344     pidRuntime.acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
 345     pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
 346 #endif // USE_ACRO_TRAINER
 347
 348 #if defined(USE_ABSOLUTE_CONTROL)
 349     pidRuntime.acGain = (float)pidProfile->abs_control_gain;
 350     pidRuntime.acLimit = (float)pidProfile->abs_control_limit;
 351     pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit;
 352     pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff;
 353     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 354         float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp;
 355         pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection);
 356     }
 357 #endif
 358
 359 #ifdef USE_DYN_LPF
 360     if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
 361         switch (pidProfile->dterm_filter_type) {
 362         case FILTER_PT1:
 363             pidRuntime.dynLpfFilter = DYN_LPF_PT1;
 364             break;
 365         case FILTER_BIQUAD:
 366             pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
 367             break;
 368         case FILTER_PT2:
 369             pidRuntime.dynLpfFilter = DYN_LPF_PT2;
 370             break;
 371         case FILTER_PT3:
 372             pidRuntime.dynLpfFilter = DYN_LPF_PT3;
 373             break;
 374         default:
 375             pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 376             break;
 377         }
 378     } else {
 379         pidRuntime.dynLpfFilter = DYN_LPF_NONE;
 380     }
 381     pidRuntime.dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
 382     pidRuntime.dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
 383     pidRuntime.dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
 384 #endif
 385
 386 #ifdef USE_LAUNCH_CONTROL
 387     pidRuntime.launchControlMode = pidProfile->launchControlMode;
 388     if (sensors(SENSOR_ACC)) {
 389         pidRuntime.launchControlAngleLimit = pidProfile->launchControlAngleLimit;
 390     } else {
 391         pidRuntime.launchControlAngleLimit = 0;
 392     }
 393     pidRuntime.launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
 394 #endif
 395
 396 #ifdef USE_INTEGRATED_YAW_CONTROL
 397     pidRuntime.useIntegratedYaw = pidProfile->use_integrated_yaw;
 398     pidRuntime.integratedYawRelax = pidProfile->integrated_yaw_relax;
 399 #endif
 400
 401 #ifdef USE_THRUST_LINEARIZATION
 402     pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
 403     pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
 404 #endif
 405
 406 #if defined(USE_D_MIN)
 407     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 408         const uint8_t dMin = pidProfile->d_min[axis];
 409         if ((dMin > 0) && (dMin < pidProfile->pid[axis].D)) {
 410             pidRuntime.dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
 411         } else {
 412             pidRuntime.dMinPercent[axis] = 0;
 413         }
 414     }
 415     pidRuntime.dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
 416     pidRuntime.dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidRuntime.pidFrequency / (100 * D_MIN_LOWPASS_HZ);
 417     // lowpass included inversely in gain since stronger lowpass decreases peak effect
 418 #endif
 419
 420 #if defined(USE_AIRMODE_LPF)
 421     pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
 422 #endif
 423
 424 #ifdef USE_FEEDFORWARD
 425     pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
 426     if (pidProfile->feedforward_smooth_factor) {
 427         pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
 428     } else {
 429         // set automatically according to boost amount, limit to 0.5 for auto
 430         pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->feedforward_boost) * 2.0f / 100.0f);
 431     }
 432     pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor;
 433     feedforwardInit(pidProfile);
 434 #endif
 435
 436     pidRuntime.levelRaceMode = pidProfile->level_race_mode;
 437 }
 438
 439 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 440 {
 441     if (dstPidProfileIndex < PID_PROFILE_COUNT && srcPidProfileIndex < PID_PROFILE_COUNT
 442         && dstPidProfileIndex != srcPidProfileIndex) {
 443         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
 444     }
 445 }
 446