Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #ifdef BOARD_CONFIG
28 #include BOARD_CONFIG
29 #endif
30 
31 #include "subsystems/imu.h"
32 #include "state.h"
33 #include "subsystems/abi.h"
34 
35 #ifdef IMU_POWER_GPIO
36 #include "mcu_periph/gpio.h"
37 
38 #ifndef IMU_POWER_GPIO_ON
39 #define IMU_POWER_GPIO_ON gpio_set
40 #endif
41 #endif
42 
43 #if PERIODIC_TELEMETRY
45 
46 static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
47 {
48  pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID,
50 }
51 
52 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
53 {
54  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
55  &imu.accel.x, &imu.accel.y, &imu.accel.z);
56 }
57 
58 static void send_accel(struct transport_tx *trans, struct link_device *dev)
59 {
60  struct FloatVect3 accel_float;
61  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
62  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
63  &accel_float.x, &accel_float.y, &accel_float.z);
64 }
65 
66 static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
67 {
68  pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID,
70 }
71 
72 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
73 {
74  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
75  &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
76 }
77 
78 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
79 {
80  struct FloatRates gyro_float;
81  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
82  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
83  &gyro_float.p, &gyro_float.q, &gyro_float.r);
84 }
85 
86 static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
87 {
88  pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID,
90 }
91 
92 static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
93 {
94  pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID,
95  &imu.mag.x, &imu.mag.y, &imu.mag.z);
96 }
97 
98 static void send_mag(struct transport_tx *trans, struct link_device *dev)
99 {
100  struct FloatVect3 mag_float;
101  MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
102  pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
103  &mag_float.x, &mag_float.y, &mag_float.z);
104 }
105 
106 #endif /* PERIODIC_TELEMETRY */
107 
108 struct Imu imu;
109 
110 void imu_init(void)
111 {
112 
113 #ifdef IMU_POWER_GPIO
115  IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
116 #endif
117 
118  /* initialises neutrals */
120 
122 
123 #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
124  VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
125 #else
126 #if USE_MAGNETOMETER && (!defined MAG_CALIB_UKF_H)
127  INFO("Magnetometer neutrals are set to zero, you should calibrate!")
128 #endif
130 #endif
131 
132  struct FloatEulers body_to_imu_eulers =
134  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
135 
136 #if PERIODIC_TELEMETRY
137  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
138  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
139  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
140  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_RAW, send_gyro_raw);
141  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
142  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
143  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
144  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
146 #endif // DOWNLINK
147 
148 }
149 
150 
152 {
153  struct FloatEulers body_to_imu_eulers;
154  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
155  body_to_imu_eulers.phi = phi;
156  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
157  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
158 }
159 
161 {
162  struct FloatEulers body_to_imu_eulers;
163  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
164  body_to_imu_eulers.theta = theta;
165  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
166  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
167 }
168 
170 {
171  struct FloatEulers body_to_imu_eulers;
172  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
173  body_to_imu_eulers.psi = psi;
174  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
175  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
176 }
177 
178 void imu_SetBodyToImuCurrent(float set)
179 {
180  imu.b2i_set_current = set;
181 
182  if (imu.b2i_set_current) {
183  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
184  struct FloatEulers body_to_imu_eulers;
185  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
186  if (stateIsAttitudeValid()) {
187  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
188  body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
189  body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
190  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
191  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
192  } else {
193  // indicate that we couldn't set to current roll/pitch
194  imu.b2i_set_current = false;
195  }
196  } else {
197  // reset to BODY_TO_IMU as defined in airframe file
198  struct FloatEulers body_to_imu_eulers =
200  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
201  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
202  }
203 }
204 
205 
206 // weak functions, used if not explicitly provided by implementation
207 
208 void WEAK imu_scale_gyro(struct Imu *_imu)
209 {
210  RATES_COPY(_imu->gyro_prev, _imu->gyro);
211  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
213  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
215  _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
217 }
218 
219 void WEAK imu_scale_accel(struct Imu *_imu)
220 {
221  VECT3_COPY(_imu->accel_prev, _imu->accel);
222  _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN *
224  _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN *
226  _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN *
228 }
229 
230 #if !defined SITL && defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
231 #include "subsystems/electrical.h"
232 void WEAK imu_scale_mag(struct Imu *_imu)
233 {
234  struct Int32Vect3 mag_correction;
235  mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current);
236  mag_correction.y = (int32_t)(IMU_MAG_Y_CURRENT_COEF * (float) electrical.current);
237  mag_correction.z = (int32_t)(IMU_MAG_Z_CURRENT_COEF * (float) electrical.current);
238  _imu->mag.x = (((_imu->mag_unscaled.x - mag_correction.x) - _imu->mag_neutral.x) *
240  _imu->mag.y = (((_imu->mag_unscaled.y - mag_correction.y) - _imu->mag_neutral.y) *
242  _imu->mag.z = (((_imu->mag_unscaled.z - mag_correction.z) - _imu->mag_neutral.z) *
244 }
245 #elif USE_MAGNETOMETER
246 void WEAK imu_scale_mag(struct Imu *_imu)
247 {
248  _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN *
250  _imu->mag.y = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN *
252  _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN *
254 }
255 #else
256 void WEAK imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
257 #endif /* MAG_x_CURRENT_COEF */
#define IMU_GYRO_Q_SENS_DEN
Definition: imu_apogee.h:68
int32_t current
current in milliamps
Definition: electrical.h:49
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:79
#define IMU_ACCEL_X_SIGN
Definition: imu_apogee.h:49
float phi
in radians
void WEAK imu_scale_gyro(struct Imu *_imu)
Definition: imu.c:208
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#define IMU_MAG_Z_SENS_DEN
Definition: imu_ardrone2.h:79
Periodic telemetry system header (includes downlink utility and generated code).
#define IMU_MAG_X_SIGN
Definition: imu_hbmini.h:55
#define IMU_ACCEL_Z_SIGN
Definition: imu_apogee.h:51
struct Int32Vect3 accel_neutral
static accelerometer bias from calibration in raw/unscaled units
Definition: imu.h:44
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:41
Some architecture independent helper functions for GPIOs.
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
#define IMU_GYRO_P_SIGN
Definition: imu_apogee.h:44
float r
in rad/s
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:58
Main include for ABI (AirBorneInterface).
bool b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:54
void WEAK imu_scale_accel(struct Imu *_imu)
Definition: imu.c:219
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:86
#define IMU_GYRO_Q_SENS_NUM
Definition: imu_apogee.h:67
float psi
in radians
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:151
#define IMU_GYRO_P_SENS_NUM
Definition: imu_apogee.h:64
void WEAK imu_scale_mag(struct Imu *_imu)
Definition: imu.c:246
#define IMU_ACCEL_Y_SENS_DEN
Definition: imu_apogee.h:87
struct Imu imu
global IMU state
Definition: imu.c:108
#define IMU_GYRO_R_SENS_NUM
Definition: imu_apogee.h:70
float q
in rad/s
float p
in rad/s
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
#define IMU_MAG_Z_SIGN
Definition: imu_hbmini.h:57
#define IMU_GYRO_P_NEUTRAL
Definition: imu_apogee.h:74
static void send_mag(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:98
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:72
#define IMU_ACCEL_Y_SIGN
Definition: imu_apogee.h:50
euler angles
struct Int32Rates gyro_neutral
static gyroscope bias from calibration in raw/unscaled units
Definition: imu.h:43
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
float theta
in radians
int32_t r
in rad/s with INT32_RATE_FRAC
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:46
#define IMU_MAG_Y_SENS_NUM
Definition: imu_ardrone2.h:75
#define IMU_MAG_X_SENS_NUM
Definition: imu_ardrone2.h:72
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:78
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
#define IMU_GYRO_P_SENS_DEN
Definition: imu_apogee.h:65
#define IMU_GYRO_R_SIGN
Definition: imu_apogee.h:46
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:703
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
Interface for electrical status: supply voltage, current, battery status, etc.
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias) in raw/unscaled units
Definition: imu.h:45
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:66
#define IMU_ACCEL_Z_SENS_DEN
Definition: imu_apogee.h:90
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:739
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
signed long int32_t
Definition: types.h:19
#define IMU_MAG_X_SENS_DEN
Definition: imu_ardrone2.h:73
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define IMU_ACCEL_Z_NEUTRAL
Definition: imu_apogee.h:95
#define INT_VECT3_ZERO(_v)
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:178
#define IMU_MAG_Y_SIGN
Definition: imu_hbmini.h:56
API to get/set the generic vehicle states.
#define IMU_ACCEL_X_SENS_DEN
Definition: imu_apogee.h:84
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
#define IMU_MAG_Z_SENS_NUM
Definition: imu_ardrone2.h:78
struct Int32Vect3 accel_prev
previous accelerometer measurements
Definition: imu.h:42
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:169
int32_t p
in rad/s with INT32_RATE_FRAC
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:751
#define IMU_ACCEL_Y_NEUTRAL
Definition: imu_apogee.h:94
struct Electrical electrical
Definition: electrical.c:65
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
abstract IMU interface providing fixed point interface
Definition: imu.h:37
#define IMU_ACCEL_Y_SENS_NUM
Definition: imu_apogee.h:86
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:80
#define IMU_GYRO_R_SENS_DEN
Definition: imu_apogee.h:71
void imu_init(void)
Definition: imu.c:110
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:92
#define IMU_GYRO_Q_NEUTRAL
Definition: imu_apogee.h:75
#define IMU_GYRO_Q_SIGN
Definition: imu_apogee.h:45
#define IMU_ACCEL_Z_SENS_NUM
Definition: imu_apogee.h:89
int32_t q
in rad/s with INT32_RATE_FRAC
#define IMU_GYRO_R_NEUTRAL
Definition: imu_apogee.h:76
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:78
angular rates
#define IMU_ACCEL_X_NEUTRAL
Definition: imu_apogee.h:93
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void imu_SetBodyToImuTheta(float theta)
Definition: imu.c:160
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
#define IMU_ACCEL_X_SENS_NUM
Definition: imu_apogee.h:83
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: imu.c:52
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
#define IMU_MAG_Y_SENS_DEN
Definition: imu_ardrone2.h:76
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).