Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 MAX_MEDIAN_DATASIZE 13
28 #define MEDIAN_DEFAULT_SIZE 5
29 
30 #include "std.h"
31 #include "math/pprz_algebra_int.h"
32 
37 };
38 
39 static inline void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size)
40 {
41  uint8_t i;
42  if (size > MAX_MEDIAN_DATASIZE){
43  filter->size = MAX_MEDIAN_DATASIZE;
44  } else if ((size % 2) == 0) {
45  // force filter to have odd number of entries so that
46  // returned median is always an entry and not an average
47  filter->size = size + 1;
48  } else {
49  filter->size = size;
50  }
51  for (i = 0; i < filter->size; i++) {
52  filter->data[i] = 0;
53  filter->sortData[i] = 0;
54  }
55  filter->dataIndex = 0;
56 }
57 
58 static inline int32_t get_median_filter_i(struct MedianFilterInt *filter)
59 {
60  if (filter->size % 2){
61  return filter->sortData[filter->size >> 1];
62  } else {
63  // this should not be used if init_median_filter was used
64  return (filter->sortData[filter->size / 2] + filter->sortData[filter->size / 2 - 1]) / 2;
65  }
66 }
67 
68 static inline int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data)
69 {
70  int temp, i, j; // used to sort array
71 
72  // Insert new data into raw data array round robin style
73  filter->data[filter->dataIndex] = new_data;
74  filter->dataIndex = (filter->dataIndex + 1) % filter->size;
75 
76  // Copy raw data to sort data array
77  memcpy(filter->sortData, filter->data, sizeof(int32_t) * filter->size);
78 
79  // Insertion Sort
80  for (i = 1; i < filter->size; i++) {
81  temp = filter->sortData[i];
82  j = i - 1;
83  while (j >= 0 && temp < filter->sortData[j]) {
84  filter->sortData[j + 1] = filter->sortData[j];
85  j = j - 1;
86  }
87  filter->sortData[j + 1] = temp;
88  }
89  // return data value in middle of sorted array
90  return get_median_filter_i(filter);
91 }
92 
94  struct MedianFilterInt mf[3];
95 };
96 
97 #define InitMedianFilterVect3Int(_f, _n) { \
98  for (int i = 0; i < 3; i++) { \
99  init_median_filter_i(&(_f.mf[i]), _n); \
100  } \
101  }
102 
103 #define InitMedianFilterEulerInt(_f, _n) InitMedianFilterVect3Int(_f, _n)
104 #define InitMedianFilterRatesInt(_f, _n) InitMedianFilterVect3Int(_f, _n)
105 
106 #define UpdateMedianFilterVect3Int(_f, _v) { \
107  (_v).x = update_median_filter_i(&(_f.mf[0]), (_v).x); \
108  (_v).y = update_median_filter_i(&(_f.mf[1]), (_v).y); \
109  (_v).z = update_median_filter_i(&(_f.mf[2]), (_v).z); \
110  }
111 
112 #define UpdateMedianFilterEulerInt(_f, _v) { \
113  (_v).phi = update_median_filter_i(&(_f.mf[0]), (_v).phi); \
114  (_v).theta = update_median_filter_i(&(_f.mf[1]), (_v).theta); \
115  (_v).psi = update_median_filter_i(&(_f.mf[2]), (_v).psi); \
116  }
117 
118 #define UpdateMedianFilterRatesInt(_f, _v) { \
119  (_v).p = update_median_filter_i(&(_f.mf[0]), (_v).p); \
120  (_v).q = update_median_filter_i(&(_f.mf[1]), (_v).q); \
121  (_v).r = update_median_filter_i(&(_f.mf[2]), (_v).r); \
122  }
123 
124 #define GetMedianFilterVect3Int(_f, _v) { \
125  (_v).x = get_median_filter_i(&(_f.mf[0])); \
126  (_v).y = get_median_filter_i(&(_f.mf[1])); \
127  (_v).z = get_median_filter_i(&(_f.mf[2])); \
128  }
129 
130 #define GetMedianFilterEulerInt(_f, _v) { \
131  (_v).phi = get_median_filter_i(&(_f.mf[0])); \
132  (_v).theta = get_median_filter_i(&(_f.mf[1])); \
133  (_v).psi = get_median_filter_i(&(_f.mf[2])); \
134  }
135 
136 #define GetMedianFilterRatesInt(_f, _v) { \
137  (_v).p = get_median_filter_i(&(_f.mf[0])); \
138  (_v).q = get_median_filter_i(&(_f.mf[1])); \
139  (_v).r = get_median_filter_i(&(_f.mf[2])); \
140  }
141 
146 };
147 
148 static inline void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
149 {
150  uint8_t i;
151  if (size > MAX_MEDIAN_DATASIZE){
152  filter->size = MAX_MEDIAN_DATASIZE;
153  } else if ((size % 2) == 0){
154  filter->size = size + 1;
155  } else {
156  filter->size = size;
157  }
158  for (i = 0; i < filter->size; i++) {
159  filter->data[i] = 0.f;
160  filter->sortData[i] = 0.f;
161  }
162  filter->dataIndex = 0;
163 }
164 
165 static inline float get_median_filter_f(struct MedianFilterFloat *filter)
166 {
167  if (filter->size % 2){
168  return filter->sortData[filter->size >> 1];
169  } else {
170  // this should not be used if init_median_filter was used
171  return (filter->sortData[filter->size / 2] + filter->sortData[filter->size / 2 - 1]) / 2;
172  }
173 }
174 
175 static inline float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
176 {
177  float temp;
178  int i, j; // used to sort array
179 
180  // Insert new data into raw data array round robin style
181  filter->data[filter->dataIndex] = new_data;
182  filter->dataIndex = (filter->dataIndex + 1) % filter->size;
183 
184  // Copy raw data to sort data array
185  memcpy(filter->sortData, filter->data, sizeof(float) * filter->size);
186 
187  // Insertion Sort
188  for (i = 1; i < filter->size; i++) {
189  temp = filter->sortData[i];
190  j = i - 1;
191  while (j >= 0 && temp < filter->sortData[j]) {
192  filter->sortData[j + 1] = filter->sortData[j];
193  j = j - 1;
194  }
195  filter->sortData[j + 1] = temp;
196  }
197  // return data value in middle of sorted array
198  return get_median_filter_f(filter);
199 }
200 
202  struct MedianFilterFloat mf[3];
203 };
204 
205 #define InitMedianFilterVect3Float(_f, _n) { \
206  for (int i = 0; i < 3; i++) { \
207  init_median_filter_f(&(_f.mf[i]), _n); \
208  } \
209  }
210 
211 #define InitMedianFilterEulerFloat(_f, _n) InitMedianFilterVect3Float(_f, _n)
212 #define InitMedianFilterRatesFloat(_f, _n) InitMedianFilterVect3Float(_f, _n)
213 
214 #define UpdateMedianFilterVect3Float(_f, _v) { \
215  (_v).x = update_median_filter_f(&(_f.mf[0]), (_v).x); \
216  (_v).y = update_median_filter_f(&(_f.mf[1]), (_v).y); \
217  (_v).z = update_median_filter_f(&(_f.mf[2]), (_v).z); \
218  }
219 
220 #define UpdateMedianFilterEulerFloat(_f, _v) { \
221  (_v).phi = update_median_filter_f(&(_f.mf[0]), (_v).phi); \
222  (_v).theta = update_median_filter_f(&(_f.mf[1]), (_v).theta); \
223  (_v).psi = update_median_filter_f(&(_f.mf[2]), (_v).psi); \
224  }
225 
226 #define UpdateMedianFilterRatesFloat(_f, _v) { \
227  (_v).p = update_median_filter_f(&(_f.mf[0]), (_v).p); \
228  (_v).q = update_median_filter_f(&(_f.mf[1]), (_v).q); \
229  (_v).r = update_median_filter_f(&(_f.mf[2]), (_v).r); \
230  }
231 
232 #define GetMedianFilterVect3Float(_f, _v) { \
233  (_v).x = get_median_filter_f(&(_f.mf[0])); \
234  (_v).y = get_median_filter_f(&(_f.mf[1])); \
235  (_v).z = get_median_filter_f(&(_f.mf[2])); \
236  }
237 
238 #define GetMedianFilterEulerFloat(_f, _v) { \
239  (_v).phi = get_median_filter_f(&(_f.mf[0])); \
240  (_v).theta = get_median_filter_f(&(_f.mf[1])); \
241  (_v).psi = get_median_filter_f(&(_f.mf[2])); \
242  }
243 
244 #define GetMedianFilterRatesFloat(_f, _v) { \
245  (_v).p = get_median_filter_f(&(_f.mf[0])); \
246  (_v).q = get_median_filter_f(&(_f.mf[1])); \
247  (_v).r = get_median_filter_f(&(_f.mf[2])); \
248  }
249 
250 #endif
static void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size)
Definition: median_filter.h:39
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
struct MedianFilterInt mf[3]
Definition: median_filter.h:94
static int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:68
float sortData[MAX_MEDIAN_DATASIZE]
struct MedianFilterFloat mf[3]
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
int32_t sortData[MAX_MEDIAN_DATASIZE]
Definition: median_filter.h:34
float data[MAX_MEDIAN_DATASIZE]
#define MAX_MEDIAN_DATASIZE
Definition: median_filter.h:27
static float get_median_filter_f(struct MedianFilterFloat *filter)
static int32_t get_median_filter_i(struct MedianFilterInt *filter)
Definition: median_filter.h:58
int32_t data[MAX_MEDIAN_DATASIZE]
Definition: median_filter.h:34
Paparazzi fixed point algebra.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98