diff options
author | Andrew Tridgell <tridge@samba.org> | 2013-08-04 15:09:16 +1000 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-04 15:43:56 +0200 |
commit | 7ddd88623efcdc83eeb93bbb592b936ac75096f0 (patch) | |
tree | f364c93f7e56515c6ecc84e540385de4c20a30ea /src/modules/mathlib/math/filter/LowPassFilter2p.hpp | |
parent | e931d3b9cda723d63bf352ca866dc499d8df21f5 (diff) | |
download | px4-firmware-7ddd88623efcdc83eeb93bbb592b936ac75096f0.tar.gz px4-firmware-7ddd88623efcdc83eeb93bbb592b936ac75096f0.tar.bz2 px4-firmware-7ddd88623efcdc83eeb93bbb592b936ac75096f0.zip |
mathlib: added LowPassFilter2p object
used in gyro and accel drivers for better filtering
Diffstat (limited to 'src/modules/mathlib/math/filter/LowPassFilter2p.hpp')
-rw-r--r-- | src/modules/mathlib/math/filter/LowPassFilter2p.hpp | 78 |
1 files changed, 78 insertions, 0 deletions
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 000000000..208ec98d4 --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/// @file LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall <LeonardTHall@gmail.com> +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math |