Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: Support changing update period

This commit is contained in:
Leonard Hall 2022-12-05 16:48:51 +10:30 committed by Peter Barker
parent 1382923db0
commit f0822e5121
2 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/Filter/LowPassFilter.cpp
View File

@ -9,6 +9,7 @@
#pragma GCC optimize("O2")
#endif
#include "LowPassFilter.h"
#include <AP_InternalError/AP_InternalError.h>
////////////////////////////////////////////////////////////////////////////////////////////
// DigitalLPF
@ -23,10 +24,18 @@ DigitalLPF<T>::DigitalLPF() {
// add a new raw value to the filter, retrieve the filtered result
template <class T>
T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
if (cutoff_freq <= 0.0f || dt <= 0.0f) {
if (is_negative(cutoff_freq) || is_negative(dt)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
_output = sample;
return _output;
}
if (is_zero(cutoff_freq)) {
_output = sample;
return _output;
}
if (is_zero(dt)) {
return _output;
}
float rc = 1.0f/(M_2PI*cutoff_freq);
alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha;

4
libraries/Filter/SlewLimiter.cpp
View File

@ -40,6 +40,10 @@ SlewLimiter::SlewLimiter(const float &_slew_rate_max, const float &_slew_rate_ta
*/
float SlewLimiter::modifier(float sample, float dt)
{
if (!is_positive(dt)) {
return 1.0;
}
if (slew_rate_max <= 0) {
_output_slew_rate = 0.0;
return 1.0;