Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: split the DerivativeFilter steps into update() and slope()

Browse Source 
this allows us to apply new data at a different rate than we calculate
the slope.
This commit is contained in:
Andrew Tridgell 2012-07-06 15:04:54 +10:00
parent 3c7e521f70
commit 520d762382
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/Filter/DerivativeFilter.cpp
View File

@ -16,15 +16,20 @@
#include <DerivativeFilter.h> #include <DerivativeFilter.h>
template <class T, uint8_t FILTER_SIZE> template <class T, uint8_t FILTER_SIZE>
float DerivativeFilter<T,FILTER_SIZE>::apply(T sample, uint32_t timestamp) void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
{ {
float result = 0;
// add timestamp before we apply to FilterWithBuffer // add timestamp before we apply to FilterWithBuffer
_timestamps[FilterWithBuffer<T,FILTER_SIZE>::sample_index] = timestamp; _timestamps[FilterWithBuffer<T,FILTER_SIZE>::sample_index] = timestamp;
// call parent's apply function to get the sample into the array // call parent's apply function to get the sample into the array
FilterWithBuffer<T,FILTER_SIZE>::apply(sample); FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
}
template <class T, uint8_t FILTER_SIZE>
float DerivativeFilter<T,FILTER_SIZE>::slope(void)
{
float result = 0;
// use f() to make the code match the maths a bit better. Note // use f() to make the code match the maths a bit better. Note
// that unlike an average filter, we care about the order of the elements // that unlike an average filter, we care about the order of the elements
@ -81,13 +86,16 @@ void DerivativeFilter<T,FILTER_SIZE>::reset(void)
} }
// add new instances as needed here // add new instances as needed here
template float DerivativeFilter<float,5>::apply(float sample, uint32_t timestamp); template void DerivativeFilter<float,5>::update(float sample, uint32_t timestamp);
template float DerivativeFilter<float,5>::slope(void);
template void DerivativeFilter<float,5>::reset(void); template void DerivativeFilter<float,5>::reset(void);
template float DerivativeFilter<float,7>::apply(float sample, uint32_t timestamp); template void DerivativeFilter<float,7>::update(float sample, uint32_t timestamp);
template float DerivativeFilter<float,7>::slope(void);
template void DerivativeFilter<float,7>::reset(void); template void DerivativeFilter<float,7>::reset(void);
template float DerivativeFilter<float,9>::apply(float sample, uint32_t timestamp); template void DerivativeFilter<float,9>::update(float sample, uint32_t timestamp);
template float DerivativeFilter<float,9>::slope(void);
template void DerivativeFilter<float,9>::reset(void); template void DerivativeFilter<float,9>::reset(void);

7
libraries/Filter/DerivativeFilter.h
View File

@ -26,8 +26,11 @@ class DerivativeFilter : public FilterWithBuffer<T,FILTER_SIZE>
// constructor // constructor
DerivativeFilter() : FilterWithBuffer<T,FILTER_SIZE>() {}; DerivativeFilter() : FilterWithBuffer<T,FILTER_SIZE>() {};
// apply - Add a new raw value to the filter, retrieve the filtered result // update - Add a new raw value to the filter, but don't recalculate
virtual float apply(T sample, uint32_t timestamp); virtual void update(T sample, uint32_t timestamp);
// return the derivative value
virtual float slope(void);
// reset - clear the filter // reset - clear the filter
virtual void reset(); virtual void reset();