Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
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#if defined(IMU_GYRO_CALIB) && (defined(IMU_GYRO_P_SIGN) || defined(IMU_GYRO_Q_SIGN) || defined(IMU_GYRO_R_SIGN))
44#warning "The IMU_GYRO_?_SIGN's aren't compatible with the IMU_GYRO_CALIB define in the airframe"
45#endif
46#ifndef IMU_GYRO_P_SIGN
47#define IMU_GYRO_P_SIGN 1
48#endif
49#ifndef IMU_GYRO_Q_SIGN
50#define IMU_GYRO_Q_SIGN 1
51#endif
52#ifndef IMU_GYRO_R_SIGN
53#define IMU_GYRO_R_SIGN 1
54#endif
55
57#if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
58#define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
59PRINT_CONFIG_MSG("Using single IMU gyro neutral calibration")
60#endif
61
62#if defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
63#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}}
64PRINT_CONFIG_MSG("Using single IMU gyro sensitivity calibration")
65#endif
66
67#if defined(GYRO_NEUTRAL) && defined(GYRO_SCALE)
68#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=GYRO_NEUTRAL, .scale=GYRO_SCALE}}
69#elif defined(GYRO_NEUTRAL)
70#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=GYRO_NEUTRAL}}
71#elif defined(GYRO_SCALE)
72#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=GYRO_SCALE}}
73#elif !defined(IMU_GYRO_CALIB)
74#define IMU_GYRO_CALIB {}
75#endif
76
77#if defined(IMU_IMU_ACCEL_CALIB)
78#error IMU_IMU_ACCEL_CALIB defined. Rename it "ACCEL_CALIB" if your calibration is in a section with the "IMU_" prefix.
79#endif
80
82#if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN))
83#warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe"
84#endif
85#ifndef IMU_ACCEL_X_SIGN
86#define IMU_ACCEL_X_SIGN 1
87#endif
88#ifndef IMU_ACCEL_Y_SIGN
89#define IMU_ACCEL_Y_SIGN 1
90#endif
91#ifndef IMU_ACCEL_Z_SIGN
92#define IMU_ACCEL_Z_SIGN 1
93#endif
94
96#if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
97#define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
98PRINT_CONFIG_MSG("Using single IMU accel neutral calibration")
99#endif
100
101#if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
102#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}}
103PRINT_CONFIG_MSG("Using single IMU accel sensitivity calibration")
104#endif
105
106#if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
107#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
108#elif defined(ACCEL_NEUTRAL)
109#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
110#elif defined(ACCEL_SCALE)
111#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
112#elif !defined(IMU_ACCEL_CALIB)
113#define IMU_ACCEL_CALIB {}
114#endif
115
117#if defined(IMU_MAG_CALIB) && (defined(IMU_MAG_X_SIGN) || defined(IMU_MAG_Y_SIGN) || defined(IMU_MAG_Z_SIGN))
118#warning "The IMU_MAG_?_SIGN's aren't compatible with the IMU_MAG_CALIB define in the airframe"
119#endif
120#ifndef IMU_MAG_X_SIGN
121#define IMU_MAG_X_SIGN 1
122#endif
123#ifndef IMU_MAG_Y_SIGN
124#define IMU_MAG_Y_SIGN 1
125#endif
126#ifndef IMU_MAG_Z_SIGN
127#define IMU_MAG_Z_SIGN 1
128#endif
129
131#if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
132#define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
133PRINT_CONFIG_MSG("Using single IMU mag neutral calibration")
134#endif
135
136#if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
137#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}}
138PRINT_CONFIG_MSG("Using single IMU mag sensitivity calibration")
139#endif
140
141#if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
142#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
143#elif defined(MAG_NEUTRAL)
144#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
145#elif defined(MAG_SCALE)
146#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
147#elif !defined(IMU_MAG_CALIB)
148#define IMU_MAG_CALIB {}
149#endif
150
151
153#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
154#define IMU_BODY_TO_IMU_PHI 0
155#define IMU_BODY_TO_IMU_THETA 0
156#define IMU_BODY_TO_IMU_PSI 0
157#endif
161
162
163#ifndef IMU_GYRO_ABI_SEND_ID
164#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
165#endif
167
168
169#ifndef IMU_ACCEL_ABI_SEND_ID
170#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
171#endif
173
174
175#ifndef IMU_MAG_ABI_SEND_ID
176#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
177#endif
179
180
181#ifndef IMU_LOG_HIGHSPEED_DEVICE
182#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
183#endif
184
185
186#if PERIODIC_TELEMETRY
188
189static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
190{
192 return;
193 static uint8_t id = 0;
197 return;
198 id++;
199 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
200 id = 0;
201}
202
203static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
204{
206 return;
207 static uint8_t id = 0;
209 &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
211 return;
212 id++;
213 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
214 id = 0;
215}
216
217static void send_accel(struct transport_tx *trans, struct link_device *dev)
218{
220 return;
221 static uint8_t id = 0;
222 struct FloatVect3 accel_float;
227 return;
228 id++;
229 if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
230 id = 0;
231}
232
233static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
234{
236 return;
237 static uint8_t id = 0;
239 &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
241 return;
242 id++;
243 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
244 id = 0;
245}
246
247static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
248{
250 return;
251 static uint8_t id = 0;
253 &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
255 return;
256 id++;
257 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
258 id = 0;
259}
260
261static void send_gyro(struct transport_tx *trans, struct link_device *dev)
262{
264 return;
265 static uint8_t id = 0;
266 struct FloatRates gyro_float;
269 &gyro_float.p, &gyro_float.q, &gyro_float.r);
271 return;
272 id++;
273 if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
274 id = 0;
275}
276
277static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
278{
280 return;
281 static uint8_t id = 0;
283 &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
285 return;
286 id++;
287 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
288 id = 0;
289}
290
291static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
292{
294 return;
295 static uint8_t id = 0;
297 &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
299 return;
300 id++;
301 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
302 id = 0;
303}
304
305static void send_mag(struct transport_tx *trans, struct link_device *dev)
306{
308 return;
309 static uint8_t id = 0;
310 struct FloatVect3 mag_float;
315 return;
316 id++;
317 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
318 id = 0;
319}
320
321static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
322{
324 return;
325 static uint8_t id = 0;
327 &imu.mags[id].abi_id,
328 &imu.mags[id].unscaled.x,
329 &imu.mags[id].unscaled.y,
330 &imu.mags[id].unscaled.z,
333 return;
334 id++;
335 if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
336 id = 0;
337}
338
339#endif /* PERIODIC_TELEMETRY */
340
341#if USE_SHELL
342#include "modules/core/shell.h"
343#include "printf.h"
344#include "string.h"
345
346static void show_calibrated(shell_stream_t *sh, struct imu_calib_t *calibrated) {
347 chprintf(sh, " calibrated: neutral %d, scale %d, rotation %d, current %d, filter %d\r\n",
348 calibrated->neutral, calibrated->scale, calibrated->rotation, calibrated->current, calibrated->filter);
349}
350
351static void show_vect3(shell_stream_t *sh, char* name, struct Int32Vect3 *v) {
352 chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, v->x, v->y, v->z);
353}
354
355static void show_vect3f(shell_stream_t *sh, char* name, struct FloatVect3 *v) {
356 chprintf(sh, " %s: %f, %f, %f\r\n", name, v->x, v->y, v->z);
357}
358
359static void show_rates(shell_stream_t *sh, char* name, struct Int32Rates *r) {
360 chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, r->p, r->q, r->r);
361}
362
363
364static void show_matrix(shell_stream_t *sh, char* name, struct Int32RMat *m) {
365 chprintf(sh, " %s:\r\n", name);
366 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 0, 0), MAT33_ELMT(*m, 0, 1), MAT33_ELMT(*m, 0, 2));
367 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 1, 0), MAT33_ELMT(*m, 1, 1), MAT33_ELMT(*m, 1, 2));
368 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 2, 0), MAT33_ELMT(*m, 2, 1), MAT33_ELMT(*m, 2, 2));
369}
370
371static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[])
372{
373 (void) argv;
374 int i = 0;
375 if (argc > 0) {
376 chprintf(sh, "Usage: imu\r\n");
377 return;
378 }
379 chprintf(sh, "GYRO IMU data\r\n");
380 for (i = 0; i < IMU_MAX_SENSORS; i++) {
381 if (imu.gyros[i].abi_id != 0) {
382 chprintf(sh, " Gyro id: %u, time %lu\r\n", imu.gyros[i].abi_id, imu.gyros[i].last_stamp);
384 show_rates(sh, "neutral", &imu.gyros[i].neutral);
385 show_vect3f(sh, "scale", &imu.gyros[i].scale_f);
386 show_matrix(sh, "body_to_sensor", &imu.gyros[i].body_to_sensor);
387 show_rates(sh, "unscaled", &imu.gyros[i].unscaled);
388 show_rates(sh, "scaled", &imu.gyros[i].scaled);
389 struct FloatRates gf;
391 chprintf(sh, " -> gyro (rad/s): %.3f, %.3f, %.3f\r\n", gf.p, gf.q, gf.r);
392 }
393 }
394 chprintf(sh, "ACCEL IMU data\r\n");
395 for (i = 0; i < IMU_MAX_SENSORS; i++) {
396 if (imu.accels[i].abi_id != 0) {
397 chprintf(sh, " Accel id: %u, time %lu\r\n", imu.accels[i].abi_id, imu.accels[i].last_stamp);
399 show_vect3(sh, "neutral", &imu.accels[i].neutral);
400 show_vect3f(sh, "scale", &imu.accels[i].scale_f);
401 show_matrix(sh, "body_to_sensor", &imu.accels[i].body_to_sensor);
402 show_vect3(sh, "unscaled", &imu.accels[i].unscaled);
403 show_vect3(sh, "scaled", &imu.accels[i].scaled);
404 struct FloatVect3 af;
406 chprintf(sh, " -> accel (m/s2): %.3f, %.3f, %.3f\r\n", af.x, af.y, af.z);
407 }
408 }
409 chprintf(sh, "MAG IMU data\r\n");
410 for (i = 0; i < IMU_MAX_SENSORS; i++) {
411 if (imu.mags[i].abi_id != 0) {
412 chprintf(sh, " Mag id: %u\r\n", imu.mags[i].abi_id);
414 show_vect3(sh, "neutral", &imu.mags[i].neutral);
415 show_vect3f(sh, "scale", &imu.mags[i].scale_f);
416 show_matrix(sh, "body_to_sensor", &imu.mags[i].body_to_sensor);
417 show_vect3(sh, "unscaled", &imu.mags[i].unscaled);
418 show_vect3(sh, "scaled", &imu.mags[i].scaled);
419 struct FloatVect3 mf;
421 chprintf(sh, " -> mag (unit): %.3f, %.3f, %.3f\r\n", mf.x, mf.y, mf.z);
422 }
423 }
424}
425
426#endif
427
428struct Imu imu = {0};
430static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp);
431static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp);
432static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
434
435void imu_init(void)
436{
437 // Set the Body to IMU rotation
440 struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
441
442
443 // Set the calibrated sensors
444 struct Int32RMat body_to_sensor;
445 const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
446 const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
447 const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
448 const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
449 const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
450 const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
451
452 // Initialize the sensors to default values
453 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
454 /* Copy gyro calibration if needed */
455 if(i >= gyro_calib_len) {
457 imu.gyros[i].calibrated.neutral = false;
458 imu.gyros[i].calibrated.scale = false;
459 imu.gyros[i].calibrated.scale_f = false;
460 imu.gyros[i].calibrated.rotation = false;
461 imu.gyros[i].calibrated.filter = false;
462 } else {
463 imu.gyros[i] = gyro_calib[i];
464 }
465
466 // Set the default safe values if not calibrated
467 if(!imu.gyros[i].calibrated.neutral) {
469 }
470
471 if(!imu.gyros[i].calibrated.scale_f) {
472 if(imu.gyros[i].calibrated.scale) {
473 imu.gyros[i].scale_f.p = (float)imu.gyros[i].scale[0].p / (float)imu.gyros[i].scale[1].p;
474 imu.gyros[i].scale_f.q = (float)imu.gyros[i].scale[0].q / (float)imu.gyros[i].scale[1].q;
475 imu.gyros[i].scale_f.r = (float)imu.gyros[i].scale[0].r / (float)imu.gyros[i].scale[1].r;
476 imu.gyros[i].calibrated.scale_f = true;
477 } else {
479 }
480 }
481
482 if(!imu.gyros[i].calibrated.rotation) {
484 }
485 imu.gyros[i].last_stamp = 0;
486 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
488
489 if(imu.gyros[i].calibrated.filter) {
490 float tau = 1.0 / (2.0 * M_PI * imu.gyros[i].filter_freq);
492 for(uint8_t j = 0; j < 3; j++)
494 }
495
496 /* Copy accel calibration if needed */
497 if(i >= accel_calib_len) {
499 imu.accels[i].calibrated.neutral = false;
500 imu.accels[i].calibrated.scale = false;
501 imu.accels[i].calibrated.scale_f = false;
502 imu.accels[i].calibrated.rotation = false;
503 imu.accels[i].calibrated.filter = false;
504 } else {
505 imu.accels[i] = accel_calib[i];
506 }
507
508 // Set the default safe values if not calibrated
509 if(!imu.accels[i].calibrated.neutral) {
511 }
512
513 if(!imu.accels[i].calibrated.scale_f) {
514 if(imu.accels[i].calibrated.scale) {
515 imu.accels[i].scale_f.x = (float)imu.accels[i].scale[0].x / (float)imu.accels[i].scale[1].x;
516 imu.accels[i].scale_f.y = (float)imu.accels[i].scale[0].y / (float)imu.accels[i].scale[1].y;
517 imu.accels[i].scale_f.z = (float)imu.accels[i].scale[0].z / (float)imu.accels[i].scale[1].z;
518 imu.accels[i].calibrated.scale_f = true;
519 } else {
521 }
522 }
523
524 if(!imu.accels[i].calibrated.rotation) {
526 }
527 imu.accels[i].last_stamp = 0;
528 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
530
531 if(imu.accels[i].calibrated.filter) {
532 float tau = 1.0 / (2.0 * M_PI * imu.accels[i].filter_freq);
534 for(uint8_t j = 0; j < 3; j++)
536 }
537
538 /* Copy mag calibrated if needed */
539 if(i >= mag_calib_len) {
541 imu.mags[i].calibrated.neutral = false;
542 imu.mags[i].calibrated.scale = false;
543 imu.mags[i].calibrated.scale_f = false;
544 imu.mags[i].calibrated.rotation = false;
545 imu.mags[i].calibrated.current = false;
546 } else {
547 imu.mags[i] = mag_calib[i];
548 }
549
550 // Set the default safe values if not calibrated
551 if(!imu.mags[i].calibrated.neutral) {
553 }
554
555 if(!imu.mags[i].calibrated.scale_f) {
556 if(imu.mags[i].calibrated.scale) {
557 imu.mags[i].scale_f.x = (float)imu.mags[i].scale[0].x / (float)imu.mags[i].scale[1].x;
558 imu.mags[i].scale_f.y = (float)imu.mags[i].scale[0].y / (float)imu.mags[i].scale[1].y;
559 imu.mags[i].scale_f.z = (float)imu.mags[i].scale[0].z / (float)imu.mags[i].scale[1].z;
560 imu.mags[i].calibrated.scale_f = true;
561 }else {
563 }
564 }
565 if(!imu.mags[i].calibrated.rotation) {
567 }
568 if(!imu.mags[i].calibrated.current) {
570 }
571 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
573 }
574
575 // Bind to raw measurements
579
580#if PERIODIC_TELEMETRY
591#endif // DOWNLINK
592
593#if USE_SHELL
594 shell_add_entry("imu", cmd_imu);
595#endif
596
597 // Set defaults
601 imu.initialized = true;
602}
603
613{
614 // Find the correct gyro
615 struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
616 if(gyro == NULL)
617 return;
618
619 // Copy the defaults
620 if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
621 struct Int32RMat body_to_sensor;
622 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
623 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
624 RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
625 }
626 if(neutral != NULL && !gyro->calibrated.neutral)
627 RATES_COPY(gyro->neutral, *neutral);
628 if(scale_f != NULL && !gyro->calibrated.scale_f) {
629 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);
630 }
631}
632
641void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
642{
643 // Find the correct accel
644 struct imu_accel_t *accel = imu_get_accel(abi_id, true);
645 if(accel == NULL)
646 return;
647
648 // Copy the defaults
649 if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
650 struct Int32RMat body_to_sensor;
651 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
652 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
653 RMAT_COPY(accel->body_to_sensor, body_to_sensor);
654 }
655 if(neutral != NULL && !accel->calibrated.neutral)
656 VECT3_COPY(accel->neutral, *neutral);
657 if(scale_f != NULL && !accel->calibrated.scale_f) {
658 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);
659 }
660}
661
670void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
671{
672 // Find the correct mag
673 struct imu_mag_t *mag = imu_get_mag(abi_id, true);
674 if(mag == NULL)
675 return;
676
677 // Copy the defaults
678 if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
679 struct Int32RMat body_to_sensor;
680 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
681 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
682 RMAT_COPY(mag->body_to_sensor, body_to_sensor);
683 }
684 if(neutral != NULL && !mag->calibrated.neutral)
685 VECT3_COPY(mag->neutral, *neutral);
686 if(scale_f != NULL && !mag->calibrated.scale_f) {
687 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);
688 }
689}
690
691static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
692{
693 // Find the correct gyro
694 struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
695 if(gyro == NULL || samples < 1)
696 return;
697
698 // Filter the gyro
700 if(gyro->calibrated.filter) {
701 for(uint8_t i = 0; i < samples; i++) {
702 data_filtered[i].p = update_butterworth_2_low_pass(&gyro->filter[0], data[i].p);
703 data_filtered[i].q = update_butterworth_2_low_pass(&gyro->filter[1], data[i].q);
704 data_filtered[i].r = update_butterworth_2_low_pass(&gyro->filter[2], data[i].r);
705 }
706 data = data_filtered;
707 }
708
709 // Copy last sample as unscaled
710 RATES_COPY(gyro->unscaled, data[samples-1]);
711
712 // Scale the gyro
713 struct Int32Rates scaled, scaled_rot;
714 scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale_f.p;
715 scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale_f.q;
716 scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale_f.r;
717
718 // Rotate the sensor
720
721#if IMU_INTEGRATION
722 // Only integrate if we have gotten a previous measurement and didn't overflow the timer
723 if(!isnan(rate) && gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
724 struct FloatRates integrated;
725
726 // Trapezoidal integration (TODO: coning correction)
727 integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
728 integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
729 integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
730
731 // Only perform multiple integrations in sensor frame if needed
732 if(samples > 1) {
734 struct FloatRMat body_to_sensor;
735 // Rotate back to sensor frame
736 RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
737 float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
738
739 // Add all the other samples
740 for(uint8_t i = 0; i < samples-1; i++) {
741 struct FloatRates f_sample;
742 f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale_f.p);
743 f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale_f.q);
744 f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale_f.r);
745
746
747#if IMU_LOG_HIGHSPEED
749#endif
750
754 }
755
756 // Rotate to body frame
757 float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
758 }
759
760 // Divide by the time of the collected samples
761 integrated.p = integrated.p * (1.f / rate);
762 integrated.q = integrated.q * (1.f / rate);
763 integrated.r = integrated.r * (1.f / rate);
764
765 // Send the integrated values
766 uint16_t dt = (1e6 / rate) * samples;
767 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
768 }
769#else
770 (void)rate; // Surpress compile warning not used
771#endif
772
773#if IMU_LOG_HIGHSPEED
774 struct FloatRates f_sample;
777#endif
778
779 // Copy and send
780 gyro->temperature = temp;
783 gyro->last_stamp = stamp;
784}
785
786static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
787{
788 // Find the correct accel
789 struct imu_accel_t *accel = imu_get_accel(sender_id, true);
790 if(accel == NULL || samples < 1)
791 return;
792
793 // Filter the accel
795 if(accel->calibrated.filter) {
796 for(uint8_t i = 0; i < samples; i++) {
797 data_filtered[i].x = update_butterworth_2_low_pass(&accel->filter[0], data[i].x);
798 data_filtered[i].y = update_butterworth_2_low_pass(&accel->filter[1], data[i].y);
799 data_filtered[i].z = update_butterworth_2_low_pass(&accel->filter[2], data[i].z);
800 }
801 data = data_filtered;
802 }
803
804 // Copy last sample as unscaled
805 VECT3_COPY(accel->unscaled, data[samples-1]);
806
807 // Scale the accel
808 struct Int32Vect3 scaled, scaled_rot;
809 scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale_f.x;
810 scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale_f.y;
811 scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale_f.z;
812
813
814 // Rotate the sensor
816
817#if IMU_INTEGRATION
818 // Only integrate if we have gotten a previous measurement and didn't overflow the timer
819 if(!isnan(rate) && accel->last_stamp > 0 && stamp > accel->last_stamp) {
820 struct FloatVect3 integrated;
821
822 // Trapezoidal integration
823 integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
824 integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
825 integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
826
827 // Only perform multiple integrations in sensor frame if needed
828 if(samples > 1) {
830 struct FloatRMat body_to_sensor;
831 // Rotate back to sensor frame
832 RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
833 float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
834
835 // Add all the other samples
836 for(uint8_t i = 0; i < samples-1; i++) {
837 struct FloatVect3 f_sample;
838 f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale_f.x);
839 f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale_f.y);
840 f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale_f.z);
841
842
843#if IMU_LOG_HIGHSPEED
845#endif
846
850 }
851
852 // Rotate to body frame
853 float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
854 }
855
856 // Divide by the time of the collected samples
857 integrated.x = integrated.x * (1.f / rate);
858 integrated.y = integrated.y * (1.f / rate);
859 integrated.z = integrated.z * (1.f / rate);
860
861 // Send the integrated values
862 uint16_t dt = (1e6 / rate) * samples;
863 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
864 }
865#else
866 (void)rate; // Surpress compile warning not used
867#endif
868
869#if IMU_LOG_HIGHSPEED
870 struct FloatVect3 f_sample;
873#endif
874
875 // Copy and send
876 accel->temperature = temp;
879 accel->last_stamp = stamp;
880}
881
883{
884 // Find the correct mag
885 struct imu_mag_t *mag = imu_get_mag(sender_id, true);
886 if(mag == NULL)
887 return;
888
889 // Calculate current compensation
894
895 // Copy last sample as unscaled
896 VECT3_COPY(mag->unscaled, *data);
897
898 // Scale the mag
899 struct Int32Vect3 scaled;
900 scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale_f.x;
901 scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale_f.y;
902 scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale_f.z;
903
904
905 // Rotate the sensor
906 int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
908}
909
918 // Find the correct gyro or create index
919 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
920 struct imu_gyro_t *gyro = NULL;
921 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
922 if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
923 gyro = &imu.gyros[i];
924 gyro->abi_id = sender_id;
925 break;
926 } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
927 gyro = &imu.gyros[i];
928 }
929 }
930 return gyro;
931}
932
941 // Find the correct accel
942 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
943 struct imu_accel_t *accel = NULL;
944 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
946 accel = &imu.accels[i];
947 accel->abi_id = sender_id;
948 break;
949 } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
950 accel = &imu.accels[i];
951 }
952 }
953
954 return accel;
955}
956
965 // Find the correct mag
966 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
967 struct imu_mag_t *mag = NULL;
968 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
969 if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
970 mag = &imu.mags[i];
971 mag->abi_id = sender_id;
972 break;
973 } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
974 mag = &imu.mags[i];
975 }
976 }
977
978 return mag;
979}
980
1014
1022
1030
1038
1040{
1041 imu.b2i_set_current = set;
1042
1043 if (imu.b2i_set_current) {
1044 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1047 if (stateIsAttitudeValid()) {
1048 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1052 } else {
1053 // indicate that we couldn't set to current roll/pitch
1054 imu.b2i_set_current = false;
1055 }
1056 } else {
1057 // reset to BODY_TO_IMU as defined in airframe file
1060 }
1061}
1062
Main include for ABI (AirBorneInterface).
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition abi_common.h:64
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:58
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
struct FloatVect3 accel_float
struct FloatVect3 mag_float
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
float current
current in A
Definition electrical.h:47
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_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
euler angles
rotation matrix
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
#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 MAT33_ELMT(_m, _row, _col)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
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:1224
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
#define IMU_MAG_ABI_SEND_ID
Which mag measurements to send over telemetry/logging.
Definition imu.c:176
#define IMU_GYRO_ABI_SEND_ID
Which gyro measurements to send over telemetry/logging.
Definition imu.c:164
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
Definition imu.c:148
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:691
#define IMU_MAG_Y_SIGN
Definition imu.c:124
#define IMU_BODY_TO_IMU_THETA
Definition imu.c:155
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:277
#define IMU_GYRO_P_SIGN
By default gyro signs are positive for single IMU with old format or defaults.
Definition imu.c:47
#define IMU_ACCEL_Y_SIGN
Definition imu.c:89
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:612
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:917
static abi_event imu_accel_raw_ev
Definition imu.c:429
static abi_event imu_mag_raw_ev
Definition imu.c:429
#define IMU_MAG_X_SIGN
By default mag signs are positive for single IMU with old format and defaults.
Definition imu.c:121
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:940
void imu_SetBodyToImuTheta(float theta)
Definition imu.c:1023
#define IMU_ACCEL_X_SIGN
By default accel signs are positive for single IMU with old format and defaults.
Definition imu.c:86
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians)
Definition imu.c:154
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:786
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:641
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
Definition imu.c:113
#define IMU_LOG_HIGHSPEED_DEVICE
By default log highspeed on the flightrecorder.
Definition imu.c:182
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:986
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:291
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:321
struct Imu imu
global IMU state
Definition imu.c:428
#define IMU_BODY_TO_IMU_PSI
Definition imu.c:156
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
Definition imu.c:74
#define IMU_MAG_Z_SIGN
Definition imu.c:127
void imu_init(void)
External functions.
Definition imu.c:435
void imu_SetBodyToImuPsi(float psi)
Definition imu.c:1031
static abi_event imu_gyro_raw_ev
Definition imu.c:429
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:189
#define IMU_GYRO_R_SIGN
Definition imu.c:53
void imu_SetBodyToImuCurrent(float set)
Definition imu.c:1039
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:964
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:247
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:261
void imu_SetBodyToImuPhi(float phi)
Definition imu.c:1015
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:217
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
Definition imu.c:882
#define IMU_ACCEL_Z_SIGN
Definition imu.c:92
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:203
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:305
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:233
#define IMU_ACCEL_ABI_SEND_ID
Which accel measurements to send over telemetry/logging.
Definition imu.c:170
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:670
#define IMU_GYRO_Q_SIGN
Definition imu.c:50
Inertial Measurement Unit interface.
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:52
struct Int32Rates scaled
Last scaled values in body frame.
Definition imu.h:54
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:76
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:84
struct FloatVect3 current_scale
Current scaling multiplying.
Definition imu.h:90
struct imu_accel_t accels[IMU_MAX_SENSORS]
The accelerometer sensors.
Definition imu.h:99
uint8_t abi_id
ABI sensor ID.
Definition imu.h:51
bool scale
Scale calibrated.
Definition imu.h:43
float temperature
Temperature in degrees celcius.
Definition imu.h:56
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:79
float filter_freq
Lowpass filter frequency (Hz)
Definition imu.h:77
struct FloatRates scale_f
Scaling.
Definition imu.h:59
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:68
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition imu.h:62
struct FloatVect3 scale_f
Scaling.
Definition imu.h:89
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:87
uint8_t mag_abi_send_id
Filter out and send only a specific ABI id in telemetry for the magnetometer.
Definition imu.h:104
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
Definition imu.h:101
bool filter
Enable the lowpass filter.
Definition imu.h:47
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:71
float temperature
Temperature in degrees celcius.
Definition imu.h:72
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:57
bool current
Current calibrated.
Definition imu.h:46
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:91
bool initialized
Check if the IMU is initialized.
Definition imu.h:97
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:63
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:69
struct FloatVect3 scale_f
Scaling.
Definition imu.h:75
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:88
bool scale_f
Scale calibrated with floating point.
Definition imu.h:44
struct Int32Rates scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:58
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition imu.h:78
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:70
struct imu_gyro_t gyros[IMU_MAX_SENSORS]
The gyro sensors.
Definition imu.h:98
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:60
struct Int32Rates unscaled
Last unscaled values in sensor frame.
Definition imu.h:55
uint8_t abi_id
ABI sensor ID.
Definition imu.h:83
struct imu_mag_t mags[IMU_MAX_SENSORS]
The magnetometer sensors.
Definition imu.h:100
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:74
bool neutral
Neutral values calibrated.
Definition imu.h:42
bool b2i_set_current
flag for adjusting body_to_imu via settings.
Definition imu.h:109
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:73
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:86
float filter_freq
Filter frequency.
Definition imu.h:61
#define IMU_MAX_SENSORS
Definition imu.h:38
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:53
uint8_t accel_abi_send_id
Filter out and send only a specific ABI id in telemetry for the accelerometer.
Definition imu.h:103
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:85
bool rotation
Rotation calibrated.
Definition imu.h:45
uint8_t gyro_abi_send_id
Filter out and send only a specific ABI id in telemetry for the gyro.
Definition imu.h:102
uint8_t abi_id
ABI sensor ID.
Definition imu.h:67
abstract IMU interface providing fixed point interface
Definition imu.h:96
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
static TRICAL_instance_t mag_calib
uint16_t foo
Definition main_demo5.c:58
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Definition pprzlog_tp.c:29
Mini printf-like functionality.
void shell_add_entry(char *cmd_name, shell_cmd_t *cmd)
Add dynamic entry.
Definition shell_arch.c:92
BaseSequentialStream shell_stream_t
Definition shell_arch.h:31
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
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
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.