Paparazzi UAS  v6.2_unstable
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 
95 static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, float tau, float sample_time)
96 {
97  filter->time_const = 2.0f * tau / sample_time;
98 }
99 
137  float a[2];
138  float b[2];
139  float i[2];
140  float o[2];
141 };
142 
143 
152 static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
153  float value)
154 {
155  float K = tanf(sample_time / (2.0f * tau));
156  float poly = K * K + K / Q + 1.0f;
157  filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
158  filter->a[1] = (K * K - K / Q + 1.0f) / poly;
159  filter->b[0] = K * K / poly;
160  filter->b[1] = 2.0f * filter->b[0];
161  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
162 }
163 
170 static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
171 {
172  float out = filter->b[0] * value
173  + filter->b[1] * filter->i[0]
174  + filter->b[0] * filter->i[1]
175  - filter->a[0] * filter->o[0]
176  - filter->a[1] * filter->o[1];
177  filter->i[1] = filter->i[0];
178  filter->i[0] = value;
179  filter->o[1] = filter->o[0];
180  filter->o[0] = out;
181  return out;
182 }
183 
189 static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
190 {
191  return filter->o[0];
192 }
193 
195  int32_t a[2];
196  int32_t b[2];
197  int32_t i[2];
198  int32_t o[2];
200 };
201 
210 static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
211  float sample_time, int32_t value)
212 {
213  struct SecondOrderLowPass filter_temp;
214  float tau = 7.0f / (44.0f * cut_off);
215  float K = sample_time / (2.0f * tau);
216  float poly = K * K + K / Q + 1.0f;
217  float loop_gain_f;
218 
219  filter_temp.a[0] = 2.0f * (K * K - 1.0f) / poly;
220  filter_temp.a[1] = (K * K - K / Q + 1.0f) / poly;
221  filter_temp.b[0] = K * K / poly;
222  filter_temp.b[1] = 2.0f * filter_temp.b[0];
223  loop_gain_f = 1.0f / filter_temp.b[0];
224 
225  filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC);
226  filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC);
227  filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC);
228  filter->b[1] = 2 * filter->b[0];
229  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
230  filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC);
231 }
232 
240 {
241  int32_t out = filter->b[0] * value
242  + filter->b[1] * filter->i[0]
243  + filter->b[0] * filter->i[1]
244  - filter->a[0] * filter->o[0]
245  - filter->a[1] * filter->o[1];
246 
247  filter->i[1] = filter->i[0];
248  filter->i[0] = value;
249  filter->o[1] = filter->o[0];
250  filter->o[0] = out / (filter->loop_gain);
251  return filter->o[0];
252 }
253 
260 {
261  return filter->o[0];
262 }
263 
267 
280 static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
281 {
282  init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
283 }
284 
291 static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
292 {
293  return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
294 }
295 
302 {
303  return filter->o[0];
304 }
305 
309 
322 static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
323  int32_t value)
324 {
325  init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
326 }
327 
335 {
336  return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
337 }
338 
345 {
346  return filter->o[0];
347 }
348 
353 typedef struct {
354  struct SecondOrderLowPass lp1;
355  struct SecondOrderLowPass lp2;
357 
371 static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
372 {
373  init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
374  init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
375 }
376 
385 static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
386 {
387  float tmp = update_second_order_low_pass(&filter->lp1, value);
388  return update_second_order_low_pass(&filter->lp2, tmp);
389 }
390 
397 {
398  return filter->lp2.o[0];
399 }
400 
405 typedef struct {
409 
423 static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
424  int32_t value)
425 {
426  init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
427  init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value);
428 }
429 
439 {
440  int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
441  return update_second_order_low_pass_int(&filter->lp2, tmp);
442 }
443 
450 {
451  return filter->lp2.o[0];
452 }
453 
454 #endif
455 
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:152
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:291
Butterworth4LowPass_int
Fourth order Butterworth low pass filter(fixed point version).
Definition: low_pass_filter.h:405
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:449
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:139
SecondOrderLowPass
Second order low pass filter structure.
Definition: low_pass_filter.h:136
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:396
SecondOrderLowPass_int::o
int32_t o[2]
output history
Definition: low_pass_filter.h:198
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:210
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:170
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:334
SecondOrderLowPass::o
float o[2]
output history
Definition: low_pass_filter.h:140
SecondOrderLowPass_int::b
int32_t b[2]
numerator gains
Definition: low_pass_filter.h:196
SecondOrderLowPass_int::a
int32_t a[2]
denominator gains
Definition: low_pass_filter.h:195
SecondOrderLowPass_int::i
int32_t i[2]
input history
Definition: low_pass_filter.h:197
SecondOrderLowPass::a
float a[2]
denominator gains
Definition: low_pass_filter.h:137
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:438
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:385
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:259
Butterworth4LowPass::lp2
struct SecondOrderLowPass lp2
Definition: low_pass_filter.h:355
Butterworth4LowPass::lp1
struct SecondOrderLowPass lp1
Definition: low_pass_filter.h:354
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:206
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:344
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
Butterworth4LowPass_int::lp1
struct SecondOrderLowPass_int lp1
Definition: low_pass_filter.h:406
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:322
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:301
SecondOrderLowPass_int::loop_gain
int32_t loop_gain
loop gain
Definition: low_pass_filter.h:199
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:280
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:371
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:423
SecondOrderLowPass::b
float b[2]
numerator gains
Definition: low_pass_filter.h:138
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:189
Butterworth4LowPass
Fourth order Butterworth low pass filter.
Definition: low_pass_filter.h:353
SecondOrderLowPass_int
Definition: low_pass_filter.h:194
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:239
Butterworth4LowPass_int::lp2
struct SecondOrderLowPass_int lp2
Definition: low_pass_filter.h:407
INT32_FILT_FRAC
#define INT32_FILT_FRAC
Definition: low_pass_filter.h:33
update_first_order_low_pass_tau
static void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, float tau, float sample_time)
Update time constant (tau parameter) for first order low pass filter.
Definition: low_pass_filter.h:95