Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
123  float a[2];
124  float b[2];
125  float i[2];
126  float o[2];
127 };
128 
129 
138 static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
139  float value)
140 {
141  float K = sample_time / (2.0f * tau);
142  float poly = K * K + K / Q + 1.0f;
143  filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
144  filter->a[1] = (K * K - K / Q + 1.0f) / poly;
145  filter->b[0] = K * K / poly;
146  filter->b[1] = 2.0f * filter->b[0];
147  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
148 }
149 
156 static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
157 {
158  float out = filter->b[0] * value
159  + filter->b[1] * filter->i[0]
160  + filter->b[0] * filter->i[1]
161  - filter->a[0] * filter->o[0]
162  - filter->a[1] * filter->o[1];
163  filter->i[1] = filter->i[0];
164  filter->i[0] = value;
165  filter->o[1] = filter->o[0];
166  filter->o[0] = out;
167  return out;
168 }
169 
175 static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
176 {
177  return filter->o[0];
178 }
179 
181  int32_t a[2];
182  int32_t b[2];
183  int32_t i[2];
184  int32_t o[2];
186 };
187 
196 static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
197  float sample_time, int32_t value)
198 {
199  struct SecondOrderLowPass filter_temp;
200  float tau = 7.0f / (44.0f * cut_off);
201  float K = sample_time / (2.0f * tau);
202  float poly = K * K + K / Q + 1.0f;
203  float loop_gain_f;
204 
205  filter_temp.a[0] = 2.0f * (K * K - 1.0f) / poly;
206  filter_temp.a[1] = (K * K - K / Q + 1.0f) / poly;
207  filter_temp.b[0] = K * K / poly;
208  filter_temp.b[1] = 2.0f * filter_temp.b[0];
209  loop_gain_f = 1.0f / filter_temp.b[0];
210 
211  filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC);
212  filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC);
213  filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC);
214  filter->b[1] = 2 * filter->b[0];
215  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
216  filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC);
217 }
218 
226 {
227  int32_t out = filter->b[0] * value
228  + filter->b[1] * filter->i[0]
229  + filter->b[0] * filter->i[1]
230  - filter->a[0] * filter->o[0]
231  - filter->a[1] * filter->o[1];
232 
233  filter->i[1] = filter->i[0];
234  filter->i[0] = value;
235  filter->o[1] = filter->o[0];
236  filter->o[0] = out / (filter->loop_gain);
237  return filter->o[0];
238 }
239 
246 {
247  return filter->o[0];
248 }
249 
253 
266 static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
267 {
268  init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
269 }
270 
277 static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
278 {
279  return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
280 }
281 
288 {
289  return filter->o[0];
290 }
291 
295 
308 static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
309  int32_t value)
310 {
311  init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
312 }
313 
321 {
322  return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
323 }
324 
331 {
332  return filter->o[0];
333 }
334 
339 typedef struct {
340  struct SecondOrderLowPass lp1;
341  struct SecondOrderLowPass lp2;
343 
357 static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
358 {
359  init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
360  init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
361 }
362 
371 static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
372 {
373  float tmp = update_second_order_low_pass(&filter->lp1, value);
374  return update_second_order_low_pass(&filter->lp2, tmp);
375 }
376 
383 {
384  return filter->lp2.o[0];
385 }
386 
391 typedef struct {
395 
409 static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
410  int32_t value)
411 {
412  init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
413  init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value);
414 }
415 
425 {
426  int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
427  return update_second_order_low_pass_int(&filter->lp2, tmp);
428 }
429 
436 {
437  return filter->lp2.o[0];
438 }
439 
440 #endif
441 
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.
#define Q
Definition: hf_float.c:74
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
Update second order low pass filter state with a new value.
Fourth order Butterworth low pass filter.
int32_t b[2]
numerator gains
static void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
Init a fourth order Butterworth filter.
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.
First order low pass filter structure.
int32_t loop_gain
loop gain
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).
Fourth order Butterworth low pass filter(fixed point version).
float o[2]
output history
#define BFP_OF_REAL(_vr, _frac)
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).
float i[2]
input history
int32_t o[2]
output history
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).
struct SecondOrderLowPass_int lp1
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)...
static float get_butterworth_2_low_pass(Butterworth2LowPass *filter)
Get current value of the second order Butterworth low pass filter.
static float get_first_order_low_pass(struct FirstOrderLowPass *filter)
Get current value of the first order low pass filter.
float a[2]
denominator gains
signed long int32_t
Definition: types.h:19
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).
static float get_butterworth_4_low_pass(Butterworth4LowPass *filter)
Get current value of the fourth order Butterworth low pass filter.
static float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
Update fourth order Butterworth low pass filter state with a new value.
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).
#define INT32_FILT_FRAC
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)...
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
struct SecondOrderLowPass lp2
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).
Second order low pass filter structure.
float b[2]
numerator gains
static float get_second_order_low_pass(struct SecondOrderLowPass *filter)
Get current value of the second order low pass filter.
int32_t a[2]
denominator gains
int32_t i[2]
input history
struct SecondOrderLowPass_int lp2
struct SecondOrderLowPass lp1
Paparazzi fixed point algebra.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.