Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Ehh its just smith predictor #553

Merged
merged 6 commits into from
May 16, 2021
Merged

Ehh its just smith predictor #553

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
ca2bf9c
ehh its just smith predictor
Quick-Flash May 3, 2021
8f9793e
fix issues
Quick-Flash May 4, 2021
0cb3c77
Merge branch 'master' into smith_predictor
nerdCopter May 4, 2021
4aa7f8d
Merge branch 'master' into smith_predictor
nerdCopter May 5, 2021
4cda959
Merge branch 'master' into smith_predictor
nerdCopter May 10, 2021
bdd8c92
Merge branch 'master' into smith_predictor
Quick-Flash May 13, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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