diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..8c518152a1e 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 @@ -1812,6 +1842,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 @@ -5992,36 +6052,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 6c16ce93b21..eb3b5e42c90 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1978,6 +1978,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 @@ -2267,24 +2288,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 f1b2f567812..c020f3d1bc9 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; @@ -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 = { @@ -309,9 +309,13 @@ 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, + + .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, @@ -352,24 +356,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 +1220,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 = smithPredictorApply(&pidState[axis].measurementSmithPredictor, pidState[axis].gyroRate); #endif } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 26aeb86990d..8639ca1a7a1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -151,11 +151,14 @@ typedef struct pidProfile_s { uint8_t fwAltControlResponseFactor; #ifdef USE_SMITH_PREDICTOR - float smithPredictorStrength; - float smithPredictorDelay; - uint16_t smithPredictorFilterHz; -#endif + float measurementSmithPredictor; + float measurementSmithPredictorDelay; + uint16_t measurementSmithPredictorFilterHz; + float dtermSmithPredictor; + float dtermSmithPredictorDelay; + uint16_t dtermSmithPredictorFilterHz; +#endif uint16_t fwItermLockTimeMaxMs; uint8_t fwItermLockRateLimit; diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index 6a363c4f231..b2d1608d0f6 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 smithPredictorApply(smithPredictor_t *predictor, float sample) { if (predictor->enabled) { predictor->data[predictor->idx] = sample; @@ -46,7 +45,7 @@ float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sampl // 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; } @@ -58,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 1d8ebcb13be..6862c6f9959 100644 --- a/src/main/flight/smith_predictor.h +++ b/src/main/flight/smith_predictor.h @@ -38,8 +38,8 @@ typedef struct smithPredictor_s { uint8_t idx; float data[MAX_SMITH_SAMPLES + 1]; pt1Filter_t smithPredictorFilter; - float smithPredictorStrength; + float measurementSmithPredictor; } smithPredictor_t; -float applySmithPredictor(uint8_t axis, 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