Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Implement throttle based dynamic gyro and dterm filters.

Browse source
This commit is contained in:
Kenneth Mitchell 2018-09-18 20:22:38 -04:00
parent c222f692ea
commit ca460e842b
No known key found for this signature in database
GPG key ID: E27133AAF586AB21
7 changed files with 213 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

70
src/main/flight/pid.c
View file

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