Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
filter_1euro_imu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 #include "filters/1e_filter.h"
28 #include "math/pprz_algebra_int.h"
30 #include "modules/core/abi.h"
31 #include "generated/airframe.h"
32 
34 #ifndef FILTER_1EURO_ENABLED
35 #define FILTER_1EURO_ENABLED TRUE
36 #endif
37 
39 #ifndef FILTER_1EURO_GYRO_MINCUTOFF
40 #define FILTER_1EURO_GYRO_MINCUTOFF 10.f
41 #endif
42 
44 #ifndef FILTER_1EURO_GYRO_BETA
45 #define FILTER_1EURO_GYRO_BETA 0.1f
46 #endif
47 
49 #ifndef FILTER_1EURO_GYRO_DCUTOFF
50 #define FILTER_1EURO_GYRO_DCUTOFF 1.f
51 #endif
52 
54 #ifndef FILTER_1EURO_ACCEL_MINCUTOFF
55 #define FILTER_1EURO_ACCEL_MINCUTOFF 0.1f
56 #endif
57 
59 #ifndef FILTER_1EURO_ACCEL_BETA
60 #define FILTER_1EURO_ACCEL_BETA 0.01f
61 #endif
62 
64 #ifndef FILTER_1EURO_ACCEL_DCUTOFF
65 #define FILTER_1EURO_ACCEL_DCUTOFF 1.f
66 #endif
67 
69 #ifndef FILTER_1EURO_FREQ
70 #if defined AHRS_PROPAGATE_FREQUENCY
71 #define FILTER_1EURO_FREQ AHRS_PROPAGATE_FREQUENCY
72 PRINT_CONFIG_VAR(FILTER_1EURO_FREQ)
73 #elif defined INS_PROPAGATE_FREQUENCY
74 #define FILTER_1EURO_FREQ INS_PROPAGATE_FREQUENCY
75 PRINT_CONFIG_VAR(FILTER_1EURO_FREQ)
76 #else
77 PRINT_CONFIG_MSG("FILTER_1EURO_FREQ not defined, using timestamp")
78 // init freq at periodic frequency, value can't be zero
79 #define FILTER_1EURO_FREQ PERIODIC_FREQUENCY
80 #endif
81 #endif
82 
87 
91 static struct OneEuroFilter gyro_1e[3];
92 
96 static struct OneEuroFilter accel_1e[3];
97 
105 #ifndef IMU_F1E_BIND_ID
106 #define IMU_F1E_BIND_ID ABI_BROADCAST
107 #endif
108 PRINT_CONFIG_VAR(IMU_F1E_BIND_ID)
109 
112 static abi_event mag_ev; // only passthrough
113 
114 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
115 {
116  if (sender_id == IMU_F1E_ID) {
117  return; // don't process own data
118  }
119 
120  if (filter_1e_imu.enabled) {
121  struct FloatRates gyro_f;
122  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
123  // compute filters
124 #ifdef FILTER_1EURO_FREQ
125  gyro_f.p = update_1e_filter(&gyro_1e[0], gyro_f.p);
126  gyro_f.q = update_1e_filter(&gyro_1e[1], gyro_f.q);
127  gyro_f.r = update_1e_filter(&gyro_1e[2], gyro_f.r);
128 #else
129  // use timestamp
130  gyro_f.p = update_1e_filter_at_time(&gyro_1e[0], gyro_f.p, stamp);
131  gyro_f.q = update_1e_filter_at_time(&gyro_1e[1], gyro_f.q, stamp);
132  gyro_f.r = update_1e_filter_at_time(&gyro_1e[2], gyro_f.r, stamp);
133 #endif
134  // send filtered data
135  struct Int32Rates gyro_i;
136  RATES_BFP_OF_REAL(gyro_i, gyro_f);
137  AbiSendMsgIMU_GYRO(IMU_F1E_ID, stamp, &gyro_i);
138  } else {
139  AbiSendMsgIMU_GYRO(IMU_F1E_ID, stamp, gyro);
140  }
141 }
142 
143 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
144 {
145  if (sender_id == IMU_F1E_ID) {
146  return; // don't process own data
147  }
148 
149  if (filter_1e_imu.enabled) {
150  struct FloatVect3 accel_f;
151  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
152  // compute filters
153 #ifdef FILTER_1EURO_FREQ
154  accel_f.x = update_1e_filter(&accel_1e[0], accel_f.x);
155  accel_f.y = update_1e_filter(&accel_1e[1], accel_f.y);
156  accel_f.z = update_1e_filter(&accel_1e[2], accel_f.z);
157 #else
158  // use timestamp
159  accel_f.x = update_1e_filter_at_time(&accel_1e[0], accel_f.x, stamp);
160  accel_f.y = update_1e_filter_at_time(&accel_1e[1], accel_f.y, stamp);
161  accel_f.z = update_1e_filter_at_time(&accel_1e[2], accel_f.z, stamp);
162 #endif
163  // send filtered data
164  struct Int32Vect3 accel_i;
165  ACCELS_BFP_OF_REAL(accel_i, accel_f);
166  AbiSendMsgIMU_ACCEL(IMU_F1E_ID, stamp, &accel_i);
167  } else {
168  AbiSendMsgIMU_ACCEL(IMU_F1E_ID, stamp, accel);
169  }
170 }
171 
172 static void mag_cb(uint8_t sender_id __attribute__((unused)),
173  uint32_t stamp, struct Int32Vect3 *mag)
174 {
175  if (sender_id == IMU_F1E_ID) {
176  return; // don't process own data
177  }
178 
179  AbiSendMsgIMU_MAG(IMU_F1E_ID, stamp, mag);
180 }
181 
186 {
194 
195  for (int i = 0; i < 3; i++) {
206  }
207 
208  AbiBindMsgIMU_GYRO(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb);
209  AbiBindMsgIMU_ACCEL(IMU_F1E_BIND_ID, &accel_ev, accel_cb);
210  AbiBindMsgIMU_MAG(IMU_F1E_BIND_ID, &mag_ev, mag_cb);
211 }
212 
217 void filter_1euro_imu_reset(float enabled)
218 {
219  filter_1e_imu.enabled = enabled;
220  for (int i = 0; i < 3; i++) {
223  }
224 }
225 
227 {
228  filter_1e_imu.gyro_mincutoff = mincutoff;
229  for (int i = 0; i < 3; i++) {
230  gyro_1e[i].mincutoff = mincutoff;
231  }
232 }
233 
235 {
236  filter_1e_imu.gyro_beta = beta;
237  for (int i = 0; i < 3; i++) {
238  gyro_1e[i].beta = beta;
239  }
240 }
241 
243 {
244  filter_1e_imu.gyro_dcutoff = dcutoff;
245  for (int i = 0; i < 3; i++) {
246  gyro_1e[i].dcutoff = dcutoff;
247  }
248 }
249 
251 {
252  filter_1e_imu.accel_mincutoff = mincutoff;
253  for (int i = 0; i < 3; i++) {
254  accel_1e[i].mincutoff = mincutoff;
255  }
256 }
257 
259 {
260  filter_1e_imu.accel_beta = beta;
261  for (int i = 0; i < 3; i++) {
262  accel_1e[i].beta = beta;
263  }
264 }
265 
267 {
268  filter_1e_imu.accel_dcutoff = dcutoff;
269  for (int i = 0; i < 3; i++) {
270  accel_1e[i].dcutoff = dcutoff;
271  }
272 }
Implementation of the 1 euro filter.
float mincutoff
min cutoff freq (in Hz)
Definition: 1e_filter.h:54
static float update_1e_filter_at_time(struct OneEuroFilter *filter, float x, uint32_t timestamp)
Filter a float using the given One Euro Filter and the given timestamp.
Definition: 1e_filter.h:142
float dcutoff
derivative cutoff freq (in Hz)
Definition: 1e_filter.h:56
static void reset_1e_filter(struct OneEuroFilter *filter)
Reset filter (gains and parameters unchanged)
Definition: 1e_filter.h:84
static void init_1e_filter(struct OneEuroFilter *filter, float rate, float mincutoff, float beta, float dcutoff)
Initialize a 1 Euro Filter instance.
Definition: 1e_filter.h:65
static float update_1e_filter(struct OneEuroFilter *filter, float x)
Filter a float using the given One Euro Filter.
Definition: 1e_filter.h:124
float beta
cutoff slope
Definition: 1e_filter.h:55
configuration parameters.
Definition: 1e_filter.h:51
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define IMU_F1E_ID
void filter_1euro_imu_update_gyro_mincutoff(float mincutoff)
struct Filter1eImu filter_1e_imu
configuration structure
#define FILTER_1EURO_ACCEL_MINCUTOFF
Default accel min cutoff freq.
void filter_1euro_imu_update_gyro_beta(float beta)
#define FILTER_1EURO_FREQ
Auto freq if not defined.
void filter_1euro_imu_update_accel_dcutoff(float dcutoff)
#define FILTER_1EURO_ENABLED
Enable by default.
void filter_1euro_imu_update_gyro_dcutoff(float dcutoff)
#define FILTER_1EURO_GYRO_DCUTOFF
Default gyro dcutoff (not recommanded to change)
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define FILTER_1EURO_GYRO_MINCUTOFF
Default gyro min cutoff freq.
static struct OneEuroFilter gyro_1e[3]
array of 1 euro filters for gyrometer
#define FILTER_1EURO_ACCEL_BETA
Default accel beta coef.
static abi_event gyro_ev
void filter_1euro_imu_reset(float enabled)
settings handlers
void filter_1euro_imu_update_accel_beta(float beta)
void filter_1euro_imu_update_accel_mincutoff(float mincutoff)
#define FILTER_1EURO_GYRO_BETA
Default gyro beta coef.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define IMU_F1E_BIND_ID
ABI bindings.
#define FILTER_1EURO_ACCEL_DCUTOFF
Default accel dcutoff (not recommanded to change)
void filter_1euro_imu_init(void)
Init and bindings.
static struct OneEuroFilter accel_1e[3]
array of 1 euro filters for accelerometer
float gyro_mincutoff
float accel_mincutoff
float q
in rad/s
float p
in rad/s
float r
in rad/s
angular rates
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
angular rates
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98