Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: Use positive and negative direction changes for slew limiting

Browse Source
This commit is contained in:
Paul Riseborough 2021-04-06 20:02:59 +10:00 committed by Andrew Tridgell
parent fa88967b6e
commit 4622e33255
2 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/Filter/SlewLimiter.cpp
View File

@ -44,19 +44,34 @@ float SlewLimiter::modifier(float sample, float dt)
return 1.0; return 1.0;
} }
// Calculate the slew rate amplitude produced by the unmodified sample
// calculate a low pass filtered slew rate // calculate a low pass filtered slew rate
float Pterm_slew_rate = slew_filter.apply((fabsf(sample - last_sample)/ dt), dt); const float slew_rate = slew_filter.apply((sample - last_sample) / dt, dt);
last_sample = sample;
// rectify and apply a decaying envelope filter. The 10 in the // Rectify and apply a decaying envelope filter. The 10 in the
// constrain limits the modifier to be between 0.1 and 1.0, so we // constrain limits the modifier to be between 0.1 and 1.0, so we
// never drop PID gains below 10% of configured value // never drop PID gains below 10% of configured value
float alpha = 1.0 - constrain_float(dt/slew_rate_tau, 0.0, 1.0); // Do this separately for the positive and negative direction and use the smaller of
slew_amplitude = constrain_float(Pterm_slew_rate, alpha * slew_amplitude, 10 * slew_rate_max); // these to avoid single direction transients causing unwanted gain compression
if (slew_rate > _max_pos_slew_rate) {
_max_pos_slew_rate = fminf(slew_rate, 10.0f * slew_rate_max);
} else {
_max_pos_slew_rate *= (1.0f - fminf(dt, slew_rate_tau) / slew_rate_tau);
}
if (slew_rate < -_max_neg_slew_rate) {
_max_neg_slew_rate = fminf(-slew_rate, 10.0f * slew_rate_max);
} else {
_max_neg_slew_rate *= (1.0f - fminf(dt, slew_rate_tau) / slew_rate_tau);
}
const float slew_rate_amplitude = fminf(_max_pos_slew_rate,_max_neg_slew_rate);
// Calculate the gain adjustment // Calculate the gain adjustment
float mod = slew_rate_max / fmaxf(slew_amplitude, slew_rate_max); float mod;
last_sample = mod * sample; if (slew_rate_amplitude > slew_rate_max) {
mod = slew_rate_max / fmaxf(slew_rate_amplitude, slew_rate_max);
} else {
mod = 1.0f;
}
return mod; return mod;
} }

2
libraries/Filter/SlewLimiter.h
View File

@ -31,4 +31,6 @@ private:
LowPassFilterFloat slew_filter; LowPassFilterFloat slew_filter;
float slew_amplitude; float slew_amplitude;
float last_sample; float last_sample;
float _max_pos_slew_rate;
float _max_neg_slew_rate;
}; };