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

Filter: Support changing update period

Browse Source
This commit is contained in:
Leonard Hall 2022-12-05 16:48:51 +10:30 committed by Andrew Tridgell
parent f07448a0ac
commit a11a1db58b
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") #pragma GCC optimize("O2")
#endif #endif
#include "LowPassFilter.h" #include "LowPassFilter.h"
#include <AP_InternalError/AP_InternalError.h>
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
// DigitalLPF // DigitalLPF
@ -23,10 +24,18 @@ DigitalLPF<T>::DigitalLPF() {
// add a new raw value to the filter, retrieve the filtered result // add a new raw value to the filter, retrieve the filtered result
template <class T> template <class T>
T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) { 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; _output = sample;
return _output; 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); float rc = 1.0f/(M_2PI*cutoff_freq);
alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha; _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) float SlewLimiter::modifier(float sample, float dt)
{ {
if (!is_positive(dt)) {
return 1.0;
}
if (slew_rate_max <= 0) { if (slew_rate_max <= 0) {
_output_slew_rate = 0.0; _output_slew_rate = 0.0;
return 1.0; return 1.0;