diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h new file mode 100644 index 0000000000..e719a08f67 --- /dev/null +++ b/libraries/Filter/LowPassFilter.h @@ -0,0 +1,89 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// @file LowPassFilter.h +/// @brief A class to implement a low pass filter without losing precision even for int types +/// the downside being that it's a little slower as it internally uses a float +/// and it consumes an extra 4 bytes of memory to hold the constant gain + +#ifndef LowPassFilter_h +#define LowPassFilter_h + +#include +#include + +// 1st parameter is the type of data being filtered. +template +class LowPassFilter : public Filter +{ + public: + // constructor + LowPassFilter(float gain); + + // apply - Add a new raw value to the filter, retrieve the filtered result + virtual T apply(T sample); + + // reset - clear the filter - next sample added will become the new base value + virtual void reset() { _base_value_set = false; }; + + // reset - clear the filter and provide the new base value + virtual void reset( T new_base_value ) { _base_value = new_base_value; _base_value_set = true;}; + + private: + float _gain; // gain value (like 0.02) applied to each new value + 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 +}; + +// Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size) +typedef LowPassFilter LowPassFilterInt8; +typedef LowPassFilter LowPassFilterUInt8; + +typedef LowPassFilter LowPassFilterInt16; +typedef LowPassFilter LowPassFilterUInt16; + +typedef LowPassFilter LowPassFilterInt32; +typedef LowPassFilter LowPassFilterUInt32; + +typedef LowPassFilter LowPassFilterFloat; + +// Constructor ////////////////////////////////////////////////////////////// + +template +LowPassFilter::LowPassFilter(float gain) : + Filter(), + _gain(gain), + _base_value_set(false) +{ + // bounds checking on _gain + if( _gain > 1.0) { + _gain = 1.0; + }else if( _gain < 0.0 ) { + _gain = 0.0; + } +}; + +// Public Methods ////////////////////////////////////////////////////////////// + +template +T LowPassFilter::apply(T sample) +{ + // initailise _base_value if required + if( ! _base_value_set ) { + _base_value = sample; + _base_value_set = true; + } + + // do the filtering + _base_value = _gain * (float)sample + (1.0 - _gain) * _base_value; + + // return the value. Should be no need to check limits + return (T)_base_value; +} + +#endif \ No newline at end of file diff --git a/libraries/Filter/examples/Filter/Filter.pde b/libraries/Filter/examples/Filter/Filter.pde index 295389f377..3ee1094479 100644 --- a/libraries/Filter/examples/Filter/Filter.pde +++ b/libraries/Filter/examples/Filter/Filter.pde @@ -21,6 +21,8 @@ int16_t rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000}; ModeFilterInt16_Size5 mfilter(2); // buffer of 5 values, result will be from buffer element 2 (ie. the 3rd element which is the middle) //AverageFilterInt16_Size5 mfilter; // buffer of 5 values. result will be average of these 5 +AverageFilterUInt16_Size4 _temp_filter; + // Function to print contents of a filter // we need to ues FilterWithBuffer class because we want to access the individual elements void printFilter(FilterWithBufferInt16_Size5& filter) @@ -44,13 +46,38 @@ void setup() delay(500); } +// Read Raw Temperature values +void ReadTemp() +{ + static uint8_t next_num = 0; + static int32_t RawTemp = 0; + uint8_t buf[2]; + uint16_t _temp_sensor; + + next_num++; + buf[0] = next_num; //next_num; + buf[1] = 0xFF; + + _temp_sensor = buf[0]; + _temp_sensor = (_temp_sensor << 8) | buf[1]; + + RawTemp = _temp_filter.apply(_temp_sensor); + + Serial.printf("RT: %ld\n",RawTemp); +} + //Main loop where the action takes place void loop() { uint8_t i = 0; int16_t filtered_value; - while( i < 9 ) { + int16_t j; + + for(j=0; j<0xFF; j++ ) { + ReadTemp(); + } + /*while( i < 9 ) { // output to user Serial.printf("applying: %d\n",(int)rangevalue[i]); @@ -68,8 +95,8 @@ void loop() Serial.printf("The filtered value is: %d\n\n",(int)filtered_value); i++; - } - delay(100000); + }*/ + delay(10000); } diff --git a/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde new file mode 100644 index 0000000000..eff3d7bdee --- /dev/null +++ b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde @@ -0,0 +1,60 @@ +/* + Example sketch to demonstrate use of LowPassFilter library. + Code by Randy Mackay. DIYDrones.com +*/ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // Filter library +#include // LowPassFilter class (inherits from Filter class) + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +FastSerialPort0(Serial); // FTDI/console + +// create a global instance of the class instead of local to avoid memory fragmentation +LowPassFilterInt16 low_pass_filter(0.02); // simple low pass filter which applies 2% of new data to old data + +// setup routine +void setup() +{ + // Open up a serial connection + Serial.begin(115200); + + // introduction + Serial.printf("ArduPilot LowPassFilter test ver 1.0\n\n"); + + // Wait for the serial connection + delay(500); +} + +//Main loop where the action takes place +void loop() +{ + int16_t i; + int16_t new_value; + int16_t filtered_value; + + // reset value to 100. If not reset the filter will start at the first value entered + low_pass_filter.reset(100); + + for( i=0; i<210; i++ ) { + + // new data value + new_value = 105; + + // output to user + Serial.printf("applying: %d",(int)new_value); + + // apply new value and retrieved filtered result + filtered_value = low_pass_filter.apply(new_value); + + // display results + Serial.printf("\toutput: %d\n\n",(int)filtered_value); + } + delay(10000); +} + + diff --git a/libraries/Filter/examples/LowPassFilter/Makefile b/libraries/Filter/examples/LowPassFilter/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/Filter/examples/LowPassFilter/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk