Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
high_pass_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Ewoud Smeur
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 HIGH_PASS_FILTER_H
28 #define HIGH_PASS_FILTER_H
29 
30 #include "std.h"
31 #include "math/pprz_algebra_int.h"
32 
38 {
39  float time_const;
40  float last_in;
41  float last_out;
42 };
43 
56 static inline void init_first_order_high_pass(struct FirstOrderHighPass *filter,
57  float tau, float sample_time, float value)
58 {
59  filter->last_in = value;
60  filter->last_out = value;
61  filter->time_const = 2.0f * tau / sample_time;
62 }
63 
70 static inline float update_first_order_high_pass(
71  struct FirstOrderHighPass *filter, float value)
72 {
73  float out = (filter->time_const * (value - filter->last_in)
74  + (filter->time_const - 1.0f) * filter->last_out)
75  / (1.0f + filter->time_const);
76  filter->last_in = value;
77  filter->last_out = out;
78  return out;
79 }
80 
86 static inline float get_first_order_high_pass(struct FirstOrderHighPass *filter)
87 {
88  return filter->last_out;
89 }
90 
100  double a[4];
101  double b[4];
102  double i[4];
103  double o[4];
104 };
105 
106 
112 static inline void init_fourth_order_high_pass(struct FourthOrderHighPass *filter, double *a, double *b, double value)
113 {
114  filter->a[0] = a[0];
115  filter->a[1] = a[1];
116  filter->a[2] = a[2];
117  filter->a[3] = a[3];
118  filter->b[0] = b[0];
119  filter->b[1] = b[1];
120  filter->b[2] = b[2];
121  filter->b[3] = b[3];
122  filter->i[0] = filter->i[1] = filter->i[2] = filter->i[3] = filter->o[0] = filter->o[1] = filter->o[2] = filter->o[3] = value;
123 }
124 
131 static inline double update_fourth_order_high_pass(struct FourthOrderHighPass *filter, double value)
132 {
133  double out = filter->b[0] * value
134  + filter->b[1] * filter->i[0]
135  + filter->b[2] * filter->i[1]
136  + filter->b[3] * filter->i[2]
137  + filter->b[0] * filter->i[3]
138  - filter->a[0] * filter->o[0]
139  - filter->a[1] * filter->o[1]
140  - filter->a[2] * filter->o[2]
141  - filter->a[3] * filter->o[3];
142 
143  filter->i[3] = filter->i[2];
144  filter->i[2] = filter->i[1];
145  filter->i[1] = filter->i[0];
146  filter->i[0] = value;
147  filter->o[3] = filter->o[2];
148  filter->o[2] = filter->o[1];
149  filter->o[1] = filter->o[0];
150  filter->o[0] = out;
151  return out;
152 }
153 
159 static inline double get_fourth_order_high_pass(struct FourthOrderHighPass *filter)
160 {
161  return filter->o[0];
162 }
163 
164 #endif
165 
static double update_fourth_order_high_pass(struct FourthOrderHighPass *filter, double value)
Update fourth order high pass filter state with a new value.
double b[4]
numerator gains
static void init_fourth_order_high_pass(struct FourthOrderHighPass *filter, double *a, double *b, double value)
Init fourth order high pass filter.
double a[4]
denominator gains
static void init_first_order_high_pass(struct FirstOrderHighPass *filter, float tau, float sample_time, float value)
Init first order high pass filter.
static float get_first_order_high_pass(struct FirstOrderHighPass *filter)
Get current value of the first order high pass filter.
static float update_first_order_high_pass(struct FirstOrderHighPass *filter, float value)
Update first order high pass filter state with a new value.
double o[4]
output history
static double get_fourth_order_high_pass(struct FourthOrderHighPass *filter)
Get current value of the fourth order high pass filter.
double i[4]
input history
First order high pass filter structure.
Fourth order filter structure.
Paparazzi fixed point algebra.
float b
Definition: wedgebug.c:202