Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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;
62  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
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;
102  pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
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
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);
145  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
146 #endif // DOWNLINK
147 
148  imu_impl_init();
149 }
150 
151 
153 {
154  struct FloatEulers body_to_imu_eulers;
155  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
156  body_to_imu_eulers.phi = phi;
157  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
158  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
159 }
160 
162 {
163  struct FloatEulers body_to_imu_eulers;
164  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
165  body_to_imu_eulers.theta = theta;
166  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
167  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
168 }
169 
171 {
172  struct FloatEulers body_to_imu_eulers;
173  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
174  body_to_imu_eulers.psi = psi;
175  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
176  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
177 }
178 
179 void imu_SetBodyToImuCurrent(float set)
180 {
181  imu.b2i_set_current = set;
182 
183  if (imu.b2i_set_current) {
184  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
185  struct FloatEulers body_to_imu_eulers;
186  body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
187  if (stateIsAttitudeValid()) {
188  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
189  body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
190  body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
191  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
192  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
193  } else {
194  // indicate that we couldn't set to current roll/pitch
196  }
197  } else {
198  // reset to BODY_TO_IMU as defined in airframe file
199  struct FloatEulers body_to_imu_eulers =
201  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
202  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
203  }
204 }
205 
206 
207 // weak functions, used if not explicitly provided by implementation
208 
209 void WEAK imu_periodic(void)
210 {
211 }
212 
213 void WEAK imu_scale_gyro(struct Imu *_imu)
214 {
215  RATES_COPY(_imu->gyro_prev, _imu->gyro);
216  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
218  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
220  _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
222 }
223 
224 void WEAK imu_scale_accel(struct Imu *_imu)
225 {
226  VECT3_COPY(_imu->accel_prev, _imu->accel);
227  _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN *
229  _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN *
231  _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN *
233 }
234 
235 #if defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
236 #include "subsystems/electrical.h"
237 void WEAK imu_scale_mag(struct Imu *_imu)
238 {
239  struct Int32Vect3 mag_correction;
240  mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current);
241  mag_correction.y = (int32_t)(IMU_MAG_Y_CURRENT_COEF * (float) electrical.current);
242  mag_correction.z = (int32_t)(IMU_MAG_Z_CURRENT_COEF * (float) electrical.current);
243  _imu->mag.x = (((_imu->mag_unscaled.x - mag_correction.x) - _imu->mag_neutral.x) *
245  _imu->mag.y = (((_imu->mag_unscaled.y - mag_correction.y) - _imu->mag_neutral.y) *
247  _imu->mag.z = (((_imu->mag_unscaled.z - mag_correction.z) - _imu->mag_neutral.z) *
249 }
250 #elif USE_MAGNETOMETER
251 void WEAK imu_scale_mag(struct Imu *_imu)
252 {
253  _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN *
255  _imu->mag.y = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN *
257  _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN *
259 }
260 #else
261 void WEAK imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
262 #endif /* MAG_x_CURRENT_COEF */
#define IMU_GYRO_Q_SENS_DEN
Definition: imu_apogee.h:63
int32_t current
current in milliamps
Definition: electrical.h:49
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:83
#define IMU_ACCEL_X_SIGN
Definition: imu_apogee.h:46
float phi
in radians
Generic transmission transport header.
Definition: transport.h:89
void WEAK imu_scale_gyro(struct Imu *_imu)
Definition: imu.c:213
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
struct FloatVect3 accel_float
#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
static bool_t stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1038
#define IMU_ACCEL_Z_SIGN
Definition: imu_apogee.h:48
struct Int32Vect3 accel_neutral
accelerometer bias
Definition: imu.h:48
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:45
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:41
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
Main include for ABI (AirBorneInterface).
void WEAK imu_scale_accel(struct Imu *_imu)
Definition: imu.c:224
struct FloatVect3 mag_float
#define IMU_GYRO_Q_SENS_NUM
Definition: imu_apogee.h:62
float psi
in radians
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:152
#define IMU_GYRO_P_SENS_NUM
Definition: imu_apogee.h:59
void WEAK imu_scale_mag(struct Imu *_imu)
Definition: imu.c:251
#define IMU_ACCEL_Y_SENS_DEN
Definition: imu_apogee.h:86
struct Imu imu
global IMU state
Definition: imu.c:108
#define IMU_GYRO_R_SENS_NUM
Definition: imu_apogee.h:65
#define IMU_MAG_Z_SIGN
Definition: imu_hbmini.h:57
#define IMU_GYRO_P_NEUTRAL
Definition: imu_apogee.h:69
#define FALSE
Definition: std.h:5
bool_t b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:58
#define IMU_ACCEL_Y_SIGN
Definition: imu_apogee.h:47
euler angles
struct Int32Rates gyro_neutral
gyroscope bias
Definition: imu.h:47
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:52
float theta
in radians
void imu_impl_init(void)
must be defined by underlying hardware
Definition: imu_apogee.c:79
int32_t r
in rad/s with INT32_RATE_FRAC
#define IMU_MAG_Y_SENS_NUM
Definition: imu_ardrone2.h:75
#define IMU_MAG_X_SENS_NUM
Definition: imu_ardrone2.h:72
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:50
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:43
#define IMU_GYRO_P_SENS_DEN
Definition: imu_apogee.h:60
#define IMU_GYRO_R_SIGN
Definition: imu_apogee.h:43
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:692
Interface for electrical status: supply voltage, current, battery status, etc.
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias)
Definition: imu.h:49
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define IMU_ACCEL_Z_SENS_DEN
Definition: imu_apogee.h:89
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:44
Inertial Measurement Unit interface.
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:728
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:53
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:69
#define IMU_ACCEL_Z_NEUTRAL
Definition: imu_apogee.h:94
#define INT_VECT3_ZERO(_v)
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:179
#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:83
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:46
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:170
int32_t p
in rad/s with INT32_RATE_FRAC
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:51
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:740
#define IMU_ACCEL_Y_NEUTRAL
Definition: imu_apogee.h:93
void WEAK imu_periodic(void)
optional.
Definition: imu.c:209
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:41
#define IMU_ACCEL_Y_SENS_NUM
Definition: imu_apogee.h:85
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:84
#define IMU_GYRO_R_SENS_DEN
Definition: imu_apogee.h:66
void imu_init(void)
Definition: imu.c:110
#define IMU_GYRO_Q_NEUTRAL
Definition: imu_apogee.h:70
#define IMU_GYRO_Q_SIGN
Definition: imu_apogee.h:42
#define IMU_ACCEL_Z_SENS_NUM
Definition: imu_apogee.h:88
int32_t q
in rad/s with INT32_RATE_FRAC
#define IMU_GYRO_R_NEUTRAL
Definition: imu_apogee.h:71
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:82
angular rates
void gpio_setup_output(uint32_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_ardrone.c:102
#define IMU_ACCEL_X_NEUTRAL
Definition: imu_apogee.h:92
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:161
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
#define IMU_ACCEL_X_SENS_NUM
Definition: imu_apogee.h:82
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:42
#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).