Smith Predictor

src/main/build/debug.c

@@ -99,5 +99,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
-    "RX_STATE_TIME"
+    "RX_STATE_TIME",
+    "SMITH_PREDICTOR",
 };

src/main/build/debug.h

@@ -98,6 +98,7 @@ typedef enum {
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
     DEBUG_RX_STATE_TIME,
+    DEBUG_SMITH_PREDICTOR,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -702,7 +702,11 @@ const clivalue_t valueTable[] = {
     { "gyro_lpf1_dyn_expo",         VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_dyn_expo) },
 #endif
     { "gyro_filter_debug_axis",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) },
-
+#ifdef USE_SMITH_PREDICTOR
+    { "smith_predict_str",          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 },    PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorStrength) },
+    { "smith_predict_delay",        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 },    PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorDelay) },
+    { "smith_predict_filt_hz",      VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 10000 },  PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorFilterHz) },
+#endif // USE_SMITH_PREDICTOR
 // PG_ACCELEROMETER_CONFIG
 #if defined(USE_ACC)
     { PARAM_NAME_ACC_HARDWARE,      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },

src/main/cms/cms_menu_imu.c

@@ -777,6 +777,12 @@ static uint16_t dtermLpfDynMax;
 static uint8_t dtermLpfDynExpo;
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+static uint8_t smithPredictor_strength;
+static uint8_t smithPredictor_delay;
+static uint16_t smithPredictor_filt_hz;
+#endif // USE_SMITH_PREDICTOR
+
 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 {
     UNUSED(pDisp);
@@ -797,6 +803,12 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+    smithPredictor_strength  = gyroConfig()->smithPredictorStrength;
+    smithPredictor_delay     = gyroConfig()->smithPredictorDelay;
+    smithPredictor_filt_hz   = gyroConfig()->smithPredictorFilterHz;
+#endif
+
     return NULL;
 }
 
@@ -821,6 +833,12 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+    gyroConfigMutable()->smithPredictorStrength = smithPredictor_strength;
+    gyroConfigMutable()->smithPredictorDelay = smithPredictor_delay;
+    gyroConfigMutable()->smithPredictorFilterHz = smithPredictor_filt_hz;
+#endif
+
     return NULL;
 }
 
@@ -844,6 +862,11 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+    { "SMITH STR",       OME_UINT8,  NULL, &(OSD_UINT8_t)  { &smithPredictor_strength,    0, 100, 1 } },
+    { "SMITH DELAY",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &smithPredictor_delay,       0, 60, 1 } },
+    { "SMITH FILT",      OME_UINT16,  NULL, &(OSD_UINT16_t)  { &smithPredictor_filt_hz,   1, 1000, 1 } },
+#endif
     { "BACK", OME_Back, NULL, NULL },
     { NULL, OME_END, NULL, NULL}
 };

src/main/sensors/gyro.c

@@ -110,7 +110,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyroMovementCalibrationThreshold = 48;
     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
     gyroConfig->gyro_lpf1_type = FILTER_PT1;
-    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;  
+    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
         // NOTE: dynamic lpf is enabled by default so this setting is actually
         // overridden and the static lowpass 1 is disabled. We can't set this
         // value to 0 otherwise Configurator versions 10.4 and earlier will also
@@ -133,6 +133,9 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyro_lpf1_dyn_expo = 5;
     gyroConfig->simplified_gyro_filter = true;
     gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
+    gyroConfig->smithPredictorStrength = 50;
+    gyroConfig->smithPredictorDelay = 40;
+    gyroConfig->smithPredictorFilterHz = 5;
 }
 
 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
@@ -448,6 +451,37 @@ FAST_CODE void gyroUpdate(void)
     }
 }
 
+#ifdef USE_SMITH_PREDICTOR
+FAST_CODE_NOINLINE float applySmithPredictor(smithPredictor_t *smithPredictor, float gyroFiltered, int axis) {
+  if (smithPredictor->samples > 1) {
+    smithPredictor->data[smithPredictor->idx] = gyroFiltered;
+    float input = gyroFiltered;
+
+    smithPredictor->idx++;
+    if (smithPredictor->idx > smithPredictor->samples) {
+        smithPredictor->idx = 0;
+    }
+
+    // filter the delayedGyro to help reduce the overall noise this prediction adds
+    float delayedGyro = smithPredictor->data[smithPredictor->idx];
+
+    float delayCompensatedGyro = smithPredictor->smithPredictorStrength * (gyroFiltered - delayedGyro);
+    delayCompensatedGyro = pt1FilterApply(&smithPredictor->smithPredictorFilter, delayCompensatedGyro);
+    gyroFiltered += delayCompensatedGyro;
+
+    if (axis == (int)(gyro.gyroDebugAxis)) {
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, 0, lrintf(input));
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, 1, lrintf(gyroFiltered));
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, 2, lrintf(delayedGyro));
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, 3, lrintf(delayCompensatedGyro));
+    }
+
+    return gyroFiltered;
+  }
+  return gyroFiltered;
+}
+#endif
+
 #define GYRO_FILTER_FUNCTION_NAME filterGyro
 #define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
 #define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do { UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)

src/main/sensors/gyro.h

@@ -49,6 +49,10 @@
 #define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+#define MAX_SMITH_SAMPLES 6 * 8
+#endif // USE_SMITH_PREDICTOR
+
 typedef union gyroLowpassFilter_u {
     pt1Filter_t pt1FilterState;
     biquadFilter_t biquadFilterState;
@@ -77,6 +81,20 @@ typedef struct gyroSensor_s {
     gyroCalibration_t calibration;
 } gyroSensor_t;
 
+#ifdef USE_SMITH_PREDICTOR
+typedef struct smithPredictor_s {
+    uint8_t samples;
+    uint8_t idx;
+
+    float data[MAX_SMITH_SAMPLES + 1]; // This is gonna be a ring buffer. Max of 6ms delay at 32khz
+
+    pt1Filter_t smithPredictorFilter; // filter the smith predictor output for RPY
+
+    float smithPredictorStrength;
+} smithPredictor_t;
+#endif // USE_SMITH_PREDICTOR
+
+
 typedef struct gyro_s {
     uint16_t sampleRateHz;
     uint32_t targetLooptime;
@@ -110,6 +128,10 @@ typedef struct gyro_s {
     filterApplyFnPtr notchFilter2ApplyFn;
     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 
+#ifdef USE_SMITH_PREDICTOR
+    smithPredictor_t smithPredictor[XYZ_AXIS_COUNT];
+#endif // USE_SMITH_PREDICTOR
+
     uint16_t accSampleRateHz;
     uint8_t gyroToUse;
     uint8_t gyroDebugMode;
@@ -196,6 +218,10 @@ typedef struct gyroConfig_s {
     uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
     uint8_t simplified_gyro_filter;
     uint8_t simplified_gyro_filter_multiplier;
+
+    uint8_t smithPredictorStrength;
+    uint8_t smithPredictorDelay;
+    uint16_t smithPredictorFilterHz;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/gyro_filter_impl.c

@@ -82,6 +82,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         }
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+         gyroADCf = applySmithPredictor(&gyro.smithPredictor[axis], gyroADCf, axis);
+#endif
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
 

src/main/sensors/gyro_init.c

@@ -234,6 +234,24 @@ static void dynLpfFilterInit()
 }
 #endif
 
+#ifdef USE_SMITH_PREDICTOR
+// perhaps make it do all 3 axis at once to reduce some cpu with the circular buffer
+// this would also reduce memory size as only 1 samples and strength would be stored
+void smithPredictorInit() {
+    if (gyroConfig()->smithPredictorDelay > 1) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            memset(&gyro.smithPredictor[axis], 0, sizeof(smithPredictor_t));
+            gyro.smithPredictor[axis].samples = gyroConfig()->smithPredictorDelay / (gyro.targetLooptime / 100.0f);
+            if (gyro.smithPredictor[axis].samples > MAX_SMITH_SAMPLES) {
+              gyro.smithPredictor[axis].samples = MAX_SMITH_SAMPLES;
+            }
+            gyro.smithPredictor[axis].smithPredictorStrength = gyroConfig()->smithPredictorStrength / 100.0f;
+            pt1FilterInit(&gyro.smithPredictor[axis].smithPredictorFilter, pt1FilterGain(gyroConfig()->smithPredictorFilterHz, gyro.targetLooptime * 1e-6f));
+        }
+    }
+}
+#endif // USE_SMITH_PREDICTOR
+
 void gyroInitFilters(void)
 {
     uint16_t gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_static_hz;
@@ -266,6 +284,9 @@ void gyroInitFilters(void)
 #ifdef USE_DYN_NOTCH_FILTER
     dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
 #endif
+#ifdef USE_SMITH_PREDICTOR
+    smithPredictorInit();
+#endif // USE_SMITH_PREDICTOR
 }
 
 #if defined(USE_GYRO_SLEW_LIMITER)
@@ -580,6 +601,7 @@ bool gyroInit(void)
     case DEBUG_GYRO_FILTERED:
     case DEBUG_DYN_LPF:
     case DEBUG_GYRO_SAMPLE:
+    case DEBUG_SMITH_PREDICTOR:
         gyro.gyroDebugMode = debugMode;
         break;
     case DEBUG_DUAL_GYRO_DIFF:

src/main/target/common_pre.h

@@ -421,6 +421,7 @@ extern uint8_t _dmaram_end__;
 #define USE_SIMPLIFIED_TUNING
 #define USE_RX_LINK_UPLINK_POWER
 #define USE_CRSF_V3
+#define USE_SMITH_PREDICTOR
 #endif
 
 #if (TARGET_FLASH_SIZE > 512)