Experimental, apply Smith Predictor to DTERM

iNavFlight · Jan 13, 2025 · 47731c8 · 47731c8
1 parent 5bd3fc0
commit 47731c8
Showing 1 changed file with 23 additions and 1 deletion.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -115,6 +115,7 @@ typedef struct {
     pt3Filter_t rateTargetFilter;
 
     smithPredictor_t measurementSmithPredictor;
+    smithPredictor_t dtermSmithPredictor;
 
     fwPidAttenuation_t attenuation;
 } pidState_t;
@@ -376,6 +377,22 @@ bool pidInitFilters(void)
         pidProfile()->measurementSmithPredictorFilterHz,
         getLooptime()
     );
+
+    // Dterm Smith predictor on ROLL and PITCH only
+    smithPredictorInit(
+        &pidState[FD_ROLL].dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorDelay,
+        pidProfile()->dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorFilterHz,
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_PITCH].dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorDelay,
+        pidProfile()->dtermSmithPredictor,
+        pidProfile()->dtermSmithPredictorFilterHz,
+        getLooptime()
+    );
 #endif
 
     pidFiltersConfigured = true;
@@ -735,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) {