From 79ed656fd3fe3fe9dfadfdcce88fa629d5049b43 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Sun, 18 Dec 2022 22:18:32 +0800 Subject: [PATCH] add smith predictor --- src/main/build/debug.c | 3 ++- src/main/build/debug.h | 1 + src/main/cli/settings.c | 6 ++++- src/main/cms/cms_menu_imu.c | 22 ++++++++++++++++++ src/main/sensors/gyro.c | 35 ++++++++++++++++++++++++++++- src/main/sensors/gyro.h | 25 +++++++++++++++++++++ src/main/sensors/gyro_filter_impl.c | 3 +++ src/main/sensors/gyro_init.c | 22 ++++++++++++++++++ src/main/target/common_pre.h | 1 + 9 files changed, 115 insertions(+), 3 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index d3ae5fb3f..1d0ddd692 100644 --- a/src/main/build/debug.c +++ b/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", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 849455025..cf6aea15b 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ed6887ff4..014640286 100644 --- a/src/main/cli/settings.c +++ b/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) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index ebd02c832..fc25235d8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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); @@ -796,6 +802,11 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp) dtermLpfDynMax = pidProfile->dterm_lpf1_dyn_max_hz; 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 +832,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 +861,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} }; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index abccba6d9..344204dfd 100644 --- a/src/main/sensors/gyro.c +++ b/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) @@ -447,6 +450,36 @@ FAST_CODE void gyroUpdate(void) gyro.sampleCount++; } } +#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) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5ff3eb888..29776354a 100644 --- a/src/main/sensors/gyro.h +++ b/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,19 @@ 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 +127,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 +217,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); diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c index 58d7006ba..5db4cab00 100644 --- a/src/main/sensors/gyro_filter_impl.c +++ b/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)); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 4ea4a537e..80a5bcffe 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -236,6 +236,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; @@ -268,6 +286,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) @@ -603,6 +624,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: diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 73fa9b0fe..61857ae57 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -443,6 +443,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)