Merge pull request #9759 from etracer65/pid_init_separate

Split initialization from pid.c for flash savings

make/source.mk

@@ -97,6 +97,7 @@ COMMON_SRC = \
             flight/mixer.c \
             flight/mixer_tricopter.c \
             flight/pid.c \
+            flight/pid_init.c \
             flight/rpm_filter.c \
             flight/servos.c \
             flight/servos_tricopter.c \
@@ -346,7 +347,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             osd/osd.c \
             osd/osd_elements.c \
             rx/rx_bind.c \
-            sensors/gyro_init.c
+            sensors/gyro_init.c\
+            flight/pid_init.c
 
 # Gyro driver files that only contain initialization and configuration code - not runtime code
 SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \

src/main/cms/cms_menu_imu.c

@@ -51,6 +51,7 @@
 
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pid_init.h"
 
 #include "pg/pg.h"
 

src/main/config/config.c

@@ -51,6 +51,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pid_init.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 

src/main/fc/init.c

@@ -38,6 +38,7 @@
 #include "common/maths.h"
 #include "common/printf_serial.h"
 
+#include "config/config.h"
 #include "config/config_eeprom.h"
 #include "config/feature.h"
 
@@ -83,7 +84,6 @@
 #include "drivers/vtx_table.h"
 
 #include "fc/board_info.h"
-#include "config/config.h"
 #include "fc/dispatch.h"
 #include "fc/init.h"
 #include "fc/rc_controls.h"
@@ -95,6 +95,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pid_init.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 

src/main/fc/rc.c

@@ -44,7 +44,7 @@
 #include "flight/imu.h"
 #include "flight/interpolated_setpoint.h"
 #include "flight/gps_rescue.h"
-#include "flight/pid.h"
+#include "flight/pid_init.h"
 
 #include "pg/rx.h"
 

src/main/fc/rc_adjustments.c

@@ -45,6 +45,7 @@
 #include "fc/rc.h"
 
 #include "flight/pid.h"
+#include "flight/pid_init.h"
 
 #include "io/beeper.h"
 #include "io/ledstrip.h"

src/main/flight/interpolated_setpoint.c

@@ -24,9 +24,14 @@
 #ifdef USE_INTERPOLATED_SP
 
 #include "build/debug.h"
+
 #include "common/maths.h"
+
 #include "fc/rc.h"
-#include "flight/interpolated_setpoint.h"
+
+#include "flight/pid.h"
+
+#include "interpolated_setpoint.h"
 
 static float setpointDeltaImpl[XYZ_AXIS_COUNT];
 static float setpointDelta[XYZ_AXIS_COUNT];

src/main/flight/interpolated_setpoint.h

@@ -25,14 +25,6 @@
 #include "common/axis.h"
 #include "flight/pid.h"
 
-typedef enum ffInterpolationType_e {
-    FF_INTERPOLATE_OFF,
-    FF_INTERPOLATE_ON,
-    FF_INTERPOLATE_AVG2,
-    FF_INTERPOLATE_AVG3,
-    FF_INTERPOLATE_AVG4
-} ffInterpolationType_t;
-
 void interpolatedSpInit(const pidProfile_t *pidProfile);
 float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type);
 float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint);

src/main/flight/pid.h

@@ -24,6 +24,7 @@
 #include "common/time.h"
 #include "common/filter.h"
 #include "common/axis.h"
+
 #include "pg/pg.h"
 
 #define MAX_PID_PROCESS_DENOM       16
@@ -107,6 +108,14 @@ typedef enum {
     ITERM_RELAX_TYPE_COUNT,
 } itermRelaxType_e;
 
+typedef enum ffInterpolationType_e {
+    FF_INTERPOLATE_OFF,
+    FF_INTERPOLATE_ON,
+    FF_INTERPOLATE_AVG2,
+    FF_INTERPOLATE_AVG3,
+    FF_INTERPOLATE_AVG4
+} ffInterpolationType_t;
+
 #define MAX_PROFILE_NAME_LENGTH 8u
 
 typedef struct pidProfile_s {
@@ -218,6 +227,147 @@ typedef struct pidAxisData_s {
     float Sum;
 } pidAxisData_t;
 
+typedef union dtermLowpass_u {
+    pt1Filter_t pt1Filter;
+    biquadFilter_t biquadFilter;
+} dtermLowpass_t;
+
+typedef struct pidCoefficient_s {
+    float Kp;
+    float Ki;
+    float Kd;
+    float Kf;
+} pidCoefficient_t;
+
+typedef struct pidRuntime_s {
+    float dT;
+    float pidFrequency;
+    bool pidStabilisationEnabled;
+    float previousPidSetpoint[XYZ_AXIS_COUNT];
+    filterApplyFnPtr dtermNotchApplyFn;
+    biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
+    filterApplyFnPtr dtermLowpassApplyFn;
+    dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
+    filterApplyFnPtr dtermLowpass2ApplyFn;
+    dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
+    filterApplyFnPtr ptermYawLowpassApplyFn;
+    pt1Filter_t ptermYawLowpass;
+    bool antiGravityEnabled;
+    uint8_t antiGravityMode;
+    pt1Filter_t antiGravityThrottleLpf;
+    float antiGravityOsdCutoff;
+    float antiGravityThrottleHpf;
+    float ffBoostFactor;
+    float ffSpikeLimitInverse;
+    float itermAccelerator;
+    uint16_t itermAcceleratorGain;
+    float feedForwardTransition;
+    pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
+    float levelGain;
+    float horizonGain;
+    float horizonTransition;
+    float horizonCutoffDegrees;
+    float horizonFactorRatio;
+    uint8_t horizonTiltExpertMode;
+    float maxVelocity[XYZ_AXIS_COUNT];
+    float itermWindupPointInv;
+    bool inCrashRecoveryMode;
+    timeUs_t crashDetectedAtUs;
+    timeDelta_t crashTimeLimitUs;
+    timeDelta_t crashTimeDelayUs;
+    int32_t crashRecoveryAngleDeciDegrees;
+    float crashRecoveryRate;
+    float crashGyroThreshold;
+    float crashDtermThreshold;
+    float crashSetpointThreshold;
+    float crashLimitYaw;
+    float itermLimit;
+    bool itermRotation;
+    bool zeroThrottleItermReset;
+    bool levelRaceMode;
+
+#ifdef USE_ITERM_RELAX
+    pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
+    uint8_t itermRelax;
+    uint8_t itermRelaxType;
+    uint8_t itermRelaxCutoff;
+#endif
+
+#ifdef USE_ABSOLUTE_CONTROL
+    float acCutoff;
+    float acGain;
+    float acLimit;
+    float acErrorLimit;
+    pt1Filter_t acLpf[XYZ_AXIS_COUNT];
+    float oldSetpointCorrection[XYZ_AXIS_COUNT];
+#endif
+
+#ifdef USE_D_MIN
+    biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
+    pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
+    float dMinPercent[XYZ_AXIS_COUNT];
+    float dMinGyroGain;
+    float dMinSetpointGain;
+#endif
+
+#ifdef USE_AIRMODE_LPF
+    pt1Filter_t airmodeThrottleLpf1;
+    pt1Filter_t airmodeThrottleLpf2;
+#endif
+
+#ifdef USE_RC_SMOOTHING_FILTER
+    pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
+    biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
+    bool setpointDerivativeLpfInitialized;
+    uint8_t rcSmoothingDebugAxis;
+    uint8_t rcSmoothingFilterType;
+#endif // USE_RC_SMOOTHING_FILTER
+
+#ifdef USE_ACRO_TRAINER
+    float acroTrainerAngleLimit;
+    float acroTrainerLookaheadTime;
+    uint8_t acroTrainerDebugAxis;
+    float acroTrainerGain;
+    bool acroTrainerActive;
+    int acroTrainerAxisState[2];  // only need roll and pitch
+#endif
+
+#ifdef USE_DYN_LPF
+    uint8_t dynLpfFilter;
+    uint16_t dynLpfMin;
+    uint16_t dynLpfMax;
+    uint8_t dynLpfCurveExpo;
+#endif
+
+#ifdef USE_LAUNCH_CONTROL
+    uint8_t launchControlMode;
+    uint8_t launchControlAngleLimit;
+    float launchControlKi;
+#endif
+
+#ifdef USE_INTEGRATED_YAW_CONTROL
+    bool useIntegratedYaw;
+    uint8_t integratedYawRelax;
+#endif
+
+#ifdef USE_THRUST_LINEARIZATION
+    float thrustLinearization;
+    float thrustLinearizationReciprocal;
+    float thrustLinearizationB;
+#endif
+
+#ifdef USE_AIRMODE_LPF
+    float airmodeThrottleOffsetLimit;
+#endif
+
+#ifdef USE_INTERPOLATED_SP
+    ffInterpolationType_t ffFromInterpolatedSetpoint;
+    float ffSmoothFactor;
+#endif
+} pidRuntime_t;
+
+extern pidRuntime_t pidRuntime;
+
 extern const char pidNames[];
 
 extern pidAxisData_t pidData[3];
@@ -230,15 +380,9 @@ extern pt1Filter_t throttleLpf;
 void pidResetIterm(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 void pidSetItermAccelerator(float newItermAccelerator);
-void pidInitFilters(const pidProfile_t *pidProfile);
-void pidInitConfig(const pidProfile_t *pidProfile);
-void pidInit(const pidProfile_t *pidProfile);
-void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 bool crashRecoveryModeActive(void);
 void pidAcroTrainerInit(void);
 void pidSetAcroTrainerState(bool newState);
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
-void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
 void pidUpdateAntiGravityThrottleFilter(float throttle);
 bool pidOsdAntiGravityActive(void);
 bool pidOsdAntiGravityMode(void);

src/main/flight/pid_init.c

@@ -0,0 +1,415 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/filter.h"
+#include "common/maths.h"
+
+#include "drivers/dshot_command.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/interpolated_setpoint.h"
+#include "flight/pid.h"
+#include "flight/rpm_filter.h"
+
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+
+#include "pid_init.h"
+
+#if defined(USE_D_MIN)
+#define D_MIN_RANGE_HZ 80    // Biquad lowpass input cutoff to peak D around propwash frequencies
+#define D_MIN_LOWPASS_HZ 10  // PT1 lowpass cutoff to smooth the boost effect
+#define D_MIN_GAIN_FACTOR 0.00005f
+#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
+#endif
+
+#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
+
+static void pidSetTargetLooptime(uint32_t pidLooptime)
+{
+    targetPidLooptime = pidLooptime;
+    pidRuntime.dT = targetPidLooptime * 1e-6f;
+    pidRuntime.pidFrequency = 1.0f / pidRuntime.dT;
+#ifdef USE_DSHOT
+    dshotSetPidLoopTime(targetPidLooptime);
+#endif
+}
+
+void pidInitFilters(const pidProfile_t *pidProfile)
+{
+    STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
+
+    if (targetPidLooptime == 0) {
+        // no looptime set, so set all the filters to null
+        pidRuntime.dtermNotchApplyFn = nullFilterApply;
+        pidRuntime.dtermLowpassApplyFn = nullFilterApply;
+        pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
+        pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
+        return;
+    }
+
+    const uint32_t pidFrequencyNyquist = pidRuntime.pidFrequency / 2; // No rounding needed
+
+    uint16_t dTermNotchHz;
+    if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
+        dTermNotchHz = pidProfile->dterm_notch_hz;
+    } else {
+        if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
+            dTermNotchHz = pidFrequencyNyquist;
+        } else {
+            dTermNotchHz = 0;
+        }
+    }
+
+    if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
+        pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
+        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+            biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
+        }
+    } else {
+        pidRuntime.dtermNotchApplyFn = nullFilterApply;
+    }
+
+    //1st Dterm Lowpass Filter
+    uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
+
+#ifdef USE_DYN_LPF
+    if (pidProfile->dyn_lpf_dterm_min_hz) {
+        dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
+    }
+#endif
+
+    if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
+        switch (pidProfile->dterm_filter_type) {
+        case FILTER_PT1:
+            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, pidRuntime.dT));
+            }
+            break;
+        case FILTER_BIQUAD:
+#ifdef USE_DYN_LPF
+            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
+#else
+            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
+#endif
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
+            }
+            break;
+        default:
+            pidRuntime.dtermLowpassApplyFn = nullFilterApply;
+            break;
+        }
+    } else {
+        pidRuntime.dtermLowpassApplyFn = nullFilterApply;
+    }
+
+    //2nd Dterm Lowpass Filter
+    if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
+        pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
+    } else {
+        switch (pidProfile->dterm_filter2_type) {
+        case FILTER_PT1:
+            pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
+            }
+            break;
+        case FILTER_BIQUAD:
+            pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
+            }
+            break;
+        default:
+            pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
+            break;
+        }
+    }
+
+    if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
+        pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
+    } else {
+        pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
+        pt1FilterInit(&pidRuntime.ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, pidRuntime.dT));
+    }
+
+#if defined(USE_THROTTLE_BOOST)
+    pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, pidRuntime.dT));
+#endif
+#if defined(USE_ITERM_RELAX)
+    if (pidRuntime.itermRelax) {
+        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+            pt1FilterInit(&pidRuntime.windupLpf[i], pt1FilterGain(pidRuntime.itermRelaxCutoff, pidRuntime.dT));
+        }
+    }
+#endif
+#if defined(USE_ABSOLUTE_CONTROL)
+    if (pidRuntime.itermRelax) {
+        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+            pt1FilterInit(&pidRuntime.acLpf[i], pt1FilterGain(pidRuntime.acCutoff, pidRuntime.dT));
+        }
+    }
+#endif
+#if defined(USE_D_MIN)
+
+    // Initialize the filters for all axis even if the d_min[axis] value is 0
+    // Otherwise if the pidProfile->d_min_xxx parameters are ever added to
+    // in-flight adjustments and transition from 0 to > 0 in flight the feature
+    // won't work because the filter wasn't initialized.
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        biquadFilterInitLPF(&pidRuntime.dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
+        pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT));
+     }
+#endif
+#if defined(USE_AIRMODE_LPF)
+    if (pidProfile->transient_throttle_limit) {
+        pt1FilterInit(&pidRuntime.airmodeThrottleLpf1, pt1FilterGain(7.0f, pidRuntime.dT));
+        pt1FilterInit(&pidRuntime.airmodeThrottleLpf2, pt1FilterGain(20.0f, pidRuntime.dT));
+    }
+#endif
+
+    pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
+
+    pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
+    pidRuntime.ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f;
+}
+
+void pidInit(const pidProfile_t *pidProfile)
+{
+    pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
+    pidInitFilters(pidProfile);
+    pidInitConfig(pidProfile);
+#ifdef USE_RPM_FILTER
+    rpmFilterInit(rpmFilterConfig());
+#endif
+}
+
+#ifdef USE_RC_SMOOTHING_FILTER
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
+{
+    pidRuntime.rcSmoothingDebugAxis = debugAxis;
+    pidRuntime.rcSmoothingFilterType = filterType;
+    if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
+        pidRuntime.setpointDerivativeLpfInitialized = true;
+        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+            switch (pidRuntime.rcSmoothingFilterType) {
+                case RC_SMOOTHING_DERIVATIVE_PT1:
+                    pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
+                    break;
+                case RC_SMOOTHING_DERIVATIVE_BIQUAD:
+                    biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
+                    break;
+            }
+        }
+    }
+}
+
+void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
+{
+    if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
+        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+            switch (pidRuntime.rcSmoothingFilterType) {
+                case RC_SMOOTHING_DERIVATIVE_PT1:
+                    pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
+                    break;
+                case RC_SMOOTHING_DERIVATIVE_BIQUAD:
+                    biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
+                    break;
+            }
+        }
+    }
+}
+#endif // USE_RC_SMOOTHING_FILTER
+
+void pidInitConfig(const pidProfile_t *pidProfile)
+{
+    if (pidProfile->feedForwardTransition == 0) {
+        pidRuntime.feedForwardTransition = 0;
+    } else {
+        pidRuntime.feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
+    }
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
+        pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
+        pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
+        pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
+    }
+#ifdef USE_INTEGRATED_YAW_CONTROL
+    if (!pidProfile->use_integrated_yaw)
+#endif
+    {
+        pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
+    }
+
+    pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
+    pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
+    pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
+    pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
+    pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
+    pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
+    pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
+    pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
+    pidRuntime.itermWindupPointInv = 1.0f;
+    if (pidProfile->itermWindupPointPercent < 100) {
+        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
+        pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
+    }
+    pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
+    pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
+    pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
+    pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
+    pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
+    pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold;
+    pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold;
+    pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
+    pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
+    pidRuntime.itermLimit = pidProfile->itermLimit;
+#if defined(USE_THROTTLE_BOOST)
+    throttleBoost = pidProfile->throttle_boost * 0.1f;
+#endif
+    pidRuntime.itermRotation = pidProfile->iterm_rotation;
+    pidRuntime.antiGravityMode = pidProfile->antiGravityMode;
+
+    // Calculate the anti-gravity value that will trigger the OSD display.
+    // For classic AG it's either 1.0 for off and > 1.0 for on.
+    // For the new AG it's a continuous floating value so we want to trigger the OSD
+    // display when it exceeds 25% of its possible range. This gives a useful indication
+    // of AG activity without excessive display.
+    pidRuntime.antiGravityOsdCutoff = 0.0f;
+    if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
+        pidRuntime.antiGravityOsdCutoff += ((pidRuntime.itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
+    }
+
+#if defined(USE_ITERM_RELAX)
+    pidRuntime.itermRelax = pidProfile->iterm_relax;
+    pidRuntime.itermRelaxType = pidProfile->iterm_relax_type;
+    pidRuntime.itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
+#endif
+
+#ifdef USE_ACRO_TRAINER
+    pidRuntime.acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
+    pidRuntime.acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
+    pidRuntime.acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
+    pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
+#endif // USE_ACRO_TRAINER
+
+#if defined(USE_ABSOLUTE_CONTROL)
+    pidRuntime.acGain = (float)pidProfile->abs_control_gain;
+    pidRuntime.acLimit = (float)pidProfile->abs_control_limit;
+    pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit;
+    pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff;
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp;
+        pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection);
+    }
+#endif
+
+#ifdef USE_DYN_LPF
+    if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
+        switch (pidProfile->dterm_filter_type) {
+        case FILTER_PT1:
+            pidRuntime.dynLpfFilter = DYN_LPF_PT1;
+            break;
+        case FILTER_BIQUAD:
+            pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
+            break;
+        default:
+            pidRuntime.dynLpfFilter = DYN_LPF_NONE;
+            break;
+        }
+    } else {
+        pidRuntime.dynLpfFilter = DYN_LPF_NONE;
+    }
+    pidRuntime.dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
+    pidRuntime.dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
+    pidRuntime.dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
+#endif
+
+#ifdef USE_LAUNCH_CONTROL
+    pidRuntime.launchControlMode = pidProfile->launchControlMode;
+    if (sensors(SENSOR_ACC)) {
+        pidRuntime.launchControlAngleLimit = pidProfile->launchControlAngleLimit;
+    } else {
+        pidRuntime.launchControlAngleLimit = 0;
+    }
+    pidRuntime.launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
+#endif
+
+#ifdef USE_INTEGRATED_YAW_CONTROL
+    pidRuntime.useIntegratedYaw = pidProfile->use_integrated_yaw;
+    pidRuntime.integratedYawRelax = pidProfile->integrated_yaw_relax;
+#endif
+
+#ifdef USE_THRUST_LINEARIZATION
+    pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
+    if (pidRuntime.thrustLinearization != 0.0f) {
+        pidRuntime.thrustLinearizationReciprocal = 1.0f / pidRuntime.thrustLinearization;
+        pidRuntime.thrustLinearizationB = (1.0f - pidRuntime.thrustLinearization) / (2.0f * pidRuntime.thrustLinearization);
+    }
+#endif
+#if defined(USE_D_MIN)
+    for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
+        const uint8_t dMin = pidProfile->d_min[axis];
+        if ((dMin > 0) && (dMin < pidProfile->pid[axis].D)) {
+            pidRuntime.dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
+        } else {
+            pidRuntime.dMinPercent[axis] = 0;
+        }
+    }
+    pidRuntime.dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
+    pidRuntime.dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidRuntime.pidFrequency / (100 * D_MIN_LOWPASS_HZ);
+    // lowpass included inversely in gain since stronger lowpass decreases peak effect
+#endif
+#if defined(USE_AIRMODE_LPF)
+    pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
+#endif
+#ifdef USE_INTERPOLATED_SP
+    pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
+    pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
+    interpolatedSpInit(pidProfile);
+#endif
+
+    pidRuntime.levelRaceMode = pidProfile->level_race_mode;
+}
+
+void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
+{
+    if (dstPidProfileIndex < PID_PROFILE_COUNT && srcPidProfileIndex < PID_PROFILE_COUNT
+        && dstPidProfileIndex != srcPidProfileIndex) {
+        memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
+    }
+}
+

src/main/flight/pid_init.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void pidInit(const pidProfile_t *pidProfile);
+void pidInitFilters(const pidProfile_t *pidProfile);
+void pidInitConfig(const pidProfile_t *pidProfile);
+void pidSetItermAccelerator(float newItermAccelerator);
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
+void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
+void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);

src/main/msp/msp.c

@@ -82,6 +82,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pid_init.h"
 #include "flight/position.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -16,6 +16,10 @@
 #include "common/color.h"
 #include "common/maths.h"
 
+#include "config/config.h"
+#include "config/config_eeprom.h"
+#include "config/feature.h"
+
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/compass/compass.h"
 #include "drivers/bus_i2c.h"
@@ -25,13 +29,20 @@
 #include "drivers/time.h"
 #include "drivers/timer.h"
 
-#include "config/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/position.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/pid_init.h"
+#include "flight/servos.h"
+
 #include "io/motors.h"
 #include "io/servos.h"
 #include "io/gps.h"
@@ -60,16 +71,6 @@
 
 #include "telemetry/telemetry.h"
 
-#include "flight/position.h"
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/mixer.h"
-#include "flight/pid.h"
-#include "flight/servos.h"
-
-#include "config/config_eeprom.h"
-#include "config/feature.h"
-
 #include "bus_bst.h"
 #include "i2c_bst.h"
 

src/test/Makefile

@@ -376,6 +376,7 @@ pid_unittest_SRC :=  \
 		$(USER_DIR)/drivers/accgyro/gyro_sync.c \
 		$(USER_DIR)/fc/runtime_config.c \
 		$(USER_DIR)/flight/pid.c \
+		$(USER_DIR)/flight/pid_init.c \
 		$(USER_DIR)/pg/pg.c
 
 pid_unittest_DEFINES := \

src/test/unit/pid_unittest.cc

@@ -52,9 +52,10 @@ extern "C" {
     #include "fc/rc_controls.h"
     #include "fc/runtime_config.h"
 
-    #include "flight/pid.h"
     #include "flight/imu.h"
     #include "flight/mixer.h"
+    #include "flight/pid.h"
+    #include "flight/pid_init.h"
 
     #include "io/gps.h"