Filter: added in Leonards LowPassFilter2p filter

this is a backport from PX4

libraries/Filter/LowPassFilter2p.cpp

@@ -0,0 +1,54 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/// @file	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include <inttypes.h>
+#include <AP_Math.h>
+#include "LowPassFilter2p.h"
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(PI/fr);
+    float c = 1.0f+2.0f*cosf(PI/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(PI/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}

libraries/Filter/LowPassFilter2p.h

@@ -0,0 +1,58 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef LOWPASSFILTER2P_H
+#define LOWPASSFILTER2P_H
+
+/// @file	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+class 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
+};
+
+#endif // LOWPASSFILTER2P_H

libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.pde

@@ -0,0 +1,54 @@
+/*
+ *       Example sketch to demonstrate use of LowPassFilter2p library.
+ *       Code by Randy Mackay and Andrew Tridgell
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_FLYMAPLE.h>
+#include <AP_Param.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <Filter.h>                     // Filter library
+#include <LowPassFilter2p.h>
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// craete an instance with 800Hz sample rate and 30Hz cutoff
+static LowPassFilter2p low_pass_filter(800, 30);
+
+// setup routine
+static void setup()
+{
+    // introduction
+    hal.console->printf("ArduPilot LowPassFilter2p test\n\n");
+}
+
+void loop()
+{
+    int16_t i;
+    float new_value;
+    float filtered_value;
+
+    for( i=0; i<300; i++ ) {
+
+        // new data value
+        new_value = sinf((float)i*2*PI*5/50.0f);  // 5hz
+
+        // output to user
+        hal.console->printf("applying: %6.4f", new_value);
+
+        // apply new value and retrieved filtered result
+        filtered_value = low_pass_filter.apply(new_value);
+
+        // display results
+        hal.console->printf("\toutput: %6.4f\n", filtered_value);
+
+        hal.scheduler->delay(10);
+    }
+    hal.scheduler->delay(10000);
+}
+
+AP_HAL_MAIN();

libraries/Filter/examples/LowPassFilter2p/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk