Filter - first version of filter library include ModeFilter

libraries/Filter/Filter.h

@@ -0,0 +1,97 @@
+// -*- 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	Filter.h
+/// @brief	A base class for apply various filters to values
+///
+///         DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION
+
+#ifndef Filter_h
+#define Filter_h
+
+#include <inttypes.h>
+#include <AP_Common.h>
+
+#define FILTER_MAX_SAMPLES 6  // max number of samples that can be added to the filter
+
+template <class T>
+class Filter
+{
+  public:
+	Filter(uint8_t filter_size);
+	~Filter();
+
+	// apply - Add a new raw value to the filter, retrieve the filtered result
+	virtual T apply(T sample);
+	
+	// reset - clear the filter
+	virtual void reset();
+	
+	uint8_t	filter_size;	// max number of items in filter
+	T* 		samples;		// buffer of samples
+	uint8_t	sample_index;	// pointer to the next empty slot in the buffer
+
+  private:
+};
+
+// Typedef for convenience
+typedef Filter<int16_t> FilterInt16;
+
+// Constructor 
+template <class T>
+Filter<T>::Filter(uint8_t filter_size) :
+	filter_size(filter_size), sample_index(0)
+{
+	// check filter size
+    if( Filter<T>::filter_size > FILTER_MAX_SAMPLES )
+		Filter<T>::filter_size = FILTER_MAX_SAMPLES;
+
+	// create array
+	samples = (T *)malloc(Filter<T>::filter_size * sizeof(T));
+
+	// clear array
+	reset();
+}
+
+// Destructor
+template <class T>
+Filter<T>::~Filter()
+{
+	// free up the samples array
+	free(samples);
+}
+		
+// reset - clear all samples
+template <class T>
+void Filter<T>::reset()
+{
+    for( int8_t i=0; i<filter_size; i++ ) {
+	    samples[i] = 0;
+	}
+	sample_index = 0;
+}
+
+// apply - take in a new raw sample, and return the filtered results
+template <class T>
+T Filter<T>::apply(T sample){
+
+	// add sample to array
+	samples[sample_index++] = sample;
+
+	// wrap index if necessary
+	if( sample_index >= filter_size )
+		sample_index = 0;
+
+	// base class doesn't know what filtering to do so we just return the raw sample
+	return sample;
+}
+
+#endif
+
+
+

libraries/Filter/ModeFilter.h

@@ -0,0 +1,123 @@
+// -*- 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	ModeFilter.h
+/// @brief	A class to apply a mode filter which is basically picking the median value from the last x samples
+///         the filter size (i.e buffer size) should always be an odd number
+///
+///         DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION
+
+#ifndef ModeFilter_h
+#define ModeFilter_h
+
+#include <inttypes.h>
+#include <Filter.h>
+
+template <class T>
+class ModeFilter : public Filter<T>
+{
+  public:
+	ModeFilter(uint8_t filter_size, uint8_t return_element);
+
+	// apply - Add a new raw value to the filter, retrieve the filtered result
+	virtual T	apply(T sample);
+
+  private:
+    // private methods
+	uint8_t	_return_element;
+  	void 	isort(T sample, bool drop_high_sample);
+	bool	drop_high_sample;  // switch to determine whether to drop the highest or lowest sample when new value arrives
+};
+
+// Typedef for convenience
+typedef ModeFilter<int16_t> ModeFilterInt16;
+
+// Constructor    //////////////////////////////////////////////////////////////
+
+template <class T>
+ModeFilter<T>::ModeFilter(uint8_t filter_size, uint8_t return_element) :
+	Filter<T>(filter_size),
+	_return_element(return_element),
+	drop_high_sample(true)
+{
+	// ensure we have a valid return_nth_element value.  if not, revert to median
+	if( _return_element >= filter_size )
+		_return_element = filter_size / 2;
+};
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+template <class T>
+T ModeFilter<T>::apply(T sample)
+{
+	// insert the new items into the samples buffer
+	isort(sample, drop_high_sample);
+	
+	// next time drop from the other end of the sample buffer
+	drop_high_sample = !drop_high_sample;
+
+	// return results
+	if( Filter<T>::sample_index < Filter<T>::filter_size ) {
+		// middle sample if buffer is not yet full
+		return Filter<T>::samples[(Filter<T>::sample_index / 2)];
+	}else{
+		// return element specified by user in constructor
+		return Filter<T>::samples[_return_element];
+	}
+}
+
+//
+// insertion sort - takes a new sample and pushes it into the sample array
+//                  drops either the highest or lowest sample depending on the 'drop_high_sample' parameter
+//
+template <class T>
+void ModeFilter<T>::isort(T new_sample, bool drop_high_sample)
+{
+	int8_t i;
+
+	// if the buffer isn't full simply increase the #items in the buffer (i.e. sample_index)
+	// the rest is the same as dropping the high sample
+	if( Filter<T>::sample_index < Filter<T>::filter_size ) {
+		Filter<T>::sample_index++;
+		drop_high_sample = true;
+	}
+
+	if( drop_high_sample ) {  // drop highest sample from the buffer to make room for our new sample
+
+		// start from top. Note: sample_index always points to the next open space so we start from sample_index-1
+		i = Filter<T>::sample_index-1;
+
+		// if the next element is higher than our new sample, push it up one position
+		while( Filter<T>::samples[i-1] > new_sample && i > 0 ) {
+			Filter<T>::samples[i] = Filter<T>::samples[i-1];
+			i--;
+		}
+
+		// add our new sample to the buffer
+		Filter<T>::samples[i] = new_sample;
+		
+	}else{ // drop lowest sample from the buffer to make room for our new sample
+
+		// start from the bottom
+		i = 0;
+
+		// if the element is lower than our new sample, push it down one position
+		while( Filter<T>::samples[i+1] < new_sample && i < Filter<T>::sample_index-1 ) {
+			Filter<T>::samples[i] = Filter<T>::samples[i+1];
+			i++;
+		}
+
+		// add our new sample to the buffer
+		Filter<T>::samples[i] = new_sample;
+	}
+}
+
+#endif
+
+
+

libraries/Filter/SumFilter.h

@@ -0,0 +1,54 @@
+// -*- 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	SumFilter.h
+/// @brief	A class to apply an average filter (but we save some calc time by not averaging the values but instead save one division by just adding the values up
+///
+///         DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION
+
+#ifndef SumFilter_h
+#define SumFilter_h
+
+#include <inttypes.h>
+#include <Filter.h>
+
+template <class T>
+class SumFilter : public Filter<T>
+{
+  public:
+	SumFilter(uint8_t filter_size) : Filter<T>(filter_size) {};
+
+	// apply - Add a new raw value to the filter, retrieve the filtered result
+	virtual T apply(T sample);
+
+  private:
+};
+
+// Typedef for convenience
+typedef SumFilter<int16_t> SumFilterInt16;
+
+// Public Methods //////////////////////////////////////////////////////////////
+template <class T>
+T SumFilter<T>::apply(T sample){
+
+	T result = 0;
+
+	// call parent's apply function to get the sample into the array
+	Filter<T>::apply(sample);
+
+	// get sum of all values - there is a risk of overflow here that we ignore
+	for(int8_t i=0; i<Filter<T>::filter_size; i++)
+		result += Filter<T>::samples[i];
+
+	return result;
+}
+
+#endif
+
+
+

libraries/Filter/examples/Filter/Filter.pde

@@ -0,0 +1,77 @@
+/*
+	Example of Filter library.
+	Code by Randy Mackay and Jason Short. 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 <ModeFilter.h>		// ModeFilter library (inherits from Filter class)
+#include <SumFilter.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+FastSerialPort0(Serial);        // FTDI/console
+
+//typedef ModeFilter<int16_t> IntModeFilter;
+//typedef SumFilter<int16_t> IntSumFilter;
+
+int16_t rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000};
+
+// create a global instance of the class instead of local to avoid memory fragmentation
+ModeFilterInt16 mfilter(5,2);  // buffer of 5 values, result will be from buffer element 2 (ie. the 3rd element which is the middle)
+//SumFilterInt16 mfilter(5);
+
+//Function to print contents of a filter
+void printFilter(Filter<int16_t>& filter)
+{
+	for(int8_t i=0; i < filter.filter_size; i++)
+	{
+		Serial.printf("%d ",(int)filter.samples[i]);
+	}
+	Serial.println();
+}
+
+void setup()
+{
+	// Open up a serial connection
+	Serial.begin(115200);
+	
+	// introduction
+	Serial.printf("ArduPilot ModeFilter library test ver 1.0\n\n");
+
+	// Wait for the serial connection
+	delay(500);
+}
+
+//Main loop where the action takes place
+void loop()
+{
+    int8_t i = 0;
+	int16_t filtered_value;
+
+	while( i < 9 ) {
+
+		// output to user
+		Serial.printf("applying: %d\n",(int)rangevalue[i]);
+
+		// display original
+		Serial.printf("before: ");
+		printFilter(mfilter);
+
+		// apply new value and retrieved filtered result
+		filtered_value = mfilter.apply(rangevalue[i]);
+
+		// display results
+		Serial.printf("after: ");
+		printFilter(mfilter);
+		Serial.printf("The filtered value is: %d\n\n",(int)filtered_value);
+
+		i++;
+	}
+	delay(100000);
+}
+
+

libraries/Filter/examples/Filter/Makefile

@@ -0,0 +1 @@
+include ../../../AP_Common/Arduino.mk

libraries/Filter/keywords.txt

@@ -0,0 +1,8 @@
+Filter	KEYWORD1
+ModeFilter	KEYWORD1
+SumFilter	KEYWORD1
+apply		KEYWORD2
+reset		KEYWORD2
+max_samples	KEYWORD2
+samples		KEYWORD2
+sample_index	KEYWORD2