Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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"
32
38
39static 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
58static 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
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
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
147
148static 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
165static 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
175static 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
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
uint16_t foo
Definition main_demo5.c:58
static void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size)
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
struct MedianFilterInt mf[3]
static int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data)
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]
float data[MAX_MEDIAN_DATASIZE]
#define MAX_MEDIAN_DATASIZE
static float get_median_filter_f(struct MedianFilterFloat *filter)
static int32_t get_median_filter_i(struct MedianFilterInt *filter)
int32_t data[MAX_MEDIAN_DATASIZE]
Paparazzi fixed point algebra.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.