Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: SlewLimiter: always caculate slew limit

Browse Source
This commit is contained in:
Iampete1 2023-04-25 02:27:16 +01:00 committed by Andrew Tridgell
parent b8b2bfafdf
commit c161875659
1 changed files with 37 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
libraries/Filter/SlewLimiter.cpp
View File

@ -43,19 +43,46 @@ float SlewLimiter::modifier(float sample, float dt)
if (!is_positive(dt)) { if (!is_positive(dt)) {
return 1.0; return 1.0;
} }
if (slew_rate_max <= 0) {
_output_slew_rate = 0.0;
return 1.0;
}
// Calculate a low pass filtered slew rate // Calculate a low pass filtered slew rate
const float slew_rate = slew_filter.apply((sample - last_sample) / dt, dt); const float slew_rate = slew_filter.apply((sample - last_sample) / dt, dt);
last_sample = sample; last_sample = sample;
uint32_t now_ms = AP_HAL::millis(); uint32_t now_ms = AP_HAL::millis();
// Apply a filter to decay maximum seen slew rate once the value had left the window period
const float decay_alpha = fminf(dt, slew_rate_tau) / slew_rate_tau; const float decay_alpha = fminf(dt, slew_rate_tau) / slew_rate_tau;
// Apply a filter to increases in slew rate only to reduce the effect of gusts and large controller setpoint changes
const float attack_alpha = fminf(2.0f * decay_alpha, 1.0f);
// Decay the peak positive and negative slew rate if they are outside the window
// Never drop PID gains below 10% of configured value
if (slew_rate > _max_pos_slew_rate) {
_max_pos_slew_rate = slew_rate;
_max_pos_slew_event_ms = now_ms;
} else if (now_ms - _max_pos_slew_event_ms > WINDOW_MS) {
_max_pos_slew_rate *= (1.0f - decay_alpha);
}
if (-slew_rate > _max_neg_slew_rate) {
_max_neg_slew_rate = -slew_rate;
_max_neg_slew_event_ms = now_ms;
} else if (now_ms - _max_neg_slew_event_ms > WINDOW_MS) {
_max_neg_slew_rate *= (1.0f - decay_alpha);
}
const float raw_slew_rate = 0.5f*(_max_pos_slew_rate + _max_neg_slew_rate);
_output_slew_rate = (1.0f - attack_alpha) * _output_slew_rate + attack_alpha * raw_slew_rate;
_output_slew_rate = fminf(_output_slew_rate, raw_slew_rate);
if (slew_rate_max <= 0) {
return 1.0;
}
// Constrain slew rate used for calculation
const float limited_raw_slew_rate = 0.5f*(fminf(_max_pos_slew_rate, 10.0f * slew_rate_max) + fminf(_max_neg_slew_rate, 10.0f * slew_rate_max));
// Store a series of positive slew rate exceedance events // Store a series of positive slew rate exceedance events
if (!_pos_event_stored && slew_rate > slew_rate_max) { if (!_pos_event_stored && slew_rate > slew_rate_max) {
if (_pos_event_index >= N_EVENTS) { if (_pos_event_index >= N_EVENTS) {
@ -68,7 +95,7 @@ float SlewLimiter::modifier(float sample, float dt)
} }
// Store a series of negative slew rate exceedance events // Store a series of negative slew rate exceedance events
if (!_neg_event_stored && slew_rate < - slew_rate_max) { if (!_neg_event_stored && -slew_rate > slew_rate_max) {
if (_neg_event_index >= N_EVENTS) { if (_neg_event_index >= N_EVENTS) {
_neg_event_index = 0; _neg_event_index = 0;
} }
@ -81,57 +108,26 @@ float SlewLimiter::modifier(float sample, float dt)
// Find the oldest event time // Find the oldest event time
uint32_t oldest_ms = now_ms; uint32_t oldest_ms = now_ms;
for (uint8_t index = 0; index < N_EVENTS; index++) { for (uint8_t index = 0; index < N_EVENTS; index++) {
if (_pos_event_ms[index] < oldest_ms) { oldest_ms = MIN(oldest_ms, _pos_event_ms[index]);
oldest_ms = _pos_event_ms[index]; oldest_ms = MIN(oldest_ms, _neg_event_ms[index]);
}
if (_neg_event_ms[index] < oldest_ms) {
oldest_ms = _neg_event_ms[index];
}
} }
// Decay the peak positive and negative slew rate if they are outside the window
// Never drop PID gains below 10% of configured value
if (slew_rate > _max_pos_slew_rate) {
_max_pos_slew_rate = fminf(slew_rate, 10.0f * slew_rate_max);
_max_pos_slew_event_ms = now_ms;
} else if (now_ms - _max_pos_slew_event_ms > WINDOW_MS) {
_max_pos_slew_rate *= (1.0f - decay_alpha);
}
if (slew_rate < -_max_neg_slew_rate) {
_max_neg_slew_rate = fminf(-slew_rate, 10.0f * slew_rate_max);
_max_neg_slew_event_ms = now_ms;
} else if (now_ms - _max_neg_slew_event_ms > WINDOW_MS) {
_max_neg_slew_rate *= (1.0f - decay_alpha);
}
const float raw_slew_rate = 0.5f*(_max_pos_slew_rate + _max_neg_slew_rate);
// Apply a further reduction when the oldest exceedance event falls outside the window required for the // Apply a further reduction when the oldest exceedance event falls outside the window required for the
// specified number of exceedance events. This prevents spikes due to control mode changed, etc causing // specified number of exceedance events. This prevents spikes due to control mode changed, etc causing
// unwanted gain reduction and is only applied to the slew rate used for gain reduction // unwanted gain reduction and is only applied to the slew rate used for gain reduction
float modifier_input = raw_slew_rate; float modifier_input = limited_raw_slew_rate;
if (now_ms - oldest_ms > (N_EVENTS + 1) * WINDOW_MS) { if (now_ms - oldest_ms > (N_EVENTS + 1) * WINDOW_MS) {
const float oldest_time_from_window = 0.001f*(float)(now_ms - oldest_ms - (N_EVENTS + 1) * WINDOW_MS); const float oldest_time_from_window = 0.001f*(float)(now_ms - oldest_ms - (N_EVENTS + 1) * WINDOW_MS);
modifier_input *= expf(-oldest_time_from_window / slew_rate_tau); modifier_input *= expf(-oldest_time_from_window / slew_rate_tau);
} }
// Apply a filter to increases in slew rate only to reduce the effect of gusts and large controller
// setpoint changes
const float attack_alpha = fminf(2.0f * decay_alpha, 1.0f);
_modifier_slew_rate = (1.0f - attack_alpha) * _modifier_slew_rate + attack_alpha * modifier_input; _modifier_slew_rate = (1.0f - attack_alpha) * _modifier_slew_rate + attack_alpha * modifier_input;
_modifier_slew_rate = fminf(_modifier_slew_rate, modifier_input); _modifier_slew_rate = fminf(_modifier_slew_rate, modifier_input);
_output_slew_rate = (1.0f - attack_alpha) * _output_slew_rate + attack_alpha * raw_slew_rate;
_output_slew_rate = fminf(_output_slew_rate, raw_slew_rate);
// Calculate the gain adjustment // Calculate the gain adjustment
float mod; float mod = 1.0f;
if (_modifier_slew_rate > slew_rate_max) { if (_modifier_slew_rate > slew_rate_max) {
mod = slew_rate_max / (slew_rate_max + MODIFIER_GAIN * (_modifier_slew_rate - slew_rate_max)); mod = slew_rate_max / (slew_rate_max + MODIFIER_GAIN * (_modifier_slew_rate - slew_rate_max));
} else {
mod = 1.0f;
} }
return mod; return mod;