add smith predictor
This commit is contained in:
parent
f592c65433
commit
79ed656fd3
|
@ -99,5 +99,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
||||||
"TIMING_ACCURACY",
|
"TIMING_ACCURACY",
|
||||||
"RX_EXPRESSLRS_SPI",
|
"RX_EXPRESSLRS_SPI",
|
||||||
"RX_EXPRESSLRS_PHASELOCK",
|
"RX_EXPRESSLRS_PHASELOCK",
|
||||||
"RX_STATE_TIME"
|
"RX_STATE_TIME",
|
||||||
|
"SMITH_PREDICTOR",
|
||||||
};
|
};
|
||||||
|
|
|
@ -98,6 +98,7 @@ typedef enum {
|
||||||
DEBUG_RX_EXPRESSLRS_SPI,
|
DEBUG_RX_EXPRESSLRS_SPI,
|
||||||
DEBUG_RX_EXPRESSLRS_PHASELOCK,
|
DEBUG_RX_EXPRESSLRS_PHASELOCK,
|
||||||
DEBUG_RX_STATE_TIME,
|
DEBUG_RX_STATE_TIME,
|
||||||
|
DEBUG_SMITH_PREDICTOR,
|
||||||
DEBUG_COUNT
|
DEBUG_COUNT
|
||||||
} debugType_e;
|
} debugType_e;
|
||||||
|
|
||||||
|
|
|
@ -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) },
|
{ "gyro_lpf1_dyn_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_dyn_expo) },
|
||||||
#endif
|
#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) },
|
{ "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
|
// PG_ACCELEROMETER_CONFIG
|
||||||
#if defined(USE_ACC)
|
#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) },
|
{ PARAM_NAME_ACC_HARDWARE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },
|
||||||
|
|
|
@ -777,6 +777,12 @@ static uint16_t dtermLpfDynMax;
|
||||||
static uint8_t dtermLpfDynExpo;
|
static uint8_t dtermLpfDynExpo;
|
||||||
#endif
|
#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)
|
static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
|
||||||
{
|
{
|
||||||
UNUSED(pDisp);
|
UNUSED(pDisp);
|
||||||
|
@ -796,6 +802,11 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
|
||||||
dtermLpfDynMax = pidProfile->dterm_lpf1_dyn_max_hz;
|
dtermLpfDynMax = pidProfile->dterm_lpf1_dyn_max_hz;
|
||||||
dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
|
dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_SMITH_PREDICTOR
|
||||||
|
smithPredictor_strength = gyroConfig()->smithPredictorStrength;
|
||||||
|
smithPredictor_delay = gyroConfig()->smithPredictorDelay;
|
||||||
|
smithPredictor_filt_hz = gyroConfig()->smithPredictorFilterHz;
|
||||||
|
#endif
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
@ -821,6 +832,12 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
|
||||||
pidProfile->dterm_lpf1_dyn_expo = dtermLpfDynExpo;
|
pidProfile->dterm_lpf1_dyn_expo = dtermLpfDynExpo;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_SMITH_PREDICTOR
|
||||||
|
gyroConfigMutable()->smithPredictorStrength = smithPredictor_strength;
|
||||||
|
gyroConfigMutable()->smithPredictorDelay = smithPredictor_delay;
|
||||||
|
gyroConfigMutable()->smithPredictorFilterHz = smithPredictor_filt_hz;
|
||||||
|
#endif
|
||||||
|
|
||||||
return NULL;
|
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 } },
|
{ "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo, 0, 10, 1 } },
|
||||||
#endif
|
#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 },
|
{ "BACK", OME_Back, NULL, NULL },
|
||||||
{ NULL, OME_END, NULL, NULL}
|
{ NULL, OME_END, NULL, NULL}
|
||||||
};
|
};
|
||||||
|
|
|
@ -110,7 +110,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
||||||
gyroConfig->gyroMovementCalibrationThreshold = 48;
|
gyroConfig->gyroMovementCalibrationThreshold = 48;
|
||||||
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
|
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
|
||||||
gyroConfig->gyro_lpf1_type = FILTER_PT1;
|
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
|
// 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
|
// 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
|
// 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->gyro_lpf1_dyn_expo = 5;
|
||||||
gyroConfig->simplified_gyro_filter = true;
|
gyroConfig->simplified_gyro_filter = true;
|
||||||
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
|
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)
|
FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
||||||
|
@ -447,6 +450,36 @@ FAST_CODE void gyroUpdate(void)
|
||||||
gyro.sampleCount++;
|
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_FUNCTION_NAME filterGyro
|
||||||
#define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
|
#define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
|
||||||
|
|
|
@ -49,6 +49,10 @@
|
||||||
#define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
|
#define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_SMITH_PREDICTOR
|
||||||
|
#define MAX_SMITH_SAMPLES 6 * 8
|
||||||
|
#endif // USE_SMITH_PREDICTOR
|
||||||
|
|
||||||
typedef union gyroLowpassFilter_u {
|
typedef union gyroLowpassFilter_u {
|
||||||
pt1Filter_t pt1FilterState;
|
pt1Filter_t pt1FilterState;
|
||||||
biquadFilter_t biquadFilterState;
|
biquadFilter_t biquadFilterState;
|
||||||
|
@ -77,6 +81,19 @@ typedef struct gyroSensor_s {
|
||||||
gyroCalibration_t calibration;
|
gyroCalibration_t calibration;
|
||||||
} gyroSensor_t;
|
} 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 {
|
typedef struct gyro_s {
|
||||||
uint16_t sampleRateHz;
|
uint16_t sampleRateHz;
|
||||||
uint32_t targetLooptime;
|
uint32_t targetLooptime;
|
||||||
|
@ -110,6 +127,10 @@ typedef struct gyro_s {
|
||||||
filterApplyFnPtr notchFilter2ApplyFn;
|
filterApplyFnPtr notchFilter2ApplyFn;
|
||||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
#ifdef USE_SMITH_PREDICTOR
|
||||||
|
smithPredictor_t smithPredictor[XYZ_AXIS_COUNT];
|
||||||
|
#endif // USE_SMITH_PREDICTOR
|
||||||
|
|
||||||
uint16_t accSampleRateHz;
|
uint16_t accSampleRateHz;
|
||||||
uint8_t gyroToUse;
|
uint8_t gyroToUse;
|
||||||
uint8_t gyroDebugMode;
|
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 gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
|
||||||
uint8_t simplified_gyro_filter;
|
uint8_t simplified_gyro_filter;
|
||||||
uint8_t simplified_gyro_filter_multiplier;
|
uint8_t simplified_gyro_filter_multiplier;
|
||||||
|
|
||||||
|
uint8_t smithPredictorStrength;
|
||||||
|
uint8_t smithPredictorDelay;
|
||||||
|
uint16_t smithPredictorFilterHz;
|
||||||
} gyroConfig_t;
|
} gyroConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||||
|
|
|
@ -82,6 +82,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
||||||
}
|
}
|
||||||
#endif
|
#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.
|
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
|
||||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
|
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
|
||||||
|
|
||||||
|
|
|
@ -236,6 +236,24 @@ static void dynLpfFilterInit()
|
||||||
}
|
}
|
||||||
#endif
|
#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)
|
void gyroInitFilters(void)
|
||||||
{
|
{
|
||||||
uint16_t gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_static_hz;
|
uint16_t gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_static_hz;
|
||||||
|
@ -268,6 +286,9 @@ void gyroInitFilters(void)
|
||||||
#ifdef USE_DYN_NOTCH_FILTER
|
#ifdef USE_DYN_NOTCH_FILTER
|
||||||
dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
|
dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_SMITH_PREDICTOR
|
||||||
|
smithPredictorInit();
|
||||||
|
#endif // USE_SMITH_PREDICTOR
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(USE_GYRO_SLEW_LIMITER)
|
#if defined(USE_GYRO_SLEW_LIMITER)
|
||||||
|
@ -603,6 +624,7 @@ bool gyroInit(void)
|
||||||
case DEBUG_GYRO_FILTERED:
|
case DEBUG_GYRO_FILTERED:
|
||||||
case DEBUG_DYN_LPF:
|
case DEBUG_DYN_LPF:
|
||||||
case DEBUG_GYRO_SAMPLE:
|
case DEBUG_GYRO_SAMPLE:
|
||||||
|
case DEBUG_SMITH_PREDICTOR:
|
||||||
gyro.gyroDebugMode = debugMode;
|
gyro.gyroDebugMode = debugMode;
|
||||||
break;
|
break;
|
||||||
case DEBUG_DUAL_GYRO_DIFF:
|
case DEBUG_DUAL_GYRO_DIFF:
|
||||||
|
|
|
@ -443,6 +443,7 @@ extern uint8_t _dmaram_end__;
|
||||||
#define USE_SIMPLIFIED_TUNING
|
#define USE_SIMPLIFIED_TUNING
|
||||||
#define USE_RX_LINK_UPLINK_POWER
|
#define USE_RX_LINK_UPLINK_POWER
|
||||||
#define USE_CRSF_V3
|
#define USE_CRSF_V3
|
||||||
|
#define USE_SMITH_PREDICTOR
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if (TARGET_FLASH_SIZE > 512)
|
#if (TARGET_FLASH_SIZE > 512)
|
||||||
|
|
Loading…
Reference in New Issue