Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
median_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012 Ted Carancho. (AeroQuad)
3  * (c) 2012 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi 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 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi 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 paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
24 #ifndef MEDIAN_H
25 #define MEDIAN_H
26 
27 #define MEDIAN_DATASIZE 5
28 
29 #include "std.h"
30 #include "math/pprz_algebra_int.h"
31 
35 };
36 
37 inline void init_median_filter(struct MedianFilterInt *filter);
38 inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data);
39 inline int32_t get_median_filter(struct MedianFilterInt *filter);
40 
41 inline void init_median_filter(struct MedianFilterInt *filter)
42 {
43  int i;
44  for (i = 0; i < MEDIAN_DATASIZE; i++) {
45  filter->data[i] = 0;
46  filter->sortData[i] = 0;
47  }
48  filter->dataIndex = 0;
49 }
50 
51 inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data)
52 {
53  int temp, i, j; // used to sort array
54 
55  // Insert new data into raw data array round robin style
56  filter->data[filter->dataIndex] = new_data;
57  if (filter->dataIndex < (MEDIAN_DATASIZE - 1)) {
58  filter->dataIndex++;
59  } else {
60  filter->dataIndex = 0;
61  }
62 
63  // Copy raw data to sort data array
64  memcpy(filter->sortData, filter->data, sizeof(filter->data));
65 
66  // Insertion Sort
67  for (i = 1; i <= (MEDIAN_DATASIZE - 1); i++) {
68  temp = filter->sortData[i];
69  j = i - 1;
70  while (temp < filter->sortData[j] && j >= 0) {
71  filter->sortData[j + 1] = filter->sortData[j];
72  j = j - 1;
73  }
74  filter->sortData[j + 1] = temp;
75  }
76  return filter->sortData[(MEDIAN_DATASIZE) >> 1]; // return data value in middle of sorted array
77 }
78 
80 {
81  return filter->sortData[(MEDIAN_DATASIZE) >> 1];
82 }
83 
85  struct MedianFilterInt mf[3];
86 };
87 
88 #define InitMedianFilterVect3Int(_f) { \
89  for (int i = 0; i < 3; i++) { \
90  init_median_filter(&(_f.mf[i])); \
91  } \
92  }
93 
94 #define InitMedianFilterEulerInt(_f) InitMedianFilterVect3Int(_f)
95 #define InitMedianFilterRatesInt(_f) InitMedianFilterVect3Int(_f)
96 
97 #define UpdateMedianFilterVect3Int(_f, _v) { \
98  (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \
99  (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \
100  (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \
101  }
102 
103 #define UpdateMedianFilterEulerInt(_f, _v) { \
104  (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \
105  (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \
106  (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \
107  }
108 
109 #define UpdateMedianFilterRatesInt(_f, _v) { \
110  (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \
111  (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \
112  (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \
113  }
114 
115 #define GetMedianFilterVect3Int(_f, _v) { \
116  (_v).x = get_median_filter(&(_f.mf[0])); \
117  (_v).y = get_median_filter(&(_f.mf[1])); \
118  (_v).z = get_median_filter(&(_f.mf[2])); \
119  }
120 
121 #define GetMedianFilterEulerInt(_f, _v) { \
122  (_v).phi = get_median_filter(&(_f.mf[0])); \
123  (_v).theta = get_median_filter(&(_f.mf[1])); \
124  (_v).psi = get_median_filter(&(_f.mf[2])); \
125  }
126 
127 #define GetMedianFilterRatesInt(_f, _v) { \
128  (_v).p = get_median_filter(&(_f.mf[0])); \
129  (_v).q = get_median_filter(&(_f.mf[1])); \
130  (_v).r = get_median_filter(&(_f.mf[2])); \
131  }
132 
133 #endif
int32_t data[MEDIAN_DATASIZE]
Definition: median_filter.h:33
struct MedianFilterInt mf[3]
Definition: median_filter.h:85
int32_t sortData[MEDIAN_DATASIZE]
Definition: median_filter.h:33
int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:51
signed long int32_t
Definition: types.h:19
int32_t get_median_filter(struct MedianFilterInt *filter)
Definition: median_filter.h:79
#define MEDIAN_DATASIZE
Definition: median_filter.h:27
signed char int8_t
Definition: types.h:15
void init_median_filter(struct MedianFilterInt *filter)
Definition: median_filter.h:41
Paparazzi fixed point algebra.