Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
imu.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2022 The Paparazzi Team
3 * Freek van Tienen <freek.v.tienen@gmail.com>
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
22
27
28#ifdef BOARD_CONFIG
29#include BOARD_CONFIG
30#endif
31
32#include "modules/imu/imu.h"
33#include "state.h"
34#include "modules/core/abi.h"
36
38#ifndef IMU_INTEGRATION
39#define IMU_INTEGRATION false
40#endif
41
43#ifndef IMU_RATE_EST_WINDOW
44#define IMU_RATE_EST_WINDOW 1000000
45#endif
46
48#ifndef IMU_RATE_EST_ALPHA
49#define IMU_RATE_EST_ALPHA 0.1f
50#endif
51
53#ifndef IMU_RATE_EST_BOUND
54#define IMU_RATE_EST_BOUND 0.03f
55#endif
56
58#ifndef IMU_RATE_EST_JITTER
59#define IMU_RATE_EST_JITTER 2000
60#endif
61
63#ifndef IMU_RATE_EST_REPORT
64#define IMU_RATE_EST_REPORT 30
65#endif
66
68#if defined(IMU_GYRO_CALIB) && (defined(IMU_GYRO_P_SIGN) || defined(IMU_GYRO_Q_SIGN) || defined(IMU_GYRO_R_SIGN))
69#warning "The IMU_GYRO_?_SIGN's aren't compatible with the IMU_GYRO_CALIB define in the airframe"
70#endif
71#ifndef IMU_GYRO_P_SIGN
72#define IMU_GYRO_P_SIGN 1
73#endif
74#ifndef IMU_GYRO_Q_SIGN
75#define IMU_GYRO_Q_SIGN 1
76#endif
77#ifndef IMU_GYRO_R_SIGN
78#define IMU_GYRO_R_SIGN 1
79#endif
80
82#if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
83#define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
84PRINT_CONFIG_MSG("Using single IMU gyro neutral calibration")
85#endif
86
87#if defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
88#define GYRO_SCALE {{IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM, IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM, IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM}, {IMU_GYRO_P_SENS_DEN, IMU_GYRO_Q_SENS_DEN, IMU_GYRO_R_SENS_DEN}}
89PRINT_CONFIG_MSG("Using single IMU gyro sensitivity calibration")
90#endif
91
92#if defined(GYRO_NEUTRAL) && defined(GYRO_SCALE)
93#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=GYRO_NEUTRAL, .scale=GYRO_SCALE}}
94#elif defined(GYRO_NEUTRAL)
95#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=GYRO_NEUTRAL}}
96#elif defined(GYRO_SCALE)
97#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=GYRO_SCALE}}
98#elif !defined(IMU_GYRO_CALIB)
99#define IMU_GYRO_CALIB {}
100#endif
101
102#if defined(IMU_IMU_ACCEL_CALIB)
103#error IMU_IMU_ACCEL_CALIB defined. Rename it "ACCEL_CALIB" if your calibration is in a section with the "IMU_" prefix.
104#endif
105
107#if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN))
108#warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe"
109#endif
110#ifndef IMU_ACCEL_X_SIGN
111#define IMU_ACCEL_X_SIGN 1
112#endif
113#ifndef IMU_ACCEL_Y_SIGN
114#define IMU_ACCEL_Y_SIGN 1
115#endif
116#ifndef IMU_ACCEL_Z_SIGN
117#define IMU_ACCEL_Z_SIGN 1
118#endif
119
121#if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
122#define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
123PRINT_CONFIG_MSG("Using single IMU accel neutral calibration")
124#endif
125
126#if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
127#define ACCEL_SCALE {{IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM, IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM, IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM}, {IMU_ACCEL_X_SENS_DEN, IMU_ACCEL_Y_SENS_DEN, IMU_ACCEL_Z_SENS_DEN}}
128PRINT_CONFIG_MSG("Using single IMU accel sensitivity calibration")
129#endif
130
131#if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
132#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
133#elif defined(ACCEL_NEUTRAL)
134#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
135#elif defined(ACCEL_SCALE)
136#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
137#elif !defined(IMU_ACCEL_CALIB)
138#define IMU_ACCEL_CALIB {}
139#endif
140
142#if defined(IMU_MAG_CALIB) && (defined(IMU_MAG_X_SIGN) || defined(IMU_MAG_Y_SIGN) || defined(IMU_MAG_Z_SIGN))
143#warning "The IMU_MAG_?_SIGN's aren't compatible with the IMU_MAG_CALIB define in the airframe"
144#endif
145#ifndef IMU_MAG_X_SIGN
146#define IMU_MAG_X_SIGN 1
147#endif
148#ifndef IMU_MAG_Y_SIGN
149#define IMU_MAG_Y_SIGN 1
150#endif
151#ifndef IMU_MAG_Z_SIGN
152#define IMU_MAG_Z_SIGN 1
153#endif
154
156#if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
157#define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
158PRINT_CONFIG_MSG("Using single IMU mag neutral calibration")
159#endif
160
161#if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
162#define MAG_SCALE {{IMU_MAG_X_SIGN*IMU_MAG_X_SENS_NUM, IMU_MAG_Y_SIGN*IMU_MAG_Y_SENS_NUM, IMU_MAG_Z_SIGN*IMU_MAG_Z_SENS_NUM}, {IMU_MAG_X_SENS_DEN, IMU_MAG_Y_SENS_DEN, IMU_MAG_Z_SENS_DEN}}
163PRINT_CONFIG_MSG("Using single IMU mag sensitivity calibration")
164#endif
165
166#if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
167#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
168#elif defined(MAG_NEUTRAL)
169#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
170#elif defined(MAG_SCALE)
171#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
172#elif !defined(IMU_MAG_CALIB)
173#define IMU_MAG_CALIB {}
174#endif
175
176
178#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
179#define IMU_BODY_TO_IMU_PHI 0
180#define IMU_BODY_TO_IMU_THETA 0
181#define IMU_BODY_TO_IMU_PSI 0
182#endif
186
187
188#ifndef IMU_GYRO_ABI_SEND_ID
189#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
190#endif
192
193
194#ifndef IMU_ACCEL_ABI_SEND_ID
195#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
196#endif
198
199
200#ifndef IMU_MAG_ABI_SEND_ID
201#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
202#endif
204
205
206#ifndef IMU_LOG_HIGHSPEED_DEVICE
207#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
208#endif
209
210
211#if PERIODIC_TELEMETRY
213
214static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
215{
216 if(imu.accel_abi_send_id == ABI_DISABLE)
217 return;
218 static uint8_t id = 0;
219 pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
220 &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
221 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
222 return;
223 id++;
224 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
225 id = 0;
226}
227
228static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
229{
230 if(imu.accel_abi_send_id == ABI_DISABLE)
231 return;
232 static uint8_t id = 0;
233 pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
234 &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
235 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
236 return;
237 id++;
238 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
239 id = 0;
240}
241
242static void send_accel(struct transport_tx *trans, struct link_device *dev)
243{
244 if(imu.accel_abi_send_id == ABI_DISABLE)
245 return;
246 static uint8_t id = 0;
247 struct FloatVect3 accel_float;
248 ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
249 pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
251 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
252 return;
253 id++;
254 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
255 id = 0;
256}
257
258static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
259{
260 if(imu.gyro_abi_send_id == ABI_DISABLE)
261 return;
262 static uint8_t id = 0;
263 pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
264 &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
265 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
266 return;
267 id++;
268 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
269 id = 0;
270}
271
272static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
273{
274 if(imu.gyro_abi_send_id == ABI_DISABLE)
275 return;
276 static uint8_t id = 0;
277 pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
278 &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
279 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
280 return;
281 id++;
282 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
283 id = 0;
284}
285
286static void send_gyro(struct transport_tx *trans, struct link_device *dev)
287{
288 if(imu.gyro_abi_send_id == ABI_DISABLE)
289 return;
290 static uint8_t id = 0;
291 struct FloatRates gyro_float;
292 RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
293 pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
294 &gyro_float.p, &gyro_float.q, &gyro_float.r);
295 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
296 return;
297 id++;
298 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
299 id = 0;
300}
301
302static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
303{
304 if(imu.mag_abi_send_id == ABI_DISABLE)
305 return;
306 static uint8_t id = 0;
307 pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
308 &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
309 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
310 return;
311 id++;
312 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
313 id = 0;
314}
315
316static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
317{
318 if(imu.mag_abi_send_id == ABI_DISABLE)
319 return;
320 static uint8_t id = 0;
321 pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
322 &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
323 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
324 return;
325 id++;
326 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
327 id = 0;
328}
329
330static void send_mag(struct transport_tx *trans, struct link_device *dev)
331{
332 if(imu.mag_abi_send_id == ABI_DISABLE)
333 return;
334 static uint8_t id = 0;
335 struct FloatVect3 mag_float;
336 MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
337 pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
338 &mag_float.x, &mag_float.y, &mag_float.z);
339 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
340 return;
341 id++;
342 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
343 id = 0;
344}
345
346static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
347{
348 if(imu.mag_abi_send_id == ABI_DISABLE)
349 return;
350 static uint8_t id = 0;
351 pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
352 &imu.mags[id].abi_id,
353 &imu.mags[id].unscaled.x,
354 &imu.mags[id].unscaled.y,
355 &imu.mags[id].unscaled.z,
356 &electrical.current);
357 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
358 return;
359 id++;
360 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
361 id = 0;
362}
363
364#endif /* PERIODIC_TELEMETRY */
365
366#if USE_SHELL
367#include "modules/core/shell.h"
368#include "printf.h"
369#include "string.h"
370
371static void show_calibrated(shell_stream_t *sh, struct imu_calib_t *calibrated) {
372 chprintf(sh, " calibrated: neutral %d, scale %d, scale_f %d, rotation %d, current %d, filter %d\r\n",
373 calibrated->neutral, calibrated->scale, calibrated->scale_f, calibrated->rotation, calibrated->current, calibrated->filter);
374}
375
376static void show_vect3(shell_stream_t *sh, char* name, struct Int32Vect3 *v) {
377 chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, v->x, v->y, v->z);
378}
379
380static void show_vect3f(shell_stream_t *sh, char* name, struct FloatVect3 *v) {
381 chprintf(sh, " %s: %f, %f, %f\r\n", name, v->x, v->y, v->z);
382}
383
384static void show_rates(shell_stream_t *sh, char* name, struct Int32Rates *r) {
385 chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, r->p, r->q, r->r);
386}
387
388static void show_ratesf(shell_stream_t *sh, char* name, struct FloatRates *r) {
389 chprintf(sh, " %s: %f, %f, %f\r\n", name, r->p, r->q, r->r);
390}
391
392
393static void show_matrix(shell_stream_t *sh, char* name, struct Int32RMat *m) {
394 chprintf(sh, " %s:\r\n", name);
395 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 0, 0), MAT33_ELMT(*m, 0, 1), MAT33_ELMT(*m, 0, 2));
396 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 1, 0), MAT33_ELMT(*m, 1, 1), MAT33_ELMT(*m, 1, 2));
397 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 2, 0), MAT33_ELMT(*m, 2, 1), MAT33_ELMT(*m, 2, 2));
398}
399
400static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[])
401{
402 (void) argv;
403 int i = 0;
404 if (argc > 0) {
405 chprintf(sh, "Usage: imu\r\n");
406 return;
407 }
408 chprintf(sh, "GYRO IMU data\r\n");
409 for (i = 0; i < IMU_MAX_SENSORS; i++) {
410 if (imu.gyros[i].abi_id != 0) {
411#if IMU_INTEGRATION
412 chprintf(sh, " Gyro id: %u, time %lu, measured rate %.2f Hz (0: not measured yet)\r\n", imu.gyros[i].abi_id, imu.gyros[i].last_stamp, imu.gyros[i].rate_est.rate);
413#else
414 chprintf(sh, " Gyro id: %u, time %lu\r\n", imu.gyros[i].abi_id, imu.gyros[i].last_stamp);
415#endif
416 show_calibrated(sh, &imu.gyros[i].calibrated);
417 show_rates(sh, "neutral", &imu.gyros[i].neutral);
418 show_ratesf(sh, "scale", &imu.gyros[i].scale_f);
419 show_matrix(sh, "body_to_sensor", &imu.gyros[i].body_to_sensor);
420 show_rates(sh, "unscaled", &imu.gyros[i].unscaled);
421 show_rates(sh, "scaled", &imu.gyros[i].scaled);
422 struct FloatRates gf;
423 RATES_FLOAT_OF_BFP(gf, imu.gyros[i].scaled);
424 chprintf(sh, " -> gyro (rad/s): %.3f, %.3f, %.3f\r\n", gf.p, gf.q, gf.r);
425 }
426 }
427 chprintf(sh, "ACCEL IMU data\r\n");
428 for (i = 0; i < IMU_MAX_SENSORS; i++) {
429 if (imu.accels[i].abi_id != 0) {
430#if IMU_INTEGRATION
431 chprintf(sh, " Accel id: %u, time %lu, measured rate %.2f Hz (0: not measured yet)\r\n", imu.accels[i].abi_id, imu.accels[i].last_stamp, imu.accels[i].rate_est.rate);
432#else
433 chprintf(sh, " Accel id: %u, time %lu\r\n", imu.accels[i].abi_id, imu.accels[i].last_stamp);
434#endif
435 show_calibrated(sh, &imu.accels[i].calibrated);
436 show_vect3(sh, "neutral", &imu.accels[i].neutral);
437 show_vect3f(sh, "scale", &imu.accels[i].scale_f);
438 show_matrix(sh, "body_to_sensor", &imu.accels[i].body_to_sensor);
439 show_vect3(sh, "unscaled", &imu.accels[i].unscaled);
440 show_vect3(sh, "scaled", &imu.accels[i].scaled);
441 struct FloatVect3 af;
442 ACCELS_FLOAT_OF_BFP(af, imu.accels[i].scaled);
443 chprintf(sh, " -> accel (m/s2): %.3f, %.3f, %.3f\r\n", af.x, af.y, af.z);
444 }
445 }
446 chprintf(sh, "MAG IMU data\r\n");
447 for (i = 0; i < IMU_MAX_SENSORS; i++) {
448 if (imu.mags[i].abi_id != 0) {
449 chprintf(sh, " Mag id: %u\r\n", imu.mags[i].abi_id);
450 show_calibrated(sh, &imu.mags[i].calibrated);
451 show_vect3(sh, "neutral", &imu.mags[i].neutral);
452 show_vect3f(sh, "scale", &imu.mags[i].scale_f);
453 show_matrix(sh, "body_to_sensor", &imu.mags[i].body_to_sensor);
454 show_vect3(sh, "unscaled", &imu.mags[i].unscaled);
455 show_vect3(sh, "scaled", &imu.mags[i].scaled);
456 struct FloatVect3 mf;
457 MAGS_FLOAT_OF_BFP(mf, imu.mags[i].scaled);
458 chprintf(sh, " -> mag (unit): %.3f, %.3f, %.3f\r\n", mf.x, mf.y, mf.z);
459 }
460 }
461}
462
463#endif
464
465struct Imu imu = {0};
467static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp);
468static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp);
469static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
470static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
471
480static bool imu_filter_init(Butterworth2LowPass filter[3], float freq, float sample_freq, const float init[3])
481{
482 if (!(freq > 0.f) || !(sample_freq > 0.f) || !isfinite(sample_freq)) {
483 return false;
484 }
485 float tau = 1.0 / (2.0 * M_PI * freq);
486 float sample_time = 1.0 / sample_freq;
487 for (uint8_t j = 0; j < 3; j++) {
488 init_butterworth_2_low_pass(&filter[j], tau, sample_time, init[j]);
489 }
490 return true;
491}
492
493#if IMU_INTEGRATION
496#include <stdio.h>
497
506static void imu_rate_est_info(const char *name, uint8_t id, float rate, float ref, bool rejected)
507{
508 char msg[100];
509 int len;
510 if(rejected) {
511 len = snprintf(msg, sizeof(msg), "IMU %s %u: measured rate %.2f Hz rejected, too far from %.2f Hz (check sample_freq)",
512 name, id, rate, ref);
513 } else {
514 len = snprintf(msg, sizeof(msg), "IMU %s %u: measured rate %.2f Hz (ref %.2f Hz)", name, id, rate, ref);
515 }
516 if(len <= 0) {
517 return;
518 }
519 len = Min(len, (int)sizeof(msg) - 1);
520 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, len, msg);
521#if FLIGHTRECORDER_SDLOG
522 pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, len, msg);
523#endif
524}
525
543static float imu_rate_est_update(struct imu_rate_est_t *est, const char *name, uint8_t id, float ref,
544 uint32_t stamp, uint32_t last_stamp, uint8_t samples)
545{
546 if(!isfinite(ref) || !(ref > 0.f)) {
547 return ref;
548 }
549
550 // No previous timestamp or the timer overflowed
551 if(last_stamp == 0 || stamp <= last_stamp) {
552 est->start = stamp;
553 est->samples = 0;
554 return 0.f;
555 }
556
557 const float bound = IMU_RATE_EST_BOUND * ref;
558 const float max_interval = samples * (1e6f / ref) * (1.f + IMU_RATE_EST_BOUND) + IMU_RATE_EST_JITTER;
559 if((float)(stamp - last_stamp) > max_interval) {
560 // Start a new window from this callback (its samples are from before the timestamp)
561 est->start = stamp;
562 est->samples = 0;
563 } else {
564 est->samples += samples;
565 const uint32_t window = stamp - est->start;
566 if(window >= IMU_RATE_EST_WINDOW) {
567 const float window_rate = (float)est->samples * 1e6f / (float)window;
568 if(fabsf(window_rate - ref) <= bound) {
569 est->rate = (est->rate > 0.f) ? est->rate + IMU_RATE_EST_ALPHA * (window_rate - est->rate) : window_rate;
571 imu_rate_est_info(name, id, est->rate, ref, false);
572 }
573 } else if(!est->rejected) {
574 est->rejected = true;
575 imu_rate_est_info(name, id, window_rate, ref, true);
576 }
577 est->start = stamp;
578 est->samples = 0;
579 }
580 }
581
582 return (est->rate > 0.f && fabsf(est->rate - ref) <= bound) ? est->rate : ref;
583}
584
596static inline void imu_integrate_sample(struct FloatVect3 *alpha, struct FloatVect3 *beta, struct FloatVect3 *last_sample,
597 struct FloatVect3 *last_delta, const struct FloatVect3 *sample, float dt)
598{
599 struct FloatVect3 delta;
600 VECT3_SUM(delta, *last_sample, *sample);
601 VECT3_SMUL(delta, delta, 0.5f * dt);
602 if(beta != NULL) {
603 struct FloatVect3 coning_arm, coning;
604 VECT3_SUM_SCALED(coning_arm, *alpha, *last_delta, 1.f / 6.f);
605 VECT3_CROSS_PRODUCT(coning, coning_arm, delta);
606 VECT3_ADD_SCALED(*beta, coning, 0.5f);
607 *last_delta = delta;
608 }
609 VECT3_ADD(*alpha, delta);
610 *last_sample = *sample;
611}
612#endif
613
614/*
615 * Preflight check: warn for accelerometers and magnetometers that send data without a calibration
616 * in the airframe (IMU_ACCEL_CALIB, IMU_MAG_CALIB), the estimators (e.g. ins_ekf2) need it.
617 * Not for the simulated sensors of NPS and HITL (IMU_NPS_ID), which are ideal.
618 */
619#if PREFLIGHT_CHECKS
621
622static struct preflight_check_t imu_pfc;
623
624static void imu_preflight(struct preflight_result_t *result)
625{
626 uint16_t fail_cnt = result->fail_cnt;
627 uint16_t warning_cnt = result->warning_cnt;
628 for (uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
629 // only the real sensors that sent data (their ABI id is then set)
630 if (imu.accels[i].abi_id != ABI_DISABLE && imu.accels[i].abi_id != ABI_BROADCAST && imu.accels[i].abi_id != IMU_NPS_ID
631 && !(imu.accels[i].calibrated.neutral && imu.accels[i].calibrated.scale_f)) {
632 preflight_warning(result, "IMU accel %d not calibrated", imu.accels[i].abi_id);
633 }
634 if (imu.mags[i].abi_id != ABI_DISABLE && imu.mags[i].abi_id != ABI_BROADCAST && imu.mags[i].abi_id != IMU_NPS_ID
635 && !(imu.mags[i].calibrated.neutral && imu.mags[i].calibrated.scale_f)) {
636 preflight_warning(result, "IMU mag %d not calibrated", imu.mags[i].abi_id);
637 }
638 }
639 if (result->fail_cnt == fail_cnt && result->warning_cnt == warning_cnt) {
640 preflight_success(result, "IMU calibrated");
641 }
642}
643#endif
644
645void imu_init(void)
646{
647 // Set the Body to IMU rotation
649 orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
650 struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
651
652
653 // Set the calibrated sensors
654 struct Int32RMat body_to_sensor;
655 const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
656 const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
657 const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
658 const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
659 const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
660 const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
661
662 // Initialize the sensors to default values
663 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
664 /* Copy gyro calibration if needed */
665 if(i >= gyro_calib_len) {
666 imu.gyros[i].abi_id = ABI_DISABLE;
667 imu.gyros[i].calibrated.neutral = false;
668 imu.gyros[i].calibrated.scale = false;
669 imu.gyros[i].calibrated.scale_f = false;
670 imu.gyros[i].calibrated.rotation = false;
671 imu.gyros[i].calibrated.rot_euler = false;
672 imu.gyros[i].calibrated.filter = false;
673 } else {
674 imu.gyros[i] = gyro_calib[i];
675 }
676
677 // Set the default safe values if not calibrated
678 if(!imu.gyros[i].calibrated.neutral) {
679 INT_RATES_ZERO(imu.gyros[i].neutral);
680 }
681
682 if(!imu.gyros[i].calibrated.scale_f) {
683 if(imu.gyros[i].calibrated.scale) {
684 imu.gyros[i].scale_f.p = (float)imu.gyros[i].scale[0].p / (float)imu.gyros[i].scale[1].p;
685 imu.gyros[i].scale_f.q = (float)imu.gyros[i].scale[0].q / (float)imu.gyros[i].scale[1].q;
686 imu.gyros[i].scale_f.r = (float)imu.gyros[i].scale[0].r / (float)imu.gyros[i].scale[1].r;
687 imu.gyros[i].calibrated.scale_f = true;
688 } else {
690 }
691 }
692
693 if(!imu.gyros[i].calibrated.rotation) {
694 if (imu.gyros[i].calibrated.rot_euler) {
695 struct Int32Eulers eulers_i;
696 EULERS_BFP_OF_REAL(eulers_i, imu.gyros[i].body_to_sensor_f);
697 int32_rmat_of_eulers(&imu.gyros[i].body_to_sensor, &eulers_i);
698 imu.gyros[i].calibrated.rotation = true;
699 } else {
700 int32_rmat_identity(&imu.gyros[i].body_to_sensor);
701 }
702 }
703 imu.gyros[i].last_stamp = 0;
704 FLOAT_VECT3_ZERO(imu.gyros[i].last_sample);
705 FLOAT_VECT3_ZERO(imu.gyros[i].last_delta_alpha);
706 imu.gyros[i].rate_est = (struct imu_rate_est_t) {0};
707 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
708 RMAT_COPY(imu.gyros[i].body_to_sensor, body_to_sensor);
709
710 /* Copy accel calibration if needed */
711 if(i >= accel_calib_len) {
712 imu.accels[i].abi_id = ABI_DISABLE;
713 imu.accels[i].calibrated.neutral = false;
714 imu.accels[i].calibrated.scale = false;
715 imu.accels[i].calibrated.scale_f = false;
716 imu.accels[i].calibrated.rotation = false;
717 imu.accels[i].calibrated.rot_euler = false;
718 imu.accels[i].calibrated.filter = false;
719 } else {
720 imu.accels[i] = accel_calib[i];
721 }
722
723 // Set the default safe values if not calibrated
724 if(!imu.accels[i].calibrated.neutral) {
725 INT_VECT3_ZERO(imu.accels[i].neutral);
726 }
727
728 if(!imu.accels[i].calibrated.scale_f) {
729 if(imu.accels[i].calibrated.scale) {
730 imu.accels[i].scale_f.x = (float)imu.accels[i].scale[0].x / (float)imu.accels[i].scale[1].x;
731 imu.accels[i].scale_f.y = (float)imu.accels[i].scale[0].y / (float)imu.accels[i].scale[1].y;
732 imu.accels[i].scale_f.z = (float)imu.accels[i].scale[0].z / (float)imu.accels[i].scale[1].z;
733 imu.accels[i].calibrated.scale_f = true;
734 } else {
736 }
737 }
738
739 if(!imu.accels[i].calibrated.rotation) {
740 if (imu.accels[i].calibrated.rot_euler) {
741 struct Int32Eulers eulers_i;
742 EULERS_BFP_OF_REAL(eulers_i, imu.accels[i].body_to_sensor_f);
743 int32_rmat_of_eulers(&imu.accels[i].body_to_sensor, &eulers_i);
744 imu.accels[i].calibrated.rotation = true;
745 } else {
746 int32_rmat_identity(&imu.accels[i].body_to_sensor);
747 }
748 }
749 imu.accels[i].last_stamp = 0;
750 FLOAT_VECT3_ZERO(imu.accels[i].last_sample);
751 imu.accels[i].rate_est = (struct imu_rate_est_t) {0};
752 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
753 RMAT_COPY(imu.accels[i].body_to_sensor, body_to_sensor);
754
755 /* Copy mag calibrated if needed */
756 if(i >= mag_calib_len) {
757 imu.mags[i].abi_id = ABI_DISABLE;
758 imu.mags[i].calibrated.neutral = false;
759 imu.mags[i].calibrated.scale = false;
760 imu.mags[i].calibrated.scale_f = false;
761 imu.mags[i].calibrated.rotation = false;
762 imu.mags[i].calibrated.rot_euler = false;
763 imu.mags[i].calibrated.current = false;
764 imu.mags[i].strength = 0;
765 } else {
766 imu.mags[i] = mag_calib[i];
767 }
768
769 // Set the default safe values if not calibrated
770 if(!imu.mags[i].calibrated.neutral) {
771 INT_VECT3_ZERO(imu.mags[i].neutral);
772 }
773
774 if(!imu.mags[i].calibrated.scale_f) {
775 if(imu.mags[i].calibrated.scale) {
776 imu.mags[i].scale_f.x = (float)imu.mags[i].scale[0].x / (float)imu.mags[i].scale[1].x;
777 imu.mags[i].scale_f.y = (float)imu.mags[i].scale[0].y / (float)imu.mags[i].scale[1].y;
778 imu.mags[i].scale_f.z = (float)imu.mags[i].scale[0].z / (float)imu.mags[i].scale[1].z;
779 imu.mags[i].calibrated.scale_f = true;
780 }else {
782 }
783 }
784 if(!imu.mags[i].calibrated.rotation) {
785 if (imu.mags[i].calibrated.rot_euler) {
786 struct Int32Eulers eulers_i;
787 EULERS_BFP_OF_REAL(eulers_i, imu.mags[i].body_to_sensor_f);
788 int32_rmat_of_eulers(&imu.mags[i].body_to_sensor, &eulers_i);
789 imu.mags[i].calibrated.rotation = true;
790 } else {
791 int32_rmat_identity(&imu.mags[i].body_to_sensor);
792 }
793 }
794 if(!imu.mags[i].calibrated.current) {
795 FLOAT_VECT3_ZERO(imu.mags[i].current_scale);
796 }
797 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
798 RMAT_COPY(imu.mags[i].body_to_sensor, body_to_sensor);
799 }
800
801 // Bind to raw measurements
802 AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
803 AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
804 AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
805
806#if PERIODIC_TELEMETRY
808 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
811 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
814 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
816 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
817#endif // DOWNLINK
818
819#if USE_SHELL
820 shell_add_entry("imu", cmd_imu);
821#endif
822
823 // Set defaults
824 imu.gyro_abi_send_id = IMU_GYRO_ABI_SEND_ID;
825 imu.accel_abi_send_id = IMU_ACCEL_ABI_SEND_ID;
826 imu.mag_abi_send_id = IMU_MAG_ABI_SEND_ID;
827 imu.initialized = true;
828
829#if PREFLIGHT_CHECKS
830 preflight_check_register(&imu_pfc, imu_preflight);
831#endif
832}
833
842void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
843{
844 // Find the correct gyro
845 struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
846 if(gyro == NULL)
847 return;
848
849 // Copy the defaults
850 if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
851 struct Int32RMat body_to_sensor;
852 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
853 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
854 RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
855 }
856 if(neutral != NULL && !gyro->calibrated.neutral)
857 RATES_COPY(gyro->neutral, *neutral);
858 if(scale_f != NULL && !gyro->calibrated.scale_f) {
859 RATES_ASSIGN(gyro->scale_f, IMU_GYRO_P_SIGN*scale_f->p, IMU_GYRO_Q_SIGN*scale_f->q, IMU_GYRO_R_SIGN*scale_f->r);
860 }
861}
862
871void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
872{
873 // Find the correct accel
874 struct imu_accel_t *accel = imu_get_accel(abi_id, true);
875 if(accel == NULL)
876 return;
877
878 // Copy the defaults
879 if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
880 struct Int32RMat body_to_sensor;
881 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
882 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
883 RMAT_COPY(accel->body_to_sensor, body_to_sensor);
884 }
885 if(neutral != NULL && !accel->calibrated.neutral)
886 VECT3_COPY(accel->neutral, *neutral);
887 if(scale_f != NULL && !accel->calibrated.scale_f) {
888 VECT3_ASSIGN(accel->scale_f, IMU_ACCEL_X_SIGN*scale_f->x, IMU_ACCEL_Y_SIGN*scale_f->y, IMU_ACCEL_Z_SIGN*scale_f->z);
889 }
890}
891
900void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
901{
902 // Find the correct mag
903 struct imu_mag_t *mag = imu_get_mag(abi_id, true);
904 if(mag == NULL)
905 return;
906
907 // Copy the defaults
908 if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
909 struct Int32RMat body_to_sensor;
910 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
911 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
912 RMAT_COPY(mag->body_to_sensor, body_to_sensor);
913 }
914 if(neutral != NULL && !mag->calibrated.neutral)
915 VECT3_COPY(mag->neutral, *neutral);
916 if(scale_f != NULL && !mag->calibrated.scale_f) {
917 VECT3_ASSIGN(mag->scale_f, IMU_MAG_X_SIGN*scale_f->x, IMU_MAG_Y_SIGN*scale_f->y, IMU_MAG_Z_SIGN*scale_f->z);
918 }
919}
920
921static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
922{
923 // Find the correct gyro
924 struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
925 if(gyro == NULL || samples < 1)
926 return;
927
928 // Reference rate for the filter and the measured integration rate:
929 // the calibrated sample frequency (e.g. measured from a log) instead of the nominal one of the driver
930 if(gyro->sample_freq > 0.f) {
931 rate = gyro->sample_freq;
932 }
933
934 // Initialize the filter on the first sample, it is disabled without a valid cutoff and sample frequency
935 if(gyro->calibrated.filter && gyro->last_stamp == 0) {
936 const float init[3] = {data[0].p, data[0].q, data[0].r};
937 gyro->calibrated.filter = imu_filter_init(gyro->filter, gyro->filter_freq, rate, init);
938 }
939
940 // Filter the gyro (data points to the filtered samples after the block, only allocate them if needed)
941 struct Int32Rates data_filtered[gyro->calibrated.filter ? samples : 1];
942 if(gyro->calibrated.filter) {
943 for(uint8_t i = 0; i < samples; i++) {
944 data_filtered[i].p = update_butterworth_2_low_pass(&gyro->filter[0], data[i].p);
945 data_filtered[i].q = update_butterworth_2_low_pass(&gyro->filter[1], data[i].q);
946 data_filtered[i].r = update_butterworth_2_low_pass(&gyro->filter[2], data[i].r);
947 }
948 data = data_filtered;
949 }
950
951 // Copy last sample as unscaled
952 RATES_COPY(gyro->unscaled, data[samples-1]);
953
954 // Scale the gyro
955 struct Int32Rates scaled, scaled_rot;
956 scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale_f.p;
957 scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale_f.q;
958 scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale_f.r;
959
960 // Rotate the sensor
961 int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
962
963#if IMU_INTEGRATION
964 // Only integrate if we have gotten a previous measurement and didn't overflow the timer (rate 0)
965 // and the rate is valid (positive and finite, NaN if unknown)
966 // The measured sample rate is used if it is within IMU_RATE_EST_BOUND of the calibrated or driver rate
967 const float int_rate = imu_rate_est_update(&gyro->rate_est, "gyro", sender_id, rate, stamp, gyro->last_stamp, samples);
968 const bool integrate = isfinite(int_rate) && int_rate > 0.f;
969
970 // Integrate in the sensor frame (coning is independent of a fixed rotation), rotate to body frame at the end
971 struct FloatVect3 alpha = {0.f, 0.f, 0.f}, beta = {0.f, 0.f, 0.f};
972 const float sample_dt = integrate ? 1.f / int_rate : 0.f;
973 for(uint8_t i = 0; i < samples; i++) {
974 struct FloatVect3 f_sample;
975 f_sample.x = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale_f.p);
976 f_sample.y = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale_f.q);
977 f_sample.z = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale_f.r);
978
979#if IMU_LOG_HIGHSPEED
980 // Scaled values in the sensor frame (not rotated to body frame), unlike the IMU_GYRO telemetry
981 // The last sample is logged below
982 if(i < samples - 1) {
983 pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
984 }
985#endif
986
987 if(integrate) {
988 imu_integrate_sample(&alpha, &beta, &gyro->last_sample, &gyro->last_delta_alpha, &f_sample, sample_dt);
989 } else {
990 // Restart the integration and the coning correction from this sample
991 gyro->last_sample = f_sample;
993 }
994 }
995
996 if(integrate) {
997 // Coning corrected delta angle rotated to the body frame
998 struct FloatRMat body_to_sensor;
999 struct FloatRates integrated, integrated_sensor;
1000 RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
1001 RATES_ASSIGN(integrated_sensor, alpha.x + beta.x, alpha.y + beta.y, alpha.z + beta.z);
1002 float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
1003
1004 // Send the integrated values
1005 // Only if dt (rounded to us) fits in the uint16_t of the ABI message (up to 65.5ms)
1006 float dt = (1e6f / int_rate) * samples + 0.5f;
1007 if(dt < UINT16_MAX + 1.f) {
1008 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, (uint16_t)dt);
1009 }
1010 }
1011#else
1012 (void)rate; // Surpress compile warning not used
1013#endif
1014
1015#if IMU_LOG_HIGHSPEED
1016 // Scaled values in the sensor frame (not rotated to body frame), unlike the IMU_GYRO telemetry
1017 struct FloatRates f_sample;
1018 RATES_FLOAT_OF_BFP(f_sample, scaled);
1019 pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r);
1020#endif
1021
1022 // Copy and send
1023 gyro->temperature = temp;
1024 RATES_COPY(gyro->scaled, scaled_rot);
1025 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
1026 gyro->last_stamp = stamp;
1027}
1028
1029static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
1030{
1031 // Find the correct accel
1032 struct imu_accel_t *accel = imu_get_accel(sender_id, true);
1033 if(accel == NULL || samples < 1)
1034 return;
1035
1036 // Reference rate for the filter and the measured integration rate:
1037 // the calibrated sample frequency (e.g. measured from a log) instead of the nominal one of the driver
1038 if(accel->sample_freq > 0.f) {
1039 rate = accel->sample_freq;
1040 }
1041
1042 // Initialize the filter on the first sample, it is disabled without a valid cutoff and sample frequency
1043 if(accel->calibrated.filter && accel->last_stamp == 0) {
1044 const float init[3] = {data[0].x, data[0].y, data[0].z};
1045 accel->calibrated.filter = imu_filter_init(accel->filter, accel->filter_freq, rate, init);
1046 }
1047
1048 // Filter the accel (data points to the filtered samples after the block, only allocate them if needed)
1049 struct Int32Vect3 data_filtered[accel->calibrated.filter ? samples : 1];
1050 if(accel->calibrated.filter) {
1051 for(uint8_t i = 0; i < samples; i++) {
1052 data_filtered[i].x = update_butterworth_2_low_pass(&accel->filter[0], data[i].x);
1053 data_filtered[i].y = update_butterworth_2_low_pass(&accel->filter[1], data[i].y);
1054 data_filtered[i].z = update_butterworth_2_low_pass(&accel->filter[2], data[i].z);
1055 }
1056 data = data_filtered;
1057 }
1058
1059 // Copy last sample as unscaled
1060 VECT3_COPY(accel->unscaled, data[samples-1]);
1061
1062 // Scale the accel
1063 struct Int32Vect3 scaled, scaled_rot;
1064 scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale_f.x;
1065 scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale_f.y;
1066 scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale_f.z;
1067
1068
1069 // Rotate the sensor
1070 int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
1071
1072#if IMU_INTEGRATION
1073 // Only integrate if we have gotten a previous measurement and didn't overflow the timer (rate 0)
1074 // and the rate is valid (positive and finite, NaN if unknown)
1075 // The measured sample rate is used if it is within IMU_RATE_EST_BOUND of the calibrated or driver rate
1076 const float int_rate = imu_rate_est_update(&accel->rate_est, "accel", sender_id, rate, stamp, accel->last_stamp, samples);
1077 const bool integrate = isfinite(int_rate) && int_rate > 0.f;
1078
1079 // Integrate in the sensor frame, rotate to body frame at the end
1080 struct FloatVect3 alpha = {0.f, 0.f, 0.f};
1081 const float sample_dt = integrate ? 1.f / int_rate : 0.f;
1082 for(uint8_t i = 0; i < samples; i++) {
1083 struct FloatVect3 f_sample;
1084 f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale_f.x);
1085 f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale_f.y);
1086 f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale_f.z);
1087
1088#if IMU_LOG_HIGHSPEED
1089 // Scaled values in the sensor frame (not rotated to body frame), unlike the IMU_ACCEL telemetry
1090 // The last sample is logged below
1091 if(i < samples - 1) {
1092 pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
1093 }
1094#endif
1095
1096 if(integrate) {
1097 imu_integrate_sample(&alpha, NULL, &accel->last_sample, NULL, &f_sample, sample_dt);
1098 } else {
1099 accel->last_sample = f_sample;
1100 }
1101 }
1102
1103 if(integrate) {
1104 // Delta velocity rotated to the body frame
1105 struct FloatRMat body_to_sensor;
1106 struct FloatVect3 integrated;
1107 RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
1108 float_rmat_transp_vmult(&integrated, &body_to_sensor, &alpha);
1109
1110 // Send the integrated values
1111 // Only if dt (rounded to us) fits in the uint16_t of the ABI message (up to 65.5ms)
1112 float dt = (1e6f / int_rate) * samples + 0.5f;
1113 if(dt < UINT16_MAX + 1.f) {
1114 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, (uint16_t)dt);
1115 }
1116 }
1117#else
1118 (void)rate; // Surpress compile warning not used
1119#endif
1120
1121#if IMU_LOG_HIGHSPEED
1122 // Scaled values in the sensor frame (not rotated to body frame), unlike the IMU_ACCEL telemetry
1123 struct FloatVect3 f_sample;
1124 ACCELS_FLOAT_OF_BFP(f_sample, scaled);
1125 pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
1126#endif
1127
1128 // Copy and send
1129 accel->temperature = temp;
1130 VECT3_COPY(accel->scaled, scaled_rot);
1131 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
1132 accel->last_stamp = stamp;
1133}
1134
1135static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
1136{
1137 // Find the correct mag
1138 struct imu_mag_t *mag = imu_get_mag(sender_id, true);
1139 if(mag == NULL)
1140 return;
1141
1142 // Calculate current compensation
1143 struct Int32Vect3 mag_correction;
1144 mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
1145 mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
1146 mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
1147
1148 // Copy last sample as unscaled
1149 VECT3_COPY(mag->unscaled, *data);
1150
1151 // Scale the mag
1152 struct Int32Vect3 scaled;
1153 scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale_f.x;
1154 scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale_f.y;
1155 scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale_f.z;
1156
1157
1158 // Rotate the sensor
1159 int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
1160 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
1161}
1162
1170struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
1171 // Find the correct gyro or create index
1172 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
1173 struct imu_gyro_t *gyro = NULL;
1174 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
1175 if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
1176 gyro = &imu.gyros[i];
1177 gyro->abi_id = sender_id;
1178 break;
1179 } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
1180 gyro = &imu.gyros[i];
1181 }
1182 }
1183 return gyro;
1184}
1185
1193struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
1194 // Find the correct accel
1195 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
1196 struct imu_accel_t *accel = NULL;
1197 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
1198 if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
1199 accel = &imu.accels[i];
1200 accel->abi_id = sender_id;
1201 break;
1202 } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
1203 accel = &imu.accels[i];
1204 }
1205 }
1206
1207 return accel;
1208}
1209
1217struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
1218 // Find the correct mag
1219 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
1220 struct imu_mag_t *mag = NULL;
1221 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
1222 if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
1223 mag = &imu.mags[i];
1224 mag->abi_id = sender_id;
1225 break;
1226 } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
1227 mag = &imu.mags[i];
1228 }
1229 }
1230
1231 return mag;
1232}
1233
1239static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
1240{
1241 struct Int32RMat new_body_to_imu, diff_body_to_imu;
1242 struct Int32Eulers body_to_imu_eulers_i;
1243 // Convert to RMat
1244 struct Int32RMat *old_body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
1245 EULERS_BFP_OF_REAL(body_to_imu_eulers_i, *body_to_imu_eulers);
1246 int32_rmat_of_eulers(&new_body_to_imu, &body_to_imu_eulers_i);
1247
1248 // Calculate the difference between old and new
1249 int32_rmat_comp_inv(&diff_body_to_imu, &new_body_to_imu, old_body_to_imu);
1250
1251 // Apply the difference to all sensors
1252 struct Int32RMat old_rmat;
1253 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
1254 old_rmat = imu.gyros[i].body_to_sensor;
1255 int32_rmat_comp(&imu.gyros[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
1256
1257 old_rmat = imu.accels[i].body_to_sensor;
1258 int32_rmat_comp(&imu.accels[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
1259
1260 old_rmat = imu.mags[i].body_to_sensor;
1261 int32_rmat_comp(&imu.mags[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
1262 }
1263
1264 // Set the current body to imu
1265 orientationSetEulers_f(&imu.body_to_imu, body_to_imu_eulers);
1266}
1267
1268void imu_SetBodyToImuPhi(float phi)
1269{
1270 struct FloatEulers body_to_imu_eulers;
1271 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1272 body_to_imu_eulers.phi = phi;
1273 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1274}
1275
1277{
1278 struct FloatEulers body_to_imu_eulers;
1279 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1280 body_to_imu_eulers.theta = theta;
1281 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1282}
1283
1285{
1286 struct FloatEulers body_to_imu_eulers;
1287 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1288 body_to_imu_eulers.psi = psi;
1289 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1290}
1291
1293{
1294 imu.b2i_set_current = set;
1295
1296 if (imu.b2i_set_current) {
1297 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1298 struct FloatEulers body_to_imu_eulers;
1299 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1300 if (stateIsAttitudeValid()) {
1301 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1302 body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
1303 body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
1304 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1305 } else {
1306 // indicate that we couldn't set to current roll/pitch
1307 imu.b2i_set_current = false;
1308 }
1309 } else {
1310 // reset to BODY_TO_IMU as defined in airframe file
1312 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1313 }
1314}
1315
Main include for ABI (AirBorneInterface).
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition abi_common.h:86
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define IMU_NPS_ID
struct FloatVect3 accel_float
struct FloatVect3 mag_float
if(GpsFixValid() &&e_identification_started)
static struct uart_periph * dev
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
#define Min(x, y)
Definition esc_dshot.c:110
void chprintf(BaseSequentialStream *lchp, const char *fmt,...)
Definition printf.c:395
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a, const struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
#define FLOAT_VECT3_ZERO(_v)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
euler angles
rotation matrix
angular rates
#define VECT3_SUM(_c, _a, _b)
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
#define VECT3_SMUL(_vo, _vi, _s)
#define RMAT_COPY(_o, _i)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_COPY(_a, _b)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define VECT3_ADD(_a, _b)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
#define MAT33_ELMT(_m, _row, _col)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define VECT3_ADD_SCALED(_a, _b, _s)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
#define INT_RATES_ZERO(_e)
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
#define RATE_FLOAT_OF_BFP(_ai)
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
#define INT_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
#define ACCEL_FLOAT_OF_BFP(_ai)
euler angles
rotation matrix
angular rates
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition state.h:1243
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
#define IMU_RATE_EST_BOUND
Maximum relative deviation of the measured sample rate from the calibrated sample_freq or driver rate...
Definition imu.c:54
#define IMU_MAG_ABI_SEND_ID
Which mag measurements to send over telemetry/logging.
Definition imu.c:201
#define IMU_GYRO_ABI_SEND_ID
Which gyro measurements to send over telemetry/logging.
Definition imu.c:189
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
Definition imu.c:173
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
Definition imu.c:921
#define IMU_MAG_Y_SIGN
Definition imu.c:149
#define IMU_BODY_TO_IMU_THETA
Definition imu.c:180
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:302
#define IMU_GYRO_P_SIGN
By default gyro signs are positive for single IMU with old format or defaults.
Definition imu.c:72
#define IMU_RATE_EST_REPORT
Number of accepted windows after which the measured sample rate is reported once (INFO_MSG).
Definition imu.c:64
#define IMU_RATE_EST_WINDOW
Window to measure the sample rate for the integration (us).
Definition imu.c:44
#define IMU_ACCEL_Y_SIGN
Definition imu.c:114
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:842
struct imu_gyro_t * imu_get_gyro(uint8_t sender_id, bool create)
Find or create the gyro in the imu structure.
Definition imu.c:1170
static abi_event imu_accel_raw_ev
Definition imu.c:466
static abi_event imu_mag_raw_ev
Definition imu.c:466
void imu_SetBodyToImuCurrent(bool set)
Definition imu.c:1292
struct imu_accel_t * imu_get_accel(uint8_t sender_id, bool create)
Find or create the accel in the imu structure.
Definition imu.c:1193
void imu_SetBodyToImuTheta(float theta)
Definition imu.c:1276
#define IMU_ACCEL_X_SIGN
By default accel signs are positive for single IMU with old format and defaults.
Definition imu.c:111
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians).
Definition imu.c:179
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
Definition imu.c:1029
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:871
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
Definition imu.c:138
#define IMU_LOG_HIGHSPEED_DEVICE
By default log highspeed on the flightrecorder.
Definition imu.c:207
static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
Set the body to IMU rotation in eulers This will update all the sensor values.
Definition imu.c:1239
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:316
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:346
struct Imu imu
global IMU state
Definition imu.c:465
#define IMU_BODY_TO_IMU_PSI
Definition imu.c:181
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
Definition imu.c:99
#define IMU_MAG_Z_SIGN
Definition imu.c:152
void imu_init(void)
External functions.
Definition imu.c:645
void imu_SetBodyToImuPsi(float psi)
Definition imu.c:1284
static abi_event imu_gyro_raw_ev
Definition imu.c:466
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:214
#define IMU_RATE_EST_JITTER
Timestamp jitter (us) allowed before a callback interval is seen as a gap with lost samples.
Definition imu.c:59
#define IMU_RATE_EST_ALPHA
Low-pass factor of the measured sample rate per window (0.1: averaged over ~10 windows).
Definition imu.c:49
#define IMU_GYRO_R_SIGN
Definition imu.c:78
struct imu_mag_t * imu_get_mag(uint8_t sender_id, bool create)
Find or create the mag in the imu structure.
Definition imu.c:1217
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:272
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:286
static bool imu_filter_init(Butterworth2LowPass filter[3], float freq, float sample_freq, const float init[3])
Initialize the lowpass filters of a sensor on its first sample.
Definition imu.c:480
void imu_SetBodyToImuPhi(float phi)
Definition imu.c:1268
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:242
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
Definition imu.c:1135
#define IMU_ACCEL_Z_SIGN
Definition imu.c:117
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:228
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:330
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:258
#define IMU_ACCEL_ABI_SEND_ID
Which accel measurements to send over telemetry/logging.
Definition imu.c:195
void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition imu.c:900
#define IMU_GYRO_Q_SIGN
Definition imu.c:75
Inertial Measurement Unit interface.
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:66
struct Int32Rates scaled
Last scaled values in body frame.
Definition imu.h:68
struct Int32RMat body_to_sensor
Rotation from body to sensor frame, set as imu to sensor in the airframe calibration and combined wit...
Definition imu.h:94
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:105
float sample_freq
Calibrated sample frequency (Hz) for the filter, and reference of the measured integration rate (the ...
Definition imu.h:77
struct FloatVect3 current_scale
Current scaling multiplying.
Definition imu.h:111
uint8_t abi_id
ABI sensor ID.
Definition imu.h:65
bool scale
Scale calibrated.
Definition imu.h:47
float temperature
Temperature in degrees celcius.
Definition imu.h:70
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:98
float filter_freq
Lowpass filter cutoff frequency (Hz).
Definition imu.h:96
struct FloatVect3 last_delta_alpha
Last integrated sample delta angle in sensor frame for the coning correction (IMU_INTEGRATION).
Definition imu.h:80
struct imu_rate_est_t rate_est
Measured sample rate for the integration (IMU_INTEGRATION).
Definition imu.h:81
struct FloatRates scale_f
Scaling.
Definition imu.h:73
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:86
float rate
Filtered measured sample rate (Hz), 0 until the first window.
Definition imu.h:59
struct FloatVect3 scale_f
Scaling.
Definition imu.h:110
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:108
bool filter
Enable the lowpass filter (gyro and accel only, ignored for the mag).
Definition imu.h:52
struct FloatVect3 last_sample
Last scaled sample in sensor frame for the integration (IMU_INTEGRATION).
Definition imu.h:79
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:89
float temperature
Temperature in degrees celcius.
Definition imu.h:90
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:71
bool current
Current calibrated.
Definition imu.h:51
struct Int32RMat body_to_sensor
Rotation from body to sensor frame, set as imu to sensor in the airframe calibration and combined wit...
Definition imu.h:112
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:78
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:87
struct FloatVect3 scale_f
Scaling.
Definition imu.h:93
uint32_t samples
Samples received since the window start.
Definition imu.h:58
bool scale_f
Scale calibrated with floating point.
Definition imu.h:48
bool rejected
A window was rejected (out of bounds), only warned once.
Definition imu.h:61
uint16_t windows
Accepted windows, up to IMU_RATE_EST_REPORT when the rate is reported.
Definition imu.h:60
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:88
struct Int32RMat body_to_sensor
Rotation from body to sensor frame, set as imu to sensor in the airframe calibration and combined wit...
Definition imu.h:74
struct Int32Rates unscaled
Last unscaled values in sensor frame.
Definition imu.h:69
uint8_t abi_id
ABI sensor ID.
Definition imu.h:104
float sample_freq
Calibrated sample frequency (Hz) for the filter, and reference of the measured integration rate (the ...
Definition imu.h:97
bool neutral
Neutral values calibrated.
Definition imu.h:46
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:91
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:107
float filter_freq
Lowpass filter cutoff frequency (Hz).
Definition imu.h:76
struct FloatVect3 last_sample
Last scaled sample in sensor frame for the integration (IMU_INTEGRATION).
Definition imu.h:99
#define IMU_MAX_SENSORS
Definition imu.h:42
struct imu_rate_est_t rate_est
Measured sample rate for the integration (IMU_INTEGRATION).
Definition imu.h:100
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:67
uint32_t start
Timestamp of the current window start (us).
Definition imu.h:57
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:106
bool rotation
Rotation calibrated.
Definition imu.h:49
uint8_t abi_id
ABI sensor ID.
Definition imu.h:85
abstract IMU interface providing fixed point interface
Definition imu.h:119
Measured sample rate of a sensor, counted against the timestamps.
Definition imu.h:56
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
static TRICAL_instance_t mag_calib
#define IMU_MAG_X_SIGN
Definition max7456.c:99
window
Definition gaia.py:210
bool init
Definition nav_gls.c:57
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Definition pprzlog_tp.c:29
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_warning(struct preflight_result_t *result, const char *fmt,...)
Register a preflight warning used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Mini printf-like functionality.
void shell_add_entry(char *cmd_name, shell_cmd_t *cmd)
Add dynamic entry.
Definition shell_arch.c:91
BaseSequentialStream shell_stream_t
Definition shell_arch.h:31
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
uint16_t ref[TCOUPLE_NB]
float alpha
Definition textons.c:133
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.