diff --git a/libraries/Filter/SlewLimiter.cpp b/libraries/Filter/SlewLimiter.cpp index d179140cac..455b001230 100644 --- a/libraries/Filter/SlewLimiter.cpp +++ b/libraries/Filter/SlewLimiter.cpp @@ -26,6 +26,10 @@ */ #include "SlewLimiter.h" +#define WINDOW_MS 300 // time in msec required for a half cycle of the slowest oscillation frequency expected +#define MODIFIER_GAIN 1.5f // ratio of modifier reduction to slew rate exceedance ratio +#define DERIVATIVE_CUTOFF_FREQ 25.0f + SlewLimiter::SlewLimiter(const float &_slew_rate_max, const float &_slew_rate_tau) : slew_rate_max(_slew_rate_max), slew_rate_tau(_slew_rate_tau) @@ -85,7 +89,7 @@ float SlewLimiter::modifier(float sample, float dt) // Store a series of positive slew rate exceedance events if (!_pos_event_stored && slew_rate > slew_rate_max) { - if (_pos_event_index >= N_EVENTS) { + if (_pos_event_index >= SLEWLIMITER_N_EVENTS) { _pos_event_index = 0; } _pos_event_ms[_pos_event_index] = now_ms; @@ -96,7 +100,7 @@ float SlewLimiter::modifier(float sample, float dt) // Store a series of negative slew rate exceedance events if (!_neg_event_stored && -slew_rate > slew_rate_max) { - if (_neg_event_index >= N_EVENTS) { + if (_neg_event_index >= SLEWLIMITER_N_EVENTS) { _neg_event_index = 0; } _neg_event_ms[_neg_event_index] = now_ms; @@ -107,7 +111,7 @@ float SlewLimiter::modifier(float sample, float dt) // Find the oldest event time uint32_t oldest_ms = now_ms; - for (uint8_t index = 0; index < N_EVENTS; index++) { + for (uint8_t index = 0; index < SLEWLIMITER_N_EVENTS; index++) { oldest_ms = MIN(oldest_ms, _pos_event_ms[index]); oldest_ms = MIN(oldest_ms, _neg_event_ms[index]); } @@ -116,8 +120,8 @@ float SlewLimiter::modifier(float sample, float dt) // 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 float modifier_input = limited_raw_slew_rate; - 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); + if (now_ms - oldest_ms > (SLEWLIMITER_N_EVENTS + 1) * WINDOW_MS) { + const float oldest_time_from_window = 0.001f*(float)(now_ms - oldest_ms - (SLEWLIMITER_N_EVENTS + 1) * WINDOW_MS); modifier_input *= expf(-oldest_time_from_window / slew_rate_tau); } diff --git a/libraries/Filter/SlewLimiter.h b/libraries/Filter/SlewLimiter.h index 34be2501e5..a35ed08806 100644 --- a/libraries/Filter/SlewLimiter.h +++ b/libraries/Filter/SlewLimiter.h @@ -8,10 +8,7 @@ #include #include "LowPassFilter.h" -#define N_EVENTS 2 // number of positive and negative consecutive slew rate exceedance events recorded where a value of 2 corresponds to a complete cycle -#define WINDOW_MS 300 // time in msec required for a half cycle of the slowest oscillation frequency expected -#define MODIFIER_GAIN 1.5f // ratio of modifier reduction to slew rate exceedance ratio -#define DERIVATIVE_CUTOFF_FREQ 25.0f +#define SLEWLIMITER_N_EVENTS 2 // number of positive and negative consecutive slew rate exceedance events recorded where a value of 2 corresponds to a complete cycle class SlewLimiter { public: @@ -45,8 +42,8 @@ private: uint32_t _max_neg_slew_event_ms; uint8_t _pos_event_index; uint8_t _neg_event_index; - uint32_t _pos_event_ms[N_EVENTS]; - uint32_t _neg_event_ms[N_EVENTS]; + uint32_t _pos_event_ms[SLEWLIMITER_N_EVENTS]; + uint32_t _neg_event_ms[SLEWLIMITER_N_EVENTS]; bool _pos_event_stored; bool _neg_event_stored; };