Paparazzi UAS  v5.18.0_stable
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 "subsystems/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_INT32(IMU_F1E_ID, stamp, &gyro_i);
138  } else {
139  AbiSendMsgIMU_GYRO_INT32(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_INT32(IMU_F1E_ID, stamp, &accel_i);
167  } else {
168  AbiSendMsgIMU_ACCEL_INT32(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_INT32(IMU_F1E_ID, stamp, mag);
180 }
181 
186 {
194 
195  for (int i = 0; i < 3; i++) {
206  }
207 
208  AbiBindMsgIMU_GYRO_INT32(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb);
209  AbiBindMsgIMU_ACCEL_INT32(IMU_F1E_BIND_ID, &accel_ev, accel_cb);
210  AbiBindMsgIMU_MAG_INT32(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 }
273 
filter_1euro_imu.h
Filter1eImu::accel_dcutoff
float accel_dcutoff
Definition: filter_1euro_imu.h:38
filter_1e_imu
struct Filter1eImu filter_1e_imu
configuration structure
Definition: filter_1euro_imu.c:86
FILTER_1EURO_ACCEL_DCUTOFF
#define FILTER_1EURO_ACCEL_DCUTOFF
Default accel dcutoff (not recommanded to change)
Definition: filter_1euro_imu.c:65
filter_1euro_imu_reset
void filter_1euro_imu_reset(float enabled)
settings handlers
Definition: filter_1euro_imu.c:217
gyro_ev
static abi_event gyro_ev
Definition: filter_1euro_imu.c:110
FILTER_1EURO_GYRO_MINCUTOFF
#define FILTER_1EURO_GYRO_MINCUTOFF
Default gyro min cutoff freq.
Definition: filter_1euro_imu.c:40
Filter1eImu
Definition: filter_1euro_imu.h:31
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
filter_1euro_imu_update_gyro_beta
void filter_1euro_imu_update_gyro_beta(float beta)
Definition: filter_1euro_imu.c:234
abi.h
filter_1euro_imu_update_accel_dcutoff
void filter_1euro_imu_update_accel_dcutoff(float dcutoff)
Definition: filter_1euro_imu.c:266
FILTER_1EURO_ACCEL_MINCUTOFF
#define FILTER_1EURO_ACCEL_MINCUTOFF
Default accel min cutoff freq.
Definition: filter_1euro_imu.c:55
OneEuroFilter::dcutoff
float dcutoff
derivative cutoff freq (in Hz)
Definition: 1e_filter.h:56
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
OneEuroFilter::mincutoff
float mincutoff
min cutoff freq (in Hz)
Definition: 1e_filter.h:54
pprz_algebra_float.h
Paparazzi floating point algebra.
pprz_algebra_int.h
Paparazzi fixed point algebra.
OneEuroFilter::beta
float beta
cutoff slope
Definition: 1e_filter.h:55
FloatVect3
Definition: pprz_algebra_float.h:54
Filter1eImu::accel_mincutoff
float accel_mincutoff
Definition: filter_1euro_imu.h:36
filter_1euro_imu_init
void filter_1euro_imu_init(void)
Init and bindings.
Definition: filter_1euro_imu.c:185
update_1e_filter_at_time
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
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
IMU_F1E_BIND_ID
#define IMU_F1E_BIND_ID
ABI bindings.
Definition: filter_1euro_imu.c:106
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: filter_1euro_imu.c:172
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: filter_1euro_imu.c:143
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
FILTER_1EURO_ACCEL_BETA
#define FILTER_1EURO_ACCEL_BETA
Default accel beta coef.
Definition: filter_1euro_imu.c:60
filter_1euro_imu_update_accel_beta
void filter_1euro_imu_update_accel_beta(float beta)
Definition: filter_1euro_imu.c:258
Int32Vect3
Definition: pprz_algebra_int.h:88
filter_1euro_imu_update_accel_mincutoff
void filter_1euro_imu_update_accel_mincutoff(float mincutoff)
Definition: filter_1euro_imu.c:250
gyro_1e
static struct OneEuroFilter gyro_1e[3]
array of 1 euro filters for gyrometer
Definition: filter_1euro_imu.c:91
uint8_t
unsigned char uint8_t
Definition: types.h:14
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
FILTER_1EURO_FREQ
#define FILTER_1EURO_FREQ
Auto freq if not defined.
Definition: filter_1euro_imu.c:79
FILTER_1EURO_GYRO_DCUTOFF
#define FILTER_1EURO_GYRO_DCUTOFF
Default gyro dcutoff (not recommanded to change)
Definition: filter_1euro_imu.c:50
reset_1e_filter
static void reset_1e_filter(struct OneEuroFilter *filter)
Reset filter (gains and parameters unchanged)
Definition: 1e_filter.h:84
Filter1eImu::gyro_dcutoff
float gyro_dcutoff
Definition: filter_1euro_imu.h:35
FILTER_1EURO_ENABLED
#define FILTER_1EURO_ENABLED
Enable by default.
Definition: filter_1euro_imu.c:35
accel_ev
static abi_event accel_ev
Definition: filter_1euro_imu.c:111
Filter1eImu::gyro_mincutoff
float gyro_mincutoff
Definition: filter_1euro_imu.h:33
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
mag_ev
static abi_event mag_ev
Definition: filter_1euro_imu.c:112
FILTER_1EURO_GYRO_BETA
#define FILTER_1EURO_GYRO_BETA
Default gyro beta coef.
Definition: filter_1euro_imu.c:45
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
accel_1e
static struct OneEuroFilter accel_1e[3]
array of 1 euro filters for accelerometer
Definition: filter_1euro_imu.c:96
filter_1euro_imu_update_gyro_mincutoff
void filter_1euro_imu_update_gyro_mincutoff(float mincutoff)
Definition: filter_1euro_imu.c:226
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: filter_1euro_imu.c:114
init_1e_filter
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
update_1e_filter
static float update_1e_filter(struct OneEuroFilter *filter, float x)
Filter a float using the given One Euro Filter.
Definition: 1e_filter.h:124
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
Filter1eImu::enabled
bool enabled
Definition: filter_1euro_imu.h:32
1e_filter.h
Implementation of the 1 euro filter.
OneEuroFilter
configuration parameters.
Definition: 1e_filter.h:51
Filter1eImu::accel_beta
float accel_beta
Definition: filter_1euro_imu.h:37
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
Filter1eImu::gyro_beta
float gyro_beta
Definition: filter_1euro_imu.h:34
IMU_F1E_ID
#define IMU_F1E_ID
Definition: abi_sender_ids.h:352
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
filter_1euro_imu_update_gyro_dcutoff
void filter_1euro_imu_update_gyro_dcutoff(float dcutoff)
Definition: filter_1euro_imu.c:242