root / quad1 / AeroQuad / Filter.h @ 9240aaa3
History | View | Annotate | Download (2.27 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
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 | }; |