rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add smith predictor

This commit is contained in:
Hugo Chiang 2022-12-18 22:18:32 +08:00
parent f592c65433
commit 79ed656fd3
9 changed files with 115 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/build/debug.c
View File

@ -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",
};

1
src/main/build/debug.h
View File

@ -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;

6
src/main/cli/settings.c
View File

@ -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) },

22
src/main/cms/cms_menu_imu.c
View File

@ -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}
};

35
src/main/sensors/gyro.c
View File

@ -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)

25
src/main/sensors/gyro.h
View File

@ -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);

3
src/main/sensors/gyro_filter_impl.c
View File

@ -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));

22
src/main/sensors/gyro_init.c
View File

@ -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:

1
src/main/target/common_pre.h
View File

@ -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)