Smith Predictor on D-term

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
@@ -1892,6 +1922,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
@@ -6142,36 +6202,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

src/main/fc/settings.yaml

@@ -1989,6 +1989,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
@@ -2284,24 +2305,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

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

src/main/flight/pid.c

@@ -114,7 +114,8 @@ typedef struct {
 
     pt3Filter_t rateTargetFilter;
 
-    smithPredictor_t smithPredictor;
+    smithPredictor_t measurementSmithPredictor;
+    smithPredictor_t dtermSmithPredictor;
 
     fwPidAttenuation_t attenuation;
 } pidState_t;
@@ -309,9 +310,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 +357,40 @@ 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].measurementSmithPredictor,
+        pidProfile()->measurementSmithPredictorDelay,
+        pidProfile()->measurementSmithPredictor,
+        pidProfile()->measurementSmithPredictorFilterHz,
         getLooptime()
     );
     smithPredictorInit(
-        &pidState[FD_PITCH].smithPredictor,
-        pidProfile()->smithPredictorDelay,
-        pidProfile()->smithPredictorStrength,
-        pidProfile()->smithPredictorFilterHz,
+        &pidState[FD_YAW].measurementSmithPredictor,
+        pidProfile()->measurementSmithPredictorDelay,
+        pidProfile()->measurementSmithPredictor,
+        pidProfile()->measurementSmithPredictorFilterHz,
         getLooptime()
     );
+
+    // Dterm Smith predictor on ROLL and PITCH only
     smithPredictorInit(
-        &pidState[FD_YAW].smithPredictor,
-        pidProfile()->smithPredictorDelay,
-        pidProfile()->smithPredictorStrength,
-        pidProfile()->smithPredictorFilterHz,
+        &pidState[FD_ROLL].dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorDelay,
+        pidProfile()->dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorFilterHz,
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_PITCH].dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorDelay,
+        pidProfile()->dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorFilterHz,
         getLooptime()
     );
 #endif
@@ -731,8 +752,13 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
 
         // Calculate derivative
         newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
+
+        //Apply D-term smith predictor
+#ifdef USE_SMITH_PREDICTOR
+        newDTerm = smithPredictorApply(&pidState->dtermSmithPredictor, newDTerm);
+#endif
     }
-    return(newDTerm);
+    return newDTerm;
 }
 
 static void applyItermLimiting(pidState_t *pidState) {
@@ -1216,7 +1242,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
     }
 

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;

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;

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);