First cut on Smith Compensator on PID measurement

src/main/CMakeLists.txt

@@ -320,6 +320,8 @@ main_sources(COMMON_SRC
     flight/imu.h
     flight/kalman.c
     flight/kalman.h
+    flight/smith_predictor.c
+    flight/smith_predictor.h
     flight/mixer.c
     flight/mixer.h
     flight/pid.c

src/main/build/debug.h

@@ -83,5 +83,6 @@ typedef enum {
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
+    DEBUG_SMITH_COMPENSATOR,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/settings.yaml

@@ -90,7 +90,8 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
+      "SMITH_COMPENSATOR"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -2058,6 +2059,27 @@ groups:
         field: fixedWingLevelTrim
         min: -10
         max: 10
+      - name: smith_predictor_strength
+        description: "The strength factor of a Smith Predictor of PID measurement. In percents"
+        default_value: 50
+        field: smithPredictorStrength
+        condition: USE_SMITH_PREDICTOR
+        min: 0
+        max: 100
+      - name: smith_predictor_delay
+        description: "Expected delay of the gyro signal. In tenths of a millisecond. 32 means 3.2ms"
+        default_value: 32
+        field: smithPredictorDelay
+        condition: USE_SMITH_PREDICTOR
+        min: 0
+        max: 80
+      - name: smith_predictor_lpf_hz
+        description: "Cutoff frequency for the Smith Predictor Low Pass Filter"
+        default_value: 50
+        field: smithPredictorFilterHz
+        condition: USE_SMITH_PREDICTOR
+        min: 1
+        max: 500
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/rpm_filter.h"
 #include "flight/secondary_imu.h"
 #include "flight/kalman.h"
+#include "flight/smith_predictor.h"
 
 #include "io/gps.h"
 
@@ -109,6 +110,8 @@ typedef struct {
     bool itermFreezeActive;
 
     biquadFilter_t rateTargetFilter;
+
+    smithPredictor_t smithPredictor;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -349,6 +352,30 @@ bool pidInitFilters(void)
         }
     }
 
+#ifdef USE_SMITH_PREDICTOR
+    smithPredictorInit(
+        &pidState[FD_ROLL].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_PITCH].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_YAW].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+#endif
+
     pidFiltersConfigured = true;
 
     return true;
@@ -1052,6 +1079,13 @@ void FAST_CODE pidController(float dT)
             pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget);
         }
 #endif
+
+#ifdef USE_SMITH_PREDICTOR
+        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate);
+        pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate);
+#endif
+
         DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);
     }
 

src/main/flight/pid.h

@@ -161,6 +161,10 @@ typedef struct pidProfile_s {
 #endif
 
     float fixedWingLevelTrim;
+
+    uint8_t smithPredictorStrength;
+    uint8_t smithPredictorDelay;
+    uint16_t smithPredictorFilterHz;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/flight/smith_predictor.c

@@ -0,0 +1,69 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ * 
+ * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight
+ *  
+ */
+
+#include "platform.h"
+#ifdef USE_SMITH_PREDICTOR
+
+#include <stdbool.h>
+#include "common/axis.h"
+#include "flight/smith_predictor.h"
+#include "build/debug.h"
+
+FUNCTION_COMPILE_FOR_SPEED
+float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
+    UNUSED(axis);
+    if (predictor->enabled) {
+        predictor->data[predictor->idx] = sample;
+
+        predictor->idx++;
+        if (predictor->idx > predictor->samples) {
+            predictor->idx = 0;
+        }
+
+        // filter the delayedGyro to help reduce the overall noise this prediction adds
+        float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]);
+        float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed);
+
+        sample += delayCompensatedSample;
+    }
+    return sample;
+}
+
+FUNCTION_COMPILE_FOR_SIZE
+void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime) {
+    if (delay > 0) {
+        predictor->enabled = true;
+        predictor->samples = delay / (looptime / 100.0f);
+        predictor->idx = 0;
+        predictor->smithPredictorStrength = strength / 100.0f;
+        pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f);
+    } else {
+        predictor->enabled = false;
+    }
+}
+
+#endif

src/main/flight/smith_predictor.h

@@ -0,0 +1,45 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ * 
+ * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight
+ * 
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "common/filter.h"
+
+#define MAX_SMITH_SAMPLES 64
+
+typedef struct smithPredictor_s {
+    bool enabled;
+    uint8_t samples;
+    uint8_t idx;
+    float data[MAX_SMITH_SAMPLES + 1];
+    pt1Filter_t smithPredictorFilter;
+    float smithPredictorStrength;
+} smithPredictor_t;
+
+float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample);
+void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime);

src/main/target/common.h

@@ -86,6 +86,7 @@
 
 #define USE_DYNAMIC_FILTERS
 #define USE_GYRO_KALMAN
+#define USE_SMITH_PREDICTOR
 #define USE_EXTENDED_CMS_MENUS
 #define USE_HOTT_TEXTMODE