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

Merge pull request #9365 from IllusionFpv/dterm_lpf_configurable_curve 

Dterm dynamic lowpass filter configurable curve
This commit is contained in:
Michael Keller 2020-01-17 02:23:57 +13:00 committed by GitHub
parent 02be6011f9 3ae5830eb0
commit 39800e0b13
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -995,6 +995,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_DYN_LPF
{ "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
{ "dyn_lpf_dterm_curve_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_curve_expo) },
#endif
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },
{ "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) },

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

@ -217,6 +217,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.ff_max_rate_limit = 100,
.ff_smooth_factor = 37,
.ff_boost = 15,
.dyn_lpf_curve_expo = 0,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;
@ -588,6 +589,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
static FAST_RAM_ZERO_INIT uint8_t dynLpfCurveExpo;
#endif
#ifdef USE_D_MIN
@ -703,6 +705,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
}
dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
#endif
#ifdef USE_LAUNCH_CONTROL
@ -1597,8 +1600,13 @@ bool pidAntiGravityEnabled(void)
#ifdef USE_DYN_LPF
void dynLpfDTermUpdate(float throttle)
{
static unsigned int cutoffFreq;
if (dynLpfFilter != DYN_LPF_NONE) {
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (dynLpfCurveExpo > 0) {
cutoffFreq = dynDtermLpfCutoffFreq(throttle, dynLpfMin, dynLpfMax, dynLpfCurveExpo);
} else {
cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
}
if (dynLpfFilter == DYN_LPF_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -1613,6 +1621,13 @@ void dynLpfDTermUpdate(float throttle)
}
#endif
float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
const float expof = expo / 10.0f;
static float curve;
curve = 2 * throttle * (1 - throttle) * expof + powerf(throttle, 2);
return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
}
void pidSetItermReset(bool enabled)
{
zeroThrottleItermReset = enabled;

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

@ -185,6 +185,7 @@ typedef struct pidProfile_s {
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -264,3 +265,4 @@ float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetFfSmoothFactor();
float pidGetSpikeLimitInverse();
float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);