From 952e5d7bbcaec65a117c848039a005470fcb3059 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:31:56 +0900 Subject: [PATCH 01/11] AP_Mount - #include FastSerial first to allow it to compile under Arduino 1.0 --- libraries/AP_Mount/AP_Mount.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 89c576a729..b8333ea339 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -20,8 +20,9 @@ #ifndef AP_Mount_H #define AP_Mount_H -//#include +#include #include +#include #include #include #include From c14d7768f7237cc542b27fc21db41b4a5ea2f2b2 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:34:05 +0900 Subject: [PATCH 02/11] Filter - first version of filter library include ModeFilter --- libraries/Filter/Filter.h | 97 +++++++++++++++ libraries/Filter/ModeFilter.h | 123 ++++++++++++++++++++ libraries/Filter/SumFilter.h | 54 +++++++++ libraries/Filter/examples/Filter/Filter.pde | 77 ++++++++++++ libraries/Filter/examples/Filter/Makefile | 1 + libraries/Filter/keywords.txt | 8 ++ 6 files changed, 360 insertions(+) create mode 100644 libraries/Filter/Filter.h create mode 100644 libraries/Filter/ModeFilter.h create mode 100644 libraries/Filter/SumFilter.h create mode 100644 libraries/Filter/examples/Filter/Filter.pde create mode 100644 libraries/Filter/examples/Filter/Makefile create mode 100644 libraries/Filter/keywords.txt 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 From f264e29814e817c49398a44ffa19503ed3ade6d6 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:34:50 +0900 Subject: [PATCH 03/11] AP_RangeFinder - cut over to use new ModeFilter from Filter library --- libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h | 2 +- libraries/AP_RangeFinder/RangeFinder.cpp | 2 +- libraries/AP_RangeFinder/RangeFinder.h | 6 +++--- .../AP_RangeFinder_test/AP_RangeFinder_test.pde | 10 +++++++++- 7 files changed, 17 insertions(+), 9 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 20915092d6..e5e865e615 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -35,7 +35,7 @@ // Constructor ////////////////////////////////////////////////////////////// -AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter): +AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter): RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h index 12780a6dfc..63b8065e00 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h @@ -24,7 +24,7 @@ class AP_RangeFinder_MaxsonarXL : public RangeFinder { public: - AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); + AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter); int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm float calculate_scaler(int sonar_type, float adc_refence_voltage); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp index 3f08bb7dc7..1e43441159 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp @@ -35,7 +35,7 @@ // Constructor ////////////////////////////////////////////////////////////// -AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) : +AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter) : RangeFinder(source, filter) { max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h index 4f5a328979..b5d92f3f16 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h @@ -9,7 +9,7 @@ class AP_RangeFinder_SharpGP2Y : public RangeFinder { public: - AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter); + AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter); int convert_raw_to_distance(int _raw_value) { if( _raw_value == 0 ) return max_distance; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 63945e5ab7..6b1ae09370 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -43,7 +43,7 @@ int RangeFinder::read() // ensure distance is within min and max temp_dist = constrain(temp_dist, min_distance, max_distance); - distance = _mode_filter->get_filtered_with_sample(temp_dist); + distance = _mode_filter->apply(temp_dist); return distance; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 1870cd5a94..03768ba181 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -4,7 +4,7 @@ #include #include #include "../AP_AnalogSource/AP_AnalogSource.h" -#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library +#include "../Filter/Filter.h" // Filter library /* #define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0 @@ -22,7 +22,7 @@ class RangeFinder { protected: - RangeFinder(AP_AnalogSource * source, ModeFilter *filter) : + RangeFinder(AP_AnalogSource * source, FilterInt16 *filter) : _analog_source(source), _mode_filter(filter) {} public: @@ -38,6 +38,6 @@ class RangeFinder virtual int read(); // read value from sensor and return distance in cm AP_AnalogSource *_analog_source; - ModeFilter *_mode_filter; + FilterInt16 *_mode_filter; }; #endif diff --git a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde index b26d1aab44..2d5864baef 100644 --- a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde +++ b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde @@ -4,6 +4,9 @@ */ // includes +#include +#include +#include #include // Range finder library #include #include @@ -11,6 +14,11 @@ #include #include // mode filter +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +FastSerialPort0(Serial); // FTDI/console + // comment out line below if using APM2 or analog pin instead of APM1's built in ADC #define USE_ADC_ADS7844 // use APM1's built in ADC and connect sonar to pitot tube @@ -24,7 +32,7 @@ // declare global instances Arduino_Mega_ISR_Registry isr_registry; -ModeFilter mode_filter; +ModeFilterInt16 mode_filter(5,2); #ifdef USE_ADC_ADS7844 AP_TimerProcess adc_scheduler; AP_ADC_ADS7844 adc; From 69132e0fe74d4c06c03f47f02e230f00d2966209 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:35:14 +0900 Subject: [PATCH 04/11] ArduCopter - change sonar to use new mode filter from Filter library --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e8c3188a5d..d6f0b34c5f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -78,7 +78,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // RC Channel Library #include // Range finder library #include // Optical Flow library -#include +#include // Mode Filter from Filter library #include // APM relay #include // MAVLink GCS definitions #include @@ -268,7 +268,7 @@ GCS_MAVLINK gcs3; // SONAR selection //////////////////////////////////////////////////////////////////////////////// // -ModeFilter sonar_mode_filter; +ModeFilterInt16 sonar_mode_filter(5,2); #if CONFIG_SONAR == ENABLED #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); From 21d7a77009b74ce88d6c5b6e8f36f486d1d06213 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:35:42 +0900 Subject: [PATCH 05/11] ArduPlane - switch sonar to use new mode filter from filter library --- ArduPlane/ArduPlane.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index edc7963f3b..1fa3643e20 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -224,7 +224,7 @@ GCS_MAVLINK gcs3; // PITOT selection //////////////////////////////////////////////////////////////////////////////// // -ModeFilter sonar_mode_filter; +ModeFilterInt16 sonar_mode_filter(5,2); #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC AP_AnalogSource_ADC pitot_analog_source( &adc, From 74f7d7a1c0ecd6b49ebf6bbd8626dc0b976a953d Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 15:36:23 +0900 Subject: [PATCH 06/11] ModeFilter - remove older ModeFilter library (new library is now in Filter directory) --- libraries/ModeFilter/ModeFilter.cpp | 87 ------------------- libraries/ModeFilter/ModeFilter.h | 26 ------ .../ModeFilter/examples/ModeFilter/Makefile | 2 - .../examples/ModeFilter/ModeFilter.pde | 51 ----------- libraries/ModeFilter/keywords.txt | 8 -- 5 files changed, 174 deletions(-) delete mode 100644 libraries/ModeFilter/ModeFilter.cpp delete mode 100644 libraries/ModeFilter/ModeFilter.h delete mode 100644 libraries/ModeFilter/examples/ModeFilter/Makefile delete mode 100644 libraries/ModeFilter/examples/ModeFilter/ModeFilter.pde delete mode 100644 libraries/ModeFilter/keywords.txt diff --git a/libraries/ModeFilter/ModeFilter.cpp b/libraries/ModeFilter/ModeFilter.cpp deleted file mode 100644 index cf282d56f0..0000000000 --- a/libraries/ModeFilter/ModeFilter.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* - ModeFilter.cpp - Mode Filter Library for Ardupilot Mega. Arduino - Code by Jason Short. DIYDrones.com - Adapted from code by Jason Lessels(June 6, 2011), Bill Gentles (Nov. 12, 2010) - - - This library 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. - - -*/ -#include "ModeFilter.h" - -#include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - - -// Constructors //////////////////////////////////////////////////////////////// - -ModeFilter::ModeFilter() : - _sample_index(0) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// - //Sorting function - // sort function (Author: Bill Gentles, Nov. 12, 2010) - // *a is an array pointer function - -int ModeFilter::get_filtered_with_sample(int _sample){ - _samples[_sample_index] = _sample; - - _sample_index++; - - if (_sample_index >= MOD_FILTER_SIZE) - _sample_index = 0; - - isort(); - - return mode(); -} - - -void ModeFilter::isort() -{ - for (int i = 1; i < MOD_FILTER_SIZE; ++i) { - int j = _samples[i]; - int k; - for (k = i - 1; (k >= 0) && (j < _samples[k]); k--){ - _samples[k + 1] = _samples[k]; - } - _samples[k + 1] = j; - } -} - -//Mode function, returning the mode or median. -int16_t ModeFilter::mode(){ - int fmode = 0; - byte i = 0; - byte count = 0; - byte maxCount = 0; - byte bimodal = 0; - - while(count > maxCount){ - fmode = _samples[i]; - maxCount = count; - bimodal = 0; - } - - if(count == 0) i++; - - if(count == maxCount){ //If the dataset has 2 or more modes. - bimodal = 1; - } - - if(fmode == 0 || bimodal == 1){ //Return the median if there is no mode. - fmode = _samples[(MOD_FILTER_SIZE / 2)]; - } - - return fmode; -} diff --git a/libraries/ModeFilter/ModeFilter.h b/libraries/ModeFilter/ModeFilter.h deleted file mode 100644 index 7cd93017ae..0000000000 --- a/libraries/ModeFilter/ModeFilter.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef ModeFilter_h -#define ModeFilter_h - -#define MOD_FILTER_SIZE 6 - -#include - -class ModeFilter -{ - private: - public: - ModeFilter(); - - int get_filtered_with_sample(int _sample); - int16_t _samples[MOD_FILTER_SIZE]; - - private: - void isort(); - int16_t mode(); - int8_t _sample_index; -}; - -#endif - - - diff --git a/libraries/ModeFilter/examples/ModeFilter/Makefile b/libraries/ModeFilter/examples/ModeFilter/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/ModeFilter/examples/ModeFilter/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/ModeFilter/examples/ModeFilter/ModeFilter.pde b/libraries/ModeFilter/examples/ModeFilter/ModeFilter.pde deleted file mode 100644 index f07b442184..0000000000 --- a/libraries/ModeFilter/examples/ModeFilter/ModeFilter.pde +++ /dev/null @@ -1,51 +0,0 @@ -/* - Example of APM_RC library. - Code by Jordi MuÒoz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library - -int rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000}; - -ModeFilter mfilter; -byte i = 0; - -void setup() -{ - //Open up a serial connection - Serial.begin(115200); - //Wait for the serial connection - delay(500); -} - -//Main loop where the action takes place -void loop() -{ - while(i < 9){ - printArray(mfilter._samples, 6); - int modE = mfilter.get_filtered_with_sample(rangevalue[i]); - i++; - - Serial.print("The mode/median is: "); - Serial.print(modE); - Serial.println(); - } - delay(100000); -} - -/*-----------Functions------------*/ -//Function to print the arrays. -void printArray(int *a, int n) -{ - for (int i = 0; i < n; i++) - { - Serial.print(a[i], DEC); - Serial.print(' '); - } - - Serial.println(); -} - diff --git a/libraries/ModeFilter/keywords.txt b/libraries/ModeFilter/keywords.txt deleted file mode 100644 index 3efcc0fa52..0000000000 --- a/libraries/ModeFilter/keywords.txt +++ /dev/null @@ -1,8 +0,0 @@ -APM_RC KEYWORD1 -begin KEYWORD2 -InputCh KEYWORD2 -OutputCh KEYWORD2 -GetState KEYWORD2 -Force_Out0_Out1 KEYWORD2 -Force_Out2_Out3 KEYWORD2 -Force_Out6_Out7 KEYWORD2 From 8193c786ef08a55b9efad5541ebe30c8f3db7233 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 17:05:41 +0900 Subject: [PATCH 07/11] ArduCopter - added #include to unbreak the build! --- ArduCopter/ArduCopter.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d6f0b34c5f..bca0a82125 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -78,6 +78,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // RC Channel Library #include // Range finder library #include // Optical Flow library +#include // Filter library #include // Mode Filter from Filter library #include // APM relay #include // MAVLink GCS definitions From e1114bf7f757435cf4f884b4c6c20bbd7a9ef37a Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 17:05:56 +0900 Subject: [PATCH 08/11] ArduPlane - added Filter.h to unbreak the build! --- ArduPlane/ArduPlane.pde | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 1fa3643e20..d93670297f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -44,7 +44,8 @@ version 2.1 of the License, or (at your option) any later version. #include // PID library #include // RC Channel Library #include // Range finder library -#include +#include // Filter library +#include // Mode Filter from Filter library #include // APM relay #include // Camera/Antenna mount #include // MAVLink GCS definitions From f53ff8de97f5c40cd2d8e11d6c96120d4e3129aa Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 17:17:46 +0900 Subject: [PATCH 09/11] Filter - removed shadowing of variables in constructors for Filter, ModeFilter and SumFilter (sorry tridge!) increased maximum sample buffer size from 6 to 10 --- libraries/Filter/Filter.h | 10 +++++----- libraries/Filter/ModeFilter.h | 10 +++++----- libraries/Filter/SumFilter.h | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/Filter/Filter.h b/libraries/Filter/Filter.h index 8d47ca100f..77ee3c9834 100644 --- a/libraries/Filter/Filter.h +++ b/libraries/Filter/Filter.h @@ -17,13 +17,13 @@ #include #include -#define FILTER_MAX_SAMPLES 6 // max number of samples that can be added to the filter +#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 filter_size); + Filter(uint8_t requested_size); ~Filter(); // apply - Add a new raw value to the filter, retrieve the filtered result @@ -44,8 +44,8 @@ typedef Filter FilterInt16; // Constructor template -Filter::Filter(uint8_t filter_size) : - filter_size(filter_size), sample_index(0) +Filter::Filter(uint8_t requested_size) : + filter_size(requested_size), sample_index(0) { // check filter size if( Filter::filter_size > FILTER_MAX_SAMPLES ) @@ -58,7 +58,7 @@ Filter::Filter(uint8_t filter_size) : reset(); } -// Destructor +// Destructor - THIS SHOULD NEVER BE CALLED OR IT COULD LEAD TO MEMORY FRAGMENTATION template Filter::~Filter() { diff --git a/libraries/Filter/ModeFilter.h b/libraries/Filter/ModeFilter.h index b4613290ae..00137b652e 100644 --- a/libraries/Filter/ModeFilter.h +++ b/libraries/Filter/ModeFilter.h @@ -22,7 +22,7 @@ template class ModeFilter : public Filter { public: - ModeFilter(uint8_t filter_size, uint8_t return_element); + ModeFilter(uint8_t requested_size, uint8_t return_element); // apply - Add a new raw value to the filter, retrieve the filtered result virtual T apply(T sample); @@ -40,14 +40,14 @@ typedef ModeFilter ModeFilterInt16; // Constructor ////////////////////////////////////////////////////////////// template -ModeFilter::ModeFilter(uint8_t filter_size, uint8_t return_element) : - Filter(filter_size), +ModeFilter::ModeFilter(uint8_t requested_size, uint8_t return_element) : + Filter(requested_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; + if( _return_element >= Filter::filter_size ) + _return_element = Filter::filter_size / 2; }; // Public Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/Filter/SumFilter.h b/libraries/Filter/SumFilter.h index 300124d1c1..cbbbcb6d89 100644 --- a/libraries/Filter/SumFilter.h +++ b/libraries/Filter/SumFilter.h @@ -21,7 +21,7 @@ template class SumFilter : public Filter { public: - SumFilter(uint8_t filter_size) : Filter(filter_size) {}; + 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); From b9b88ba23732749bd2b1444103b027b9c2943d44 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 17:34:36 +0900 Subject: [PATCH 10/11] Filter - added AverageFilter - this will be used in place of SumFilter because it removes the possibility of overflows --- libraries/Filter/AverageFilter.h | 82 ++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 libraries/Filter/AverageFilter.h diff --git a/libraries/Filter/AverageFilter.h b/libraries/Filter/AverageFilter.h new file mode 100644 index 0000000000..df9305a745 --- /dev/null +++ b/libraries/Filter/AverageFilter.h @@ -0,0 +1,82 @@ +// -*- 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 AverageFilter.h +/// @brief A class to provide the average of a number of samples +/// +/// DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION + +#ifndef AverageFilter_h +#define AverageFilter_h + +#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 +{ + public: + AverageFilter(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); + + // reset - clear the filter + virtual void reset(); + + private: + uint8_t _num_samples; +}; + +// 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; + +// Public Methods ////////////////////////////////////////////////////////////// + +template +T AverageFilter::apply(T sample) +{ + U result = 0; + + // call parent's apply function to get the sample into the array + Filter::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; + + // 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 (T)(result / _num_samples); +} + +// reset - clear all samples +template +void AverageFilter::reset() +{ + // call parent's apply function to get the sample into the array + Filter::reset(); + + // clear our variable + _num_samples = 0; +} + +#endif + + + From 297d2ae45776800196037f6eddf786ecb6ea144b Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 26 Feb 2012 17:57:28 +0900 Subject: [PATCH 11/11] ModeFilter - corrected shadowed variable compiler warning for drop_high_sample --- libraries/Filter/ModeFilter.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/Filter/ModeFilter.h b/libraries/Filter/ModeFilter.h index 00137b652e..b9a76b293a 100644 --- a/libraries/Filter/ModeFilter.h +++ b/libraries/Filter/ModeFilter.h @@ -76,7 +76,7 @@ T ModeFilter::apply(T sample) // 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) +void ModeFilter::isort(T new_sample, bool drop_high) { int8_t i; @@ -84,10 +84,10 @@ void ModeFilter::isort(T new_sample, bool drop_high_sample) // the rest is the same as dropping the high sample if( Filter::sample_index < Filter::filter_size ) { Filter::sample_index++; - drop_high_sample = true; + drop_high = true; } - if( drop_high_sample ) { // drop highest sample from the buffer to make room for our new sample + 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;