diff --git a/make/source.mk b/make/source.mk
index 016656ede..b8ea12da2 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 \
@@ -346,7 +347,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
osd/osd.c \
osd/osd_elements.c \
rx/rx_bind.c \
- sensors/gyro_init.c
+ sensors/gyro_init.c\
+ flight/pid_init.c
# Gyro driver files that only contain initialization and configuration code - not runtime code
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 702cb51be..c0e9e56d2 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 95e49dfe6..bf9ef30eb 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 ec2c6a84d..434645011 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, ¤tPidSetpoint);
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 ba0c0d76a..95e65ccb0 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
@@ -107,6 +108,14 @@ typedef enum {
ITERM_RELAX_TYPE_COUNT,
} itermRelaxType_e;
+typedef enum ffInterpolationType_e {
+ FF_INTERPOLATE_OFF,
+ FF_INTERPOLATE_ON,
+ FF_INTERPOLATE_AVG2,
+ FF_INTERPOLATE_AVG3,
+ FF_INTERPOLATE_AVG4
+} ffInterpolationType_t;
+
#define MAX_PROFILE_NAME_LENGTH 8u
typedef struct pidProfile_s {
@@ -218,6 +227,147 @@ typedef struct pidAxisData_s {
float Sum;
} pidAxisData_t;
+typedef union dtermLowpass_u {
+ pt1Filter_t pt1Filter;
+ biquadFilter_t biquadFilter;
+} dtermLowpass_t;
+
+typedef struct pidCoefficient_s {
+ float Kp;
+ float Ki;
+ float Kd;
+ float Kf;
+} pidCoefficient_t;
+
+typedef struct pidRuntime_s {
+ float dT;
+ float pidFrequency;
+ bool pidStabilisationEnabled;
+ float previousPidSetpoint[XYZ_AXIS_COUNT];
+ filterApplyFnPtr dtermNotchApplyFn;
+ biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
+ filterApplyFnPtr dtermLowpassApplyFn;
+ dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
+ filterApplyFnPtr dtermLowpass2ApplyFn;
+ dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
+ filterApplyFnPtr ptermYawLowpassApplyFn;
+ pt1Filter_t ptermYawLowpass;
+ bool antiGravityEnabled;
+ uint8_t antiGravityMode;
+ pt1Filter_t antiGravityThrottleLpf;
+ float antiGravityOsdCutoff;
+ float antiGravityThrottleHpf;
+ float ffBoostFactor;
+ float ffSpikeLimitInverse;
+ float itermAccelerator;
+ uint16_t itermAcceleratorGain;
+ float feedForwardTransition;
+ pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
+ float levelGain;
+ float horizonGain;
+ float horizonTransition;
+ float horizonCutoffDegrees;
+ float horizonFactorRatio;
+ uint8_t horizonTiltExpertMode;
+ float maxVelocity[XYZ_AXIS_COUNT];
+ float itermWindupPointInv;
+ bool inCrashRecoveryMode;
+ timeUs_t crashDetectedAtUs;
+ timeDelta_t crashTimeLimitUs;
+ timeDelta_t crashTimeDelayUs;
+ int32_t crashRecoveryAngleDeciDegrees;
+ float crashRecoveryRate;
+ float crashGyroThreshold;
+ float crashDtermThreshold;
+ float crashSetpointThreshold;
+ float crashLimitYaw;
+ float itermLimit;
+ bool itermRotation;
+ bool zeroThrottleItermReset;
+ bool levelRaceMode;
+
+#ifdef USE_ITERM_RELAX
+ pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
+ uint8_t itermRelax;
+ uint8_t itermRelaxType;
+ uint8_t itermRelaxCutoff;
+#endif
+
+#ifdef USE_ABSOLUTE_CONTROL
+ float acCutoff;
+ float acGain;
+ float acLimit;
+ float acErrorLimit;
+ pt1Filter_t acLpf[XYZ_AXIS_COUNT];
+ float oldSetpointCorrection[XYZ_AXIS_COUNT];
+#endif
+
+#ifdef USE_D_MIN
+ biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
+ pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
+ float dMinPercent[XYZ_AXIS_COUNT];
+ float dMinGyroGain;
+ float dMinSetpointGain;
+#endif
+
+#ifdef USE_AIRMODE_LPF
+ pt1Filter_t airmodeThrottleLpf1;
+ pt1Filter_t airmodeThrottleLpf2;
+#endif
+
+#ifdef USE_RC_SMOOTHING_FILTER
+ pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
+ biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
+ bool setpointDerivativeLpfInitialized;
+ uint8_t rcSmoothingDebugAxis;
+ uint8_t rcSmoothingFilterType;
+#endif // USE_RC_SMOOTHING_FILTER
+
+#ifdef USE_ACRO_TRAINER
+ float acroTrainerAngleLimit;
+ float acroTrainerLookaheadTime;
+ uint8_t acroTrainerDebugAxis;
+ float acroTrainerGain;
+ bool acroTrainerActive;
+ int acroTrainerAxisState[2]; // only need roll and pitch
+#endif
+
+#ifdef USE_DYN_LPF
+ uint8_t dynLpfFilter;
+ uint16_t dynLpfMin;
+ uint16_t dynLpfMax;
+ uint8_t dynLpfCurveExpo;
+#endif
+
+#ifdef USE_LAUNCH_CONTROL
+ uint8_t launchControlMode;
+ uint8_t launchControlAngleLimit;
+ float launchControlKi;
+#endif
+
+#ifdef USE_INTEGRATED_YAW_CONTROL
+ bool useIntegratedYaw;
+ uint8_t integratedYawRelax;
+#endif
+
+#ifdef USE_THRUST_LINEARIZATION
+ float thrustLinearization;
+ float thrustLinearizationReciprocal;
+ float thrustLinearizationB;
+#endif
+
+#ifdef USE_AIRMODE_LPF
+ float airmodeThrottleOffsetLimit;
+#endif
+
+#ifdef USE_INTERPOLATED_SP
+ ffInterpolationType_t ffFromInterpolatedSetpoint;
+ float ffSmoothFactor;
+#endif
+} pidRuntime_t;
+
+extern pidRuntime_t pidRuntime;
+
extern const char pidNames[];
extern pidAxisData_t pidData[3];
@@ -230,15 +380,9 @@ extern pt1Filter_t throttleLpf;
void pidResetIterm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
-void pidInitFilters(const pidProfile_t *pidProfile);
-void pidInitConfig(const pidProfile_t *pidProfile);
-void pidInit(const pidProfile_t *pidProfile);
-void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
bool crashRecoveryModeActive(void);
void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
-void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void);
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 .
+ */
+
+#include
+#include
+#include
+#include
+
+#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 .
+ */
+
+#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 0b0211aac..0017c2da9 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 4b377e416..5f934f1aa 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 51120398d..bf801dbc5 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"