Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
32 #define INT32_FILT_FRAC 8
33 
39  float time_const;
40  float last_in;
41  float last_out;
42 };
43 
56 static inline void init_first_order_low_pass(struct FirstOrderLowPass * filter, float tau, float sample_time, float value) {
57  filter->last_in = value;
58  filter->last_out = value;
59  filter->time_const = 2.0f * tau / sample_time;
60 }
61 
68 static inline float update_first_order_low_pass(struct FirstOrderLowPass * filter, float value) {
69  float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const);
70  filter->last_in = value;
71  filter->last_out = out;
72  return out;
73 }
74 
80 static inline float get_first_order_low_pass(struct FirstOrderLowPass * filter) {
81  return filter->last_out;
82 }
83 
118  float a[2];
119  float b[2];
120  float i[2];
121  float o[2];
122 };
123 
124 
133 static inline void init_second_order_low_pass(struct SecondOrderLowPass * filter, float tau, float Q, float sample_time, float value) {
134  float K = sample_time / (2.0f * tau);
135  float poly = K*K + K/Q + 1.0f;
136  filter->a[0] = 2.0f * (K*K - 1.0f) / poly;
137  filter->a[1] = (K*K - K/Q + 1.0f) / poly;
138  filter->b[0] = K*K / poly;
139  filter->b[1] = 2.0f * filter->b[0];
140  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
141 }
142 
149 static inline float update_second_order_low_pass(struct SecondOrderLowPass * filter, float value) {
150  float out = filter->b[0] * value
151  + filter->b[1] * filter->i[0]
152  + filter->b[0] * filter->i[1]
153  - filter->a[0] * filter->o[0]
154  - filter->a[1] * filter->o[1];
155  filter->i[1] = filter->i[0];
156  filter->i[0] = value;
157  filter->o[1] = filter->o[0];
158  filter->o[0] = out;
159  return out;
160 }
161 
167 static inline float get_second_order_low_pass(struct SecondOrderLowPass * filter) {
168  return filter->o[0];
169 }
170 
172  int32_t a[2];
173  int32_t b[2];
174  int32_t i[2];
175  int32_t o[2];
177 };
178 
187 static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int* filter, float cut_off, float Q, float sample_time, int32_t value) {
188  struct SecondOrderLowPass filter_temp;
189  float tau = 7.0f / (44.0f * cut_off);
190  float K = sample_time / (2.0f * tau);
191  float poly = K*K + K/Q + 1.0f;
192  float loop_gain_f;
193 
194  filter_temp.a[0] = 2.0f * (K*K - 1.0f) / poly;
195  filter_temp.a[1] = (K*K - K/Q + 1.0f) / poly;
196  filter_temp.b[0] = K*K / poly;
197  filter_temp.b[1] = 2.0f * filter_temp.b[0];
198  loop_gain_f = 1.0f / filter_temp.b[0];
199 
200  filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC);
201  filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC);
202  filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC);
203  filter->b[1] = 2 * filter->b[0];
204  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
205  filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC);
206 }
207 
215  int32_t out = filter->b[0] * value
216  + filter->b[1] * filter->i[0]
217  + filter->b[0] * filter->i[1]
218  - filter->a[0] * filter->o[0]
219  - filter->a[1] * filter->o[1];
220 
221  filter->i[1] = filter->i[0];
222  filter->i[0] = value;
223  filter->o[1] = filter->o[0];
224  filter->o[0] = out / (filter->loop_gain);
225  return filter->o[0];
226 }
227 
234  return filter->o[0];
235 }
236 
240 
253 static inline void init_butterworth_2_low_pass(Butterworth2LowPass * filter, float tau, float sample_time, float value) {
254  init_second_order_low_pass((struct SecondOrderLowPass*)filter, tau, 0.7071, sample_time, value);
255 }
256 
263 static inline float update_butterworth_2_low_pass(Butterworth2LowPass * filter, float value) {
264  return update_second_order_low_pass((struct SecondOrderLowPass*)filter, value);
265 }
266 
272 static inline float get_butterworth_2_low_pass(Butterworth2LowPass * filter) {
273  return filter->o[0];
274 }
275 
279 
292 static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter, float cut_off, float sample_time, int32_t value) {
293  init_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, cut_off, 0.7071, sample_time, value);
294 }
295 
303  return update_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, value);
304 }
305 
312  return filter->o[0];
313 }
314 
319 typedef struct {
320  struct SecondOrderLowPass lp1;
321  struct SecondOrderLowPass lp2;
323 
337 static inline void init_butterworth_4_low_pass(Butterworth4LowPass * filter, float tau, float sample_time, float value) {
338  init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
339  init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
340 }
341 
350 static inline float update_butterworth_4_low_pass(Butterworth4LowPass * filter, float value) {
351  float tmp = update_second_order_low_pass(&filter->lp1, value);
352  return update_second_order_low_pass(&filter->lp2, tmp);
353 }
354 
360 static inline float get_butterworth_4_low_pass(Butterworth4LowPass * filter) {
361  return filter->lp2.o[0];
362 }
363 
368 typedef struct {
372 
386 static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter, float cut_off, float sample_time, int32_t value) {
387  init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
388  init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value);
389 }
390 
400  int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
401  return update_second_order_low_pass_int(&filter->lp2, tmp);
402 }
403 
410  return filter->lp2.o[0];
411 }
412 
413 #endif
414 
int32_t i[2]
input history
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.
Fourth order Butterworth low pass filter(fixed point version).
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
int32_t b[2]
numerator gains
static float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
Update second order low pass filter state with a new value.
uint16_t value
Definition: adc_arch.c:585
struct SecondOrderLowPass_int lp1
struct SecondOrderLowPass lp2
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.
float b[2]
numerator gains
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).
struct SecondOrderLowPass_int lp2
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).
Second order low pass filter structure.
int32_t a[2]
denominator gains
First order low pass filter structure.
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).
int32_t loop_gain
loop gain
#define BFP_OF_REAL(_vr, _frac)
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.
float a[2]
denominator gains
static float get_first_order_low_pass(struct FirstOrderLowPass *filter)
Get current value of the first order low pass filter.
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.
float o[2]
output history
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)...
struct SecondOrderLowPass lp1
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
Fourth order Butterworth low pass filter.
float i[2]
input history
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).
int32_t o[2]
output history
static float get_second_order_low_pass(struct SecondOrderLowPass *filter)
Get current value of the second order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.