From 22484476d048232132145dde058384671b07546a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 17 Jun 2024 21:05:04 +0200 Subject: [PATCH 1/4] refactor applySmithPredictor --- src/main/flight/pid.c | 2 +- src/main/flight/smith_predictor.c | 3 +-- src/main/flight/smith_predictor.h | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 98aa47ebbfa..82ba6f31b74 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1216,7 +1216,7 @@ void FAST_CODE pidController(float dT) #endif #ifdef USE_SMITH_PREDICTOR - pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate); + pidState[axis].gyroRate = applySmithPredictor(&pidState[axis].smithPredictor, pidState[axis].gyroRate); #endif } diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index 6a363c4f231..6c8410def21 100644 --- a/src/main/flight/smith_predictor.c +++ b/src/main/flight/smith_predictor.c @@ -34,8 +34,7 @@ #include "flight/smith_predictor.h" #include "build/debug.h" -float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { - UNUSED(axis); +float applySmithPredictor(smithPredictor_t *predictor, float sample) { if (predictor->enabled) { predictor->data[predictor->idx] = sample; diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h index 1d8ebcb13be..b57548874e5 100644 --- a/src/main/flight/smith_predictor.h +++ b/src/main/flight/smith_predictor.h @@ -41,5 +41,5 @@ typedef struct smithPredictor_s { float smithPredictorStrength; } smithPredictor_t; -float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); +float applySmithPredictor(smithPredictor_t *predictor, float sample); void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file From b108e8a7e2477de4bac237045f0a676c2e259261 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 17 Jun 2024 21:08:52 +0200 Subject: [PATCH 2/4] Further refactor smith predictor --- src/main/flight/pid.c | 2 +- src/main/flight/smith_predictor.c | 2 +- src/main/flight/smith_predictor.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 82ba6f31b74..26a7d5d7517 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1216,7 +1216,7 @@ void FAST_CODE pidController(float dT) #endif #ifdef USE_SMITH_PREDICTOR - pidState[axis].gyroRate = applySmithPredictor(&pidState[axis].smithPredictor, pidState[axis].gyroRate); + pidState[axis].gyroRate = smithPredictorApply(&pidState[axis].smithPredictor, pidState[axis].gyroRate); #endif } diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index 6c8410def21..3b049715c2a 100644 --- a/src/main/flight/smith_predictor.c +++ b/src/main/flight/smith_predictor.c @@ -34,7 +34,7 @@ #include "flight/smith_predictor.h" #include "build/debug.h" -float applySmithPredictor(smithPredictor_t *predictor, float sample) { +float smithPredictorApply(smithPredictor_t *predictor, float sample) { if (predictor->enabled) { predictor->data[predictor->idx] = sample; diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h index b57548874e5..32d807f2743 100644 --- a/src/main/flight/smith_predictor.h +++ b/src/main/flight/smith_predictor.h @@ -41,5 +41,5 @@ typedef struct smithPredictor_s { float smithPredictorStrength; } smithPredictor_t; -float applySmithPredictor(smithPredictor_t *predictor, float sample); +float smithPredictorApply(smithPredictor_t *predictor, float sample); void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file From c602992a399c1d914c4bacb3bde784a34aa43778 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 17 Jun 2024 21:33:30 +0200 Subject: [PATCH 3/4] Rename settings to reflect function --- src/main/fc/settings.yaml | 12 +++++------ src/main/flight/ez_tune.c | 2 +- src/main/flight/pid.c | 34 +++++++++++++++---------------- src/main/flight/pid.h | 6 +++--- src/main/flight/smith_predictor.c | 4 ++-- src/main/flight/smith_predictor.h | 2 +- 6 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0d358f58ffb..958a76d1dc5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2252,24 +2252,24 @@ groups: field: fixedWingLevelTrim min: -10 max: 10 - - name: smith_predictor_strength + - name: gyro_predictor_strength description: "The strength factor of a Smith Predictor of PID measurement. In percents" default_value: 0.5 - field: smithPredictorStrength + field: measurementSmithPredictor condition: USE_SMITH_PREDICTOR min: 0 max: 1 - - name: smith_predictor_delay + - name: gyro_predictor_delay description: "Expected delay of the gyro signal. In milliseconds" default_value: 0 - field: smithPredictorDelay + field: measurementSmithPredictorDelay condition: USE_SMITH_PREDICTOR min: 0 max: 8 - - name: smith_predictor_lpf_hz + - name: gyro_predictor_lpf_hz description: "Cutoff frequency for the Smith Predictor Low Pass Filter" default_value: 50 - field: smithPredictorFilterHz + field: measurementSmithPredictorFilterHz condition: USE_SMITH_PREDICTOR min: 1 max: 500 diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index e67e3fe9b47..f65b9cebcf7 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -88,7 +88,7 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT; //Enable Smith predictor - pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz); + pidProfileMutable()->measurementSmithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz); #ifdef USE_DYNAMIC_FILTERS //Enable dynamic notch diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 26a7d5d7517..add21fffa39 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,7 @@ typedef struct { pt3Filter_t rateTargetFilter; - smithPredictor_t smithPredictor; + smithPredictor_t measurementSmithPredictor; fwPidAttenuation_t attenuation; } pidState_t; @@ -309,9 +309,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT, #ifdef USE_SMITH_PREDICTOR - .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, - .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, - .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT, + .measurementSmithPredictor = SETTING_GYRO_PREDICTOR_STRENGTH_DEFAULT, + .measurementSmithPredictorDelay = SETTING_GYRO_PREDICTOR_DELAY_DEFAULT, + .measurementSmithPredictorFilterHz = SETTING_GYRO_PREDICTOR_LPF_HZ_DEFAULT, #endif .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT, .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT, @@ -352,24 +352,24 @@ bool pidInitFilters(void) #ifdef USE_SMITH_PREDICTOR smithPredictorInit( - &pidState[FD_ROLL].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_ROLL].measurementSmithPredictor, + pidProfile()->measurementSmithPredictorDelay, + pidProfile()->measurementSmithPredictor, + pidProfile()->measurementSmithPredictorFilterHz, getLooptime() ); smithPredictorInit( - &pidState[FD_PITCH].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_PITCH].measurementSmithPredictor, + pidProfile()->measurementSmithPredictorDelay, + pidProfile()->measurementSmithPredictor, + pidProfile()->measurementSmithPredictorFilterHz, getLooptime() ); smithPredictorInit( - &pidState[FD_YAW].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_YAW].measurementSmithPredictor, + pidProfile()->measurementSmithPredictorDelay, + pidProfile()->measurementSmithPredictor, + pidProfile()->measurementSmithPredictorFilterHz, getLooptime() ); #endif @@ -1216,7 +1216,7 @@ void FAST_CODE pidController(float dT) #endif #ifdef USE_SMITH_PREDICTOR - pidState[axis].gyroRate = smithPredictorApply(&pidState[axis].smithPredictor, pidState[axis].gyroRate); + pidState[axis].gyroRate = smithPredictorApply(&pidState[axis].measurementSmithPredictor, pidState[axis].gyroRate); #endif } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 26aeb86990d..75fa589638f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -151,9 +151,9 @@ typedef struct pidProfile_s { uint8_t fwAltControlResponseFactor; #ifdef USE_SMITH_PREDICTOR - float smithPredictorStrength; - float smithPredictorDelay; - uint16_t smithPredictorFilterHz; + float measurementSmithPredictor; + float measurementSmithPredictorDelay; + uint16_t measurementSmithPredictorFilterHz; #endif diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index 3b049715c2a..b2d1608d0f6 100644 --- a/src/main/flight/smith_predictor.c +++ b/src/main/flight/smith_predictor.c @@ -45,7 +45,7 @@ float smithPredictorApply(smithPredictor_t *predictor, float sample) { // filter the delayed data to help reduce the overall noise this prediction adds float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); - float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); + float delayCompensatedSample = predictor->measurementSmithPredictor * (sample - delayed); sample += delayCompensatedSample; } @@ -57,7 +57,7 @@ void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength predictor->enabled = true; predictor->samples = (delay * 1000) / looptime; predictor->idx = 0; - predictor->smithPredictorStrength = strength; + predictor->measurementSmithPredictor = strength; pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, US2S(looptime)); } else { predictor->enabled = false; diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h index 32d807f2743..6862c6f9959 100644 --- a/src/main/flight/smith_predictor.h +++ b/src/main/flight/smith_predictor.h @@ -38,7 +38,7 @@ typedef struct smithPredictor_s { uint8_t idx; float data[MAX_SMITH_SAMPLES + 1]; pt1Filter_t smithPredictorFilter; - float smithPredictorStrength; + float measurementSmithPredictor; } smithPredictor_t; float smithPredictorApply(smithPredictor_t *predictor, float sample); From a87301ab7e9f703988c677ac04adf14aab90576a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 17 Jun 2024 21:46:34 +0200 Subject: [PATCH 4/4] Docs update --- docs/Settings.md | 90 ++++++++++++++++++++++++++------------- src/main/fc/settings.yaml | 21 +++++++++ src/main/flight/pid.c | 6 ++- src/main/flight/pid.h | 5 ++- 4 files changed, 90 insertions(+), 32 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 296d86931ee..47251d82674 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -792,6 +792,36 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, --- +### dterm_predictor_delay + +Expected delay of the dterm signal. In milliseconds + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 8 | + +--- + +### dterm_predictor_lpf_hz + +Cutoff frequency for the D-term Smith Predictor Low Pass Filter + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 1 | 500 | + +--- + +### dterm_predictor_strength + +The strength factor of a Smith Predictor of PID D-term. In percents + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 1 | + +--- + ### dynamic_gyro_notch_3d_q Q factor for 3D dynamic notches @@ -1712,6 +1742,36 @@ Software based gyro main lowpass filter. Value is cutoff frequency (Hz) --- +### gyro_predictor_delay + +Expected delay of the gyro signal. In milliseconds + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 8 | + +--- + +### gyro_predictor_lpf_hz + +Cutoff frequency for the Smith Predictor Low Pass Filter + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 1 | 500 | + +--- + +### gyro_predictor_strength + +The strength factor of a Smith Predictor of PID measurement. In percents + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 1 | + +--- + ### gyro_to_use On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro @@ -5832,36 +5892,6 @@ _// TODO_ --- -### smith_predictor_delay - -Expected delay of the gyro signal. In milliseconds - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 8 | - ---- - -### smith_predictor_lpf_hz - -Cutoff frequency for the Smith Predictor Low Pass Filter - -| Default | Min | Max | -| --- | --- | --- | -| 50 | 1 | 500 | - ---- - -### smith_predictor_strength - -The strength factor of a Smith Predictor of PID measurement. In percents - -| Default | Min | Max | -| --- | --- | --- | -| 0.5 | 0 | 1 | - ---- - ### spektrum_sat_bind 0 = disabled. Used to bind the spektrum satellite to RX diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 958a76d1dc5..c253522e33b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1963,6 +1963,27 @@ groups: default_value: "PT2" field: dterm_lpf_type table: filter_type_full + - name: dterm_predictor_strength + description: "The strength factor of a Smith Predictor of PID D-term. In percents" + default_value: 0.5 + field: dtermSmithPredictor + condition: USE_SMITH_PREDICTOR + min: 0 + max: 1 + - name: dterm_predictor_delay + description: "Expected delay of the dterm signal. In milliseconds" + default_value: 0 + field: dtermSmithPredictorDelay + condition: USE_SMITH_PREDICTOR + min: 0 + max: 8 + - name: dterm_predictor_lpf_hz + description: "Cutoff frequency for the D-term Smith Predictor Low Pass Filter" + default_value: 50 + field: dtermSmithPredictorFilterHz + condition: USE_SMITH_PREDICTOR + min: 1 + max: 500 - name: yaw_lpf_hz description: "Yaw P term low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index add21fffa39..aa6a7a6dbb0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -312,6 +312,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .measurementSmithPredictor = SETTING_GYRO_PREDICTOR_STRENGTH_DEFAULT, .measurementSmithPredictorDelay = SETTING_GYRO_PREDICTOR_DELAY_DEFAULT, .measurementSmithPredictorFilterHz = SETTING_GYRO_PREDICTOR_LPF_HZ_DEFAULT, + + .dtermSmithPredictor = SETTING_DTERM_PREDICTOR_STRENGTH_DEFAULT, + .dtermSmithPredictorDelay = SETTING_DTERM_PREDICTOR_DELAY_DEFAULT, + .dtermSmithPredictorFilterHz = SETTING_DTERM_PREDICTOR_LPF_HZ_DEFAULT, #endif .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT, .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 75fa589638f..8639ca1a7a1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -154,8 +154,11 @@ typedef struct pidProfile_s { float measurementSmithPredictor; float measurementSmithPredictorDelay; uint16_t measurementSmithPredictorFilterHz; -#endif + float dtermSmithPredictor; + float dtermSmithPredictorDelay; + uint16_t dtermSmithPredictorFilterHz; +#endif uint16_t fwItermLockTimeMaxMs; uint8_t fwItermLockRateLimit;