27#define MAX_MEDIAN_DATASIZE 13
28#define MEDIAN_DEFAULT_SIZE 5
44 }
else if ((size % 2) == 0) {
47 filter->
size = size + 1;
51 for (i = 0; i < filter->
size; i++) {
60 if (filter->
size % 2){
80 for (i = 1; i < filter->
size; i++) {
97#define InitMedianFilterVect3Int(_f, _n) { \
98 for (int i = 0; i < 3; i++) { \
99 init_median_filter_i(&(_f.mf[i]), _n); \
103#define InitMedianFilterEulerInt(_f, _n) InitMedianFilterVect3Int(_f, _n)
104#define InitMedianFilterRatesInt(_f, _n) InitMedianFilterVect3Int(_f, _n)
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); \
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); \
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); \
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])); \
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])); \
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])); \
153 }
else if ((size % 2) == 0){
154 filter->
size = size + 1;
158 for (i = 0; i < filter->
size; i++) {
159 filter->
data[i] = 0.f;
167 if (filter->
size % 2){
188 for (i = 1; i < filter->
size; i++) {
205#define InitMedianFilterVect3Float(_f, _n) { \
206 for (int i = 0; i < 3; i++) { \
207 init_median_filter_f(&(_f.mf[i]), _n); \
211#define InitMedianFilterEulerFloat(_f, _n) InitMedianFilterVect3Float(_f, _n)
212#define InitMedianFilterRatesFloat(_f, _n) InitMedianFilterVect3Float(_f, _n)
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); \
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); \
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); \
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])); \
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])); \
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])); \
Paparazzi fixed point algebra.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.