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

uncrustify libraries/Filter/DerivativeFilter.cpp

This commit is contained in:
uncrustify 2012-08-16 23:22:10 -07:00 committed by Pat Hickey
parent e5b317cb42
commit 973dcba9ab
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/Filter/DerivativeFilter.cpp
View File

@ -29,12 +29,12 @@ void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
// this is not a new timestamp - ignore // this is not a new timestamp - ignore
return; return;
} }
// add timestamp before we apply to FilterWithBuffer // add timestamp before we apply to FilterWithBuffer
_timestamps[i] = 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; _new_data = true;
} }
@ -47,12 +47,12 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
return _last_slope; 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
// 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+1)+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+1)+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
@ -63,28 +63,28 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
switch (FILTER_SIZE) { switch (FILTER_SIZE) {
case 5: case 5:
result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1)) result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1))
+ 4*1*(f(2) - f(-2)) / (x(2) - x(-2)); + 4*1*(f(2) - f(-2)) / (x(2) - x(-2));
result /= 8; result /= 8;
break; break;
case 7: case 7:
result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1)) result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1))
+ 4*4*(f(2) - f(-2)) / (x(2) - x(-2)) + 4*4*(f(2) - f(-2)) / (x(2) - x(-2))
+ 6*1*(f(3) - f(-3)) / (x(3) - x(-3)); + 6*1*(f(3) - f(-3)) / (x(3) - x(-3));
result /= 32; result /= 32;
break; break;
case 9: case 9:
result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1)) result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1))
+ 4*14*(f(2) - f(-2)) / (x(2) - x(-2)) + 4*14*(f(2) - f(-2)) / (x(2) - x(-2))
+ 6* 6*(f(3) - f(-3)) / (x(3) - x(-3)) + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3))
+ 8* 1*(f(4) - f(-4)) / (x(4) - x(-4)); + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4));
result /= 128; result /= 128;
break; break;
case 11: case 11:
result = 2*42*(f(1) - f(-1)) / (x(1) - x(-1)) result = 2*42*(f(1) - f(-1)) / (x(1) - x(-1))
+ 4*48*(f(2) - f(-2)) / (x(2) - x(-2)) + 4*48*(f(2) - f(-2)) / (x(2) - x(-2))
+ 6*27*(f(3) - f(-3)) / (x(3) - x(-3)) + 6*27*(f(3) - f(-3)) / (x(3) - x(-3))
+ 8* 8*(f(4) - f(-4)) / (x(4) - x(-4)) + 8* 8*(f(4) - f(-4)) / (x(4) - x(-4))
+ 10* 1*(f(5) - f(-5)) / (x(5) - x(-5)); + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5));
result /= 512; result /= 512;
break; break;
default: default:
@ -107,8 +107,8 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
template <class T, uint8_t FILTER_SIZE> template <class T, uint8_t FILTER_SIZE>
void DerivativeFilter<T,FILTER_SIZE>::reset(void) void DerivativeFilter<T,FILTER_SIZE>::reset(void)
{ {
// 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>::reset(); FilterWithBuffer<T,FILTER_SIZE>::reset();
} }
// add new instances as needed here // add new instances as needed here