Skip to content
Permalink
Browse files

Implement throttle based dynamic gyro and dterm filters.

  • Loading branch information...
kmitchel committed Sep 19, 2018
1 parent 6adfd34 commit 0cb490da8e9abb8935da198603f6b4d49f7d4f0b
@@ -813,7 +813,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}

pidUpdateAntiGravityThrottleFilter(throttle);

#ifdef USE_DYN_LPF
dynLpfGyroUpdate(throttle);
dynLpfDTermUpdate(throttle);
#endif

#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
@@ -167,7 +167,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dyn_lpf_dterm_max_hz = 200,
.dyn_lpf_dterm_idle = 20,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150;
pidProfile->dterm_lowpass2_hz = 180;
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_type = FILTER_BIQUAD;
#endif
}

void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
@@ -280,7 +288,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
break;
case FILTER_BIQUAD:
#ifdef USE_DYN_LPF
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
#else
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
}
@@ -436,6 +448,15 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
}
}

#ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
#endif

void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@@ -510,6 +531,31 @@ void pidInitConfig(const pidProfile_t *pidProfile)
acLimit = (float)pidProfile->abs_control_limit;
acErrorLimit = (float)pidProfile->abs_control_error_limit;
#endif

#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) {
if (pidProfile->dterm_lowpass_hz > 0 ) {
dynLpfMin = pidProfile->dterm_lowpass_hz;
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
}
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint);
dynLpfDiff = pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin;
#endif
}

void pidInit(const pidProfile_t *pidProfile)
@@ -1130,3 +1176,27 @@ bool pidAntiGravityEnabled(void)
{
return antiGravityEnabled;
}

#ifdef USE_DYN_LPF
void dynLpfDTermUpdate(float throttle)
{
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff;
}

if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, gyroDt));
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, gyro.targetLooptime);
}
}
}
}
#endif
@@ -149,6 +149,8 @@ typedef struct pidProfile_s {
uint8_t abs_control_limit; // Limit to the correction
uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
uint16_t dyn_lpf_dterm_max_hz;
uint8_t dyn_lpf_dterm_idle;
} pidProfile_t;

PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
@@ -214,3 +216,5 @@ float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
float calcHorizonLevelStrength(void);
#endif
void dynLpfDTermUpdate(float throttle);

@@ -550,6 +550,12 @@ const clivalue_t valueTable[] = {
{ "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
{ "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
#endif
#ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
{ "dyn_lpf_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
{ "dyn_lpf_dterm_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) },
#endif

// PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
@@ -183,38 +183,44 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)

PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);

#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
#endif

PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_align = ALIGN_DEFAULT,
.gyroCalibrationDuration = 125, // 1.25 seconds
.gyroMovementCalibrationThreshold = 48,
.gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
.gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
.gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
.gyro_lowpass_type = FILTER_PT1,
.gyro_lowpass_hz = 100,
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz = 300,
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
.gyro_soft_notch_hz_1 = 0,
.gyro_soft_notch_cutoff_1 = 0,
.gyro_soft_notch_hz_2 = 0,
.gyro_soft_notch_cutoff_2 = 0,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
.dyn_filter_width_percent = 40,
.dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
.dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
);
void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
{
gyroConfig->gyro_align = ALIGN_DEFAULT;
gyroConfig->gyroCalibrationDuration = 125; // 1gyroConfig->25 seconds
gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_lowpass_type = FILTER_PT1;
gyroConfig->gyro_lowpass_hz = 100;
gyroConfig->gyro_lowpass2_type = FILTER_PT1;
gyroConfig->gyro_lowpass2_hz = 300;
gyroConfig->gyro_high_fsr = false;
gyroConfig->gyro_use_32khz = false;
gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
gyroConfig->gyro_soft_notch_hz_1 = 0;
gyroConfig->gyro_soft_notch_cutoff_1 = 0;
gyroConfig->gyro_soft_notch_hz_2 = 0;
gyroConfig->gyro_soft_notch_cutoff_2 = 0;
gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = true;
gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_filter_width_percent = 40;
gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS;
gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM;
gyroConfig->dyn_lpf_gyro_max_hz = 400;
gyroConfig->dyn_lpf_gyro_idle = 20;
#ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 150;
#endif
}

#ifdef USE_MULTI_GYRO
#define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
@@ -533,6 +539,41 @@ bool gyroInit(void)
return ret;
}

#ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;

static void dynLpfFilterInit()
{
if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) {
if (gyroConfig()->gyro_lowpass_hz > 0 ) {
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
}
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint);
dynLpfDiff = gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin;
}
#endif

void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{
filterApplyFnPtr *lowpassFilterApplyFn;
@@ -574,7 +615,11 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
}
break;
case FILTER_BIQUAD:
#ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
#endif
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
}
@@ -682,6 +727,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamicNotch(gyroSensor);
#endif
#ifdef USE_DYN_LPF
dynLpfFilterInit();
#endif
}

void gyroInitFilters(void)
@@ -1150,3 +1198,44 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
}
#endif // USE_GYRO_REGISTER_DUMP

#ifdef USE_DYN_LPF
void dynLpfGyroUpdate(float throttle)
{
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff;
}
if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
#else
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
#else
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
#endif
}
}
}
}
#endif
@@ -53,6 +53,12 @@ enum {
DYN_FILTER_RANGE_LOW
} ;

enum {
DYN_LPF_NONE = 0,
DYN_LPF_PT1,
DYN_LPF_BIQUAD
} ;

#define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
@@ -95,6 +101,8 @@ typedef struct gyroConfig_s {
uint8_t dyn_filter_width_percent;
uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
uint16_t dyn_lpf_gyro_max_hz;
uint8_t dyn_lpf_gyro_idle;
} gyroConfig_t;

PG_DECLARE(gyroConfig_t, gyroConfig);
@@ -117,3 +125,6 @@ bool gyroOverflowDetected(void);
bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
#ifdef USE_DYN_LPF
void dynLpfGyroUpdate(float throttle);
#endif
@@ -189,6 +189,7 @@
#define USE_THROTTLE_BOOST
#define USE_RC_SMOOTHING_FILTER
#define USE_ITERM_RELAX
//#define USE_DYN_LPF

#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND

0 comments on commit 0cb490d

Please sign in to comment.
You can’t perform that action at this time.