mirror of https://github.com/ArduPilot/ardupilot
Filter: add some comments to LowPassFilter
This commit is contained in:
parent
738b1967ad
commit
22e1156c31
|
@ -26,6 +26,7 @@
|
||||||
#include <AP_Math.h>
|
#include <AP_Math.h>
|
||||||
#include "FilterClass.h"
|
#include "FilterClass.h"
|
||||||
|
|
||||||
|
// DigitalLPF implements the filter math
|
||||||
class DigitalLPF
|
class DigitalLPF
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -39,6 +40,7 @@ public:
|
||||||
float alpha;
|
float alpha;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// add a new raw value to the filter, retrieve the filtered result
|
||||||
float apply(float sample, float cutoff_freq, float dt) {
|
float apply(float sample, float cutoff_freq, float dt) {
|
||||||
if (cutoff_freq <= 0.0f) {
|
if (cutoff_freq <= 0.0f) {
|
||||||
_output = sample;
|
_output = sample;
|
||||||
|
@ -51,6 +53,7 @@ public:
|
||||||
return _output;
|
return _output;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get latest filtered value from filter (equal to the value returned by latest call to apply method)
|
||||||
float get() const {
|
float get() const {
|
||||||
return _output;
|
return _output;
|
||||||
}
|
}
|
||||||
|
@ -61,6 +64,7 @@ private:
|
||||||
float _output;
|
float _output;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// LPF base class
|
||||||
class LowPassFilter
|
class LowPassFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -84,6 +88,7 @@ protected:
|
||||||
float _cutoff_freq;
|
float _cutoff_freq;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// LPF for a single float
|
||||||
class LowPassFilterFloat : public LowPassFilter
|
class LowPassFilterFloat : public LowPassFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -108,6 +113,7 @@ private:
|
||||||
DigitalLPF _filter;
|
DigitalLPF _filter;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// LPF for a 2D vector
|
||||||
class LowPassFilterVector2f : public LowPassFilter
|
class LowPassFilterVector2f : public LowPassFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -134,6 +140,7 @@ private:
|
||||||
DigitalLPF _filter_y;
|
DigitalLPF _filter_y;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// LPF for 3D vector
|
||||||
class LowPassFilterVector3f : public LowPassFilter
|
class LowPassFilterVector3f : public LowPassFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
Loading…
Reference in New Issue