Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: fixed a off-by-one DerivativeFilter bug 

many thanks to Jon Challinger for noticing this!
This commit is contained in:
Andrew Tridgell 2012-08-08 11:03:55 +10:00
parent 40839fdea2
commit c9a56e2223
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/Filter/DerivativeFilter.cpp
View File

@ -33,8 +33,8 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
// 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
#define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i)+3*FILTER_SIZE/2) % FILTER_SIZE] #define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i+1)+3*FILTER_SIZE/2) % FILTER_SIZE]
#define x(i) _timestamps[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i)+3*FILTER_SIZE/2) % FILTER_SIZE] #define x(i) _timestamps[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)+i+1)+3*FILTER_SIZE/2) % FILTER_SIZE]
if (_timestamps[FILTER_SIZE-1] == _timestamps[FILTER_SIZE-2]) { if (_timestamps[FILTER_SIZE-1] == _timestamps[FILTER_SIZE-2]) {
// we haven't filled the buffer yet - assume zero derivative // we haven't filled the buffer yet - assume zero derivative
@ -98,5 +98,9 @@ template void DerivativeFilter<float,9>::update(float sample, uint32_t timestamp
template float DerivativeFilter<float,9>::slope(void); template float DerivativeFilter<float,9>::slope(void);
template void DerivativeFilter<float,9>::reset(void); template void DerivativeFilter<float,9>::reset(void);
template void DerivativeFilter<float,11>::update(float sample, uint32_t timestamp);
template float DerivativeFilter<float,11>::slope(void);
template void DerivativeFilter<float,11>::reset(void);