Paparazzi UAS  v6.2_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"
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}
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 #if PERIODIC_TELEMETRY
160 #include "modules/datalink/telemetry.h"
161 
162 static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
163 {
164  static uint8_t id = 0;
165  pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
166  &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
167  id++;
168  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
169  id = 0;
170 }
171 
172 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
173 {
174  static uint8_t id = 0;
175  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
176  &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
177  id++;
178  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
179  id = 0;
180 }
181 
182 static void send_accel(struct transport_tx *trans, struct link_device *dev)
183 {
184  static uint8_t id = 0;
185  struct FloatVect3 accel_float;
186  ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
187  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
188  &accel_float.x, &accel_float.y, &accel_float.z);
189  id++;
190  if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
191  id = 0;
192 }
193 
194 static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
195 {
196  static uint8_t id = 0;
197  pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
198  &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
199  id++;
200  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
201  id = 0;
202 }
203 
204 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
205 {
206  static uint8_t id = 0;
207  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
208  &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
209  id++;
210  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
211  id = 0;
212 }
213 
214 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
215 {
216  static uint8_t id = 0;
217  struct FloatRates gyro_float;
218  RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
219  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
220  &gyro_float.p, &gyro_float.q, &gyro_float.r);
221  id++;
222  if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
223  id = 0;
224 }
225 
226 static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
227 {
228  static uint8_t id = 0;
229  pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
230  &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
231  id++;
232  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
233  id = 0;
234 }
235 
236 static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
237 {
238  static uint8_t id = 0;
239  pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
240  &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
241  id++;
242  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
243  id = 0;
244 }
245 
246 static void send_mag(struct transport_tx *trans, struct link_device *dev)
247 {
248  static uint8_t id = 0;
249  struct FloatVect3 mag_float;
250  MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
251  pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
252  &mag_float.x, &mag_float.y, &mag_float.z);
253  id++;
254  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
255  id = 0;
256 }
257 
258 static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
259 {
260  static uint8_t id = 0;
261  pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
262  &imu.mags[id].abi_id,
263  &imu.mags[id].unscaled.x,
264  &imu.mags[id].unscaled.y,
265  &imu.mags[id].unscaled.z,
266  &electrical.current);
267  id++;
268  if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
269  id = 0;
270 }
271 
272 #endif /* PERIODIC_TELEMETRY */
273 
274 struct Imu imu = {0};
275 static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
276 static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp);
277 static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp);
278 static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
279 static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
280 
281 void imu_init(void)
282 {
283  // Set the Body to IMU rotation
284  struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
285  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
286  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
287 
288 
289  // Set the calibrated sensors
290  struct Int32RMat body_to_sensor;
291  const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
292  const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
293  const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
294  const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
295  const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
296  const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
297 
298  // Initialize the sensors to default values
299  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
300  /* Copy gyro calibration if needed */
301  if(i >= gyro_calib_len) {
302  imu.gyros[i].abi_id = ABI_DISABLE;
303  imu.gyros[i].calibrated.neutral = false;
304  imu.gyros[i].calibrated.scale = false;
305  imu.gyros[i].calibrated.rotation = false;
306  } else {
307  imu.gyros[i] = gyro_calib[i];
308  }
309 
310  // Set the default safe values if not calibrated
311  if(!imu.gyros[i].calibrated.neutral) {
312  INT_RATES_ZERO(imu.gyros[i].neutral);
313  }
314  if(!imu.gyros[i].calibrated.scale) {
315  RATES_ASSIGN(imu.gyros[i].scale[0], IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN);
316  RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1);
317  }
318  if(!imu.gyros[i].calibrated.rotation) {
319  int32_rmat_identity(&imu.gyros[i].body_to_sensor);
320  }
321  imu.gyros[i].last_stamp = 0;
322  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
323  RMAT_COPY(imu.gyros[i].body_to_sensor, body_to_sensor);
324 
325 
326  /* Copy accel calibration if needed */
327  if(i >= accel_calib_len) {
328  imu.accels[i].abi_id = ABI_DISABLE;
329  imu.accels[i].calibrated.neutral = false;
330  imu.accels[i].calibrated.scale = false;
331  imu.accels[i].calibrated.rotation = false;
332  } else {
333  imu.accels[i] = accel_calib[i];
334  }
335 
336  // Set the default safe values if not calibrated
337  if(!imu.accels[i].calibrated.neutral) {
338  INT_VECT3_ZERO(imu.accels[i].neutral);
339  }
340  if(!imu.accels[i].calibrated.scale) {
341  VECT3_ASSIGN(imu.accels[i].scale[0], IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN);
342  VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1);
343  }
344  if(!imu.accels[i].calibrated.rotation) {
345  int32_rmat_identity(&imu.accels[i].body_to_sensor);
346  }
347  imu.accels[i].last_stamp = 0;
348  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
349  RMAT_COPY(imu.accels[i].body_to_sensor, body_to_sensor);
350 
351 
352  /* Copy mag calibrated if needed */
353  if(i >= mag_calib_len) {
354  imu.mags[i].abi_id = ABI_DISABLE;
355  imu.mags[i].calibrated.neutral = false;
356  imu.mags[i].calibrated.scale = false;
357  imu.mags[i].calibrated.rotation = false;
358  imu.mags[i].calibrated.current = false;
359  } else {
360  imu.mags[i] = mag_calib[i];
361  }
362 
363  // Set the default safe values if not calibrated
364  if(!imu.mags[i].calibrated.neutral) {
365  INT_VECT3_ZERO(imu.mags[i].neutral);
366  }
367  if(!imu.mags[i].calibrated.scale) {
368  VECT3_ASSIGN(imu.mags[i].scale[0], IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN);
369  VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1);
370  }
371  if(!imu.mags[i].calibrated.rotation) {
372  int32_rmat_identity(&imu.mags[i].body_to_sensor);
373  }
374  if(!imu.mags[i].calibrated.current) {
375  INT_VECT3_ZERO(imu.mags[i].current_scale);
376  }
377  int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
378  RMAT_COPY(imu.mags[i].body_to_sensor, body_to_sensor);
379  }
380 
381  // Bind to raw measurements
382  AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
383  AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
384  AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
385 
386 #if PERIODIC_TELEMETRY
387  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
388  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
389  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
390  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_RAW, send_gyro_raw);
391  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
392  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
393  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
394  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
395  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
396  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
397 #endif // DOWNLINK
398 
399  imu.initialized = true;
400 }
401 
410 void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
411 {
412  // Find the correct gyro
413  struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
414  if(gyro == NULL)
415  return;
416 
417  // Copy the defaults
418  if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
419  struct Int32RMat body_to_sensor;
420  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
421  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
422  RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
423  }
424  if(neutral != NULL && !gyro->calibrated.neutral)
425  RATES_COPY(gyro->neutral, *neutral);
426  if(scale != NULL && !gyro->calibrated.scale) {
427  RATES_ASSIGN(gyro->scale[0], IMU_GYRO_P_SIGN*scale[0].p, IMU_GYRO_Q_SIGN*scale[0].q, IMU_GYRO_R_SIGN*scale[0].r);
428  RATES_COPY(gyro->scale[1], scale[1]);
429  }
430 }
431 
440 void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
441 {
442  // Find the correct accel
443  struct imu_accel_t *accel = imu_get_accel(abi_id, true);
444  if(accel == NULL)
445  return;
446 
447  // Copy the defaults
448  if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
449  struct Int32RMat body_to_sensor;
450  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
451  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
452  RMAT_COPY(accel->body_to_sensor, body_to_sensor);
453  }
454  if(neutral != NULL && !accel->calibrated.neutral)
455  VECT3_COPY(accel->neutral, *neutral);
456  if(scale != NULL && !accel->calibrated.scale) {
457  VECT3_ASSIGN(accel->scale[0], IMU_ACCEL_X_SIGN*scale[0].x, IMU_ACCEL_Y_SIGN*scale[0].y, IMU_ACCEL_Z_SIGN*scale[0].z);
458  VECT3_COPY(accel->scale[1], scale[1]);
459  }
460 }
461 
470 void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
471 {
472  // Find the correct mag
473  struct imu_mag_t *mag = imu_get_mag(abi_id, true);
474  if(mag == NULL)
475  return;
476 
477  // Copy the defaults
478  if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
479  struct Int32RMat body_to_sensor;
480  struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
481  int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
482  RMAT_COPY(mag->body_to_sensor, body_to_sensor);
483  }
484  if(neutral != NULL && !mag->calibrated.neutral)
485  VECT3_COPY(mag->neutral, *neutral);
486  if(scale != NULL && !mag->calibrated.scale) {
487  VECT3_ASSIGN(mag->scale[0], IMU_MAG_X_SIGN*scale[0].x, IMU_MAG_Y_SIGN*scale[0].y, IMU_MAG_Z_SIGN*scale[0].z);
488  VECT3_COPY(mag->scale[1], scale[1]);
489  }
490 }
491 
492 static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
493 {
494  // Find the correct gyro
495  struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
496  if(gyro == NULL || samples < 1)
497  return;
498 
499  // Copy last sample as unscaled
500  RATES_COPY(gyro->unscaled, data[samples-1]);
501 
502  // Scale the gyro
503  struct Int32Rates scaled, scaled_rot;
504  scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p;
505  scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q;
506  scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r;
507 
508  // Rotate the sensor
509  int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
510 
511 #if IMU_INTEGRATION
512  // Only integrate if we have gotten a previous measurement and didn't overflow the timer
513  if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
514  struct FloatRates integrated;
515  uint16_t dt = stamp - gyro->last_stamp;
516 
517  // Trapezoidal integration (TODO: coning correction)
518  integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
519  integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
520  integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
521 
522  // Only perform multiple integrations in sensor frame if needed
523  if(samples > 1) {
524  struct FloatRates integrated_sensor;
525  struct FloatRMat body_to_sensor;
526  // Rotate back to sensor frame
527  RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
528  float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
529 
530  // Add all the other samples
531  for(uint8_t i = 0; i < samples-1; i++) {
532  integrated_sensor.p += RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
533  integrated_sensor.q += RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
534  integrated_sensor.r += RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
535  }
536 
537  // Rotate to body frame
538  float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
539  }
540 
541  // Divide by the amount of samples and multiply by the delta time
542  integrated.p = integrated.p / samples * ((float)dt * 1e-6f);
543  integrated.q = integrated.q / samples * ((float)dt * 1e-6f);
544  integrated.r = integrated.r / samples * ((float)dt * 1e-6f);
545 
546  // Send the integrated values
547  AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
548  }
549 #endif
550 
551  // Copy and send
552  gyro->temperature = temp;
553  RATES_COPY(gyro->scaled, scaled_rot);
554  AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
555  gyro->last_stamp = stamp;
556 }
557 
558 static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp)
559 {
560  // Find the correct accel
561  struct imu_accel_t *accel = imu_get_accel(sender_id, true);
562  if(accel == NULL || samples < 1)
563  return;
564 
565  // Copy last sample as unscaled
566  VECT3_COPY(accel->unscaled, data[samples-1]);
567 
568  // Scale the accel
569  struct Int32Vect3 scaled, scaled_rot;
570  scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x;
571  scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y;
572  scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z;
573 
574  // Rotate the sensor
575  int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
576 
577 #if IMU_INTEGRATION
578  // Only integrate if we have gotten a previous measurement and didn't overflow the timer
579  if(accel->last_stamp > 0 && stamp > accel->last_stamp) {
580  struct FloatVect3 integrated;
581  uint16_t dt = stamp - accel->last_stamp;
582 
583  // Trapezoidal integration
584  integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
585  integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
586  integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
587 
588  // Only perform multiple integrations in sensor frame if needed
589  if(samples > 1) {
590  struct FloatVect3 integrated_sensor;
591  struct FloatRMat body_to_sensor;
592  // Rotate back to sensor frame
593  RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
594  float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
595 
596  // Add all the other samples
597  for(uint8_t i = 0; i < samples-1; i++) {
598  integrated_sensor.x += ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
599  integrated_sensor.y += ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
600  integrated_sensor.z += ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
601  }
602 
603  // Rotate to body frame
604  float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
605  }
606 
607  // Divide by the amount of samples and multiply by the delta time
608  integrated.x = integrated.x / samples * ((float)dt * 1e-6f);
609  integrated.y = integrated.y / samples * ((float)dt * 1e-6f);
610  integrated.z = integrated.z / samples * ((float)dt * 1e-6f);
611 
612  // Send the integrated values
613  AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
614  }
615 #endif
616 
617  // Copy and send
618  accel->temperature = temp;
619  VECT3_COPY(accel->scaled, scaled_rot);
620  AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
621  accel->last_stamp = stamp;
622 }
623 
624 static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
625 {
626  // Find the correct mag
627  struct imu_mag_t *mag = imu_get_mag(sender_id, true);
628  if(mag == NULL)
629  return;
630 
631  // Calculate current compensation
632  struct Int32Vect3 mag_correction;
633  mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
634  mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
635  mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
636 
637  // Copy last sample as unscaled
638  VECT3_COPY(mag->unscaled, *data);
639 
640  // Scale the mag
641  struct Int32Vect3 scaled;
642  scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale[0].x / mag->scale[1].x;
643  scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale[0].y / mag->scale[1].y;
644  scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale[0].z / mag->scale[1].z;
645 
646  // Rotate the sensor
647  int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
648  AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
649 }
650 
658 struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
659  // Find the correct gyro or create index
660  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
661  struct imu_gyro_t *gyro = NULL;
662  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
663  if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
664  gyro = &imu.gyros[i];
665  gyro->abi_id = sender_id;
666  break;
667  } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
668  gyro = &imu.gyros[i];
669  }
670  }
671  return gyro;
672 }
673 
681 struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
682  // Find the correct accel
683  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
684  struct imu_accel_t *accel = NULL;
685  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
686  if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
687  accel = &imu.accels[i];
688  accel->abi_id = sender_id;
689  break;
690  } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
691  accel = &imu.accels[i];
692  }
693  }
694 
695  return accel;
696 }
697 
705 struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
706  // Find the correct mag
707  // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
708  struct imu_mag_t *mag = NULL;
709  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
710  if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
711  mag = &imu.mags[i];
712  mag->abi_id = sender_id;
713  break;
714  } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
715  mag = &imu.mags[i];
716  }
717  }
718 
719  return mag;
720 }
721 
727 static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
728 {
729  struct Int32RMat new_body_to_imu, diff_body_to_imu;
730  struct Int32Eulers body_to_imu_eulers_i;
731  // Convert to RMat
732  struct Int32RMat *old_body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
733  EULERS_BFP_OF_REAL(body_to_imu_eulers_i, *body_to_imu_eulers);
734  int32_rmat_of_eulers(&new_body_to_imu, &body_to_imu_eulers_i);
735 
736  // Calculate the difference between old and new
737  int32_rmat_comp_inv(&diff_body_to_imu, &new_body_to_imu, old_body_to_imu);
738 
739  // Apply the difference to all sensors
740  struct Int32RMat old_rmat;
741  for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
742  old_rmat = imu.gyros[i].body_to_sensor;
743  int32_rmat_comp(&imu.gyros[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
744 
745  old_rmat = imu.accels[i].body_to_sensor;
746  int32_rmat_comp(&imu.accels[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
747 
748  old_rmat = imu.mags[i].body_to_sensor;
749  int32_rmat_comp(&imu.mags[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
750  }
751 
752  // Set the current body to imu
753  orientationSetEulers_f(&imu.body_to_imu, body_to_imu_eulers);
754 }
755 
756 void imu_SetBodyToImuPhi(float phi)
757 {
758  struct FloatEulers body_to_imu_eulers;
759  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
760  body_to_imu_eulers.phi = phi;
761  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
762 }
763 
764 void imu_SetBodyToImuTheta(float theta)
765 {
766  struct FloatEulers body_to_imu_eulers;
767  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
768  body_to_imu_eulers.theta = theta;
769  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
770 }
771 
772 void imu_SetBodyToImuPsi(float psi)
773 {
774  struct FloatEulers body_to_imu_eulers;
775  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
776  body_to_imu_eulers.psi = psi;
777  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
778 }
779 
780 void imu_SetBodyToImuCurrent(float set)
781 {
782  imu.b2i_set_current = set;
783 
784  if (imu.b2i_set_current) {
785  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
786  struct FloatEulers body_to_imu_eulers;
787  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
788  if (stateIsAttitudeValid()) {
789  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
790  body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
791  body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
792  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
793  } else {
794  // indicate that we couldn't set to current roll/pitch
795  imu.b2i_set_current = false;
796  }
797  } else {
798  // reset to BODY_TO_IMU as defined in airframe file
799  struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
800  imu_set_body_to_imu_eulers(&body_to_imu_eulers);
801  }
802 }
imu_gyro_t::last_stamp
uint32_t last_stamp
Last measurement timestamp for integration.
Definition: imu.h:49
electrical.h
mag_calib
static TRICAL_instance_t mag_calib
Definition: mag_calib_ukf.c:123
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
IMU_BODY_TO_IMU_THETA
#define IMU_BODY_TO_IMU_THETA
Definition: imu.c:151
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
imu_gyro_raw_cb
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
Definition: imu.c:492
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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
IMU_BODY_TO_IMU_PSI
#define IMU_BODY_TO_IMU_PSI
Definition: imu.c:152
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
imu_calib_t::neutral
bool neutral
Neutral values calibrated.
Definition: imu.h:41
imu_accel_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:62
IMU_MAG_Y_SIGN
#define IMU_MAG_Y_SIGN
Definition: imu.c:120
imu_mag_t::unscaled
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition: imu.h:75
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 Int32Vect3 *scale)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition: imu.c:470
IMU_GYRO_CALIB
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
Definition: imu.c:74
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
INT_RATES_ZERO
#define INT_RATES_ZERO(_e)
Definition: pprz_algebra_int.h:576
abi.h
Imu::gyros
struct imu_gyro_t gyros[IMU_MAX_SENSORS]
The gyro sensors.
Definition: imu.h:86
scale
static const float scale[]
Definition: dw1000_arduino.c:200
int32_rmat_identity
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
Definition: pprz_algebra_int.h:347
imu_accel_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:60
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
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:658
Imu::b2i_set_current
bool b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:94
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
send_gyro_scaled
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:204
imu_mag_t::scaled
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition: imu.h:74
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
imu_SetBodyToImuTheta
void imu_SetBodyToImuTheta(float theta)
Definition: imu.c:764
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
send_accel_scaled
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:172
Imu::body_to_imu
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
Definition: imu.h:89
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:705
Imu::accels
struct imu_accel_t accels[IMU_MAX_SENSORS]
The accelerometer sensors.
Definition: imu.h:87
send_accel_raw
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:162
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:117
Imu::initialized
bool initialized
Check if the IMU is initialized.
Definition: imu.h:85
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:681
IMU_MAG_CALIB
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
Definition: imu.c:144
imu_accel_t::neutral
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:66
imu_gyro_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:50
mag_float
struct FloatVect3 mag_float
Definition: ahrs_float_dcm.c:60
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:727
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
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:82
FloatVect3
Definition: pprz_algebra_float.h:54
send_mag
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:246
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
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
imu.h
imu_SetBodyToImuPhi
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:756
Imu
abstract IMU interface providing fixed point interface
Definition: imu.h:84
RATES_ASSIGN
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
imu_mag_raw_ev
static abi_event imu_mag_raw_ev
Definition: imu.c:275
imu_accel_t::scale
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:67
IMU_GYRO_R_SIGN
#define IMU_GYRO_R_SIGN
Definition: imu.c:53
RMAT_COPY
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:704
ACCEL_FLOAT_OF_BFP
#define ACCEL_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:222
imu_accel_t::last_stamp
uint32_t last_stamp
Last measurement timestamp for integration.
Definition: imu.h:61
imu_mag_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:72
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:56
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
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
send_mag_raw
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:226
imu_gyro_raw_ev
static abi_event imu_gyro_raw_ev
Definition: imu.c:275
imu_accel_raw_ev
static abi_event imu_accel_raw_ev
Definition: imu.c:275
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateIsAttitudeValid
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
imu_calib_t::current
bool current
Current calibrated.
Definition: imu.h:44
imu_mag_t
Definition: imu.h:71
Int32Vect3
Definition: pprz_algebra_int.h:88
IMU_MAX_SENSORS
#define IMU_MAX_SENSORS
Definition: imu.h:37
IMU_GYRO_Q_SIGN
#define IMU_GYRO_Q_SIGN
Definition: imu.c:50
imu_mag_t::scale
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:77
RMAT_FLOAT_OF_BFP
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:733
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
send_gyro
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:214
imu_mag_t::calibrated
struct imu_calib_t calibrated
Calibration bitmask.
Definition: imu.h:73
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
imu_gyro_t::temperature
float temperature
Temperature in degrees celcius.
Definition: imu.h:53
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:409
imu
struct Imu imu
global IMU state
Definition: imu.c:274
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
imu_gyro_t::abi_id
uint8_t abi_id
ABI sensor ID.
Definition: imu.h:48
imu_gyro_t::scale
struct Int32Rates scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:55
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
imu_accel_raw_cb
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp)
Definition: imu.c:558
imu_calib_t::scale
bool scale
Scale calibrated.
Definition: imu.h:42
imu_mag_t::neutral
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:76
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
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
imu_accel_t
Definition: imu.h:59
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:68
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 Int32Vect3 *scale)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition: imu.c:440
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
imu_init
void imu_init(void)
External functions.
Definition: imu.c:281
Imu::mags
struct imu_mag_t mags[IMU_MAX_SENSORS]
The magnetometer sensors.
Definition: imu.h:88
imu_gyro_t::unscaled
struct Int32Rates unscaled
Last unscaled values in sensor frame.
Definition: imu.h:52
IMU_ACCEL_CALIB
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
Definition: imu.c:109
IMU_BODY_TO_IMU_PHI
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians)
Definition: imu.c:150
IMU_MAG_Z_SIGN
#define IMU_MAG_Z_SIGN
Definition: imu.c:123
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:79
imu_gyro_t::scaled
struct Int32Rates scaled
Last scaled values in body frame.
Definition: imu.h:51
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
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 Int32Rates *scale)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition: imu.c:410
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
send_gyro_raw
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:194
imu_accel_t::scaled
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition: imu.h:63
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
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
imu_gyro_t
Definition: imu.h:47
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
send_mag_current
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:258
send_mag_scaled
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:236
imu_SetBodyToImuCurrent
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:780
INT_VECT3_ZERO
#define INT_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:288
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
accel_float
struct FloatVect3 accel_float
Definition: ahrs_float_dcm.c:59
imu_accel_t::unscaled
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition: imu.h:64
send_accel
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:182
electrical
struct Electrical electrical
Definition: electrical.c:66
IMU_ACCEL_Y_SIGN
#define IMU_ACCEL_Y_SIGN
Definition: imu.c:85
state.h
imu_accel_t::temperature
float temperature
Temperature in degrees celcius.
Definition: imu.h:65
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
imu_calib_t::rotation
bool rotation
Rotation calibrated.
Definition: imu.h:43
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
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
RATE_FLOAT_OF_BFP
#define RATE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:210
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
imu_SetBodyToImuPsi
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:772
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_Z_SIGN
#define IMU_ACCEL_Z_SIGN
Definition: imu.c:88
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
p
static float p[2][2]
Definition: ins_alt_float.c:257
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
orientationGetEulers_f
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
Definition: pprz_orientation_conversion.h:243
imu_gyro_t::neutral
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:54
ABI_DISABLE
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition: abi_common.h:63
Electrical::current
float current
current in A
Definition: electrical.h:46
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
imu_mag_t::current_scale
struct FloatVect3 current_scale
Current scaling multiplying.
Definition: imu.h:78
imu_mag_raw_cb
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
Definition: imu.c:624