From d17a015df117ca62e5e7a722521e41980da9b959 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 28 Feb 2012 21:01:11 +0900 Subject: [PATCH] Filter - added AverageFilter, removed SumFilter added FilterWithBuffer to allow removal of malloc/free without losing ability to pass around filter objects --- libraries/Filter/AverageFilter.h | 69 ++++++++++++------- libraries/Filter/Filter.h | 72 +++----------------- libraries/Filter/FilterWithBuffer.h | 102 ++++++++++++++++++++++++++++ libraries/Filter/ModeFilter.h | 83 +++++++++++++--------- libraries/Filter/SumFilter.h | 54 --------------- libraries/Filter/keywords.txt | 17 ++--- 6 files changed, 215 insertions(+), 182 deletions(-) create mode 100644 libraries/Filter/FilterWithBuffer.h delete mode 100644 libraries/Filter/SumFilter.h diff --git a/libraries/Filter/AverageFilter.h b/libraries/Filter/AverageFilter.h index df9305a745..5fa0406819 100644 --- a/libraries/Filter/AverageFilter.h +++ b/libraries/Filter/AverageFilter.h @@ -16,14 +16,16 @@ #include #include +#include // 1st parameter is the type of data being filtered. // 2nd parameter is a larger data type used during summation to prevent overflows -template -class AverageFilter : public Filter +template +class AverageFilter : public FilterWithBuffer { public: - AverageFilter(uint8_t requested_size) : Filter(requested_size) {}; + // constructor + AverageFilter() : FilterWithBuffer(), _num_samples(0) {}; // apply - Add a new raw value to the filter, retrieve the filtered result virtual T apply(T sample); @@ -32,51 +34,68 @@ class AverageFilter : public Filter virtual void reset(); private: - uint8_t _num_samples; + uint8_t _num_samples; // the number of samples in the filter, maxes out at size of the filter }; -// Typedef for convenience (1st argument is the data size, 2nd argument is a datasize that's bigger to handle overflows) -typedef AverageFilter AverageFilterInt8; -typedef AverageFilter AverageFilterUInt8; -typedef AverageFilter AverageFilterInt16; -typedef AverageFilter AverageFilterUInt16; -typedef AverageFilter AverageFilterInt32; -typedef AverageFilter AverageFilterUInt32; +// Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size) +typedef AverageFilter AverageFilterInt8_Size2; +typedef AverageFilter AverageFilterInt8_Size3; +typedef AverageFilter AverageFilterInt8_Size4; +typedef AverageFilter AverageFilterInt8_Size5; +typedef AverageFilter AverageFilterUInt8_Size2; +typedef AverageFilter AverageFilterUInt8_Size3; +typedef AverageFilter AverageFilterUInt8_Size4; +typedef AverageFilter AverageFilterUInt8_Size5; + +typedef AverageFilter AverageFilterInt16_Size2; +typedef AverageFilter AverageFilterInt16_Size3; +typedef AverageFilter AverageFilterInt16_Size4; +typedef AverageFilter AverageFilterInt16_Size5; +typedef AverageFilter AverageFilterUInt16_Size2; +typedef AverageFilter AverageFilterUInt16_Size3; +typedef AverageFilter AverageFilterUInt16_Size4; +typedef AverageFilter AverageFilterUInt16_Size5; + +typedef AverageFilter AverageFilterInt32_Size2; +typedef AverageFilter AverageFilterInt32_Size3; +typedef AverageFilter AverageFilterInt32_Size4; +typedef AverageFilter AverageFilterInt32_Size5; +typedef AverageFilter AverageFilterUInt32_Size2; +typedef AverageFilter AverageFilterUInt32_Size3; +typedef AverageFilter AverageFilterUInt32_Size4; +typedef AverageFilter AverageFilterUInt32_Size5; // Public Methods ////////////////////////////////////////////////////////////// -template -T AverageFilter::apply(T sample) +template +T AverageFilter::apply(T sample) { U result = 0; // call parent's apply function to get the sample into the array - Filter::apply(sample); + FilterWithBuffer::apply(sample); // increment the number of samples so far _num_samples++; - if( _num_samples > Filter::filter_size || _num_samples == 0 ) - _num_samples = Filter::filter_size; + if( _num_samples > FILTER_SIZE || _num_samples == 0 ) + _num_samples = FILTER_SIZE; // 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]; + for(uint8_t i=0; i::samples[i]; return (T)(result / _num_samples); } // reset - clear all samples -template -void AverageFilter::reset() +template +void AverageFilter::reset() { // call parent's apply function to get the sample into the array - Filter::reset(); + FilterWithBuffer::reset(); // clear our variable _num_samples = 0; } -#endif - - - +#endif \ No newline at end of file diff --git a/libraries/Filter/Filter.h b/libraries/Filter/Filter.h index 77ee3c9834..dfa45dea29 100644 --- a/libraries/Filter/Filter.h +++ b/libraries/Filter/Filter.h @@ -7,9 +7,8 @@ // /// @file Filter.h -/// @brief A base class for apply various filters to values +/// @brief A base class from which various filters classes should inherit /// -/// DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION #ifndef Filter_h #define Filter_h @@ -17,79 +16,28 @@ #include #include -#define FILTER_MAX_SAMPLES 10 // maximum size of the sample buffer (normally older values will be overwritten as new appear) - template class Filter { public: - Filter(uint8_t requested_size); - ~Filter(); + // constructor + Filter() {}; // apply - Add a new raw value to the filter, retrieve the filtered result - virtual T apply(T sample); + virtual T apply(T sample) { return 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 + virtual void reset() {}; - private: }; // Typedef for convenience +typedef Filter FilterInt8; +typedef Filter FilterUInt8; typedef Filter FilterInt16; - -// Constructor -template -Filter::Filter(uint8_t requested_size) : - filter_size(requested_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 - THIS SHOULD NEVER BE CALLED OR IT COULD LEAD TO MEMORY FRAGMENTATION -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; -} +typedef Filter FilterUInt16; +typedef Filter FilterInt32; +typedef Filter FilterUInt32; #endif diff --git a/libraries/Filter/FilterWithBuffer.h b/libraries/Filter/FilterWithBuffer.h new file mode 100644 index 0000000000..e86ff6b81f --- /dev/null +++ b/libraries/Filter/FilterWithBuffer.h @@ -0,0 +1,102 @@ +// -*- 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 FilterWithBuffer.h +/// @brief A filter with a buffer. +/// This is implemented separately to the base Filter class to get around +/// restrictions caused by the use of templates which makes different sizes essentially +/// completely different classes + +#ifndef FilterWithBuffer_h +#define FilterWithBuffer_h + +#include +#include +#include + +template +class FilterWithBuffer : public Filter +{ + public: + // constructor + FilterWithBuffer(); + + // 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(); + + // get filter size + virtual uint8_t get_filter_size() { return FILTER_SIZE; }; + + T samples[FILTER_SIZE]; // buffer of samples + uint8_t sample_index; // pointer to the next empty slot in the buffer + + private: +}; + +// Typedef for convenience +typedef FilterWithBuffer FilterWithBufferInt16_Size2; +typedef FilterWithBuffer FilterWithBufferInt16_Size3; +typedef FilterWithBuffer FilterWithBufferInt16_Size4; +typedef FilterWithBuffer FilterWithBufferInt16_Size5; +typedef FilterWithBuffer FilterWithBufferInt16_Size6; +typedef FilterWithBuffer FilterWithBufferInt16_Size7; +typedef FilterWithBuffer FilterWithBufferUInt16_Size2; +typedef FilterWithBuffer FilterWithBufferUInt16_Size3; +typedef FilterWithBuffer FilterWithBufferUInt16_Size4; +typedef FilterWithBuffer FilterWithBufferUInt16_Size5; +typedef FilterWithBuffer FilterWithBufferUInt16_Size6; +typedef FilterWithBuffer FilterWithBufferUInt16_Size7; + +// Constructor +template +FilterWithBuffer::FilterWithBuffer() : + Filter(), + sample_index(0) +{ + // clear sample buffer + reset(); +} + +// reset - clear all samples from the buffer +template +void FilterWithBuffer::reset() +{ + // call base class reset + Filter::reset(); + + // clear samples buffer + for( int8_t i=0; i +T FilterWithBuffer::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 index b9a76b293a..bdb56a4246 100644 --- a/libraries/Filter/ModeFilter.h +++ b/libraries/Filter/ModeFilter.h @@ -17,57 +17,77 @@ #include #include +#include -template -class ModeFilter : public Filter +template +class ModeFilter : public FilterWithBuffer { public: - ModeFilter(uint8_t requested_size, uint8_t return_element); + ModeFilter(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 + // private methods uint8_t _return_element; - void isort(T sample, bool drop_high_sample); + 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; +typedef ModeFilter ModeFilterInt8_Size3; +typedef ModeFilter ModeFilterInt8_Size4; +typedef ModeFilter ModeFilterInt8_Size5; +typedef ModeFilter ModeFilterInt8_Size6; +typedef ModeFilter ModeFilterInt8_Size7; +typedef ModeFilter ModeFilterUInt8_Size3; +typedef ModeFilter ModeFilterUInt8_Size4; +typedef ModeFilter ModeFilterUInt8_Size5; +typedef ModeFilter ModeFilterUInt8_Size6; +typedef ModeFilter ModeFilterUInt8_Size7; +typedef ModeFilter ModeFilterInt16_Size3; +typedef ModeFilter ModeFilterInt16_Size4; +typedef ModeFilter ModeFilterInt16_Size5; +typedef ModeFilter ModeFilterInt16_Size6; +typedef ModeFilter ModeFilterInt16_Size7; +typedef ModeFilter ModeFilterUInt16_Size3; +typedef ModeFilter ModeFilterUInt16_Size4; +typedef ModeFilter ModeFilterUInt16_Size5; +typedef ModeFilter ModeFilterUInt16_Size6; +typedef ModeFilter ModeFilterUInt16_Size7; // Constructor ////////////////////////////////////////////////////////////// -template -ModeFilter::ModeFilter(uint8_t requested_size, uint8_t return_element) : - Filter(requested_size), +template +ModeFilter::ModeFilter(uint8_t return_element) : + FilterWithBuffer(), _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::filter_size ) - _return_element = Filter::filter_size / 2; + if( _return_element >= FILTER_SIZE ) + _return_element = FILTER_SIZE / 2; }; // Public Methods ////////////////////////////////////////////////////////////// -template -T ModeFilter::apply(T sample) +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 ) { + if( FilterWithBuffer::sample_index < FILTER_SIZE ) { // middle sample if buffer is not yet full - return Filter::samples[(Filter::sample_index / 2)]; + return FilterWithBuffer::samples[(FilterWithBuffer::sample_index / 2)]; }else{ // return element specified by user in constructor - return Filter::samples[_return_element]; + return FilterWithBuffer::samples[_return_element]; } } @@ -75,49 +95,46 @@ T ModeFilter::apply(T sample) // 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) +template +void ModeFilter::isort(T new_sample, bool drop_high) { 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++; + if( FilterWithBuffer::sample_index < FILTER_SIZE ) { + FilterWithBuffer::sample_index++; drop_high = true; } if( drop_high ) { // 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; + i = FilterWithBuffer::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]; + while( FilterWithBuffer::samples[i-1] > new_sample && i > 0 ) { + FilterWithBuffer::samples[i] = FilterWithBuffer::samples[i-1]; i--; } // add our new sample to the buffer - Filter::samples[i] = new_sample; - + FilterWithBuffer::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]; + while( FilterWithBuffer::samples[i+1] < new_sample && i < FilterWithBuffer::sample_index-1 ) { + FilterWithBuffer::samples[i] = FilterWithBuffer::samples[i+1]; i++; } // add our new sample to the buffer - Filter::samples[i] = new_sample; + FilterWithBuffer::samples[i] = new_sample; } } -#endif - - - +#endif \ No newline at end of file diff --git a/libraries/Filter/SumFilter.h b/libraries/Filter/SumFilter.h deleted file mode 100644 index cbbbcb6d89..0000000000 --- a/libraries/Filter/SumFilter.h +++ /dev/null @@ -1,54 +0,0 @@ -// -*- 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 requested_size) : Filter(requested_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/keywords.txt b/libraries/Filter/keywords.txt index 4061f263dd..862c2151f4 100644 --- a/libraries/Filter/keywords.txt +++ b/libraries/Filter/keywords.txt @@ -1,8 +1,9 @@ -Filter KEYWORD1 -ModeFilter KEYWORD1 -SumFilter KEYWORD1 -apply KEYWORD2 -reset KEYWORD2 -max_samples KEYWORD2 -samples KEYWORD2 -sample_index KEYWORD2 +Filter KEYWORD1 +FilterWithBuffer KEYWORD1 +ModeFilter KEYWORD1 +AverageFilter KEYWORD1 +apply KEYWORD2 +reset KEYWORD2 +get_filter_size KEYWORD2 +samples KEYWORD2 +sample_index KEYWORD2