Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  int i;
43  for (i = 0; i < MEDIAN_DATASIZE; i++) {
44  filter->data[i] = 0;
45  filter->sortData[i] = 0;
46  }
47  filter->dataIndex = 0;
48 }
49 
50 inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new_data) {
51  int temp, i, j; // used to sort array
52 
53  // Insert new data into raw data array round robin style
54  filter->data[filter->dataIndex] = new_data;
55  if (filter->dataIndex < (MEDIAN_DATASIZE-1)) {
56  filter->dataIndex++;
57  }
58  else {
59  filter->dataIndex = 0;
60  }
61 
62  // Copy raw data to sort data array
63  memcpy(filter->sortData, filter->data, sizeof(filter->data));
64 
65  // Insertion Sort
66  for(i = 1; i <= (MEDIAN_DATASIZE-1); i++) {
67  temp = filter->sortData[i];
68  j = i-1;
69  while(temp < filter->sortData[j] && j>=0) {
70  filter->sortData[j+1] = filter->sortData[j];
71  j = j-1;
72  }
73  filter->sortData[j+1] = temp;
74  }
75  return filter->sortData[(MEDIAN_DATASIZE)>>1]; // return data value in middle of sorted array
76 }
77 
78 inline int32_t get_median_filter(struct MedianFilterInt * filter) {
79  return filter->sortData[(MEDIAN_DATASIZE)>>1];
80 }
81 
83  struct MedianFilterInt mf[3];
84 };
85 
86 #define InitMedianFilterVect3Int(_f) { \
87  for (int i = 0; i < 3; i++) { \
88  init_median_filter(&(_f.mf[i])); \
89  } \
90 }
91 
92 #define InitMedianFilterEulerInt(_f) InitMedianFilterVect3Int(_f)
93 #define InitMedianFilterRatesInt(_f) InitMedianFilterVect3Int(_f)
94 
95 #define UpdateMedianFilterVect3Int(_f, _v) { \
96  (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \
97  (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \
98  (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \
99 }
100 
101 #define UpdateMedianFilterEulerInt(_f, _v) { \
102  (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \
103  (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \
104  (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \
105 }
106 
107 #define UpdateMedianFilterRatesInt(_f, _v) { \
108  (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \
109  (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \
110  (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \
111 }
112 
113 #define GetMedianFilterVect3Int(_f, _v) { \
114  (_v).x = get_median_filter(&(_f.mf[0])); \
115  (_v).y = get_median_filter(&(_f.mf[1])); \
116  (_v).z = get_median_filter(&(_f.mf[2])); \
117 }
118 
119 #define GetMedianFilterEulerInt(_f, _v) { \
120  (_v).phi = get_median_filter(&(_f.mf[0])); \
121  (_v).theta = get_median_filter(&(_f.mf[1])); \
122  (_v).psi = get_median_filter(&(_f.mf[2])); \
123 }
124 
125 #define GetMedianFilterRatesInt(_f, _v) { \
126  (_v).p = get_median_filter(&(_f.mf[0])); \
127  (_v).q = get_median_filter(&(_f.mf[1])); \
128  (_v).r = get_median_filter(&(_f.mf[2])); \
129 }
130 
131 #endif
int32_t sortData[MEDIAN_DATASIZE]
Definition: median_filter.h:33
int32_t data[MEDIAN_DATASIZE]
Definition: median_filter.h:33
int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:50
signed long int32_t
Definition: types.h:19
int32_t get_median_filter(struct MedianFilterInt *filter)
Definition: median_filter.h:78
#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
struct MedianFilterInt mf[3]
Definition: median_filter.h:83
Paparazzi fixed point algebra.