diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9857a899a..932c1318c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 6); void resetPidProfile(pidProfile_t *pidProfile) { @@ -166,6 +166,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_gain = 0, .abs_control_limit = 90, .abs_control_error_limit = 20, + .abs_control_cutoff = 11, .antiGravityMode = ANTI_GRAVITY_SMOOTH, .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default @@ -239,6 +240,15 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelax; static FAST_RAM_ZERO_INIT uint8_t itermRelaxType; static FAST_RAM_ZERO_INIT 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]; +#endif + #ifdef USE_RC_SMOOTHING_FILTER static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT]; @@ -352,6 +362,13 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } #endif +#if defined(USE_ABSOLUTE_CONTROL) + if (itermRelax) { + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + pt1FilterInit(&acLpf[i], pt1FilterGain(acCutoff, dT)); + } + } +#endif pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT)); } @@ -424,12 +441,6 @@ static FAST_RAM_ZERO_INIT bool itermRotation; #if defined(USE_SMART_FEEDFORWARD) static FAST_RAM_ZERO_INIT bool smartFeedforward; #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; -#endif #ifdef USE_LAUNCH_CONTROL static FAST_RAM_ZERO_INIT uint8_t launchControlMode; @@ -542,6 +553,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) 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; #endif #ifdef USE_DYN_LPF @@ -918,31 +930,27 @@ void FAST_CODE applySmartFeedforward(int axis) #if defined(USE_ITERM_RELAX) #if defined(USE_ABSOLUTE_CONTROL) -STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, - const float setpointLpf, const float setpointHpf, - float *currentPidSetpoint, float *itermErrorRate) +STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate) { if (acGain > 0) { + const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint); + const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); float acErrorRate = 0; - if (itermRelaxIsEnabled) { - const float gmaxac = setpointLpf + 2 * setpointHpf; - const float gminac = setpointLpf - 2 * setpointHpf; - if (gyroRate >= gminac && gyroRate <= gmaxac) { - const float acErrorRate1 = gmaxac - gyroRate; - const float acErrorRate2 = gminac - gyroRate; - if (acErrorRate1 * axisError[axis] < 0) { - acErrorRate = acErrorRate1; - } else { - acErrorRate = acErrorRate2; - } - if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { - acErrorRate = -axisError[axis] * pidFrequency; - } + const float gmaxac = setpointLpf + 2 * setpointHpf; + const float gminac = setpointLpf - 2 * setpointHpf; + if (gyroRate >= gminac && gyroRate <= gmaxac) { + const float acErrorRate1 = gmaxac - gyroRate; + const float acErrorRate2 = gminac - gyroRate; + if (acErrorRate1 * axisError[axis] < 0) { + acErrorRate = acErrorRate1; } else { - acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; + acErrorRate = acErrorRate2; + } + if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { + acErrorRate = -axisError[axis] * pidFrequency; } } else { - acErrorRate = *itermErrorRate; + acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; } if (isAirmodeActivated()) { @@ -964,12 +972,10 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, { const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); - bool itermRelaxIsEnabled = false; if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) { - itermRelaxIsEnabled = true; const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; const bool isDecreasingI = @@ -989,11 +995,8 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); } - } else { #if defined(USE_ABSOLUTE_CONTROL) - applyAbsoluteControl(axis, gyroRate, setpointLpf, setpointHpf, itermRelaxIsEnabled, currentPidSetpoint, itermErrorRate); -#else - UNUSED(itermRelaxIsEnabled); + applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate); #endif } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 89afb0c65..37a9c4111 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -148,6 +148,7 @@ typedef struct pidProfile_s { uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for uint8_t abs_control_limit; // Limit to the correction uint8_t abs_control_error_limit; // Limit to the accumulated error + uint8_t abs_control_cutoff; // Cutoff frequency for path estimation in abs control uint8_t dterm_filter2_type; // Filter selection for 2nd dterm uint16_t dyn_lpf_dterm_min_hz; uint16_t dyn_lpf_dterm_max_hz; @@ -213,9 +214,7 @@ bool pidAntiGravityEnabled(void); extern float axisError[XYZ_AXIS_COUNT]; void applyItermRelax(const int axis, const float iterm, const float gyroRate, float *itermErrorRate, float *currentPidSetpoint); -void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, - const float setpointLpf, const float setpointHpf, - float *currentPidSetpoint, float *itermErrorRate); +void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate); void rotateItermAndAxisError(); float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 63ea5fba0..d39cd52d9 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -934,6 +934,7 @@ const clivalue_t valueTable[] = { { "abs_control_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) }, { "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) }, { "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) }, + { "abs_control_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_cutoff) }, #endif #ifdef USE_LAUNCH_CONTROL diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index f716d1f57..6c0dfccf7 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -589,29 +589,22 @@ TEST(pidControllerTest, testAbsoluteControl) { pidStabilisationState(PID_STABILISATION_ON); float gyroRate = 0; - bool itermRelaxIsEnabled = false; - float setpointLpf = 6; - float setpointHpf = 30; float itermErrorRate = 10; float currentPidSetpoint = 10; - applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, - ¤tPidSetpoint, &itermErrorRate); + applyAbsoluteControl(FD_PITCH, gyroRate, ¤tPidSetpoint, &itermErrorRate); ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); - itermRelaxIsEnabled = true; - applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, - ¤tPidSetpoint, &itermErrorRate); + applyAbsoluteControl(FD_PITCH, gyroRate, ¤tPidSetpoint, &itermErrorRate); ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); gyroRate = -53; axisError[FD_PITCH] = -60; - applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, - ¤tPidSetpoint, &itermErrorRate); + applyAbsoluteControl(FD_PITCH, gyroRate, ¤tPidSetpoint, &itermErrorRate); ASSERT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2)); ASSERT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2)); }