Filter: automatically cope with duplicate timestamps

this makes it easier for callers to avoid excessive slope calculations

Thanks to Jon Challinger for the idea
This commit is contained in:
Andrew Tridgell 2012-08-08 11:25:23 +10:00
parent e97305ea6e
commit 2de06df08d
2 changed files with 24 additions and 2 deletions

View File

@ -18,17 +18,35 @@
template <class T, uint8_t FILTER_SIZE> template <class T, uint8_t FILTER_SIZE>
void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp) void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
{ {
uint8_t i = FilterWithBuffer<T,FILTER_SIZE>::sample_index;
uint8_t i1;
if (i == 0) {
i1 = FILTER_SIZE-1;
} else {
i1 = i-1;
}
if (_timestamps[i1] == timestamp) {
// this is not a new timestamp - ignore
return;
}
// add timestamp before we apply to FilterWithBuffer // add timestamp before we apply to FilterWithBuffer
_timestamps[FilterWithBuffer<T,FILTER_SIZE>::sample_index] = timestamp; _timestamps[i] = 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);
_new_data = true;
} }
template <class T, uint8_t FILTER_SIZE> template <class T, uint8_t FILTER_SIZE>
float DerivativeFilter<T,FILTER_SIZE>::slope(void) float DerivativeFilter<T,FILTER_SIZE>::slope(void)
{ {
if (!_new_data) {
return _last_slope;
}
float result = 0; 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
@ -74,6 +92,9 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
break; break;
} }
_new_data = false;
_last_slope = result;
return result; return result;
} }

View File

@ -36,7 +36,8 @@ class DerivativeFilter : public FilterWithBuffer<T,FILTER_SIZE>
virtual void reset(); virtual void reset();
private: private:
uint32_t _last_time; bool _new_data;
float _last_slope;
// microsecond timestamps for samples. This is needed // microsecond timestamps for samples. This is needed
// to cope with non-uniform time spacing of the data // to cope with non-uniform time spacing of the data