root / quad1 / AeroQuad / Filter.h @ 9240aaa3
History  View  Annotate  Download (2.27 KB)
1 
/*


2 
AeroQuad v2.1  October 2010

3 
www.AeroQuad.com

4 
Copyright (c) 2010 Ted Carancho. All rights reserved.

5 
An Open Source Arduino based quadrocopter.

6 

7 
This program is free software: you can redistribute it and/or modify

8 
it under the terms of the GNU General Public License as published by

9 
the Free Software Foundation, either version 3 of the License, or

10 
(at your option) any later version.

11 

12 
This program is distributed in the hope that it will be useful,

13 
but WITHOUT ANY WARRANTY; without even the implied warranty of

14 
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

15 
GNU General Public License for more details.

16 

17 
You should have received a copy of the GNU General Public License

18 
along with this program. If not, see <http://www.gnu.org/licenses/>.

19 
*/

20  
21 
// Low pass filter, kept as regular C function for speed

22 
float smooth(float currentData, float previousData, float smoothFactor) { 
23 
return (previousData * (1.0  smoothFactor) + (currentData * smoothFactor)); 
24 
} 
25  
26 
// ***********************************************************************

27 
// *********************** Median Filter Class ***************************

28 
// ***********************************************************************

29 
// Median filter currently not used, but kept if needed for the future

30 
// To declar use: MedianFilter filterSomething;

31  
32 
class MedianFilter { 
33 
public:

34 
#define DATASIZE 25 
35 
float data[DATASIZE], sortData[DATASIZE];

36 
int dataIndex;

37 
MedianFilter(void) {}

38  
39 
void initialize(void) { 
40 
for (int index = 0; index < DATASIZE; index++) { 
41 
data[index] = 0;

42 
sortData[index] = 0;

43 
} 
44 
dataIndex = 0;

45 
} 
46 

47 
const float filter(float newData) { 
48 
int temp, i, j; // used to sort array 
49  
50 
// Insert new data into raw data array round robin style

51 
data[dataIndex] = newData; 
52 
if (dataIndex < (DATASIZE1)) dataIndex++; 
53 
else dataIndex = 0; 
54 

55 
// Copy raw data to sort data array

56 
memcpy(sortData, data, sizeof(data));

57 

58 
// Insertion Sort

59 
for(i=1; i<=(DATASIZE1); i++) { 
60 
temp = sortData[i]; 
61 
j = i1;

62 
while(temp<sortData[j] && j>=0) { 
63 
sortData[j+1] = sortData[j];

64 
j = j1;

65 
} 
66 
sortData[j+1] = temp;

67 
} 
68 
return data[(DATASIZE)>>1]; // return data value in middle of sorted array 
69 
} 
70 
}; 