Filter - first version of filter library include ModeFilter

This commit is contained in:
rmackay9 2012-02-26 15:34:05 +09:00
parent f6f05755d9
commit ae8fd43335
6 changed files with 360 additions and 0 deletions

97
libraries/Filter/Filter.h Normal file
View File

@ -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

View File

@ -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

View File

@ -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

View File

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

View File

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

View File

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