Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
imu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2022 The Paparazzi Team
3  * Freek van Tienen <freek.v.tienen@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
28 #ifdef BOARD_CONFIG
29 #include BOARD_CONFIG
30 #endif
31 
32 #include "modules/imu/imu.h"
33 #include "state.h"
34 #include "modules/core/abi.h"
36 
38 #ifndef IMU_INTEGRATION
39 #define IMU_INTEGRATION false
40 #endif
41 
43 #if defined(IMU_GYRO_CALIB) && (defined(IMU_GYRO_P_SIGN) || defined(IMU_GYRO_Q_SIGN) || defined(IMU_GYRO_R_SIGN))
44 #warning "The IMU_GYRO_?_SIGN's aren't compatible with the IMU_GYRO_CALIB define in the airframe"
45 #endif
46 #ifndef IMU_GYRO_P_SIGN
47 #define IMU_GYRO_P_SIGN 1
48 #endif
49 #ifndef IMU_GYRO_Q_SIGN
50 #define IMU_GYRO_Q_SIGN 1
51 #endif
52 #ifndef IMU_GYRO_R_SIGN
53 #define IMU_GYRO_R_SIGN 1
54 #endif
55 
57 #if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
58 #define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
59 PRINT_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}}
64 PRINT_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 
78 #if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN))
79 #warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe"
80 #endif
81 #ifndef IMU_ACCEL_X_SIGN
82 #define IMU_ACCEL_X_SIGN 1
83 #endif
84 #ifndef IMU_ACCEL_Y_SIGN
85 #define IMU_ACCEL_Y_SIGN 1
86 #endif
87 #ifndef IMU_ACCEL_Z_SIGN
88 #define IMU_ACCEL_Z_SIGN 1
89 #endif
90 
92 #if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
93 #define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
94 PRINT_CONFIG_MSG("Using single IMU accel neutral calibration")
95 #endif
96 
97 #if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
98 #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}}
99 PRINT_CONFIG_MSG("Using single IMU accel sensitivity calibration")
100 #endif
101 
102 #if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
103 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
104 #elif defined(ACCEL_NEUTRAL)
105 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
106 #elif defined(ACCEL_SCALE)
107 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
108 #elif !defined(IMU_ACCEL_CALIB)
109 #define IMU_ACCEL_CALIB {}
110 #endif
111 
113 #if defined(IMU_MAG_CALIB) && (defined(IMU_MAG_X_SIGN) || defined(IMU_MAG_Y_SIGN) || defined(IMU_MAG_Z_SIGN))
114 #warning "The IMU_MAG_?_SIGN's aren't compatible with the IMU_MAG_CALIB define in the airframe"
115 #endif
116 #ifndef IMU_MAG_X_SIGN
117 #define IMU_MAG_X_SIGN 1
118 #endif
119 #ifndef IMU_MAG_Y_SIGN
120 #define IMU_MAG_Y_SIGN 1
121 #endif
122 #ifndef IMU_MAG_Z_SIGN
123 #define IMU_MAG_Z_SIGN 1
124 #endif
125 
127 #if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
128 #define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
129 PRINT_CONFIG_MSG("Using single IMU mag neutral calibration")
130 #endif
131 
132 #if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
133 #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}}
134 PRINT_CONFIG_MSG("Using single IMU mag sensitivity calibration")
135 #endif
136 
137 #if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
138 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
139 #elif defined(MAG_NEUTRAL)
140 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
141 #elif defined(MAG_SCALE)
142 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
143 #elif !defined(IMU_MAG_CALIB)
144 #define IMU_MAG_CALIB {}
145 #endif
146 
147 
149 #if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
150 #define IMU_BODY_TO_IMU_PHI 0
151 #define IMU_BODY_TO_IMU_THETA 0
152 #define IMU_BODY_TO_IMU_PSI 0
153 #endif
154 PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
155 PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
156 PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
157 
158 
159 #ifndef IMU_GYRO_ABI_SEND_ID
160 #define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
161 #endif
162 PRINT_CONFIG_VAR(IMU_GYRO_ABI_SEND_ID)
163 
164 
165 #ifndef IMU_ACCEL_ABI_SEND_ID
166 #define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
167 #endif
168 PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)
169 
170 
171 #ifndef IMU_MAG_ABI_SEND_ID
172 #define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
173 #endif
174 PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)
175 
176 
177 #ifndef IMU_LOG_HIGHSPEED_DEVICE
178 #define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
179 #endif
180 
181 
182 #if PERIODIC_TELEMETRY
184 
185 static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
186 {
188  return;
189  static uint8_t id = 0;
190  pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
191  &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
193  return;
194  id++;
195  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
196  id = 0;
197 }
198 
199 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
200 {
202  return;
203  static uint8_t id = 0;
204  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
205  &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
207  return;
208  id++;
209  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
210  id = 0;
211 }
212 
213 static void send_accel(struct transport_tx *trans, struct link_device *dev)
214 {
216  return;
217  static uint8_t id = 0;
218  struct FloatVect3 accel_float;
220  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
223  return;
224  id++;
225  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
226  id = 0;
227 }
228 
229 static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
230 {
232  return;
233  static uint8_t id = 0;
234  pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
235  &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
237  return;
238  id++;
239  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
240  id = 0;
241 }
242 
243 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
244 {
246  return;
247  static uint8_t id = 0;
248  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
249  &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
251  return;
252  id++;
253  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
254  id = 0;
255 }
256 
257 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
258 {
260  return;
261  static uint8_t id = 0;
262  struct FloatRates gyro_float;
263  RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
264  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
265  &gyro_float.p, &gyro_float.q, &gyro_float.r);
267  return;
268  id++;
269  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
270  id = 0;
271 }
272 
273 static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
274 {
276  return;
277  static uint8_t id = 0;
278  pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
279  &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
281  return;
282  id++;
283  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
284  id = 0;
285 }
286 
287 static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
288 {
290  return;
291  static uint8_t id = 0;
292  pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
293  &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
295  return;
296  id++;
297  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
298  id = 0;
299 }
300 
301 static void send_mag(struct transport_tx *trans, struct link_device *dev)
302 {
304  return;
305  static uint8_t id = 0;
306  struct FloatVect3 mag_float;
308  pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
311  return;
312  id++;
313  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
314  id = 0;
315 }
316 
317 static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
318 {
320  return;
321  static uint8_t id = 0;
322  pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
323  &imu.mags[id].abi_id,
324  &imu.mags[id].unscaled.x,
325  &imu.mags[id].unscaled.y,
326  &imu.mags[id].unscaled.z,
329  return;
330  id++;
331  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
332  id = 0;
333 }
334 
335 #endif /* PERIODIC_TELEMETRY */
336 
337 struct Imu imu = {0};
339 static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp);
340 static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp);
341 static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
342 static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
343 
344 void imu_init(void)
345 {
346  // Set the Body to IMU rotation
348  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
349  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
350 
351 
352  // Set the calibrated sensors
353  struct Int32RMat body_to_sensor;
354  const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
355  const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
356  const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
357  const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
358  const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
359  const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
360 
361  // Initialize the sensors to default values
362  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
363  /* Copy gyro calibration if needed */
364  if(i >= gyro_calib_len) {
366  imu.gyros[i].calibrated.neutral = false;
367  imu.gyros[i].calibrated.scale = false;
368  imu.gyros[i].calibrated.rotation = false;
369  imu.gyros[i].calibrated.filter = false;
370  } else {
371  imu.gyros[i] = gyro_calib[i];
372  }
373 
374  // Set the default safe values if not calibrated
375  if(!imu.gyros[i].calibrated.neutral) {
377  }
378  if(!imu.gyros[i].calibrated.scale) {
380  RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1);
381  }
382  if(!imu.gyros[i].calibrated.rotation) {
384  }
385  imu.gyros[i].last_stamp = 0;
386  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
388 
389  if(imu.gyros[i].calibrated.filter) {
390  float tau = 1.0 / (2.0 * M_PI * imu.gyros[i].filter_freq);
391  float sample_time = 1 / imu.gyros[i].filter_sample_freq;
392  for(uint8_t j = 0; j < 3; j++)
393  init_butterworth_2_low_pass(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
394  }
395 
396  /* Copy accel calibration if needed */
397  if(i >= accel_calib_len) {
399  imu.accels[i].calibrated.neutral = false;
400  imu.accels[i].calibrated.scale = false;
401  imu.accels[i].calibrated.rotation = false;
402  imu.accels[i].calibrated.filter = false;
403  } else {
404  imu.accels[i] = accel_calib[i];
405  }
406 
407  // Set the default safe values if not calibrated
408  if(!imu.accels[i].calibrated.neutral) {
410  }
411  if(!imu.accels[i].calibrated.scale) {
413  VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1);
414  }
415  if(!imu.accels[i].calibrated.rotation) {
417  }
418  imu.accels[i].last_stamp = 0;
419  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
421 
422  if(imu.accels[i].calibrated.filter) {
423  float tau = 1.0 / (2.0 * M_PI * imu.accels[i].filter_freq);
424  float sample_time = 1 / imu.accels[i].filter_sample_freq;
425  for(uint8_t j = 0; j < 3; j++)
426  init_butterworth_2_low_pass(&imu.accels[i].filter[j], tau, sample_time, 0.0);
427  }
428 
429  /* Copy mag calibrated if needed */
430  if(i >= mag_calib_len) {
431  imu.mags[i].abi_id = ABI_DISABLE;
432  imu.mags[i].calibrated.neutral = false;
433  imu.mags[i].calibrated.scale = false;
434  imu.mags[i].calibrated.rotation = false;
435  imu.mags[i].calibrated.current = false;
436  } else {
437  imu.mags[i] = mag_calib[i];
438  }
439 
440  // Set the default safe values if not calibrated
441  if(!imu.mags[i].calibrated.neutral) {
443  }
444  if(!imu.mags[i].calibrated.scale) {
446  VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1);
447  }
448  if(!imu.mags[i].calibrated.rotation) {
450  }
451  if(!imu.mags[i].calibrated.current) {
453  }
454  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
456  }
457 
458  // Bind to raw measurements
459  AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
460  AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
461  AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
462 
463 #if PERIODIC_TELEMETRY
464  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
465  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
466  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
467  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_RAW, send_gyro_raw);
468  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
469  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
470  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
471  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
473  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
474 #endif // DOWNLINK
475 
476  // Set defaults
480  imu.initialized = true;
481 }
482 
491 void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
492 {
493  // Find the correct gyro
494  struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
495  if(gyro == NULL)
496  return;
497 
498  // Copy the defaults
499  if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
500  struct Int32RMat body_to_sensor;
501  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
502  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
503  RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
504  }
505  if(neutral != NULL && !gyro->calibrated.neutral)
506  RATES_COPY(gyro->neutral, *neutral);
507  if(scale != NULL && !gyro->calibrated.scale) {
509  RATES_COPY(gyro->scale[1], scale[1]);
510  }
511 }
512 
521 void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
522 {
523  // Find the correct accel
524  struct imu_accel_t *accel = imu_get_accel(abi_id, true);
525  if(accel == NULL)
526  return;
527 
528  // Copy the defaults
529  if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
530  struct Int32RMat body_to_sensor;
531  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
532  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
533  RMAT_COPY(accel->body_to_sensor, body_to_sensor);
534  }
535  if(neutral != NULL && !accel->calibrated.neutral)
536  VECT3_COPY(accel->neutral, *neutral);
537  if(scale != NULL && !accel->calibrated.scale) {
539  VECT3_COPY(accel->scale[1], scale[1]);
540  }
541 }
542 
551 void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
552 {
553  // Find the correct mag
554  struct imu_mag_t *mag = imu_get_mag(abi_id, true);
555  if(mag == NULL)
556  return;
557 
558  // Copy the defaults
559  if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
560  struct Int32RMat body_to_sensor;
561  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
562  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
563  RMAT_COPY(mag->body_to_sensor, body_to_sensor);
564  }
565  if(neutral != NULL && !mag->calibrated.neutral)
566  VECT3_COPY(mag->neutral, *neutral);
567  if(scale != NULL && !mag->calibrated.scale) {
569  VECT3_COPY(mag->scale[1], scale[1]);
570  }
571 }
572 
573 static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
574 {
575  // Find the correct gyro
576  struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
577  if(gyro == NULL || samples < 1)
578  return;
579 
580  // Filter the gyro
581  struct Int32Rates data_filtered[samples];
582  if(gyro->calibrated.filter) {
583  for(uint8_t i = 0; i < samples; i++) {
584  data_filtered[i].p = update_butterworth_2_low_pass(&gyro->filter[0], data[i].p);
585  data_filtered[i].q = update_butterworth_2_low_pass(&gyro->filter[1], data[i].q);
586  data_filtered[i].r = update_butterworth_2_low_pass(&gyro->filter[2], data[i].r);
587  }
588  data = data_filtered;
589  }
590 
591  // Copy last sample as unscaled
592  RATES_COPY(gyro->unscaled, data[samples-1]);
593 
594  // Scale the gyro
595  struct Int32Rates scaled, scaled_rot;
596  scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p;
597  scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q;
598  scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r;
599 
600  // Rotate the sensor
601  int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
602 
603 #if IMU_INTEGRATION
604  // Only integrate if we have gotten a previous measurement and didn't overflow the timer
605  if(!isnan(rate) && gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
606  struct FloatRates integrated;
607 
608  // Trapezoidal integration (TODO: coning correction)
609  integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
610  integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
611  integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
612 
613  // Only perform multiple integrations in sensor frame if needed
614  if(samples > 1) {
615  struct FloatRates integrated_sensor;
616  struct FloatRMat body_to_sensor;
617  // Rotate back to sensor frame
618  RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
619  float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
620 
621  // Add all the other samples
622  for(uint8_t i = 0; i < samples-1; i++) {
623  struct FloatRates f_sample;
624  f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
625  f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
626  f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
627 
628 #if IMU_LOG_HIGHSPEED
629  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);
630 #endif
631 
632  integrated_sensor.p += f_sample.p;
633  integrated_sensor.q += f_sample.q;
634  integrated_sensor.r += f_sample.r;
635  }
636 
637  // Rotate to body frame
638  float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
639  }
640 
641  // Divide by the time of the collected samples
642  integrated.p = integrated.p * (1.f / rate);
643  integrated.q = integrated.q * (1.f / rate);
644  integrated.r = integrated.r * (1.f / rate);
645 
646  // Send the integrated values
647  uint16_t dt = (1e6 / rate) * samples;
648  AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
649  }
650 #else
651  (void)rate; // Surpress compile warning not used
652 #endif
653 
654 #if IMU_LOG_HIGHSPEED
655  struct FloatRates f_sample;
656  RATES_FLOAT_OF_BFP(f_sample, scaled);
657  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);
658 #endif
659 
660  // Copy and send
661  gyro->temperature = temp;
662  RATES_COPY(gyro->scaled, scaled_rot);
663  AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
664  gyro->last_stamp = stamp;
665 }
666 
667 static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
668 {
669  // Find the correct accel
670  struct imu_accel_t *accel = imu_get_accel(sender_id, true);
671  if(accel == NULL || samples < 1)
672  return;
673 
674  // Filter the accel
675  struct Int32Vect3 data_filtered[samples];
676  if(accel->calibrated.filter) {
677  for(uint8_t i = 0; i < samples; i++) {
678  data_filtered[i].x = update_butterworth_2_low_pass(&accel->filter[0], data[i].x);
679  data_filtered[i].y = update_butterworth_2_low_pass(&accel->filter[1], data[i].y);
680  data_filtered[i].z = update_butterworth_2_low_pass(&accel->filter[2], data[i].z);
681  }
682  data = data_filtered;
683  }
684 
685  // Copy last sample as unscaled
686  VECT3_COPY(accel->unscaled, data[samples-1]);
687 
688  // Scale the accel
689  struct Int32Vect3 scaled, scaled_rot;
690  scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x;
691  scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y;
692  scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z;
693 
694  // Rotate the sensor
695  int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
696 
697 #if IMU_INTEGRATION
698  // Only integrate if we have gotten a previous measurement and didn't overflow the timer
699  if(!isnan(rate) && accel->last_stamp > 0 && stamp > accel->last_stamp) {
700  struct FloatVect3 integrated;
701 
702  // Trapezoidal integration
703  integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
704  integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
705  integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
706 
707  // Only perform multiple integrations in sensor frame if needed
708  if(samples > 1) {
709  struct FloatVect3 integrated_sensor;
710  struct FloatRMat body_to_sensor;
711  // Rotate back to sensor frame
712  RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
713  float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
714 
715  // Add all the other samples
716  for(uint8_t i = 0; i < samples-1; i++) {
717  struct FloatVect3 f_sample;
718  f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
719  f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
720  f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
721 
722 #if IMU_LOG_HIGHSPEED
723  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);
724 #endif
725 
726  integrated_sensor.x += f_sample.x;
727  integrated_sensor.y += f_sample.y;
728  integrated_sensor.z += f_sample.z;
729  }
730 
731  // Rotate to body frame
732  float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
733  }
734 
735  // Divide by the time of the collected samples
736  integrated.x = integrated.x * (1.f / rate);
737  integrated.y = integrated.y * (1.f / rate);
738  integrated.z = integrated.z * (1.f / rate);
739 
740  // Send the integrated values
741  uint16_t dt = (1e6 / rate) * samples;
742  AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
743  }
744 #else
745  (void)rate; // Surpress compile warning not used
746 #endif
747 
748 #if IMU_LOG_HIGHSPEED
749  struct FloatVect3 f_sample;
750  ACCELS_FLOAT_OF_BFP(f_sample, scaled);
751  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);
752 #endif
753 
754  // Copy and send
755  accel->temperature = temp;
756  VECT3_COPY(accel->scaled, scaled_rot);
757  AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
758  accel->last_stamp = stamp;
759 }
760 
761 static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
762 {
763  // Find the correct mag
764  struct imu_mag_t *mag = imu_get_mag(sender_id, true);
765  if(mag == NULL)
766  return;
767 
768  // Calculate current compensation
769  struct Int32Vect3 mag_correction;
770  mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
771  mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
772  mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
773 
774  // Copy last sample as unscaled
775  VECT3_COPY(mag->unscaled, *data);
776 
777  // Scale the mag
778  struct Int32Vect3 scaled;
779  scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale[0].x / mag->scale[1].x;
780  scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale[0].y / mag->scale[1].y;
781  scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale[0].z / mag->scale[1].z;
782 
783  // Rotate the sensor
784  int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
785  AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
786 }
787 
795 struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
796  // Find the correct gyro or create index
797  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
798  struct imu_gyro_t *gyro = NULL;
799  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
800  if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
801  gyro = &imu.gyros[i];
802  gyro->abi_id = sender_id;
803  break;
804  } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
805  gyro = &imu.gyros[i];
806  }
807  }
808  return gyro;
809 }
810 
818 struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
819  // Find the correct accel
820  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
821  struct imu_accel_t *accel = NULL;
822  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
823  if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
824  accel = &imu.accels[i];
825  accel->abi_id = sender_id;
826  break;
827  } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
828  accel = &imu.accels[i];
829  }
830  }
831 
832  return accel;
833 }
834 
842 struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
843  // Find the correct mag
844  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
845  struct imu_mag_t *mag = NULL;
846  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
847  if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
848  mag = &imu.mags[i];
849  mag->abi_id = sender_id;
850  break;
851  } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
852  mag = &imu.mags[i];
853  }
854  }
855 
856  return mag;
857 }
858 
864 static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
865 {
866  struct Int32RMat new_body_to_imu, diff_body_to_imu;
867  struct Int32Eulers body_to_imu_eulers_i;
868  // Convert to RMat
869  struct Int32RMat *old_body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
870  EULERS_BFP_OF_REAL(body_to_imu_eulers_i, *body_to_imu_eulers);
871  int32_rmat_of_eulers(&new_body_to_imu, &body_to_imu_eulers_i);
872 
873  // Calculate the difference between old and new
874  int32_rmat_comp_inv(&diff_body_to_imu, &new_body_to_imu, old_body_to_imu);
875 
876  // Apply the difference to all sensors
877  struct Int32RMat old_rmat;
878  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
879  old_rmat = imu.gyros[i].body_to_sensor;
880  int32_rmat_comp(&imu.gyros[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
881 
882  old_rmat = imu.accels[i].body_to_sensor;
883  int32_rmat_comp(&imu.accels[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
884 
885  old_rmat = imu.mags[i].body_to_sensor;
886  int32_rmat_comp(&imu.mags[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
887  }
888 
889  // Set the current body to imu
890  orientationSetEulers_f(&imu.body_to_imu, body_to_imu_eulers);
891 }
892 
893 void imu_SetBodyToImuPhi(float phi)
894 {
895  struct FloatEulers body_to_imu_eulers;
896  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
897  body_to_imu_eulers.phi = phi;
898  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
899 }
900 
902 {
903  struct FloatEulers body_to_imu_eulers;
904  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
905  body_to_imu_eulers.theta = theta;
906  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
907 }
908 
910 {
911  struct FloatEulers body_to_imu_eulers;
912  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
913  body_to_imu_eulers.psi = psi;
914  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
915 }
916 
917 void imu_SetBodyToImuCurrent(float set)
918 {
919  imu.b2i_set_current = set;
920 
921  if (imu.b2i_set_current) {
922  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
923  struct FloatEulers body_to_imu_eulers;
924  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
925  if (stateIsAttitudeValid()) {
926  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
927  body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
928  body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
929  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
930  } else {
931  // indicate that we couldn't set to current roll/pitch
932  imu.b2i_set_current = false;
933  }
934  } else {
935  // reset to BODY_TO_IMU as defined in airframe file
937  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
938  }
939 }
Main include for ABI (AirBorneInterface).
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition: abi_common.h:64
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
struct FloatVect3 accel_float
struct FloatVect3 mag_float
static const float scale[]
struct Electrical electrical
Definition: electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
float current
current in A
Definition: electrical.h:47
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
euler angles
rotation matrix
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:733
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:704
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
#define INT_RATES_ZERO(_e)
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
#define RATE_FLOAT_OF_BFP(_ai)
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
#define INT_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
#define ACCEL_FLOAT_OF_BFP(_ai)
euler angles
rotation matrix
angular rates
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#define IMU_MAG_ABI_SEND_ID
Which mag measurements to send over telemetry/logging.
Definition: imu.c:172
#define IMU_GYRO_ABI_SEND_ID
Which gyro measurements to send over telemetry/logging.
Definition: imu.c:160
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
Definition: imu.c:144
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:818
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:573
#define IMU_MAG_Y_SIGN
Definition: imu.c:120
#define IMU_BODY_TO_IMU_THETA
Definition: imu.c:151
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:273
#define IMU_GYRO_P_SIGN
By default gyro signs are positive for single IMU with old format or defaults.
Definition: imu.c:47
#define IMU_ACCEL_Y_SIGN
Definition: imu.c:85
static abi_event imu_accel_raw_ev
Definition: imu.c:338
static abi_event imu_mag_raw_ev
Definition: imu.c:338
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:795
#define IMU_MAG_X_SIGN
By default mag signs are positive for single IMU with old format and defaults.
Definition: imu.c:117
void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition: imu.c:551
void imu_SetBodyToImuTheta(float theta)
Definition: imu.c:901
#define IMU_ACCEL_X_SIGN
By default accel signs are positive for single IMU with old format and defaults.
Definition: imu.c:82
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians)
Definition: imu.c:150
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:667
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
Definition: imu.c:109
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition: imu.c:521
#define IMU_LOG_HIGHSPEED_DEVICE
By default log highspeed on the flightrecorder.
Definition: imu.c:178
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:864
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:287
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:317
struct Imu imu
global IMU state
Definition: imu.c:337
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition: imu.c:491
#define IMU_BODY_TO_IMU_PSI
Definition: imu.c:152
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
Definition: imu.c:74
#define IMU_MAG_Z_SIGN
Definition: imu.c:123
void imu_init(void)
External functions.
Definition: imu.c:344
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:909
static abi_event imu_gyro_raw_ev
Definition: imu.c:338
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:185
#define IMU_GYRO_R_SIGN
Definition: imu.c:53
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:917
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:243
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:257
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:893
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:213
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
Definition: imu.c:761
#define IMU_ACCEL_Z_SIGN
Definition: imu.c:88
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:199
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:301
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:842
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:229
#define IMU_ACCEL_ABI_SEND_ID
Which accel measurements to send over telemetry/logging.
Definition: imu.c:166
#define IMU_GYRO_Q_SIGN
Definition: imu.c:50
Inertial Measurement Unit interface.
uint32_t last_stamp
Last measurement timestamp for integration.
Definition: imu.h:51
struct Int32Rates scaled
Last scaled values in body frame.
Definition: imu.h:53
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition: imu.h:73
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:81
struct FloatVect3 current_scale
Current scaling multiplying.
Definition: imu.h:86
struct imu_accel_t accels[IMU_MAX_SENSORS]
The accelerometer sensors.
Definition: imu.h:95
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:50
bool scale
Scale calibrated.
Definition: imu.h:43
float temperature
Temperature in degrees celcius.
Definition: imu.h:55
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition: imu.h:76
float filter_freq
Lowpass filter frequency (Hz)
Definition: imu.h:74
uint32_t last_stamp
Last measurement timestamp for integration.
Definition: imu.h:66
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition: imu.h:60
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:84
uint8_t mag_abi_send_id
Filter out and send only a specific ABI id in telemetry for the magnetometer.
Definition: imu.h:100
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
Definition: imu.h:97
bool filter
Enable the lowpass filter.
Definition: imu.h:46
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition: imu.h:69
float temperature
Temperature in degrees celcius.
Definition: imu.h:70
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:56
bool current
Current calibrated.
Definition: imu.h:45
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition: imu.h:87
bool initialized
Check if the IMU is initialized.
Definition: imu.h:93
Butterworth2LowPass filter[3]
Lowpass filter optional.
Definition: imu.h:61
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:67
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:85
struct Int32Rates scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:57
float filter_sample_freq
Lowpass filter sample frequency (Hz)
Definition: imu.h:75
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition: imu.h:68
struct imu_gyro_t gyros[IMU_MAX_SENSORS]
The gyro sensors.
Definition: imu.h:94
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
Definition: imu.h:58
struct Int32Rates unscaled
Last unscaled values in sensor frame.
Definition: imu.h:54
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:80
struct imu_mag_t mags[IMU_MAX_SENSORS]
The magnetometer sensors.
Definition: imu.h:96
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:72
bool neutral
Neutral values calibrated.
Definition: imu.h:42
bool b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:105
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:71
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition: imu.h:83
float filter_freq
Filter frequency.
Definition: imu.h:59
#define IMU_MAX_SENSORS
Definition: imu.h:38
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:52
uint8_t accel_abi_send_id
Filter out and send only a specific ABI id in telemetry for the accelerometer.
Definition: imu.h:99
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition: imu.h:82
bool rotation
Rotation calibrated.
Definition: imu.h:44
uint8_t gyro_abi_send_id
Filter out and send only a specific ABI id in telemetry for the gyro.
Definition: imu.h:98
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:65
abstract IMU interface providing fixed point interface
Definition: imu.h:92
Definition: imu.h:49
Definition: imu.h:79
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
static TRICAL_instance_t mag_calib
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Definition: pprzlog_tp.c:29
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98