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
363static void show_ratesf(shell_stream_t *sh, char* name, struct FloatRates *r) {
364 chprintf(sh, " %s: %f, %f, %f\r\n", name, r->p, r->q, r->r);
365}
366
367
368static void show_matrix(shell_stream_t *sh, char* name, struct Int32RMat *m) {
369 chprintf(sh, " %s:\r\n", name);
370 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 0, 0), MAT33_ELMT(*m, 0, 1), MAT33_ELMT(*m, 0, 2));
371 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 1, 0), MAT33_ELMT(*m, 1, 1), MAT33_ELMT(*m, 1, 2));
372 chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 2, 0), MAT33_ELMT(*m, 2, 1), MAT33_ELMT(*m, 2, 2));
373}
374
375static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[])
376{
377 (void) argv;
378 int i = 0;
379 if (argc > 0) {
380 chprintf(sh, "Usage: imu\r\n");
381 return;
382 }
383 chprintf(sh, "GYRO IMU data\r\n");
384 for (i = 0; i < IMU_MAX_SENSORS; i++) {
385 if (imu.gyros[i].abi_id != 0) {
386 chprintf(sh, " Gyro id: %u, time %lu\r\n", imu.gyros[i].abi_id, imu.gyros[i].last_stamp);
387 show_calibrated(sh, &imu.gyros[i].calibrated);
388 show_rates(sh, "neutral", &imu.gyros[i].neutral);
389 show_ratesf(sh, "scale", &imu.gyros[i].scale_f);
390 show_matrix(sh, "body_to_sensor", &imu.gyros[i].body_to_sensor);
391 show_rates(sh, "unscaled", &imu.gyros[i].unscaled);
392 show_rates(sh, "scaled", &imu.gyros[i].scaled);
393 struct FloatRates gf;
394 RATES_FLOAT_OF_BFP(gf, imu.gyros[i].scaled);
395 chprintf(sh, " -> gyro (rad/s): %.3f, %.3f, %.3f\r\n", gf.p, gf.q, gf.r);
396 }
397 }
398 chprintf(sh, "ACCEL IMU data\r\n");
399 for (i = 0; i < IMU_MAX_SENSORS; i++) {
400 if (imu.accels[i].abi_id != 0) {
401 chprintf(sh, " Accel id: %u, time %lu\r\n", imu.accels[i].abi_id, imu.accels[i].last_stamp);
402 show_calibrated(sh, &imu.accels[i].calibrated);
403 show_vect3(sh, "neutral", &imu.accels[i].neutral);
404 show_vect3f(sh, "scale", &imu.accels[i].scale_f);
405 show_matrix(sh, "body_to_sensor", &imu.accels[i].body_to_sensor);
406 show_vect3(sh, "unscaled", &imu.accels[i].unscaled);
407 show_vect3(sh, "scaled", &imu.accels[i].scaled);
408 struct FloatVect3 af;
409 ACCELS_FLOAT_OF_BFP(af, imu.accels[i].scaled);
410 chprintf(sh, " -> accel (m/s2): %.3f, %.3f, %.3f\r\n", af.x, af.y, af.z);
411 }
412 }
413 chprintf(sh, "MAG IMU data\r\n");
414 for (i = 0; i < IMU_MAX_SENSORS; i++) {
415 if (imu.mags[i].abi_id != 0) {
416 chprintf(sh, " Mag id: %u\r\n", imu.mags[i].abi_id);
417 show_calibrated(sh, &imu.mags[i].calibrated);
418 show_vect3(sh, "neutral", &imu.mags[i].neutral);
419 show_vect3f(sh, "scale", &imu.mags[i].scale_f);
420 show_matrix(sh, "body_to_sensor", &imu.mags[i].body_to_sensor);
421 show_vect3(sh, "unscaled", &imu.mags[i].unscaled);
422 show_vect3(sh, "scaled", &imu.mags[i].scaled);
423 struct FloatVect3 mf;
424 MAGS_FLOAT_OF_BFP(mf, imu.mags[i].scaled);
425 chprintf(sh, " -> mag (unit): %.3f, %.3f, %.3f\r\n", mf.x, mf.y, mf.z);
426 }
427 }
428}
429
430#endif
431
432struct Imu imu = {0};
433static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
434static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp);
435static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp);
436static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
437static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
438
439void imu_init(void)
440{
441 // Set the Body to IMU rotation
442 struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
443 orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
444 struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
445
446
447 // Set the calibrated sensors
448 struct Int32RMat body_to_sensor;
449 const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
450 const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
451 const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
452 const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
453 const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
454 const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
455
456 // Initialize the sensors to default values
457 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
458 /* Copy gyro calibration if needed */
459 if(i >= gyro_calib_len) {
460 imu.gyros[i].abi_id = ABI_DISABLE;
461 imu.gyros[i].calibrated.neutral = false;
462 imu.gyros[i].calibrated.scale = false;
463 imu.gyros[i].calibrated.scale_f = false;
464 imu.gyros[i].calibrated.rotation = false;
465 imu.gyros[i].calibrated.filter = false;
466 } else {
467 imu.gyros[i] = gyro_calib[i];
468 }
469
470 // Set the default safe values if not calibrated
471 if(!imu.gyros[i].calibrated.neutral) {
472 INT_RATES_ZERO(imu.gyros[i].neutral);
473 }
474
475 if(!imu.gyros[i].calibrated.scale_f) {
476 if(imu.gyros[i].calibrated.scale) {
477 imu.gyros[i].scale_f.p = (float)imu.gyros[i].scale[0].p / (float)imu.gyros[i].scale[1].p;
478 imu.gyros[i].scale_f.q = (float)imu.gyros[i].scale[0].q / (float)imu.gyros[i].scale[1].q;
479 imu.gyros[i].scale_f.r = (float)imu.gyros[i].scale[0].r / (float)imu.gyros[i].scale[1].r;
480 imu.gyros[i].calibrated.scale_f = true;
481 } else {
482 VECT3_ASSIGN(imu.accels[i].scale_f, IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN);
483 }
484 }
485
486 if(!imu.gyros[i].calibrated.rotation) {
487 int32_rmat_identity(&imu.gyros[i].body_to_sensor);
488 }
489 imu.gyros[i].last_stamp = 0;
490 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
491 RMAT_COPY(imu.gyros[i].body_to_sensor, body_to_sensor);
492
493 if(imu.gyros[i].calibrated.filter) {
494 float tau = 1.0 / (2.0 * M_PI * imu.gyros[i].filter_freq);
495 float sample_time = 1 / imu.gyros[i].filter_sample_freq;
496 for(uint8_t j = 0; j < 3; j++)
497 init_butterworth_2_low_pass(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
498 }
499
500 /* Copy accel calibration if needed */
501 if(i >= accel_calib_len) {
502 imu.accels[i].abi_id = ABI_DISABLE;
503 imu.accels[i].calibrated.neutral = false;
504 imu.accels[i].calibrated.scale = false;
505 imu.accels[i].calibrated.scale_f = false;
506 imu.accels[i].calibrated.rotation = false;
507 imu.accels[i].calibrated.filter = false;
508 } else {
509 imu.accels[i] = accel_calib[i];
510 }
511
512 // Set the default safe values if not calibrated
513 if(!imu.accels[i].calibrated.neutral) {
514 INT_VECT3_ZERO(imu.accels[i].neutral);
515 }
516
517 if(!imu.accels[i].calibrated.scale_f) {
518 if(imu.accels[i].calibrated.scale) {
519 imu.accels[i].scale_f.x = (float)imu.accels[i].scale[0].x / (float)imu.accels[i].scale[1].x;
520 imu.accels[i].scale_f.y = (float)imu.accels[i].scale[0].y / (float)imu.accels[i].scale[1].y;
521 imu.accels[i].scale_f.z = (float)imu.accels[i].scale[0].z / (float)imu.accels[i].scale[1].z;
522 imu.accels[i].calibrated.scale_f = true;
523 } else {
524 VECT3_ASSIGN(imu.accels[i].scale_f, IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN);
525 }
526 }
527
528 if(!imu.accels[i].calibrated.rotation) {
529 int32_rmat_identity(&imu.accels[i].body_to_sensor);
530 }
531 imu.accels[i].last_stamp = 0;
532 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
533 RMAT_COPY(imu.accels[i].body_to_sensor, body_to_sensor);
534
535 if(imu.accels[i].calibrated.filter) {
536 float tau = 1.0 / (2.0 * M_PI * imu.accels[i].filter_freq);
537 float sample_time = 1 / imu.accels[i].filter_sample_freq;
538 for(uint8_t j = 0; j < 3; j++)
539 init_butterworth_2_low_pass(&imu.accels[i].filter[j], tau, sample_time, 0.0);
540 }
541
542 /* Copy mag calibrated if needed */
543 if(i >= mag_calib_len) {
544 imu.mags[i].abi_id = ABI_DISABLE;
545 imu.mags[i].calibrated.neutral = false;
546 imu.mags[i].calibrated.scale = false;
547 imu.mags[i].calibrated.scale_f = false;
548 imu.mags[i].calibrated.rotation = false;
549 imu.mags[i].calibrated.current = false;
550 } else {
551 imu.mags[i] = mag_calib[i];
552 }
553
554 // Set the default safe values if not calibrated
555 if(!imu.mags[i].calibrated.neutral) {
556 INT_VECT3_ZERO(imu.mags[i].neutral);
557 }
558
559 if(!imu.mags[i].calibrated.scale_f) {
560 if(imu.mags[i].calibrated.scale) {
561 imu.mags[i].scale_f.x = (float)imu.mags[i].scale[0].x / (float)imu.mags[i].scale[1].x;
562 imu.mags[i].scale_f.y = (float)imu.mags[i].scale[0].y / (float)imu.mags[i].scale[1].y;
563 imu.mags[i].scale_f.z = (float)imu.mags[i].scale[0].z / (float)imu.mags[i].scale[1].z;
564 imu.mags[i].calibrated.scale_f = true;
565 }else {
566 VECT3_ASSIGN(imu.mags[i].scale_f, IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN);
567 }
568 }
569 if(!imu.mags[i].calibrated.rotation) {
570 int32_rmat_identity(&imu.mags[i].body_to_sensor);
571 }
572 if(!imu.mags[i].calibrated.current) {
573 INT_VECT3_ZERO(imu.mags[i].current_scale);
574 }
575 int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
576 RMAT_COPY(imu.mags[i].body_to_sensor, body_to_sensor);
577 }
578
579 // Bind to raw measurements
580 AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
581 AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
582 AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
583
584#if PERIODIC_TELEMETRY
585 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
586 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
587 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
588 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_RAW, send_gyro_raw);
589 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
590 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
591 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
592 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
593 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
594 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
595#endif // DOWNLINK
596
597#if USE_SHELL
598 shell_add_entry("imu", cmd_imu);
599#endif
600
601 // Set defaults
602 imu.gyro_abi_send_id = IMU_GYRO_ABI_SEND_ID;
603 imu.accel_abi_send_id = IMU_ACCEL_ABI_SEND_ID;
604 imu.mag_abi_send_id = IMU_MAG_ABI_SEND_ID;
605 imu.initialized = true;
606}
607
616void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
617{
618 // Find the correct gyro
619 struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
620 if(gyro == NULL)
621 return;
622
623 // Copy the defaults
624 if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
625 struct Int32RMat body_to_sensor;
626 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
627 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
628 RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
629 }
630 if(neutral != NULL && !gyro->calibrated.neutral)
631 RATES_COPY(gyro->neutral, *neutral);
632 if(scale_f != NULL && !gyro->calibrated.scale_f) {
633 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);
634 }
635}
636
645void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
646{
647 // Find the correct accel
648 struct imu_accel_t *accel = imu_get_accel(abi_id, true);
649 if(accel == NULL)
650 return;
651
652 // Copy the defaults
653 if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
654 struct Int32RMat body_to_sensor;
655 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
656 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
657 RMAT_COPY(accel->body_to_sensor, body_to_sensor);
658 }
659 if(neutral != NULL && !accel->calibrated.neutral)
660 VECT3_COPY(accel->neutral, *neutral);
661 if(scale_f != NULL && !accel->calibrated.scale_f) {
662 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);
663 }
664}
665
674void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
675{
676 // Find the correct mag
677 struct imu_mag_t *mag = imu_get_mag(abi_id, true);
678 if(mag == NULL)
679 return;
680
681 // Copy the defaults
682 if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
683 struct Int32RMat body_to_sensor;
684 struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
685 int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
686 RMAT_COPY(mag->body_to_sensor, body_to_sensor);
687 }
688 if(neutral != NULL && !mag->calibrated.neutral)
689 VECT3_COPY(mag->neutral, *neutral);
690 if(scale_f != NULL && !mag->calibrated.scale_f) {
691 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);
692 }
693}
694
695static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
696{
697 // Find the correct gyro
698 struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
699 if(gyro == NULL || samples < 1)
700 return;
701
702 // Filter the gyro
703 struct Int32Rates data_filtered[samples];
704 if(gyro->calibrated.filter) {
705 for(uint8_t i = 0; i < samples; i++) {
706 data_filtered[i].p = update_butterworth_2_low_pass(&gyro->filter[0], data[i].p);
707 data_filtered[i].q = update_butterworth_2_low_pass(&gyro->filter[1], data[i].q);
708 data_filtered[i].r = update_butterworth_2_low_pass(&gyro->filter[2], data[i].r);
709 }
710 data = data_filtered;
711 }
712
713 // Copy last sample as unscaled
714 RATES_COPY(gyro->unscaled, data[samples-1]);
715
716 // Scale the gyro
717 struct Int32Rates scaled, scaled_rot;
718 scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale_f.p;
719 scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale_f.q;
720 scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale_f.r;
721
722 // Rotate the sensor
723 int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
724
725#if IMU_INTEGRATION
726 // Only integrate if we have gotten a previous measurement and didn't overflow the timer
727 if(!isnan(rate) && gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
728 struct FloatRates integrated;
729
730 // Trapezoidal integration (TODO: coning correction)
731 integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
732 integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
733 integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
734
735 // Only perform multiple integrations in sensor frame if needed
736 if(samples > 1) {
737 struct FloatRates integrated_sensor;
738 struct FloatRMat body_to_sensor;
739 // Rotate back to sensor frame
740 RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
741 float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
742
743 // Add all the other samples
744 for(uint8_t i = 0; i < samples-1; i++) {
745 struct FloatRates f_sample;
746 f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale_f.p);
747 f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale_f.q);
748 f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale_f.r);
749
750
751#if IMU_LOG_HIGHSPEED
752 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);
753#endif
754
755 integrated_sensor.p += f_sample.p;
756 integrated_sensor.q += f_sample.q;
757 integrated_sensor.r += f_sample.r;
758 }
759
760 // Rotate to body frame
761 float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
762 }
763
764 // Divide by the time of the collected samples
765 integrated.p = integrated.p * (1.f / rate);
766 integrated.q = integrated.q * (1.f / rate);
767 integrated.r = integrated.r * (1.f / rate);
768
769 // Send the integrated values
770 uint16_t dt = (1e6 / rate) * samples;
771 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
772 }
773#else
774 (void)rate; // Surpress compile warning not used
775#endif
776
777#if IMU_LOG_HIGHSPEED
778 struct FloatRates f_sample;
779 RATES_FLOAT_OF_BFP(f_sample, scaled);
780 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);
781#endif
782
783 // Copy and send
784 gyro->temperature = temp;
785 RATES_COPY(gyro->scaled, scaled_rot);
786 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
787 gyro->last_stamp = stamp;
788}
789
790static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
791{
792 // Find the correct accel
793 struct imu_accel_t *accel = imu_get_accel(sender_id, true);
794 if(accel == NULL || samples < 1)
795 return;
796
797 // Filter the accel
798 struct Int32Vect3 data_filtered[samples];
799 if(accel->calibrated.filter) {
800 for(uint8_t i = 0; i < samples; i++) {
801 data_filtered[i].x = update_butterworth_2_low_pass(&accel->filter[0], data[i].x);
802 data_filtered[i].y = update_butterworth_2_low_pass(&accel->filter[1], data[i].y);
803 data_filtered[i].z = update_butterworth_2_low_pass(&accel->filter[2], data[i].z);
804 }
805 data = data_filtered;
806 }
807
808 // Copy last sample as unscaled
809 VECT3_COPY(accel->unscaled, data[samples-1]);
810
811 // Scale the accel
812 struct Int32Vect3 scaled, scaled_rot;
813 scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale_f.x;
814 scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale_f.y;
815 scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale_f.z;
816
817
818 // Rotate the sensor
819 int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
820
821#if IMU_INTEGRATION
822 // Only integrate if we have gotten a previous measurement and didn't overflow the timer
823 if(!isnan(rate) && accel->last_stamp > 0 && stamp > accel->last_stamp) {
824 struct FloatVect3 integrated;
825
826 // Trapezoidal integration
827 integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
828 integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
829 integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
830
831 // Only perform multiple integrations in sensor frame if needed
832 if(samples > 1) {
833 struct FloatVect3 integrated_sensor;
834 struct FloatRMat body_to_sensor;
835 // Rotate back to sensor frame
836 RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
837 float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
838
839 // Add all the other samples
840 for(uint8_t i = 0; i < samples-1; i++) {
841 struct FloatVect3 f_sample;
842 f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale_f.x);
843 f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale_f.y);
844 f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale_f.z);
845
846
847#if IMU_LOG_HIGHSPEED
848 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);
849#endif
850
851 integrated_sensor.x += f_sample.x;
852 integrated_sensor.y += f_sample.y;
853 integrated_sensor.z += f_sample.z;
854 }
855
856 // Rotate to body frame
857 float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
858 }
859
860 // Divide by the time of the collected samples
861 integrated.x = integrated.x * (1.f / rate);
862 integrated.y = integrated.y * (1.f / rate);
863 integrated.z = integrated.z * (1.f / rate);
864
865 // Send the integrated values
866 uint16_t dt = (1e6 / rate) * samples;
867 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
868 }
869#else
870 (void)rate; // Surpress compile warning not used
871#endif
872
873#if IMU_LOG_HIGHSPEED
874 struct FloatVect3 f_sample;
875 ACCELS_FLOAT_OF_BFP(f_sample, scaled);
876 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);
877#endif
878
879 // Copy and send
880 accel->temperature = temp;
881 VECT3_COPY(accel->scaled, scaled_rot);
882 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
883 accel->last_stamp = stamp;
884}
885
886static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
887{
888 // Find the correct mag
889 struct imu_mag_t *mag = imu_get_mag(sender_id, true);
890 if(mag == NULL)
891 return;
892
893 // Calculate current compensation
894 struct Int32Vect3 mag_correction;
895 mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
896 mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
897 mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
898
899 // Copy last sample as unscaled
900 VECT3_COPY(mag->unscaled, *data);
901
902 // Scale the mag
903 struct Int32Vect3 scaled;
904 scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale_f.x;
905 scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale_f.y;
906 scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale_f.z;
907
908
909 // Rotate the sensor
910 int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
911 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
912}
913
921struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
922 // Find the correct gyro or create index
923 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
924 struct imu_gyro_t *gyro = NULL;
925 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
926 if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
927 gyro = &imu.gyros[i];
928 gyro->abi_id = sender_id;
929 break;
930 } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
931 gyro = &imu.gyros[i];
932 }
933 }
934 return gyro;
935}
936
944struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
945 // Find the correct accel
946 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
947 struct imu_accel_t *accel = NULL;
948 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
949 if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
950 accel = &imu.accels[i];
951 accel->abi_id = sender_id;
952 break;
953 } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
954 accel = &imu.accels[i];
955 }
956 }
957
958 return accel;
959}
960
968struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
969 // Find the correct mag
970 // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
971 struct imu_mag_t *mag = NULL;
972 for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
973 if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
974 mag = &imu.mags[i];
975 mag->abi_id = sender_id;
976 break;
977 } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
978 mag = &imu.mags[i];
979 }
980 }
981
982 return mag;
983}
984
1018
1026
1034
1042
1043void imu_SetBodyToImuCurrent(float set)
1044{
1045 imu.b2i_set_current = set;
1046
1047 if (imu.b2i_set_current) {
1048 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1049 struct FloatEulers body_to_imu_eulers;
1050 body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
1051 if (stateIsAttitudeValid()) {
1052 // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
1053 body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
1054 body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
1055 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1056 } else {
1057 // indicate that we couldn't set to current roll/pitch
1058 imu.b2i_set_current = false;
1059 }
1060 } else {
1061 // reset to BODY_TO_IMU as defined in airframe file
1062 struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
1063 imu_set_body_to_imu_eulers(&body_to_imu_eulers);
1064 }
1065}
1066
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:695
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:616
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:921
imu_accel_raw_ev
static abi_event imu_accel_raw_ev
Definition imu.c:433
imu_mag_raw_ev
static abi_event imu_mag_raw_ev
Definition imu.c:433
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:944
imu_SetBodyToImuTheta
void imu_SetBodyToImuTheta(float theta)
Definition imu.c:1027
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:790
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:645
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:990
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:432
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:439
imu_SetBodyToImuPsi
void imu_SetBodyToImuPsi(float psi)
Definition imu.c:1035
imu_gyro_raw_ev
static abi_event imu_gyro_raw_ev
Definition imu.c:433
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:1043
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:968
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:1019
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:886
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:674
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