Statistics
| Branch: | Revision:

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
};