From c06106e2d17c92a6bc2e8e2a03e9710c9607dacc Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Sat, 2 May 2020 12:23:26 -0400
Subject: [PATCH] Split initialization from pid.c for flash savings Move low
 performance requirements initialization code into pid_init.c and optimize
 that for size.

Saves 2688 bytes for target STM32F7X2.
---
 make/source.mk                          |   4 +-
 src/main/cms/cms_menu_imu.c             |   1 +
 src/main/config/config.c                |   1 +
 src/main/fc/init.c                      |   3 +-
 src/main/fc/rc.c                        |   2 +-
 src/main/fc/rc_adjustments.c            |   1 +
 src/main/flight/interpolated_setpoint.c |   7 +-
 src/main/flight/interpolated_setpoint.h |   8 -
 src/main/flight/pid.c                   | 853 +++++-------------------
 src/main/flight/pid.h                   | 156 ++++-
 src/main/flight/pid_init.c              | 415 ++++++++++++
 src/main/flight/pid_init.h              |  29 +
 src/main/msp/msp.c                      |   1 +
 src/main/target/COLIBRI_RACE/i2c_bst.c  |  23 +-
 src/test/Makefile                       |   1 +
 src/test/unit/pid_unittest.cc           |   3 +-
 16 files changed, 798 insertions(+), 710 deletions(-)
 create mode 100644 src/main/flight/pid_init.c
 create mode 100644 src/main/flight/pid_init.h

diff --git a/make/source.mk b/make/source.mk
index 037cb4b78..de8b7f6e5 100644
--- a/make/source.mk
+++ b/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 \
@@ -350,7 +351,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
 
 # F4 and F7 optimizations
 ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 53f963995..f37899283 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/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"
 
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 6d8e5563d..1686d2738 100644
--- a/src/main/config/config.c
+++ b/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"
 
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index f590e4b44..038abdefe 100644
--- a/src/main/fc/init.c
+++ b/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"
 
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
index e07e5abfb..a8901725c 100644
--- a/src/main/fc/rc.c
+++ b/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"
 
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index cb06279db..4ea22eb61 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/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"
diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c
index 30da75724..21f77dabd 100644
--- a/src/main/flight/interpolated_setpoint.c
+++ b/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];
diff --git a/src/main/flight/interpolated_setpoint.h b/src/main/flight/interpolated_setpoint.h
index 0b26f46c1..1b683e888 100644
--- a/src/main/flight/interpolated_setpoint.h
+++ b/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);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 47136e4c9..03034767f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -34,7 +34,6 @@
 
 #include "config/config_reset.h"
 
-#include "drivers/dshot_command.h"
 #include "drivers/pwm_output.h"
 #include "drivers/sound_beeper.h"
 #include "drivers/time.h"
@@ -77,20 +76,16 @@ const char pidNames[] =
 
 FAST_RAM_ZERO_INIT uint32_t targetPidLooptime;
 FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT];
+FAST_RAM_ZERO_INIT pidRuntime_t pidRuntime;
 
-static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
+#if defined(USE_ABSOLUTE_CONTROL)
+STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
+#endif
 
-static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
-
-static FAST_RAM_ZERO_INIT float dT;
-static FAST_RAM_ZERO_INIT float pidFrequency;
-
-static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
-static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
-static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
-static FAST_RAM_ZERO_INIT float antiGravityOsdCutoff;
-static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
-static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
+#if defined(USE_THROTTLE_BOOST)
+FAST_RAM_ZERO_INIT float throttleBoost;
+pt1Filter_t throttleLpf;
+#endif
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
 
@@ -104,13 +99,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
 #define PID_PROCESS_DENOM_DEFAULT       1
 #endif
 
-#if defined(USE_D_MIN)
-#define D_MIN_GAIN_FACTOR 0.00005f
-#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
-#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
-#endif
-
 #ifdef USE_RUNAWAY_TAKEOFF
 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
@@ -129,12 +117,6 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 #define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 #endif // USE_ACRO_TRAINER
 
-#ifdef USE_AIRMODE_LPF
-static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
-#endif
-
-#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
-
 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000  // 1 second delay before crash recovery detection is active after entering a self-level mode
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
@@ -243,328 +225,42 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
     }
 }
 
-static void pidSetTargetLooptime(uint32_t pidLooptime)
-{
-    targetPidLooptime = pidLooptime;
-    dT = targetPidLooptime * 1e-6f;
-    pidFrequency = 1.0f / dT;
-#ifdef USE_DSHOT
-    dshotSetPidLoopTime(targetPidLooptime);
-#endif
-}
-
-static FAST_RAM_ZERO_INIT float itermAccelerator;
 
 void pidSetItermAccelerator(float newItermAccelerator)
 {
-    itermAccelerator = newItermAccelerator;
+    pidRuntime.itermAccelerator = newItermAccelerator;
 }
 
 bool pidOsdAntiGravityActive(void)
 {
-    return (itermAccelerator > antiGravityOsdCutoff);
+    return (pidRuntime.itermAccelerator > pidRuntime.antiGravityOsdCutoff);
 }
 
 void pidStabilisationState(pidStabilisationState_e pidControllerState)
 {
-    pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
+    pidRuntime.pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
 }
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-typedef union dtermLowpass_u {
-    pt1Filter_t pt1Filter;
-    biquadFilter_t biquadFilter;
-} dtermLowpass_t;
-
-static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT];
-
-static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
-static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
-static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
-static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
-static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
-
-#if defined(USE_ITERM_RELAX)
-static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT uint8_t itermRelax;
-static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
-static uint8_t itermRelaxCutoff;
-#endif
-
-#if defined(USE_ABSOLUTE_CONTROL)
-STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float acGain;
-static FAST_RAM_ZERO_INIT float acLimit;
-static FAST_RAM_ZERO_INIT float acErrorLimit;
-static FAST_RAM_ZERO_INIT float acCutoff;
-static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float oldSetpointCorrection[XYZ_AXIS_COUNT];
-#endif
-
-#if defined(USE_D_MIN)
-static FAST_RAM_ZERO_INIT biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
-#endif
-
-#ifdef USE_RC_SMOOTHING_FILTER
-static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
-static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
-static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
-#endif // USE_RC_SMOOTHING_FILTER
-
-#ifdef USE_AIRMODE_LPF
-static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1;
-static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
-#endif
-
-static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
-
-static FAST_RAM_ZERO_INIT float ffBoostFactor;
-static FAST_RAM_ZERO_INIT float ffSmoothFactor;
-static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse;
-
 float pidGetSpikeLimitInverse()
 {
-    return ffSpikeLimitInverse;
+    return pidRuntime.ffSpikeLimitInverse;
 }
 
 
 float pidGetFfBoostFactor()
 {
-    return ffBoostFactor;
+    return pidRuntime.ffBoostFactor;
 }
 
+#ifdef USE_INTERPOLATED_SP
 float pidGetFfSmoothFactor()
 {
-    return ffSmoothFactor;
+    return pidRuntime.ffSmoothFactor;
 }
-
-
-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
-        dtermNotchApplyFn = nullFilterApply;
-        dtermLowpassApplyFn = nullFilterApply;
-        ptermYawLowpassApplyFn = nullFilterApply;
-        return;
-    }
-
-    const uint32_t pidFrequencyNyquist = 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) {
-        dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
-        const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
-        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-            biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
-        }
-    } else {
-        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:
-            dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
-            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT));
-            }
-            break;
-        case FILTER_BIQUAD:
-#ifdef USE_DYN_LPF
-            dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
-#else
-            dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
-#endif
-            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
-            }
-            break;
-        default:
-            dtermLowpassApplyFn = nullFilterApply;
-            break;
-        }
-    } else {
-        dtermLowpassApplyFn = nullFilterApply;
-    }
-
-    //2nd Dterm Lowpass Filter
-    if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
-    	dtermLowpass2ApplyFn = nullFilterApply;
-    } else {
-        switch (pidProfile->dterm_filter2_type) {
-        case FILTER_PT1:
-            dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
-            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
-            }
-            break;
-        case FILTER_BIQUAD:
-            dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
-            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
-            }
-            break;
-        default:
-            dtermLowpass2ApplyFn = nullFilterApply;
-            break;
-        }
-    }
-
-    if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
-        ptermYawLowpassApplyFn = nullFilterApply;
-    } else {
-        ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
-        pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
-    }
-
-#if defined(USE_THROTTLE_BOOST)
-    pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
-#endif
-#if defined(USE_ITERM_RELAX)
-    if (itermRelax) {
-        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-            pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
-        }
-    }
-#endif
-#if defined(USE_ABSOLUTE_CONTROL)
-    if (itermRelax) {
-        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-            pt1FilterInit(&acLpf[i], pt1FilterGain(acCutoff, 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(&dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
-        pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT));
-     }
-#endif
-#if defined(USE_AIRMODE_LPF)
-    if (pidProfile->transient_throttle_limit) {
-        pt1FilterInit(&airmodeThrottleLpf1, pt1FilterGain(7.0f, dT));
-        pt1FilterInit(&airmodeThrottleLpf2, pt1FilterGain(20.0f, dT));
-    }
-#endif
-
-    pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
-
-    ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
-    ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f;
-}
-
-#ifdef USE_RC_SMOOTHING_FILTER
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
-{
-    rcSmoothingDebugAxis = debugAxis;
-    rcSmoothingFilterType = filterType;
-    if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
-        setpointDerivativeLpfInitialized = true;
-        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-            switch (rcSmoothingFilterType) {
-                case RC_SMOOTHING_DERIVATIVE_PT1:
-                    pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
-                    break;
-                case RC_SMOOTHING_DERIVATIVE_BIQUAD:
-                    biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
-                    break;
-            }
-        }
-    }
-}
-
-void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
-{
-    if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
-        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-            switch (rcSmoothingFilterType) {
-                case RC_SMOOTHING_DERIVATIVE_PT1:
-                    pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
-                    break;
-                case RC_SMOOTHING_DERIVATIVE_BIQUAD:
-                    biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
-                    break;
-            }
-        }
-    }
-}
-#endif // USE_RC_SMOOTHING_FILTER
-
-typedef struct pidCoefficient_s {
-    float Kp;
-    float Ki;
-    float Kd;
-    float Kf;
-} pidCoefficient_t;
-
-static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float feedForwardTransition;
-static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
-static FAST_RAM_ZERO_INIT float itermWindupPointInv;
-static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
-static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
-static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs;
-static FAST_RAM_ZERO_INIT int32_t crashRecoveryAngleDeciDegrees;
-static FAST_RAM_ZERO_INIT float crashRecoveryRate;
-static FAST_RAM_ZERO_INIT float crashDtermThreshold;
-static FAST_RAM_ZERO_INIT float crashGyroThreshold;
-static FAST_RAM_ZERO_INIT float crashSetpointThreshold;
-static FAST_RAM_ZERO_INIT float crashLimitYaw;
-static FAST_RAM_ZERO_INIT float itermLimit;
-#if defined(USE_THROTTLE_BOOST)
-FAST_RAM_ZERO_INIT float throttleBoost;
-pt1Filter_t throttleLpf;
-#endif
-static FAST_RAM_ZERO_INIT bool itermRotation;
-
-#ifdef USE_LAUNCH_CONTROL
-static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
-static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
-static FAST_RAM_ZERO_INIT float launchControlKi;
-#endif
-
-#ifdef USE_INTEGRATED_YAW_CONTROL
-static FAST_RAM_ZERO_INIT bool useIntegratedYaw;
-static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax;
-#endif
-
-static FAST_RAM_ZERO_INIT bool levelRaceMode;
-
 void pidResetIterm(void)
 {
     for (int axis = 0; axis < 3; axis++) {
@@ -575,243 +271,42 @@ void pidResetIterm(void)
     }
 }
 
-#ifdef USE_ACRO_TRAINER
-static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit;
-static FAST_RAM_ZERO_INIT float acroTrainerLookaheadTime;
-static FAST_RAM_ZERO_INIT uint8_t acroTrainerDebugAxis;
-static FAST_RAM_ZERO_INIT bool acroTrainerActive;
-static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2];  // only need roll and pitch
-static FAST_RAM_ZERO_INIT float acroTrainerGain;
-#endif // USE_ACRO_TRAINER
-
-#ifdef USE_THRUST_LINEARIZATION
-FAST_RAM_ZERO_INIT float thrustLinearization;
-FAST_RAM_ZERO_INIT float thrustLinearizationReciprocal;
-FAST_RAM_ZERO_INIT float thrustLinearizationB;
-#endif
-
 void pidUpdateAntiGravityThrottleFilter(float throttle)
 {
-    if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
-        antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
+    if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
+        pidRuntime.antiGravityThrottleHpf = throttle - pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle);
     }
 }
 
-#ifdef USE_DYN_LPF
-static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
-static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
-static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
-static FAST_RAM_ZERO_INIT uint8_t dynLpfCurveExpo;
-#endif
-
-#ifdef USE_D_MIN
-static FAST_RAM_ZERO_INIT float dMinPercent[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float dMinGyroGain;
-static FAST_RAM_ZERO_INIT float dMinSetpointGain;
-#endif
-
-#ifdef USE_INTERPOLATED_SP
-static FAST_RAM_ZERO_INIT ffInterpolationType_t ffFromInterpolatedSetpoint;
-#endif
-
-void pidInitConfig(const pidProfile_t *pidProfile)
-{
-    if (pidProfile->feedForwardTransition == 0) {
-        feedForwardTransition = 0;
-    } else {
-        feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
-    }
-    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-        pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
-        pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
-        pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
-        pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
-    }
-#ifdef USE_INTEGRATED_YAW_CONTROL
-    if (!pidProfile->use_integrated_yaw)
-#endif
-    {
-        pidCoefficient[FD_YAW].Ki *= 2.5f;
-    }
-
-    levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
-    horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
-    horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
-    horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
-    horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
-    horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
-    maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
-    maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
-    itermWindupPointInv = 1.0f;
-    if (pidProfile->itermWindupPointPercent < 100) {
-        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
-        itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
-    }
-    itermAcceleratorGain = pidProfile->itermAcceleratorGain;
-    crashTimeLimitUs = pidProfile->crash_time * 1000;
-    crashTimeDelayUs = pidProfile->crash_delay * 1000;
-    crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
-    crashRecoveryRate = pidProfile->crash_recovery_rate;
-    crashGyroThreshold = pidProfile->crash_gthreshold;
-    crashDtermThreshold = pidProfile->crash_dthreshold;
-    crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
-    crashLimitYaw = pidProfile->crash_limit_yaw;
-    itermLimit = pidProfile->itermLimit;
-#if defined(USE_THROTTLE_BOOST)
-    throttleBoost = pidProfile->throttle_boost * 0.1f;
-#endif
-    itermRotation = pidProfile->iterm_rotation;
-    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.
-    antiGravityOsdCutoff = 0.0f;
-    if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
-        antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
-    }
-
-#if defined(USE_ITERM_RELAX)
-    itermRelax = pidProfile->iterm_relax;
-    itermRelaxType = pidProfile->iterm_relax_type;
-    itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
-#endif
-
-#ifdef USE_ACRO_TRAINER
-    acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
-    acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
-    acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
-    acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
-#endif // USE_ACRO_TRAINER
-
-#if defined(USE_ABSOLUTE_CONTROL)
-    acGain = (float)pidProfile->abs_control_gain;
-    acLimit = (float)pidProfile->abs_control_limit;
-    acErrorLimit = (float)pidProfile->abs_control_error_limit;
-    acCutoff = (float)pidProfile->abs_control_cutoff;
-    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-        float iCorrection = -acGain * PTERM_SCALE / ITERM_SCALE * pidCoefficient[axis].Kp;
-        pidCoefficient[axis].Ki = MAX(0.0f, 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:
-            dynLpfFilter = DYN_LPF_PT1;
-            break;
-        case FILTER_BIQUAD:
-            dynLpfFilter = DYN_LPF_BIQUAD;
-            break;
-        default:
-            dynLpfFilter = DYN_LPF_NONE;
-            break;
-        }
-    } else {
-        dynLpfFilter = DYN_LPF_NONE;
-    }
-    dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
-    dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
-    dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
-#endif
-
-#ifdef USE_LAUNCH_CONTROL
-    launchControlMode = pidProfile->launchControlMode;
-    if (sensors(SENSOR_ACC)) {
-        launchControlAngleLimit = pidProfile->launchControlAngleLimit;
-    } else {
-        launchControlAngleLimit = 0;
-    }
-    launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
-#endif
-
-#ifdef USE_INTEGRATED_YAW_CONTROL
-    useIntegratedYaw = pidProfile->use_integrated_yaw;
-    integratedYawRelax = pidProfile->integrated_yaw_relax;
-#endif
-
-#ifdef USE_THRUST_LINEARIZATION
-    thrustLinearization = pidProfile->thrustLinearization / 100.0f;
-    if (thrustLinearization != 0.0f) {
-        thrustLinearizationReciprocal = 1.0f / thrustLinearization;
-        thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * 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)) {
-            dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
-        } else {
-            dMinPercent[axis] = 0;
-        }
-    }
-    dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
-    dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidFrequency / (100 * D_MIN_LOWPASS_HZ);
-    // lowpass included inversely in gain since stronger lowpass decreases peak effect
-#endif
-#if defined(USE_AIRMODE_LPF)
-    airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
-#endif
-#ifdef USE_INTERPOLATED_SP
-    ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
-    ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
-    interpolatedSpInit(pidProfile);
-#endif
-
-    levelRaceMode = pidProfile->level_race_mode;
-}
-
-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_ACRO_TRAINER
 void pidAcroTrainerInit(void)
 {
-    acroTrainerAxisState[FD_ROLL] = 0;
-    acroTrainerAxisState[FD_PITCH] = 0;
+    pidRuntime.acroTrainerAxisState[FD_ROLL] = 0;
+    pidRuntime.acroTrainerAxisState[FD_PITCH] = 0;
 }
 #endif // USE_ACRO_TRAINER
 
 #ifdef USE_THRUST_LINEARIZATION
 float pidCompensateThrustLinearization(float throttle)
 {
-    if (thrustLinearization != 0.0f) {
-        throttle = throttle * (throttle * thrustLinearization + 1.0f - thrustLinearization);
+    if (pidRuntime.thrustLinearization != 0.0f) {
+        throttle = throttle * (throttle * pidRuntime.thrustLinearization + 1.0f - pidRuntime.thrustLinearization);
     }
     return throttle;
 }
 
 float pidApplyThrustLinearization(float motorOutput)
 {
-    if (thrustLinearization != 0.0f) {
+    if (pidRuntime.thrustLinearization != 0.0f) {
         if (motorOutput > 0.0f) {
-            motorOutput = sqrtf(motorOutput * thrustLinearizationReciprocal +
-                                thrustLinearizationB * thrustLinearizationB) - thrustLinearizationB;
+            motorOutput = sqrtf(motorOutput * pidRuntime.thrustLinearizationReciprocal +
+                                pidRuntime.thrustLinearizationB * pidRuntime.thrustLinearizationB) - pidRuntime.thrustLinearizationB;
         }
     }
     return motorOutput;
 }
 #endif
 
-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));
-    }
-}
-
 #if defined(USE_ACC)
 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
 STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
@@ -824,19 +319,19 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
 
     // horizonTiltExpertMode:  0 = leveling always active when sticks centered,
     //                         1 = leveling can be totally off when inverted
-    if (horizonTiltExpertMode) {
-        if (horizonTransition > 0 && horizonCutoffDegrees > 0) {
+    if (pidRuntime.horizonTiltExpertMode) {
+        if (pidRuntime.horizonTransition > 0 && pidRuntime.horizonCutoffDegrees > 0) {
                     // if d_level > 0 and horizonTiltEffect < 175
             // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
             //  for larger inclinations; 0.0 at horizonCutoffDegrees value:
-            const float inclinationLevelRatio = constrainf((horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1);
+            const float inclinationLevelRatio = constrainf((pidRuntime.horizonCutoffDegrees-currentInclination) / pidRuntime.horizonCutoffDegrees, 0, 1);
             // apply configured horizon sensitivity:
                 // when stick is near center (horizonLevelStrength ~= 1.0)
                 //  H_sensitivity value has little effect,
                 // when stick is deflected (horizonLevelStrength near 0.0)
                 //  H_sensitivity value has more effect:
-            horizonLevelStrength = (horizonLevelStrength - 1) * 100 / horizonTransition + 1;
+            horizonLevelStrength = (horizonLevelStrength - 1) * 100 / pidRuntime.horizonTransition + 1;
             // apply inclination ratio, which may lower leveling
             //  to zero regardless of stick position:
             horizonLevelStrength *= inclinationLevelRatio;
@@ -845,15 +340,15 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
         }
     } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
         float sensitFact;
-        if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0
+        if (pidRuntime.horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0
             // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
             // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
             //  for larger inclinations, goes to 1.0 at inclination==level:
-            const float inclinationLevelRatio = (180-currentInclination)/180 * (1.0f-horizonFactorRatio) + horizonFactorRatio;
+            const float inclinationLevelRatio = (180 - currentInclination) / 180 * (1.0f - pidRuntime.horizonFactorRatio) + pidRuntime.horizonFactorRatio;
             // apply ratio to configured horizon sensitivity:
-            sensitFact = horizonTransition * inclinationLevelRatio;
+            sensitFact = pidRuntime.horizonTransition * inclinationLevelRatio;
         } else { // horizonTiltEffect=0 for "old" functionality
-            sensitFact = horizonTransition;
+            sensitFact = pidRuntime.horizonTransition;
         }
 
         if (sensitFact <= 0) {           // zero means no leveling
@@ -883,54 +378,52 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
     const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
         // ANGLE mode - control is angle based
-        currentPidSetpoint = errorAngle * levelGain;
+        currentPidSetpoint = errorAngle * pidRuntime.levelGain;
     } else {
         // HORIZON mode - mix of ANGLE and ACRO modes
         // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
         const float horizonLevelStrength = calcHorizonLevelStrength();
-        currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
+        currentPidSetpoint = currentPidSetpoint + (errorAngle * pidRuntime.horizonGain * horizonLevelStrength);
     }
     return currentPidSetpoint;
 }
 
-static timeUs_t crashDetectedAtUs;
-
 static void handleCrashRecovery(
     const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
     const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
 {
-    if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
+    if (pidRuntime.inCrashRecoveryMode && cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) > pidRuntime.crashTimeDelayUs) {
         if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
             BEEP_ON;
         }
         if (axis == FD_YAW) {
-            *errorRate = constrainf(*errorRate, -crashLimitYaw, crashLimitYaw);
+            *errorRate = constrainf(*errorRate, -pidRuntime.crashLimitYaw, pidRuntime.crashLimitYaw);
         } else {
             // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
             if (sensors(SENSOR_ACC)) {
                 // errorAngle is deviation from horizontal
                 const float errorAngle =  -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
-                *currentPidSetpoint = errorAngle * levelGain;
+                *currentPidSetpoint = errorAngle * pidRuntime.levelGain;
                 *errorRate = *currentPidSetpoint - gyroRate;
             }
         }
         // reset iterm, since accumulated error before crash is now meaningless
         // and iterm windup during crash recovery can be extreme, especially on yaw axis
         pidData[axis].I = 0.0f;
-        if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
+        if (cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) > pidRuntime.crashTimeLimitUs
             || (getMotorMixRange() < 1.0f
-                   && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
-                   && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
-                   && fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) {
+                   && fabsf(gyro.gyroADCf[FD_ROLL]) < pidRuntime.crashRecoveryRate
+                   && fabsf(gyro.gyroADCf[FD_PITCH]) < pidRuntime.crashRecoveryRate
+                   && fabsf(gyro.gyroADCf[FD_YAW]) < pidRuntime.crashRecoveryRate)) {
             if (sensors(SENSOR_ACC)) {
                 // check aircraft nearly level
-                if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
-                   && ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees) {
-                    inCrashRecoveryMode = false;
+                if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < pidRuntime.crashRecoveryAngleDeciDegrees
+                   && ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < pidRuntime.crashRecoveryAngleDeciDegrees) {
+                    pidRuntime.inCrashRecoveryMode = false;
                     BEEP_OFF;
                 }
             } else {
-                inCrashRecoveryMode = false;
+                pidRuntime.inCrashRecoveryMode = false;
                 BEEP_OFF;
             }
         }
@@ -945,25 +438,25 @@ static void detectAndSetCrashRecovery(
     // no point in trying to recover if the crash is so severe that the gyro overflows
     if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
         if (ARMING_FLAG(ARMED)) {
-            if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode
-                && fabsf(delta) > crashDtermThreshold
-                && fabsf(errorRate) > crashGyroThreshold
-                && fabsf(getSetpointRate(axis)) < crashSetpointThreshold) {
+            if (getMotorMixRange() >= 1.0f && !pidRuntime.inCrashRecoveryMode
+                && fabsf(delta) > pidRuntime.crashDtermThreshold
+                && fabsf(errorRate) > pidRuntime.crashGyroThreshold
+                && fabsf(getSetpointRate(axis)) < pidRuntime.crashSetpointThreshold) {
                 if (crash_recovery == PID_CRASH_RECOVERY_DISARM) {
                     setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
                     disarm(DISARM_REASON_CRASH_PROTECTION);
                 } else {
-                    inCrashRecoveryMode = true;
-                    crashDetectedAtUs = currentTimeUs;
+                    pidRuntime.inCrashRecoveryMode = true;
+                    pidRuntime.crashDetectedAtUs = currentTimeUs;
                 }
             }
-            if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (fabsf(errorRate) < crashGyroThreshold
-                || fabsf(getSetpointRate(axis)) > crashSetpointThreshold)) {
-                inCrashRecoveryMode = false;
+            if (pidRuntime.inCrashRecoveryMode && cmpTimeUs(currentTimeUs, pidRuntime.crashDetectedAtUs) < pidRuntime.crashTimeDelayUs && (fabsf(errorRate) < pidRuntime.crashGyroThreshold
+                || fabsf(getSetpointRate(axis)) > pidRuntime.crashSetpointThreshold)) {
+                pidRuntime.inCrashRecoveryMode = false;
                 BEEP_OFF;
             }
-        } else if (inCrashRecoveryMode) {
-            inCrashRecoveryMode = false;
+        } else if (pidRuntime.inCrashRecoveryMode) {
+            pidRuntime.inCrashRecoveryMode = false;
             BEEP_OFF;
         }
     }
@@ -999,31 +492,31 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
         const int angleSign = acroTrainerSign(currentAngle);
 
-        if ((acroTrainerAxisState[axis] != 0) && (acroTrainerAxisState[axis] != setpointSign)) {  // stick has reversed - stop limiting
-            acroTrainerAxisState[axis] = 0;
+        if ((pidRuntime.acroTrainerAxisState[axis] != 0) && (pidRuntime.acroTrainerAxisState[axis] != setpointSign)) {  // stick has reversed - stop limiting
+            pidRuntime.acroTrainerAxisState[axis] = 0;
         }
 
         // Limit and correct the angle when it exceeds the limit
-        if ((fabsf(currentAngle) > acroTrainerAngleLimit) && (acroTrainerAxisState[axis] == 0)) {
+        if ((fabsf(currentAngle) > pidRuntime.acroTrainerAngleLimit) && (pidRuntime.acroTrainerAxisState[axis] == 0)) {
             if (angleSign == setpointSign) {
-                acroTrainerAxisState[axis] = angleSign;
+                pidRuntime.acroTrainerAxisState[axis] = angleSign;
                 resetIterm = true;
             }
         }
 
-        if (acroTrainerAxisState[axis] != 0) {
-            ret = constrainf(((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
+        if (pidRuntime.acroTrainerAxisState[axis] != 0) {
+            ret = constrainf(((pidRuntime.acroTrainerAngleLimit * angleSign) - currentAngle) * pidRuntime.acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
         } else {
 
         // Not currently over the limit so project the angle based on current angle and
         // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
         // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
             // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
-            float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime;
+            float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * pidRuntime.acroTrainerLookaheadTime;
             projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
             const int projectedAngleSign = acroTrainerSign(projectedAngle);
-            if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
-                ret = ((acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * acroTrainerGain;
+            if ((fabsf(projectedAngle) > pidRuntime.acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
+                ret = ((pidRuntime.acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * pidRuntime.acroTrainerGain;
                 resetIterm = true;
             }
         }
@@ -1032,9 +525,9 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
             pidData[axis].I = 0;
         }
 
-        if (axis == acroTrainerDebugAxis) {
+        if (axis == pidRuntime.acroTrainerDebugAxis) {
             DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
-            DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]);
+            DEBUG_SET(DEBUG_ACRO_TRAINER, 1, pidRuntime.acroTrainerAxisState[axis]);
             DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
             DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
         }
@@ -1049,8 +542,8 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
     static float previousSetpoint[XYZ_AXIS_COUNT];
     const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
 
-    if (fabsf(currentVelocity) > maxVelocity[axis]) {
-        currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
+    if (fabsf(currentVelocity) > pidRuntime.maxVelocity[axis]) {
+        currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + pidRuntime.maxVelocity[axis] : previousSetpoint[axis] - pidRuntime.maxVelocity[axis];
     }
 
     previousSetpoint[axis] = currentPidSetpoint;
@@ -1072,22 +565,22 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]
 
 STATIC_UNIT_TESTED void rotateItermAndAxisError()
 {
-    if (itermRotation
+    if (pidRuntime.itermRotation
 #if defined(USE_ABSOLUTE_CONTROL)
-        || acGain > 0 || debugMode == DEBUG_AC_ERROR
+        || pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR
 #endif
         ) {
-        const float gyroToAngle = dT * RAD;
+        const float gyroToAngle = pidRuntime.dT * RAD;
         float rotationRads[XYZ_AXIS_COUNT];
         for (int i = FD_ROLL; i <= FD_YAW; i++) {
             rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
         }
 #if defined(USE_ABSOLUTE_CONTROL)
-        if (acGain > 0 || debugMode == DEBUG_AC_ERROR) {
+        if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) {
             rotateVector(axisError, rotationRads);
         }
 #endif
-        if (itermRotation) {
+        if (pidRuntime.itermRotation) {
             float v[XYZ_AXIS_COUNT];
             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
                 v[i] = pidData[i].I;
@@ -1104,19 +597,19 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError()
 float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
 {
     float ret = pidSetpointDelta;
-    if (axis == rcSmoothingDebugAxis) {
+    if (axis == pidRuntime.rcSmoothingDebugAxis) {
         DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
     }
-    if (setpointDerivativeLpfInitialized) {
-        switch (rcSmoothingFilterType) {
+    if (pidRuntime.setpointDerivativeLpfInitialized) {
+        switch (pidRuntime.rcSmoothingFilterType) {
             case RC_SMOOTHING_DERIVATIVE_PT1:
-                ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
+                ret = pt1FilterApply(&pidRuntime.setpointDerivativePt1[axis], pidSetpointDelta);
                 break;
             case RC_SMOOTHING_DERIVATIVE_BIQUAD:
-                ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
+                ret = biquadFilterApplyDF1(&pidRuntime.setpointDerivativeBiquad[axis], pidSetpointDelta);
                 break;
         }
-        if (axis == rcSmoothingDebugAxis) {
+        if (axis == pidRuntime.rcSmoothingDebugAxis) {
             DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
         }
     }
@@ -1128,8 +621,8 @@ float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelt
 #if defined(USE_ABSOLUTE_CONTROL)
 STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
 {
-    if (acGain > 0 || debugMode == DEBUG_AC_ERROR) {
-        const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint);
+    if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) {
+        const float setpointLpf = pt1FilterApply(&pidRuntime.acLpf[axis], *currentPidSetpoint);
         const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
         float acErrorRate = 0;
         const float gmaxac = setpointLpf + 2 * setpointHpf;
@@ -1142,17 +635,17 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat
             } else {
                 acErrorRate = acErrorRate2;
             }
-            if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
-                acErrorRate = -axisError[axis] * pidFrequency;
+            if (fabsf(acErrorRate * pidRuntime.dT) > fabsf(axisError[axis]) ) {
+                acErrorRate = -axisError[axis] * pidRuntime.pidFrequency;
             }
         } else {
             acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
         }
 
         if (isAirmodeActivated()) {
-            axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT,
-                -acErrorLimit, acErrorLimit);
-            const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
+            axisError[axis] = constrainf(axisError[axis] + acErrorRate * pidRuntime.dT,
+                -pidRuntime.acErrorLimit, pidRuntime.acErrorLimit);
+            const float acCorrection = constrainf(axisError[axis] * pidRuntime.acGain, -pidRuntime.acLimit, pidRuntime.acLimit);
             *currentPidSetpoint += acCorrection;
             *itermErrorRate += acCorrection;
             DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10));
@@ -1168,19 +661,19 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat
 STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
 {
-    const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
+    const float setpointLpf = pt1FilterApply(&pidRuntime.windupLpf[axis], *currentPidSetpoint);
     const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
 
-    if (itermRelax) {
-        if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC) {
+    if (pidRuntime.itermRelax) {
+        if (axis < FD_YAW || pidRuntime.itermRelax == ITERM_RELAX_RPY || pidRuntime.itermRelax == ITERM_RELAX_RPY_INC) {
             const float itermRelaxFactor = MAX(0, 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD);
             const bool isDecreasingI =
                 ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
-            if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
+            if ((pidRuntime.itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
                 // Do Nothing, use the precalculed itermErrorRate
-            } else if (itermRelaxType == ITERM_RELAX_SETPOINT) {
+            } else if (pidRuntime.itermRelaxType == ITERM_RELAX_SETPOINT) {
                 *itermErrorRate *= itermRelaxFactor;
-            } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
+            } else if (pidRuntime.itermRelaxType == ITERM_RELAX_GYRO ) {
                 *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
             } else {
                 *itermErrorRate = 0.0f;
@@ -1203,25 +696,25 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
 #ifdef USE_AIRMODE_LPF
 void pidUpdateAirmodeLpf(float currentOffset)
 {
-    if (airmodeThrottleOffsetLimit == 0.0f) {
+    if (pidRuntime.airmodeThrottleOffsetLimit == 0.0f) {
         return;
     }
 
     float offsetHpf = currentOffset * 2.5f;
-    offsetHpf = offsetHpf - pt1FilterApply(&airmodeThrottleLpf2, offsetHpf);
+    offsetHpf = offsetHpf - pt1FilterApply(&pidRuntime.airmodeThrottleLpf2, offsetHpf);
 
     // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
-    pt1FilterApply(&airmodeThrottleLpf1, offsetHpf);
+    pt1FilterApply(&pidRuntime.airmodeThrottleLpf1, offsetHpf);
     // Bring offset up immediately so the filter only applies to the decline
-    if (currentOffset * airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > airmodeThrottleLpf1.state) {
-        airmodeThrottleLpf1.state = currentOffset;
+    if (currentOffset * pidRuntime.airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > pidRuntime.airmodeThrottleLpf1.state) {
+        pidRuntime.airmodeThrottleLpf1.state = currentOffset;
     }
-    airmodeThrottleLpf1.state = constrainf(airmodeThrottleLpf1.state, -airmodeThrottleOffsetLimit, airmodeThrottleOffsetLimit);
+    pidRuntime.airmodeThrottleLpf1.state = constrainf(pidRuntime.airmodeThrottleLpf1.state, -pidRuntime.airmodeThrottleOffsetLimit, pidRuntime.airmodeThrottleOffsetLimit);
 }
 
 float pidGetAirmodeThrottleOffset()
 {
-    return airmodeThrottleLpf1.state;
+    return pidRuntime.airmodeThrottleLpf1.state;
 }
 #endif
 
@@ -1240,7 +733,7 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
     // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
     // reached at 50% stick deflection. This keeps the launch control positioning consistent
     // regardless of the user's rates.
-    if ((axis == FD_PITCH) || (launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
+    if ((axis == FD_PITCH) || (pidRuntime.launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
         const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
         ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
     }
@@ -1248,13 +741,13 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
 #if defined(USE_ACC)
     // If ACC is enabled and a limit angle is set, then try to limit forward tilt
     // to that angle and slow down the rate as the limit is approached to reduce overshoot
-    if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0)) {
+    if ((axis == FD_PITCH) && (pidRuntime.launchControlAngleLimit > 0) && (ret > 0)) {
         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
-        if (currentAngle >= launchControlAngleLimit) {
+        if (currentAngle >= pidRuntime.launchControlAngleLimit) {
             ret = 0.0f;
         } else {
             //for the last 10 degrees scale the rate from the current input to 5 dps
-            const float angleDelta = launchControlAngleLimit - currentAngle;
+            const float angleDelta = pidRuntime.launchControlAngleLimit - currentAngle;
             if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
                 ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
             }
@@ -1307,7 +800,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
     levelMode_e levelMode;
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive) {
-        if (levelRaceMode && !gpsRescueIsActive) {
+        if (pidRuntime.levelRaceMode && !gpsRescueIsActive) {
             levelMode = LEVEL_MODE_R;
         } else {
             levelMode = LEVEL_MODE_RP;
@@ -1330,18 +823,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #endif
 
     // Dynamic i component,
-    if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
-        itermAccelerator = fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
-        DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
+    if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) {
+        pidRuntime.itermAccelerator = fabsf(pidRuntime.antiGravityThrottleHpf) * 0.01f * (pidRuntime.itermAcceleratorGain - 1000);
+        DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.antiGravityThrottleHpf * 1000));
     }
-    DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
+    DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));
 
-    float agGain = dT * itermAccelerator * AG_KI;
+    float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
 
     // gradually scale back integration when above windup point
-    float dynCi = dT;
-    if (itermWindupPointInv > 1.0f) {
-        dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
+    float dynCi = pidRuntime.dT;
+    if (pidRuntime.itermWindupPointInv > 1.0f) {
+        dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
     }
 
     // Precalculate gyro deta for D-term here, this allows loop unrolling
@@ -1351,9 +844,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #ifdef USE_RPM_FILTER
         gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
 #endif
-        gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
-        gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
-        gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
+        gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
+        gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *) &pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]);
+        gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]);
     }
 
     rotateItermAndAxisError();
@@ -1373,7 +866,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
 
         float currentPidSetpoint = getSetpointRate(axis);
-        if (maxVelocity[axis]) {
+        if (pidRuntime.maxVelocity[axis]) {
             currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
         }
         // Yaw control is GYRO based, direct sticks control is applied to rate PID
@@ -1398,7 +891,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #endif
 
 #ifdef USE_ACRO_TRAINER
-        if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode && !launchControlActive) {
+        if ((axis != FD_YAW) && pidRuntime.acroTrainerActive && !pidRuntime.inCrashRecoveryMode && !launchControlActive) {
             currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
         }
 #endif // USE_ACRO_TRAINER
@@ -1437,7 +930,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #endif
 
 #if defined(USE_ITERM_RELAX)
-        if (!launchControlActive && !inCrashRecoveryMode) {
+        if (!launchControlActive && !pidRuntime.inCrashRecoveryMode) {
             applyItermRelax(axis, previousIterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
             errorRate = currentPidSetpoint - gyroRate;
         }
@@ -1451,9 +944,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
 
         // -----calculate P component
-        pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
+        pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
         if (axis == FD_YAW) {
-            pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
+            pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
         }
 
         // -----calculate I component
@@ -1462,29 +955,29 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #ifdef USE_LAUNCH_CONTROL
         // if launch control is active override the iterm gains and apply iterm windup protection to all axes
         if (launchControlActive) {
-            Ki = launchControlKi;
+            Ki = pidRuntime.launchControlKi;
             axisDynCi = dynCi;
         } else 
 #endif
         {
-            Ki = pidCoefficient[axis].Ki;
-            axisDynCi = (axis == FD_YAW) ? dynCi : dT; // only apply windup protection to yaw
+            Ki = pidRuntime.pidCoefficient[axis].Ki;
+            axisDynCi = (axis == FD_YAW) ? dynCi : pidRuntime.dT; // only apply windup protection to yaw
         }
 
-        pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + agGain) * itermErrorRate, -itermLimit, itermLimit);
+        pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + agGain) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit);
 
         // -----calculate pidSetpointDelta
         float pidSetpointDelta = 0;
 #ifdef USE_INTERPOLATED_SP
-        if (ffFromInterpolatedSetpoint) {
-            pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, ffFromInterpolatedSetpoint);
+        if (pidRuntime.ffFromInterpolatedSetpoint) {
+            pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, pidRuntime.ffFromInterpolatedSetpoint);
         } else {
-            pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
+            pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
         }
 #else
-        pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
+        pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
 #endif
-        previousPidSetpoint[axis] = currentPidSetpoint;
+        pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
 
 
 #ifdef USE_RC_SMOOTHING_FILTER
@@ -1493,7 +986,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
         // -----calculate D component
         // disable D if launch control is active
-        if ((pidCoefficient[axis].Kd > 0) && !launchControlActive) {
+        if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) {
 
             // Divide rate change by dT to get differential (ie dr/dt).
             // dT is fixed and calculated from the target PID loop time
@@ -1501,7 +994,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             // calculated deltaT whenever another task causes the PID
             // loop execution to be delayed.
             const float delta =
-                - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
+                - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
 
 #if defined(USE_ACC)
             if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
@@ -1511,24 +1004,24 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
             float dMinFactor = 1.0f;
 #if defined(USE_D_MIN)
-            if (dMinPercent[axis] > 0) {
-                float dMinGyroFactor = biquadFilterApply(&dMinRange[axis], delta);
-                dMinGyroFactor = fabsf(dMinGyroFactor) * dMinGyroGain;
-                const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * dMinSetpointGain;
+            if (pidRuntime.dMinPercent[axis] > 0) {
+                float dMinGyroFactor = biquadFilterApply(&pidRuntime.dMinRange[axis], delta);
+                dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain;
+                const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain;
                 dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
-                dMinFactor = dMinPercent[axis] + (1.0f - dMinPercent[axis]) * dMinFactor;
-                dMinFactor = pt1FilterApply(&dMinLowpass[axis], dMinFactor);
+                dMinFactor = pidRuntime.dMinPercent[axis] + (1.0f - pidRuntime.dMinPercent[axis]) * dMinFactor;
+                dMinFactor = pt1FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor);
                 dMinFactor = MIN(dMinFactor, 1.0f);
                 if (axis == FD_ROLL) {
                     DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100));
                     DEBUG_SET(DEBUG_D_MIN, 1, lrintf(dMinSetpointFactor * 100));
-                    DEBUG_SET(DEBUG_D_MIN, 2, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
+                    DEBUG_SET(DEBUG_D_MIN, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
                 } else if (axis == FD_PITCH) {
-                    DEBUG_SET(DEBUG_D_MIN, 3, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
+                    DEBUG_SET(DEBUG_D_MIN, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
                 }
             }
 #endif
-            pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dMinFactor;
+            pidData[axis].D = pidRuntime.pidCoefficient[axis].Kd * delta * tpaFactor * dMinFactor;
         } else {
             pidData[axis].D = 0;
         }
@@ -1537,20 +1030,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         // -----calculate feedforward component
 #ifdef USE_ABSOLUTE_CONTROL
         // include abs control correction in FF
-        pidSetpointDelta += setpointCorrection - oldSetpointCorrection[axis];
-        oldSetpointCorrection[axis] = setpointCorrection;
+        pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
+        pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
 #endif
 
         // Only enable feedforward for rate mode and if launch control is inactive
-        const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
+        const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
         if (feedforwardGain > 0) {
             // no transition if feedForwardTransition == 0
-            float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
-            float feedForward = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
+            float transition = pidRuntime.feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedForwardTransition) : 1;
+            float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency;
 
 #ifdef USE_INTERPOLATED_SP
             pidData[axis].F = shouldApplyFfLimits(axis) ?
-                applyFfLimit(axis, feedForward, pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
+                applyFfLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
 #else
             pidData[axis].F = feedForward;
 #endif
@@ -1575,11 +1068,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         if (launchControlActive) {
             // if not using FULL mode then disable I accumulation on yaw as
             // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
-            const int launchControlYawItermLimit = (launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
+            const int launchControlYawItermLimit = (pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
             pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit);
 
             // for pitch-only mode we disable everything except pitch P/I
-            if (launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
+            if (pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
                 pidData[FD_ROLL].P = 0;
                 pidData[FD_ROLL].I = 0;
                 pidData[FD_YAW].P = 0;
@@ -1591,9 +1084,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         // calculating the PID sum
         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
 #ifdef USE_INTEGRATED_YAW_CONTROL
-        if (axis == FD_YAW && useIntegratedYaw) {
-            pidData[axis].Sum += pidSum * dT * 100.0f;
-            pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f;
+        if (axis == FD_YAW && pidRuntime.useIntegratedYaw) {
+            pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
+            pidData[axis].Sum -= pidData[axis].Sum * pidRuntime.integratedYawRelax / 100000.0f * pidRuntime.dT / 0.000125f;
         } else
 #endif
         {
@@ -1603,7 +1096,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
     // Disable PID control if at zero throttle or if gyro overflow detected
     // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
-    if (!pidStabilisationEnabled || gyroOverflowDetected()) {
+    if (!pidRuntime.pidStabilisationEnabled || gyroOverflowDetected()) {
         for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
             pidData[axis].P = 0;
             pidData[axis].I = 0;
@@ -1612,60 +1105,60 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
             pidData[axis].Sum = 0;
         }
-    } else if (zeroThrottleItermReset) {
+    } else if (pidRuntime.zeroThrottleItermReset) {
         pidResetIterm();
     }
 }
 
 bool crashRecoveryModeActive(void)
 {
-    return inCrashRecoveryMode;
+    return pidRuntime.inCrashRecoveryMode;
 }
 
 #ifdef USE_ACRO_TRAINER
 void pidSetAcroTrainerState(bool newState)
 {
-    if (acroTrainerActive != newState) {
+    if (pidRuntime.acroTrainerActive != newState) {
         if (newState) {
             pidAcroTrainerInit();
         }
-        acroTrainerActive = newState;
+        pidRuntime.acroTrainerActive = newState;
     }
 }
 #endif // USE_ACRO_TRAINER
 
 void pidSetAntiGravityState(bool newState)
 {
-    if (newState != antiGravityEnabled) {
+    if (newState != pidRuntime.antiGravityEnabled) {
         // reset the accelerator on state changes
-        itermAccelerator = 0.0f;
+        pidRuntime.itermAccelerator = 0.0f;
     }
-    antiGravityEnabled = newState;
+    pidRuntime.antiGravityEnabled = newState;
 }
 
 bool pidAntiGravityEnabled(void)
 {
-    return antiGravityEnabled;
+    return pidRuntime.antiGravityEnabled;
 }
 
 #ifdef USE_DYN_LPF
 void dynLpfDTermUpdate(float throttle)
 {
     static unsigned int cutoffFreq;
-    if (dynLpfFilter != DYN_LPF_NONE) {
-        if (dynLpfCurveExpo > 0) {
-            cutoffFreq = dynDtermLpfCutoffFreq(throttle, dynLpfMin, dynLpfMax, dynLpfCurveExpo);
+    if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) {
+        if (pidRuntime.dynLpfCurveExpo > 0) {
+            cutoffFreq = dynDtermLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo);
         } else {
-            cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
+            cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
         }
 
-         if (dynLpfFilter == DYN_LPF_PT1) {
+         if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) {
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
+                pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT));
             }
-        } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
+        } else if (pidRuntime.dynLpfFilter == DYN_LPF_BIQUAD) {
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
+                biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
             }
         }
     }
@@ -1681,20 +1174,20 @@ float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfM
 
 void pidSetItermReset(bool enabled)
 {
-    zeroThrottleItermReset = enabled;
+    pidRuntime.zeroThrottleItermReset = enabled;
 }
 
 float pidGetPreviousSetpoint(int axis)
 {
-    return previousPidSetpoint[axis];
+    return pidRuntime.previousPidSetpoint[axis];
 }
 
 float pidGetDT()
 {
-    return dT;
+    return pidRuntime.dT;
 }
 
 float pidGetPidFrequency()
 {
-    return pidFrequency;
+    return pidRuntime.pidFrequency;
 }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 0adb72e20..042b23386 100644
--- a/src/main/flight/pid.h
+++ b/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
@@ -105,6 +106,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 {
@@ -216,6 +225,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];
@@ -228,15 +378,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);
diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c
new file mode 100644
index 000000000..540a7b164
--- /dev/null
+++ b/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));
+    }
+}
+
diff --git a/src/main/flight/pid_init.h b/src/main/flight/pid_init.h
new file mode 100644
index 000000000..9ca6d2690
--- /dev/null
+++ b/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);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 5171cb850..2a4168a26 100644
--- a/src/main/msp/msp.c
+++ b/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"
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index b96a1de9c..289440257 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/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"
 
diff --git a/src/test/Makefile b/src/test/Makefile
index f7fa864ee..1241b32fe 100644
--- a/src/test/Makefile
+++ b/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 := \
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index 3a58e3341..d6085de65 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/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"