Filter - added simple LowPassFilter (simple but it's possible to make errors...

Filter - added simple LowPassFilter (simple but it's possible to make errors with simple stuff too so might as well have one)

libraries/Filter/LowPassFilter.h

+// -*- 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 <inttypes.h>
+#include <Filter.h>
+
+// 1st parameter <T> is the type of data being filtered.  
+template <class T>
+class LowPassFilter : public Filter<T>
+{
+  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<int8_t> LowPassFilterInt8;
+typedef LowPassFilter<uint8_t> LowPassFilterUInt8;
+
+typedef LowPassFilter<int16_t> LowPassFilterInt16;
+typedef LowPassFilter<uint16_t> LowPassFilterUInt16;
+
+typedef LowPassFilter<int32_t> LowPassFilterInt32;
+typedef LowPassFilter<uint32_t> LowPassFilterUInt32;
+
+typedef LowPassFilter<float> LowPassFilterFloat;
+
+// Constructor    //////////////////////////////////////////////////////////////
+
+template <class T>
+LowPassFilter<T>::LowPassFilter(float gain) :
+	Filter<T>(),
+	_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 <class T>
+T LowPassFilter<T>::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

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);
 }
 
 

libraries/Filter/examples/LowPassFilter/LowPassFilter.pde

+/*
+	Example sketch to demonstrate use of LowPassFilter library.
+	Code by Randy Mackay. DIYDrones.com
+*/
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
+#include <Filter.h>			// Filter library
+#include <LowPassFilter.h>	// 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);
+}
+
+

libraries/Filter/examples/LowPassFilter/Makefile

+include ../../../AP_Common/Arduino.mk