From 17a995e03e963bb2ab12e3078695e019dd1a3983 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 10 Dec 2018 09:05:20 +0100 Subject: [PATCH 1/5] fix abs control refactor, make abs control cutoff configurable --- src/main/flight/pid.c | 65 ++++++++++++++++++----------------- src/main/flight/pid.h | 1 + src/main/interface/settings.c | 1 + 3 files changed, 36 insertions(+), 31 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9857a899ab..aac8277b8d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 89afb0c65d..b51a7cbc07 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; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index bef291c7e7..878d0c3229 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 From e63cb48adfddafdb20a92285f8f1656ad3b3dbeb Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 10 Dec 2018 10:51:45 +0100 Subject: [PATCH 2/5] unit test fix --- src/test/unit/pid_unittest.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index f716d1f578..0520a6798e 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -589,15 +589,13 @@ 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)); From 56181a87c8765a07411d6b7a60801ee4ca5960f1 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 10 Dec 2018 13:57:35 +0100 Subject: [PATCH 3/5] change pid.h for unit test --- src/main/flight/pid.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b51a7cbc07..30ce2d5729 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -215,8 +215,7 @@ 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); + const float setpointLpf, const float setpointHpf, float *currentPidSetpoint, float *itermErrorRate); void rotateItermAndAxisError(); float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint); From b0af988a7518684cf7edb11a3f378dc59670d84e Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 10 Dec 2018 19:28:18 +0100 Subject: [PATCH 4/5] unit test bugs --- src/main/flight/pid.h | 3 +-- src/test/unit/pid_unittest.cc | 9 ++------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 30ce2d5729..37a9c41110 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -214,8 +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/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 0520a6798e..6c0dfccf78 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -589,8 +589,6 @@ TEST(pidControllerTest, testAbsoluteControl) { pidStabilisationState(PID_STABILISATION_ON); float gyroRate = 0; - float setpointLpf = 6; - float setpointHpf = 30; float itermErrorRate = 10; float currentPidSetpoint = 10; @@ -600,16 +598,13 @@ TEST(pidControllerTest, testAbsoluteControl) { 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)); } From 8bcc4dab64b75abf0f91df9837caf116125d4791 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Tue, 11 Dec 2018 08:18:51 +0100 Subject: [PATCH 5/5] fix indentation and pidProfile version number --- src/main/flight/pid.c | 2 +- src/main/interface/settings.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index aac8277b8d..932c1318cb 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) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 878d0c3229..4ff2f85cd5 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -934,7 +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) }, + { "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