Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: add get_cutoff_frequency

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-31 16:26:48 -07:00 committed by Randy Mackay
parent 1828515b3b
commit 57f8a4d29d
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/Filter/LowPassFilter.h
View File

@ -35,6 +35,7 @@ public:
LowPassFilter(); LowPassFilter();
void set_cutoff_frequency(float time_step, float cutoff_freq); void set_cutoff_frequency(float time_step, float cutoff_freq);
float get_cutoff_frequency() { return _cutoff_hz; }
void set_time_constant(float time_step, float time_constant); void set_time_constant(float time_step, float time_constant);
// apply - Add a new raw value to the filter, retrieve the filtered result // apply - Add a new raw value to the filter, retrieve the filtered result
@ -56,7 +57,8 @@ public:
private: private:
float _alpha; // gain value (like 0.02) applied to each new value float _alpha; // gain value (like 0.02) applied to each new value
bool _base_value_set; // true if the base value has been set bool _base_value_set; // true if the base value has been set
float _base_value; // the number of samples in the filter, maxes out at size of the filter float _base_value; // filter output
float _cutoff_hz; // cutoff frequency in hz
}; };
// Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size) // Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size)
@ -89,6 +91,7 @@ LowPassFilter<T>::LowPassFilter() :
template <class T> template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq) void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq)
{ {
_cutoff_hz = cutoff_freq;
// avoid divide by zero and allow removing filtering // avoid divide by zero and allow removing filtering
if (cutoff_freq <= 0.0f) { if (cutoff_freq <= 0.0f) {
_alpha = 1.0f; _alpha = 1.0f;
@ -96,7 +99,7 @@ void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq)
} }
// calculate alpha // calculate alpha
float rc = 1/(2*PI*cutoff_freq); float rc = 1/(2*M_PI_F*cutoff_freq);
_alpha = time_step / (time_step + rc); _alpha = time_step / (time_step + rc);
} }
@ -105,10 +108,13 @@ void LowPassFilter<T>::set_time_constant(float time_step, float time_constant)
{ {
// avoid divide by zero // avoid divide by zero
if (time_constant + time_step <= 0.0f) { if (time_constant + time_step <= 0.0f) {
_cutoff_hz = 0.0f;
_alpha = 1.0f; _alpha = 1.0f;
return; return;
} }
_cutoff_hz = 1/(2*M_PI_F*time_constant);
// calculate alpha // calculate alpha
_alpha = time_step / (time_constant + time_step); _alpha = time_step / (time_constant + time_step);
} }