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"
35#include "modules/energy/electrical.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
158PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
159PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
160PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
161
162
163#ifndef IMU_GYRO_ABI_SEND_ID
164#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
165#endif
166PRINT_CONFIG_VAR(IMU_GYRO_ABI_SEND_ID)
167
168
169#ifndef IMU_ACCEL_ABI_SEND_ID
170#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
171#endif
172PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)
173
174
175#ifndef IMU_MAG_ABI_SEND_ID
176#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
177#endif
178PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)
179
180
181#ifndef IMU_LOG_HIGHSPEED_DEVICE
182#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
183#endif
184
185
186#if PERIODIC_TELEMETRY
187#include "modules/datalink/telemetry.h"
188
189static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
190{
191 if(imu.accel_abi_send_id == ABI_DISABLE)
192 return;
193 static uint8_t id = 0;
194 pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
195 &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
196 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
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{
205 if(imu.accel_abi_send_id == ABI_DISABLE)
206 return;
207 static uint8_t id = 0;
208 pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
209 &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
210 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
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{
219 if(imu.accel_abi_send_id == ABI_DISABLE)
220 return;
221 static uint8_t id = 0;
222 struct FloatVect3 accel_float;
223 ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
224 pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
225 &accel_float.x, &accel_float.y, &accel_float.z);
226 if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
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{
235 if(imu.gyro_abi_send_id == ABI_DISABLE)
236 return;
237 static uint8_t id = 0;
238 pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
239 &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
240 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
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{
249 if(imu.gyro_abi_send_id == ABI_DISABLE)
250 return;
251 static uint8_t id = 0;
252 pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
253 &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
254 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
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{
263 if(imu.gyro_abi_send_id == ABI_DISABLE)
264 return;
265 static uint8_t id = 0;
266 struct FloatRates gyro_float;
267 RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
268 pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
269 &gyro_float.p, &gyro_float.q, &gyro_float.r);
270 if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
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{
279 if(imu.mag_abi_send_id == ABI_DISABLE)
280 return;
281 static uint8_t id = 0;
282 pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
283 &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
284 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
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{
293 if(imu.mag_abi_send_id == ABI_DISABLE)
294 return;
295 static uint8_t id = 0;
296 pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
297 &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
298 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
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{
307 if(imu.mag_abi_send_id == ABI_DISABLE)
308 return;
309 static uint8_t id = 0;
310 struct FloatVect3 mag_float;
311 MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
312 pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
313 &mag_float.x, &mag_float.y, &mag_float.z);
314 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
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{
323 if(imu.mag_abi_send_id == ABI_DISABLE)
324 return;
325 static uint8_t id = 0;
326 pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
327 &imu.mags[id].abi_id,
328 &imu.mags[id].unscaled.x,
329 &imu.mags[id].unscaled.y,
330 &imu.mags[id].unscaled.z,
331 &electrical.current);
332 if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
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);
383 show_calibrated(sh, &imu.gyros[i].calibrated);
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;
390 RATES_FLOAT_OF_BFP(gf, imu.gyros[i].scaled);
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);
398 show_calibrated(sh, &imu.accels[i].calibrated);
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;
405 ACCELS_FLOAT_OF_BFP(af, imu.accels[i].scaled);
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);
413 show_calibrated(sh, &imu.mags[i].calibrated);
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;
420 MAGS_FLOAT_OF_BFP(mf, imu.mags[i].scaled);
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};
429static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
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);
433static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
434
435void imu_init(void)
436{
437 // Set the Body to IMU rotation
438 struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
439 orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
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) {
456 imu.gyros[i].abi_id = ABI_DISABLE;
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) {
468 INT_RATES_ZERO(imu.gyros[i].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 {
478 VECT3_ASSIGN(imu.accels[i].scale_f, IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN);
479 }
480 }
481
482 if(!imu.gyros[i].calibrated.rotation) {
483 int32_rmat_identity(&imu.gyros[i].body_to_sensor);
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);
487 RMAT_COPY(imu.gyros[i].body_to_sensor, 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);
491 float sample_time = 1 / imu.gyros[i].filter_sample_freq;
492 for(uint8_t j = 0; j < 3; j++)
493 init_butterworth_2_low_pass(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
494 }
495
496 /* Copy accel calibration if needed */
497 if(i >= accel_calib_len) {
498 imu.accels[i].abi_id = ABI_DISABLE;
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) {
510 INT_VECT3_ZERO(imu.accels[i].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 {
520 VECT3_ASSIGN(imu.accels[i].scale_f, IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN);
521 }
522 }
523
524 if(!imu.accels[i].calibrated.rotation) {
525 int32_rmat_identity(&imu.accels[i].body_to_sensor);
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);
529 RMAT_COPY(imu.accels[i].body_to_sensor, 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);
533 float sample_time = 1 / imu.accels[i].filter_sample_freq;
534 for(uint8_t j = 0; j < 3; j++)
535 init_butterworth_2_low_pass(&imu.accels[i].filter[j], tau, sample_time, 0.0);
536 }
537
538 /* Copy mag calibrated if needed */
539 if(i >= mag_calib_len) {
540 imu.mags[i].abi_id = ABI_DISABLE;
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) {
552 INT_VECT3_ZERO(imu.mags[i].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 {
562 VECT3_ASSIGN(imu.mags[i].scale_f, IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN);
563 }
564 }
565 if(!imu.mags[i].calibrated.rotation) {
566 int32_rmat_identity(&imu.mags[i].body_to_sensor);
567 }
568 if(!imu.mags[i].calibrated.current) {
569 INT_VECT3_ZERO(imu.mags[i].current_scale);
570 }
571 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
572 RMAT_COPY(imu.mags[i].body_to_sensor, body_to_sensor);
573 }
574
575 // Bind to raw measurements
576 AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
577 AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
578 AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
579
580#if PERIODIC_TELEMETRY
581 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
582 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
583 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
584 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_RAW, send_gyro_raw);
585 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
586 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
587 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
588 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
589 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
590 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
591#endif // DOWNLINK
592
593#if USE_SHELL
594 shell_add_entry("imu", cmd_imu);
595#endif
596
597 // Set defaults
598 imu.gyro_abi_send_id = IMU_GYRO_ABI_SEND_ID;
599 imu.accel_abi_send_id = IMU_ACCEL_ABI_SEND_ID;
600 imu.mag_abi_send_id = IMU_MAG_ABI_SEND_ID;
601 imu.initialized = true;
602}
603
612void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
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
699 struct Int32Rates data_filtered[samples];
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
719 int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
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) {
733 struct FloatRates integrated_sensor;
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
748 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);
749#endif
750
751 integrated_sensor.p += f_sample.p;
752 integrated_sensor.q += f_sample.q;
753 integrated_sensor.r += f_sample.r;
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;
775 RATES_FLOAT_OF_BFP(f_sample, scaled);
776 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);
777#endif
778
779 // Copy and send
780 gyro->temperature = temp;
781 RATES_COPY(gyro->scaled, scaled_rot);
782 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
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
794 struct Int32Vect3 data_filtered[samples];
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
815 int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
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) {
829 struct FloatVect3 integrated_sensor;
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
844 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);
845#endif
846
847 integrated_sensor.x += f_sample.x;
848 integrated_sensor.y += f_sample.y;
849 integrated_sensor.z += f_sample.z;
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;
871 ACCELS_FLOAT_OF_BFP(f_sample, scaled);
872 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);
873#endif
874
875 // Copy and send
876 accel->temperature = temp;
877 VECT3_COPY(accel->scaled, scaled_rot);
878 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
879 accel->last_stamp = stamp;
880}
881
882static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
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
890 struct Int32Vect3 mag_correction;
891 mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
892 mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
893 mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
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);
907 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
908}
909
917struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
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
940struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
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++) {
945 if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
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
964struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
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
1039void imu_SetBodyToImuCurrent(float set)
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
1045 struct FloatEulers body_to_imu_eulers;
1046 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1047 if (stateIsAttitudeValid()) {
1048 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1049 body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
1050 body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
1051 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
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
1058 struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
1059 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1060 }
1061}
1062
abi.h
Main include for ABI (AirBorneInterface).
ABI_DISABLE
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition abi_common.h:64
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:58
abi_struct
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
accel_float
struct FloatVect3 accel_float
Definition ahrs_float_dcm.c:59
mag_float
struct FloatVect3 mag_float
Definition ahrs_float_dcm.c:60
electrical
struct Electrical electrical
Definition electrical.c:92
electrical.h
Interface for electrical status: supply voltage, current, battery status, etc.
Electrical::current
float current
current in A
Definition electrical.h:47
chprintf
void chprintf(BaseSequentialStream *lchp, const char *fmt,...)
Definition printf.c:395
FloatRates::q
float q
in rad/s
Definition pprz_algebra_float.h:95
FloatEulers::phi
float phi
in radians
Definition pprz_algebra_float.h:85
FloatRates::p
float p
in rad/s
Definition pprz_algebra_float.h:94
FloatRates::r
float r
in rad/s
Definition pprz_algebra_float.h:96
FloatEulers::theta
float theta
in radians
Definition pprz_algebra_float.h:86
FloatVect3::x
float x
Definition pprz_algebra_float.h:55
FloatVect3::y
float y
Definition pprz_algebra_float.h:56
FloatEulers::psi
float psi
in radians
Definition pprz_algebra_float.h:87
FloatVect3::z
float z
Definition pprz_algebra_float.h:57
float_rmat_ratemult
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
Definition pprz_algebra_float.c:150
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition pprz_algebra_float.c:160
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition pprz_algebra_float.c:120
float_rmat_vmult
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
Definition pprz_algebra_float.c:110
FloatEulers
euler angles
Definition pprz_algebra_float.h:84
FloatRMat
rotation matrix
Definition pprz_algebra_float.h:77
FloatRates
angular rates
Definition pprz_algebra_float.h:93
FloatVect3
Definition pprz_algebra_float.h:54
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition pprz_algebra.h:807
RATES_COPY
#define RATES_COPY(_a, _b)
Definition pprz_algebra.h:337
RATES_ASSIGN
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition pprz_algebra.h:330
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition pprz_algebra.h:715
RMAT_FLOAT_OF_BFP
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition pprz_algebra.h:733
RMAT_COPY
#define RMAT_COPY(_o, _i)
Definition pprz_algebra.h:704
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition pprz_algebra.h:125
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition pprz_algebra.h:140
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition pprz_algebra.h:759
MAT33_ELMT
#define MAT33_ELMT(_m, _row, _col)
Definition pprz_algebra.h:436
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition pprz_algebra.h:795
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition pprz_algebra_int.h:180
Int32Vect3::z
int32_t z
Definition pprz_algebra_int.h:91
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition pprz_algebra_int.h:182
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition pprz_algebra_int.h:181
Int32Vect3::x
int32_t x
Definition pprz_algebra_int.h:89
Int32Vect3::y
int32_t y
Definition pprz_algebra_int.h:90
INT_RATES_ZERO
#define INT_RATES_ZERO(_e)
Definition pprz_algebra_int.h:576
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition pprz_algebra_int.h:409
RATE_FLOAT_OF_BFP
#define RATE_FLOAT_OF_BFP(_ai)
Definition pprz_algebra_int.h:210
int32_rmat_comp_inv
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.
Definition pprz_algebra_int.c:91
int32_rmat_transp_ratemult
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
Definition pprz_algebra_int.c:137
INT_VECT3_ZERO
#define INT_VECT3_ZERO(_v)
Definition pprz_algebra_int.h:288
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition pprz_algebra_int.c:117
int32_rmat_comp
void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition pprz_algebra_int.c:75
int32_rmat_identity
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
Definition pprz_algebra_int.h:347
ACCEL_FLOAT_OF_BFP
#define ACCEL_FLOAT_OF_BFP(_ai)
Definition pprz_algebra_int.h:222
Int32Eulers
euler angles
Definition pprz_algebra_int.h:146
Int32RMat
rotation matrix
Definition pprz_algebra_int.h:159
Int32Rates
angular rates
Definition pprz_algebra_int.h:179
Int32Vect3
Definition pprz_algebra_int.h:88
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition pprz_orientation_conversion.h:189
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition pprz_orientation_conversion.h:207
orientationGetEulers_f
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
Definition pprz_orientation_conversion.h:243
stateIsAttitudeValid
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition state.h:1224
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
IMU_MAG_ABI_SEND_ID
#define IMU_MAG_ABI_SEND_ID
Which mag measurements to send over telemetry/logging.
Definition imu.c:176
IMU_GYRO_ABI_SEND_ID
#define IMU_GYRO_ABI_SEND_ID
Which gyro measurements to send over telemetry/logging.
Definition imu.c:164
IMU_MAG_CALIB
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
Definition imu.c:148
imu_gyro_raw_cb
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
IMU_MAG_Y_SIGN
#define IMU_MAG_Y_SIGN
Definition imu.c:124
IMU_BODY_TO_IMU_THETA
#define IMU_BODY_TO_IMU_THETA
Definition imu.c:155
send_mag_raw
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:277
IMU_GYRO_P_SIGN
#define IMU_GYRO_P_SIGN
By default gyro signs are positive for single IMU with old format or defaults.
Definition imu.c:47
IMU_ACCEL_Y_SIGN
#define IMU_ACCEL_Y_SIGN
Definition imu.c:89
imu_set_defaults_gyro
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
imu_get_gyro
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
imu_accel_raw_ev
static abi_event imu_accel_raw_ev
Definition imu.c:429
imu_mag_raw_ev
static abi_event imu_mag_raw_ev
Definition imu.c:429
IMU_MAG_X_SIGN
#define IMU_MAG_X_SIGN
By default mag signs are positive for single IMU with old format and defaults.
Definition imu.c:121
imu_get_accel
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
imu_SetBodyToImuTheta
void imu_SetBodyToImuTheta(float theta)
Definition imu.c:1023
IMU_ACCEL_X_SIGN
#define IMU_ACCEL_X_SIGN
By default accel signs are positive for single IMU with old format and defaults.
Definition imu.c:86
IMU_BODY_TO_IMU_PHI
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians)
Definition imu.c:154
imu_accel_raw_cb
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
imu_set_defaults_accel
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
IMU_ACCEL_CALIB
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
Definition imu.c:113
IMU_LOG_HIGHSPEED_DEVICE
#define IMU_LOG_HIGHSPEED_DEVICE
By default log highspeed on the flightrecorder.
Definition imu.c:182
imu_set_body_to_imu_eulers
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
send_mag_scaled
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:291
send_mag_current
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:321
imu
struct Imu imu
global IMU state
Definition imu.c:428
IMU_BODY_TO_IMU_PSI
#define IMU_BODY_TO_IMU_PSI
Definition imu.c:156
IMU_GYRO_CALIB
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
Definition imu.c:74
IMU_MAG_Z_SIGN
#define IMU_MAG_Z_SIGN
Definition imu.c:127
imu_init
void imu_init(void)
External functions.
Definition imu.c:435
imu_SetBodyToImuPsi
void imu_SetBodyToImuPsi(float psi)
Definition imu.c:1031
imu_gyro_raw_ev
static abi_event imu_gyro_raw_ev
Definition imu.c:429
send_accel_raw
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:189
IMU_GYRO_R_SIGN
#define IMU_GYRO_R_SIGN
Definition imu.c:53
imu_SetBodyToImuCurrent
void imu_SetBodyToImuCurrent(float set)
Definition imu.c:1039
imu_get_mag
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
send_gyro_scaled
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:247
send_gyro
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:261
imu_SetBodyToImuPhi
void imu_SetBodyToImuPhi(float phi)
Definition imu.c:1015
send_accel
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:217
imu_mag_raw_cb
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
Definition imu.c:882
IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_Z_SIGN
Definition imu.c:92
send_accel_scaled
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:203
send_mag
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:305
send_gyro_raw
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:233
IMU_ACCEL_ABI_SEND_ID
#define IMU_ACCEL_ABI_SEND_ID
Which accel measurements to send over telemetry/logging.
Definition imu.c:170
imu_set_defaults_mag
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
IMU_GYRO_Q_SIGN
#define IMU_GYRO_Q_SIGN
Definition imu.c:50
imu.h
Inertial Measurement Unit interface.
imu_gyro_t::last_stamp
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:52
imu_gyro_t::scaled
struct Int32Rates scaled
Last scaled values in body frame.
Definition imu.h:54
imu_accel_t::body_to_sensor
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:76
imu_mag_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:84
imu_mag_t::current_scale
struct FloatVect3 current_scale
Current scaling multiplying.
Definition imu.h:90
Imu::accels
struct imu_accel_t accels[IMU_MAX_SENSORS]
The accelerometer sensors.
Definition imu.h:99
imu_gyro_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition imu.h:51
imu_calib_t::scale
bool scale
Scale calibrated.
Definition imu.h:43
imu_gyro_t::temperature
float temperature
Temperature in degrees celcius.
Definition imu.h:56
imu_accel_t::filter
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:79
imu_accel_t::filter_freq
float filter_freq
Lowpass filter frequency (Hz)
Definition imu.h:77
imu_gyro_t::scale_f
struct FloatRates scale_f
Scaling.
Definition imu.h:59
imu_accel_t::last_stamp
uint32_t last_stamp
Last measurement timestamp for integration.
Definition imu.h:68
imu_gyro_t::filter_sample_freq
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition imu.h:62
imu_mag_t::scale_f
struct FloatVect3 scale_f
Scaling.
Definition imu.h:89
imu_mag_t::neutral
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:87
Imu::mag_abi_send_id
uint8_t mag_abi_send_id
Filter out and send only a specific ABI id in telemetry for the magnetometer.
Definition imu.h:104
Imu::body_to_imu
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
Definition imu.h:101
imu_calib_t::filter
bool filter
Enable the lowpass filter.
Definition imu.h:47
imu_accel_t::unscaled
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:71
imu_accel_t::temperature
float temperature
Temperature in degrees celcius.
Definition imu.h:72
imu_gyro_t::neutral
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:57
imu_calib_t::current
bool current
Current calibrated.
Definition imu.h:46
imu_mag_t::body_to_sensor
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:91
Imu::initialized
bool initialized
Check if the IMU is initialized.
Definition imu.h:97
imu_gyro_t::filter
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition imu.h:63
imu_accel_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:69
imu_accel_t::scale_f
struct FloatVect3 scale_f
Scaling.
Definition imu.h:75
imu_mag_t::scale
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:88
imu_calib_t::scale_f
bool scale_f
Scale calibrated with floating point.
Definition imu.h:44
imu_gyro_t::scale
struct Int32Rates scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:58
imu_accel_t::filter_sample_freq
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition imu.h:78
imu_accel_t::scaled
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:70
Imu::gyros
struct imu_gyro_t gyros[IMU_MAX_SENSORS]
The gyro sensors.
Definition imu.h:98
imu_gyro_t::body_to_sensor
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition imu.h:60
imu_gyro_t::unscaled
struct Int32Rates unscaled
Last unscaled values in sensor frame.
Definition imu.h:55
imu_mag_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition imu.h:83
Imu::mags
struct imu_mag_t mags[IMU_MAX_SENSORS]
The magnetometer sensors.
Definition imu.h:100
imu_accel_t::scale
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition imu.h:74
imu_calib_t::neutral
bool neutral
Neutral values calibrated.
Definition imu.h:42
Imu::b2i_set_current
bool b2i_set_current
flag for adjusting body_to_imu via settings.
Definition imu.h:109
imu_accel_t::neutral
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition imu.h:73
imu_mag_t::unscaled
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition imu.h:86
imu_gyro_t::filter_freq
float filter_freq
Filter frequency.
Definition imu.h:61
IMU_MAX_SENSORS
#define IMU_MAX_SENSORS
Definition imu.h:38
imu_gyro_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition imu.h:53
Imu::accel_abi_send_id
uint8_t accel_abi_send_id
Filter out and send only a specific ABI id in telemetry for the accelerometer.
Definition imu.h:103
imu_mag_t::scaled
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:85
imu_calib_t::rotation
bool rotation
Rotation calibrated.
Definition imu.h:45
Imu::gyro_abi_send_id
uint8_t gyro_abi_send_id
Filter out and send only a specific ABI id in telemetry for the gyro.
Definition imu.h:102
imu_accel_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition imu.h:67
Imu
abstract IMU interface providing fixed point interface
Definition imu.h:96
imu_accel_t
Definition imu.h:66
imu_calib_t
Definition imu.h:41
imu_gyro_t
Definition imu.h:50
imu_mag_t
Definition imu.h:82
p
static float p[2][2]
Definition ins_alt_float.c:273
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
init_butterworth_2_low_pass
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
Definition low_pass_filter.h:280
update_butterworth_2_low_pass
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Definition low_pass_filter.h:291
mag_calib
static TRICAL_instance_t mag_calib
Definition mag_calib_ukf.c:123
foo
uint16_t foo
Definition main_demo5.c:58
PRINT_CONFIG_VAR
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
pprzlog_tp
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Definition pprzlog_tp.c:29
printf.h
Mini printf-like functionality.
shell_add_entry
void shell_add_entry(char *cmd_name, shell_cmd_t *cmd)
Add dynamic entry.
Definition shell_arch.c:92
shell_stream_t
BaseSequentialStream shell_stream_t
Definition shell_arch.h:31
state.h
API to get/set the generic vehicle states.
dev
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
register_periodic_telemetry
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
telemetry.h
Periodic telemetry system header (includes downlink utility and generated code).
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition vl53l1_types.h:88
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition vl53l1_types.h:83
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition vl53l1_types.h:98