diff --git a/libraries/Filter/Filter.h b/libraries/Filter/Filter.h new file mode 100644 index 0000000000..8d47ca100f --- /dev/null +++ b/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 +#include + +#define FILTER_MAX_SAMPLES 6 // max number of samples that can be added to the filter + +template +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 FilterInt16; + +// Constructor +template +Filter::Filter(uint8_t filter_size) : + filter_size(filter_size), sample_index(0) +{ + // check filter size + if( Filter::filter_size > FILTER_MAX_SAMPLES ) + Filter::filter_size = FILTER_MAX_SAMPLES; + + // create array + samples = (T *)malloc(Filter::filter_size * sizeof(T)); + + // clear array + reset(); +} + +// Destructor +template +Filter::~Filter() +{ + // free up the samples array + free(samples); +} + +// reset - clear all samples +template +void Filter::reset() +{ + for( int8_t i=0; i +T Filter::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 + + + diff --git a/libraries/Filter/ModeFilter.h b/libraries/Filter/ModeFilter.h new file mode 100644 index 0000000000..b4613290ae --- /dev/null +++ b/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 +#include + +template +class ModeFilter : public Filter +{ + 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 ModeFilterInt16; + +// Constructor ////////////////////////////////////////////////////////////// + +template +ModeFilter::ModeFilter(uint8_t filter_size, uint8_t return_element) : + Filter(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 +T ModeFilter::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::sample_index < Filter::filter_size ) { + // middle sample if buffer is not yet full + return Filter::samples[(Filter::sample_index / 2)]; + }else{ + // return element specified by user in constructor + return Filter::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 +void ModeFilter::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::sample_index < Filter::filter_size ) { + Filter::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::sample_index-1; + + // if the next element is higher than our new sample, push it up one position + while( Filter::samples[i-1] > new_sample && i > 0 ) { + Filter::samples[i] = Filter::samples[i-1]; + i--; + } + + // add our new sample to the buffer + Filter::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::samples[i+1] < new_sample && i < Filter::sample_index-1 ) { + Filter::samples[i] = Filter::samples[i+1]; + i++; + } + + // add our new sample to the buffer + Filter::samples[i] = new_sample; + } +} + +#endif + + + diff --git a/libraries/Filter/SumFilter.h b/libraries/Filter/SumFilter.h new file mode 100644 index 0000000000..300124d1c1 --- /dev/null +++ b/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 +#include + +template +class SumFilter : public Filter +{ + public: + SumFilter(uint8_t filter_size) : Filter(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 SumFilterInt16; + +// Public Methods ////////////////////////////////////////////////////////////// +template +T SumFilter::apply(T sample){ + + T result = 0; + + // call parent's apply function to get the sample into the array + Filter::apply(sample); + + // get sum of all values - there is a risk of overflow here that we ignore + for(int8_t i=0; i::filter_size; i++) + result += Filter::samples[i]; + + return result; +} + +#endif + + + diff --git a/libraries/Filter/examples/Filter/Filter.pde b/libraries/Filter/examples/Filter/Filter.pde new file mode 100644 index 0000000000..88014523d4 --- /dev/null +++ b/libraries/Filter/examples/Filter/Filter.pde @@ -0,0 +1,77 @@ +/* + Example of Filter library. + Code by Randy Mackay and Jason Short. DIYDrones.com +*/ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // Filter library +#include // ModeFilter library (inherits from Filter class) +#include + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +FastSerialPort0(Serial); // FTDI/console + +//typedef ModeFilter IntModeFilter; +//typedef SumFilter 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& 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); +} + + diff --git a/libraries/Filter/examples/Filter/Makefile b/libraries/Filter/examples/Filter/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/Filter/examples/Filter/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/Filter/keywords.txt b/libraries/Filter/keywords.txt new file mode 100644 index 0000000000..4061f263dd --- /dev/null +++ b/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