Skip to content

Commit

Permalink
Ehh its just smith predictor (emuflight#553)
Browse files Browse the repository at this point in the history 
* 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.

* fix issues

Co-authored-by: nerdCopter <[email protected]>
  • Loading branch information
@Quick-Flash @nerdCopter
Quick-Flash and nerdCopter committed Jun 8, 2021
1 parent 5260b26 commit 97cf224
Show file tree
Hide file tree
Showing 5 changed files with 101 additions and 0 deletions.
21 changes: 21 additions & 0 deletions src/main/cms/cms_menu_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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 }
Expand Down
5 changes: 5 additions & 0 deletions src/main/interface/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -547,6 +547,11 @@ const clivalue_t valueTable[] = {
{ "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 = { 400, 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) },
Expand Down
51 changes: 51 additions & 0 deletions src/main/sensors/gyro.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -813,6 +823,19 @@ static void gyroInitABGFilter(gyroSensor_t *gyroSensor, uint16_t alpha, uint16_t
}
}

#ifdef USE_SMITH_PREDICTOR
void smithPredictorInit(gyroSensor_t *gyroSensor) {
if (gyroConfig()->smithPredictorDelay > 1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
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));
}
}
}
#endif // USE_SMITH_PREDICTOR

static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) {
#if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor);
Expand All @@ -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(gyroSensor);
#endif // USE_SMITH_PREDICTOR
}

void gyroInitFilters(void) {
Expand Down Expand Up @@ -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"
Expand Down
23 changes: 23 additions & 0 deletions src/main/sensors/gyro.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,15 @@
#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"

#ifdef USE_SMITH_PREDICTOR
#define MAX_SMITH_SAMPLES 12 * 32
#endif // USE_SMITH_PREDICTOR

extern float vGyroStdDevModulus;
typedef enum {
GYRO_NONE = 0,
Expand Down Expand Up @@ -69,6 +74,7 @@ typedef enum {
FILTER_LOWPASS = 0,
FILTER_LOWPASS2
} filterSlots;

#if defined(USE_GYRO_IMUF9001)
typedef enum {
IMUF_RATE_32K = 0,
Expand All @@ -80,6 +86,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.
Expand Down Expand Up @@ -129,6 +148,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);
Expand Down
1 change: 1 addition & 0 deletions src/main/target/common_fc_pre.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 97cf224

Please sign in to comment.