Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Filter.h
- Revision:
- 46:fd5a62296b12
- Parent:
- 40:8e852115fe55
diff -r 6251c0169f4f -r fd5a62296b12 Filter.h --- a/Filter.h Wed May 27 11:45:00 2015 +0000 +++ b/Filter.h Wed May 27 13:01:43 2015 +0000 @@ -11,25 +11,23 @@ protected: float y; float const alpha; - + public: /// Construct the filter with given coefficient. /// @param a filter coefficient (0.0 to 1.0), higher values result in more filtering. LowPassFilter(float const a) : y(0), alpha(a) {} - + /// Main processing function, passes given value to filter and returns the filter's output. /// Call repeatedly to filter an incoming stream of numbers. /// @param x one value to send to filter as input /// @return filtered output - float filter(float const x) - { + float filter(float const x) { y = x + alpha * (y - x); return y; } - + /// Resets the internal state of the filter. Use to start filtering a new stream. - void reset() - { + void reset() { y = 0; } }; @@ -45,14 +43,12 @@ public: HighPassFilter(float const a) : y(0), x_1(0), alpha(1 - a) {} - float filter(float const x) - { + float filter(float const x) { y = alpha * (y + x - x_1); x_1 = x; return y; } - void reset() - { + void reset() { y = 0; x_1 = 0; } @@ -64,19 +60,17 @@ float y; float dt; float clipmax; - + public: Integrator( float const _dt = 1, float const initial = 0, float const _max = 1000000) : y(initial), - dt(_dt), - clipmax(_max) {} - float integrate(float const x) - { + dt(_dt), + clipmax(_max) {} + float integrate(float const x) { return (y = utils::clip(-clipmax, y + x * dt, clipmax)); } - void reset(float const _dt = 1, float const initial = 0) - { + void reset(float const _dt = 1, float const initial = 0) { y = initial; dt = _dt; } @@ -86,16 +80,14 @@ { protected: float y; - + public: Differentiator() : y(0) {} - float differentiate(float const x) - { + float differentiate(float const x) { return (y = x - y); } - - void reset() - { + + void reset() { y = 0; } };