Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
low_pass_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #ifndef LOW_PASS_FILTER_H
28 #define LOW_PASS_FILTER_H
29 
30 #include "std.h"
31 #include "math/pprz_algebra_int.h"
32 
33 #define INT32_FILT_FRAC 8
34 
40  float time_const;
41  float last_in;
42  float last_out;
43 };
44 
57 static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time,
58  float value)
59 {
60  filter->last_in = value;
61  filter->last_out = value;
62  filter->time_const = 2.0f * tau / sample_time;
63 }
64 
71 static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
72 {
73  float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const);
74  filter->last_in = value;
75  filter->last_out = out;
76  return out;
77 }
78 
84 static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
85 {
86  return filter->last_out;
87 }
88 
126  float a[2];
127  float b[2];
128  float i[2];
129  float o[2];
130 };
131 
132 
141 static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
142  float value)
143 {
144  float K = tanf(sample_time / (2.0f * tau));
145  float poly = K * K + K / Q + 1.0f;
146  filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
147  filter->a[1] = (K * K - K / Q + 1.0f) / poly;
148  filter->b[0] = K * K / poly;
149  filter->b[1] = 2.0f * filter->b[0];
150  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
151 }
152 
159 static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
160 {
161  float out = filter->b[0] * value
162  + filter->b[1] * filter->i[0]
163  + filter->b[0] * filter->i[1]
164  - filter->a[0] * filter->o[0]
165  - filter->a[1] * filter->o[1];
166  filter->i[1] = filter->i[0];
167  filter->i[0] = value;
168  filter->o[1] = filter->o[0];
169  filter->o[0] = out;
170  return out;
171 }
172 
178 static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
179 {
180  return filter->o[0];
181 }
182 
184  int32_t a[2];
185  int32_t b[2];
186  int32_t i[2];
187  int32_t o[2];
189 };
190 
199 static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
200  float sample_time, int32_t value)
201 {
202  struct SecondOrderLowPass filter_temp;
203  float tau = 7.0f / (44.0f * cut_off);
204  float K = sample_time / (2.0f * tau);
205  float poly = K * K + K / Q + 1.0f;
206  float loop_gain_f;
207 
208  filter_temp.a[0] = 2.0f * (K * K - 1.0f) / poly;
209  filter_temp.a[1] = (K * K - K / Q + 1.0f) / poly;
210  filter_temp.b[0] = K * K / poly;
211  filter_temp.b[1] = 2.0f * filter_temp.b[0];
212  loop_gain_f = 1.0f / filter_temp.b[0];
213 
214  filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC);
215  filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC);
216  filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC);
217  filter->b[1] = 2 * filter->b[0];
218  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
219  filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC);
220 }
221 
229 {
230  int32_t out = filter->b[0] * value
231  + filter->b[1] * filter->i[0]
232  + filter->b[0] * filter->i[1]
233  - filter->a[0] * filter->o[0]
234  - filter->a[1] * filter->o[1];
235 
236  filter->i[1] = filter->i[0];
237  filter->i[0] = value;
238  filter->o[1] = filter->o[0];
239  filter->o[0] = out / (filter->loop_gain);
240  return filter->o[0];
241 }
242 
249 {
250  return filter->o[0];
251 }
252 
256 
269 static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
270 {
271  init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
272 }
273 
280 static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
281 {
282  return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
283 }
284 
291 {
292  return filter->o[0];
293 }
294 
298 
311 static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
312  int32_t value)
313 {
314  init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
315 }
316 
324 {
325  return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
326 }
327 
334 {
335  return filter->o[0];
336 }
337 
342 typedef struct {
343  struct SecondOrderLowPass lp1;
344  struct SecondOrderLowPass lp2;
346 
360 static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
361 {
362  init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
363  init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
364 }
365 
374 static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
375 {
376  float tmp = update_second_order_low_pass(&filter->lp1, value);
377  return update_second_order_low_pass(&filter->lp2, tmp);
378 }
379 
386 {
387  return filter->lp2.o[0];
388 }
389 
394 typedef struct {
398 
412 static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
413  int32_t value)
414 {
415  init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
416  init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value);
417 }
418 
428 {
429  int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
430  return update_second_order_low_pass_int(&filter->lp2, tmp);
431 }
432 
439 {
440  return filter->lp2.o[0];
441 }
442 
443 #endif
444 
init_second_order_low_pass
static void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time, float value)
Init second order low pass filter.
Definition: low_pass_filter.h:141
update_butterworth_2_low_pass
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Definition: low_pass_filter.h:280
Butterworth4LowPass_int
Fourth order Butterworth low pass filter(fixed point version).
Definition: low_pass_filter.h:394
get_butterworth_4_low_pass_int
static int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter)
Get current value of the fourth order Butterworth low pass filter(fixed point version).
Definition: low_pass_filter.h:438
FirstOrderLowPass::time_const
float time_const
Definition: low_pass_filter.h:40
init_first_order_low_pass
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
Definition: low_pass_filter.h:57
update_first_order_low_pass
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
Definition: low_pass_filter.h:71
SecondOrderLowPass::i
float i[2]
input history
Definition: low_pass_filter.h:128
SecondOrderLowPass
Second order low pass filter structure.
Definition: low_pass_filter.h:125
get_butterworth_4_low_pass
static float get_butterworth_4_low_pass(Butterworth4LowPass *filter)
Get current value of the fourth order Butterworth low pass filter.
Definition: low_pass_filter.h:385
SecondOrderLowPass_int::o
int32_t o[2]
output history
Definition: low_pass_filter.h:187
init_second_order_low_pass_int
static void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q, float sample_time, int32_t value)
Init second order low pass filter(fixed point version).
Definition: low_pass_filter.h:199
update_second_order_low_pass
static float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
Update second order low pass filter state with a new value.
Definition: low_pass_filter.h:159
pprz_algebra_int.h
Paparazzi fixed point algebra.
update_butterworth_2_low_pass_int
static int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
Update second order Butterworth low pass filter state with a new value(fixed point version).
Definition: low_pass_filter.h:323
SecondOrderLowPass::o
float o[2]
output history
Definition: low_pass_filter.h:129
SecondOrderLowPass_int::b
int32_t b[2]
numerator gains
Definition: low_pass_filter.h:185
SecondOrderLowPass_int::a
int32_t a[2]
denominator gains
Definition: low_pass_filter.h:184
SecondOrderLowPass_int::i
int32_t i[2]
input history
Definition: low_pass_filter.h:186
SecondOrderLowPass::a
float a[2]
denominator gains
Definition: low_pass_filter.h:126
std.h
update_butterworth_4_low_pass_int
static int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, int32_t value)
Update fourth order Butterworth low pass filter state with a new value(fixed point version).
Definition: low_pass_filter.h:427
FirstOrderLowPass::last_in
float last_in
Definition: low_pass_filter.h:41
FirstOrderLowPass::last_out
float last_out
Definition: low_pass_filter.h:42
update_butterworth_4_low_pass
static float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
Update fourth order Butterworth low pass filter state with a new value.
Definition: low_pass_filter.h:374
get_second_order_low_pass_int
static int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int *filter)
Get current value of the second order low pass filter(fixed point version).
Definition: low_pass_filter.h:248
Butterworth4LowPass::lp2
struct SecondOrderLowPass lp2
Definition: low_pass_filter.h:344
Butterworth4LowPass::lp1
struct SecondOrderLowPass lp1
Definition: low_pass_filter.h:343
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:205
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
get_butterworth_2_low_pass_int
static int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter)
Get current value of the second order Butterworth low pass filter(fixed point version).
Definition: low_pass_filter.h:333
int32_t
signed long int32_t
Definition: types.h:19
Butterworth4LowPass_int::lp1
struct SecondOrderLowPass_int lp1
Definition: low_pass_filter.h:395
K
static float K[9]
Definition: undistort_image.c:41
init_butterworth_2_low_pass_int
static void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a second order Butterworth filter.
Definition: low_pass_filter.h:311
get_butterworth_2_low_pass
static float get_butterworth_2_low_pass(Butterworth2LowPass *filter)
Get current value of the second order Butterworth low pass filter.
Definition: low_pass_filter.h:290
SecondOrderLowPass_int::loop_gain
int32_t loop_gain
loop gain
Definition: low_pass_filter.h:188
init_butterworth_2_low_pass
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
Definition: low_pass_filter.h:269
get_first_order_low_pass
static float get_first_order_low_pass(struct FirstOrderLowPass *filter)
Get current value of the first order low pass filter.
Definition: low_pass_filter.h:84
init_butterworth_4_low_pass
static void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
Init a fourth order Butterworth filter.
Definition: low_pass_filter.h:360
init_butterworth_4_low_pass_int
static void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a fourth order Butterworth filter(fixed point version).
Definition: low_pass_filter.h:412
SecondOrderLowPass::b
float b[2]
numerator gains
Definition: low_pass_filter.h:127
get_second_order_low_pass
static float get_second_order_low_pass(struct SecondOrderLowPass *filter)
Get current value of the second order low pass filter.
Definition: low_pass_filter.h:178
Butterworth4LowPass
Fourth order Butterworth low pass filter.
Definition: low_pass_filter.h:342
SecondOrderLowPass_int
Definition: low_pass_filter.h:183
FirstOrderLowPass
First order low pass filter structure.
Definition: low_pass_filter.h:39
update_second_order_low_pass_int
static int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
Update second order low pass filter state with a new value(fixed point version).
Definition: low_pass_filter.h:228
Butterworth4LowPass_int::lp2
struct SecondOrderLowPass_int lp2
Definition: low_pass_filter.h:396
INT32_FILT_FRAC
#define INT32_FILT_FRAC
Definition: low_pass_filter.h:33