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 < (DATASIZE-1)) 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<=(DATASIZE-1); i++) { |
60 |
temp = sortData[i]; |
61 |
j = i-1;
|
62 |
while(temp<sortData[j] && j>=0) { |
63 |
sortData[j+1] = sortData[j];
|
64 |
j = j-1;
|
65 |
} |
66 |
sortData[j+1] = temp;
|
67 |
} |
68 |
return data[(DATASIZE)>>1]; // return data value in middle of sorted array |
69 |
} |
70 |
}; |