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

Filter: LowPassFilter: use `calc_lowpass_alpha_dt` helper

This commit is contained in:
Iampete1 2024-08-26 11:17:14 +01:00 committed by Andrew Tridgell
parent 5453f3a0cc
commit 3c6e5691e4
1 changed files with 1 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/Filter/LowPassFilter.cpp
View File

@ -117,20 +117,7 @@ float LowPassFilter<T>::get_cutoff_freq() const {
// 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 LowPassFilter<T>::apply(const T &sample, const float &dt) { T LowPassFilter<T>::apply(const T &sample, const float &dt) {
if (is_negative(cutoff_freq) || is_negative(dt)) { const float alpha = calc_lowpass_alpha_dt(dt, cutoff_freq);
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
this->reset(sample);
return this->get();
}
if (is_zero(cutoff_freq)) {
this->reset(sample);
return this->get();
}
if (is_zero(dt)) {
return this->get();
}
const float rc = 1.0f/(M_2PI*cutoff_freq);
const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
return this->_apply(sample, alpha); return this->_apply(sample, alpha);
} }