fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

AlphaFilter: add function to set param from cutoff frequency

Browse Source
This commit is contained in:
bresch 2021-10-05 15:13:17 +02:00 committed by Mathieu Bresciani
parent b5d401b20c
commit 1b58202b0d
1 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/lib/mathlib/math/filter/AlphaFilter.hpp
View File

@ -43,6 +43,9 @@
#pragma once
#include <float.h>
#include <mathlib/math/Functions.hpp>
using namespace math;
template <typename T>
class AlphaFilter
@ -70,6 +73,21 @@ public:
}
}
void setCutoffFreq(float sample_freq, float cutoff_freq)
{
if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f)
|| !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
// No filtering
_alpha = 1.f;
_cutoff_freq = 0.f;
return;
}
setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
_cutoff_freq = cutoff_freq;
}
/**
* Set filter parameter alpha directly without time abstraction
*
@ -96,10 +114,12 @@ public:
}
const T &getState() const { return _filter_state; }
float getCutoffFreq() const { return _cutoff_freq; }
protected:
T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
float _cutoff_freq{0.f};
float _alpha{0.f};
T _filter_state{};
};