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

Merge pull request #6943 from kmitchel/dynamic_lowpass 

Implement throttle based dynamic gyro and dterm filters.
This commit is contained in:
Michael Keller 2018-10-27 19:24:16 +13:00 committed by GitHub
parent a35b310851 ff8d1a842a
commit 8609346f21
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 212 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/flight/mixer.c
View File

@ -815,7 +815,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);

76
src/main/flight/pid.c
View File

@ -121,16 +121,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
.yaw_lowpass_hz = 0,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_notch_hz = 0,
.dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.itermWindupPointPercent = 40,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
@ -167,7 +162,19 @@ 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,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 120;
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 +287,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 +447,14 @@ 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 dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
#endif
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@ -510,6 +529,30 @@ 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;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -1130,3 +1173,26 @@ bool pidAntiGravityEnabled(void)
{
return antiGravityEnabled;
}
#ifdef USE_DYN_LPF
void dynLpfDTermUpdate(float throttle)
{
if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
if (dynLpfFilter == DYN_LPF_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
}
}
}
}
#endif

4
src/main/flight/pid.h
View File

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

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

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

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

@ -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; // 1.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 = 120;
#endif
}
#ifdef USE_MULTI_GYRO
#define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
@ -533,6 +539,39 @@ 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 dynLpfInvIdlePointScaled;
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;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
}
#endif
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{
filterApplyFnPtr *lowpassFilterApplyFn;
@ -574,7 +613,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 +725,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 +1196,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_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
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

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

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

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

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