From ca2bf9c23b6465cbabc64b7c02e7aca54cb11866 Mon Sep 17 00:00:00 2001 From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com> Date: Mon, 3 May 2021 11:32:34 -0600 Subject: [PATCH 1/2] ehh its just smith predictor if a noob like me can write this code why do we even consider using this crap. EHHH people do seem excited about it though. --- src/main/cms/cms_menu_imu.c | 21 +++++++++++++++ src/main/interface/settings.c | 9 +++++-- src/main/sensors/gyro.c | 51 +++++++++++++++++++++++++++++++++++ src/main/sensors/gyro.h | 22 +++++++++++++++ 4 files changed, 101 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 988bb8a6c9..fd2f7610eb 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -500,6 +500,11 @@ static uint16_t gyroConfig_imuf_yaw_q; static uint16_t gyroConfig_imuf_w; static uint16_t gyroConfig_imuf_sharpness; #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 long cmsx_menuGyro_onEnter(void) { gyroConfig_gyro_lowpass_hz_roll = gyroConfig()->gyro_lowpass_hz[ROLL]; @@ -525,6 +530,11 @@ static long cmsx_menuGyro_onEnter(void) { gyroConfig_imuf_yaw_q = gyroConfig()->imuf_yaw_q; gyroConfig_imuf_w = gyroConfig()->imuf_w; gyroConfig_imuf_sharpness = gyroConfig()->imuf_sharpness; +#endif +#ifdef USE_SMITH_PREDICTOR + smithPredictor_strength = gyroConfig()->smithPredictorStrength; + smithPredictor_delay = gyroConfig()->smithPredictorDelay; + smithPredictor_filt_hz = gyroConfig()->smithPredictorFilterHz; #endif return 0; } @@ -553,6 +563,11 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { gyroConfigMutable()->imuf_yaw_q = gyroConfig_imuf_yaw_q; gyroConfigMutable()->imuf_w = gyroConfig_imuf_w; gyroConfigMutable()->imuf_sharpness = gyroConfig_imuf_sharpness; +#endif +#ifdef USE_SMITH_PREDICTOR + gyroConfigMutable()->smithPredictorStrength = smithPredictor_strength; + gyroConfigMutable()->smithPredictorDelay = smithPredictor_delay; + gyroConfigMutable()->smithPredictorFilterHz = smithPredictor_filt_hz; #endif return 0; } @@ -588,6 +603,12 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "GYRO ABG BOOST", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_abg_boost, 0, 2000, 1 }, 0 }, { "GYRO ABG HL", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_abg_half_life, 0, 250, 1 }, 0 }, +#ifdef USE_SMITH_PREDICTOR + { "SMITH STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &smithPredictor_strength, 0, 100, 1 }, 0 }, + { "SMITH DELAY", OME_UINT8, NULL, &(OSD_UINT8_t) { &smithPredictor_delay, 0, 120, 1 }, 0 }, + { "SMITH FILT", OME_UINT16, NULL, &(OSD_UINT16_t) { &smithPredictor_filt_hz, 1, 1000, 1 }, 0 }, +#endif + { "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index d96d5f60ac..70ae22e69d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -543,10 +543,15 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif #if defined(USE_GYRO_DATA_ANALYSE) - { "dynamic_gyro_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q_factor) }, - { "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, + { "dynamic_gyro_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q_factor) }, + { "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, { "dynamic_gyro_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 600, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) }, #endif +#ifdef USE_SMITH_PREDICTOR + { "smith_predict_str", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorStrength) }, + { "smith_predict_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorDelay) }, + { "smith_predict_filt_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 10000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, smithPredictorFilterHz) }, +#endif // USE_SMITH_PREDICTOR // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1cb63d36f8..654805b54e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -183,6 +183,10 @@ typedef struct gyroSensor_s { gyroAnalyseState_t gyroAnalyseState; float dynNotchQ; #endif + +#ifdef USE_SMITH_PREDICTOR + smithPredictor_t smithPredictor[XYZ_AXIS_COUNT]; +#endif // USE_SMITH_PREDICTOR } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; @@ -262,6 +266,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_ABG_alpha = 0, .gyro_ABG_boost = 275, .gyro_ABG_half_life = 50, + .smithPredictorStrength = 50, + .smithPredictorDelay = 40, + .smithPredictorFilterHz = 5, ); #else //USE_GYRO_IMUF9001 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, @@ -301,6 +308,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_ABG_alpha = 0, .gyro_ABG_boost = 275, .gyro_ABG_half_life = 50, + .smithPredictorStrength = 50, + .smithPredictorDelay = 40, + .smithPredictorFilterHz = 5, ); #endif //USE_GYRO_IMUF9001 @@ -813,6 +823,19 @@ static void gyroInitABGFilter(gyroSensor_t *gyroSensor, uint16_t alpha, uint16_t } } +#ifdef USE_SMITH_PREDICTOR +void smithPredictorInit() { + if (gyroConfig()->smithPredictorDelay > 1) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyro.smithPredictor[axis].samples = gyroConfig()->smithPredictorDelay / (gyro.targetLooptime / 100.0f); + gyro.smithPredictor[axis].idx = 0; + gyro.smithPredictor[axis].smithPredictorStrength = gyroConfig()->smithPredictorStrength / 100.0f; + pt1FilterInit(&gyro.smithPredictor[axis].smithPredictorFilter, pt1FilterGain(gyroConfig()->smithPredictorFilterHz, gyro.targetLooptime * 1e-6f)); + } + } +} +#endif // USE_SMITH_PREDICTOR + static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { #if defined(USE_GYRO_SLEW_LIMITER) gyroInitSlewLimiter(gyroSensor); @@ -837,6 +860,10 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { #endif gyroInitABGFilter(gyroSensor, gyroConfig()->gyro_ABG_alpha, gyroConfig()->gyro_ABG_boost, gyroConfig()->gyro_ABG_half_life); + +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit(); +#endif // USE_SMITH_PREDICTOR } void gyroInitFilters(void) { @@ -1053,6 +1080,30 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current } #endif // USE_YAW_SPIN_RECOVERY +#ifdef USE_SMITH_PREDICTOR +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; + + return gyroFiltered; + } + return gyroFiltered; +} +#endif + #define GYRO_FILTER_FUNCTION_NAME filterGyro #define GYRO_FILTER_DEBUG_SET(...) #include "gyro_filter_impl.h" diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 4f753b59ad..f41a50bec3 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -27,6 +27,10 @@ #include "drivers/bus.h" #include "drivers/sensor.h" +#ifdef USE_SMITH_PREDICTOR +#define MAX_SMITH_SAMPLES 12 * 32 +#endif // USE_SMITH_PREDICTOR + extern float vGyroStdDevModulus; typedef enum { GYRO_NONE = 0, @@ -69,6 +73,7 @@ typedef enum { FILTER_LOWPASS = 0, FILTER_LOWPASS2 } filterSlots; + #if defined(USE_GYRO_IMUF9001) typedef enum { IMUF_RATE_32K = 0, @@ -80,6 +85,19 @@ typedef enum { } imufRate_e; #endif +#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 8ms delay at 8khz + + pt1Filter_t smithPredictorFilter; // filter the smith predictor output for RPY + + float smithPredictorStrength; +} smithPredictor_t; +#endif // USE_SMITH_PREDICTOR + typedef struct gyroConfig_s { uint8_t gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. @@ -129,6 +147,10 @@ typedef struct gyroConfig_s { uint16_t imuf_yaw_q; uint16_t imuf_w; uint16_t imuf_sharpness; + + uint8_t smithPredictorStrength; + uint8_t smithPredictorDelay; + uint16_t smithPredictorFilterHz; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From 8f9793ef44278fee88e46dbd277d4c8269ef6d95 Mon Sep 17 00:00:00 2001 From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com> Date: Tue, 4 May 2021 10:22:06 -0600 Subject: [PATCH 2/2] fix issues --- src/main/sensors/gyro.c | 12 ++++++------ src/main/sensors/gyro.h | 1 + src/main/target/common_fc_pre.h | 1 + 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 654805b54e..1f95f44348 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -824,13 +824,13 @@ static void gyroInitABGFilter(gyroSensor_t *gyroSensor, uint16_t alpha, uint16_t } #ifdef USE_SMITH_PREDICTOR -void smithPredictorInit() { +void smithPredictorInit(gyroSensor_t *gyroSensor) { if (gyroConfig()->smithPredictorDelay > 1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyro.smithPredictor[axis].samples = gyroConfig()->smithPredictorDelay / (gyro.targetLooptime / 100.0f); - gyro.smithPredictor[axis].idx = 0; - gyro.smithPredictor[axis].smithPredictorStrength = gyroConfig()->smithPredictorStrength / 100.0f; - pt1FilterInit(&gyro.smithPredictor[axis].smithPredictorFilter, pt1FilterGain(gyroConfig()->smithPredictorFilterHz, gyro.targetLooptime * 1e-6f)); + gyroSensor->smithPredictor[axis].samples = gyroConfig()->smithPredictorDelay / (gyro.targetLooptime / 100.0f); + gyroSensor->smithPredictor[axis].idx = 0; + gyroSensor->smithPredictor[axis].smithPredictorStrength = gyroConfig()->smithPredictorStrength / 100.0f; + pt1FilterInit(&gyroSensor->smithPredictor[axis].smithPredictorFilter, pt1FilterGain(gyroConfig()->smithPredictorFilterHz, gyro.targetLooptime * 1e-6f)); } } } @@ -862,7 +862,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { gyroInitABGFilter(gyroSensor, gyroConfig()->gyro_ABG_alpha, gyroConfig()->gyro_ABG_boost, gyroConfig()->gyro_ABG_half_life); #ifdef USE_SMITH_PREDICTOR - smithPredictorInit(); + smithPredictorInit(gyroSensor); #endif // USE_SMITH_PREDICTOR } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index f41a50bec3..aea9a491c5 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -23,6 +23,7 @@ #include "common/axis.h" #include "common/time.h" #include "common/maths.h" +#include "common/filter.h" #include "pg/pg.h" #include "drivers/bus.h" #include "drivers/sensor.h" diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index b25b6ac998..2345ab8aba 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -156,6 +156,7 @@ #if (FLASH_SIZE > 128) #define USE_PEGASUS_UI +#define USE_SMITH_PREDICTOR #define USE_SERIALRX_SUMH // Graupner legacy protocol #define USE_CAMERA_CONTROL #define USE_CMS