Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 #ifndef IMU_H
28 #define IMU_H
29 
30 #include "math/pprz_algebra_int.h"
33 #include "generated/airframe.h"
34 
35 /* must be defined by underlying hardware */
36 extern void imu_impl_init(void);
37 extern void imu_periodic(void);
38 
40 struct Imu {
41  struct Int32Rates gyro;
42  struct Int32Vect3 accel;
43  struct Int32Vect3 mag;
53 
58 };
59 
61 struct ImuFloat {
62  struct FloatRates gyro;
63  struct FloatVect3 accel;
64  struct FloatVect3 mag;
68 };
69 
70 
71 
73 extern struct Imu imu;
74 extern struct ImuFloat imuf;
75 
76 /* underlying hardware */
77 #ifdef IMU_TYPE_H
78 #include IMU_TYPE_H
79 #endif
80 
81 extern void imu_init(void);
82 extern void imu_float_init(void);
83 extern void imu_SetBodyToImuPhi(float phi);
84 extern void imu_SetBodyToImuTheta(float theta);
85 extern void imu_SetBodyToImuPsi(float psi);
86 extern void imu_SetBodyToImuCurrent(float set);
87 extern void imu_ResetBodyToImu(float reset);
88 
89 #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
90 #define IMU_BODY_TO_IMU_PHI 0
91 #define IMU_BODY_TO_IMU_THETA 0
92 #define IMU_BODY_TO_IMU_PSI 0
93 #endif
94 
95 #if !defined IMU_GYRO_P_NEUTRAL && !defined IMU_GYRO_Q_NEUTRAL && !defined IMU_GYRO_R_NEUTRAL
96 #define IMU_GYRO_P_NEUTRAL 0
97 #define IMU_GYRO_Q_NEUTRAL 0
98 #define IMU_GYRO_R_NEUTRAL 0
99 #endif
100 
101 #if !defined IMU_ACCEL_X_NEUTRAL && !defined IMU_ACCEL_Y_NEUTRAL && !defined IMU_ACCEL_Z_NEUTRAL
102 #define IMU_ACCEL_X_NEUTRAL 0
103 #define IMU_ACCEL_Y_NEUTRAL 0
104 #define IMU_ACCEL_Z_NEUTRAL 0
105 #endif
106 
107 
108 #ifndef ImuScaleGyro
109 #define ImuScaleGyro(_imu) { \
110  RATES_COPY(_imu.gyro_prev, _imu.gyro); \
111  _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
112  _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
113  _imu.gyro.r = ((_imu.gyro_unscaled.r - _imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
114  }
115 #endif
116 
117 
118 #ifndef ImuScaleAccel
119 #define ImuScaleAccel(_imu) { \
120  VECT3_COPY(_imu.accel_prev, _imu.accel); \
121  _imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \
122  _imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \
123  _imu.accel.z = ((_imu.accel_unscaled.z - _imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN; \
124  }
125 #endif
126 
127 #ifndef ImuScaleMag
128 #if defined IMU_MAG_45_HACK
129 #define ImuScaleMag(_imu) { \
130  int32_t msx = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
131  int32_t msy = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
132  _imu.mag.x = msx - msy; \
133  _imu.mag.y = msx + msy; \
134  _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
135  }
136 #elif defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
137 #define ImuScaleMag(_imu) { \
138  struct Int32Vect3 _mag_correction; \
139  _mag_correction.x = (int32_t) (IMU_MAG_X_CURRENT_COEF * (float) electrical.current); \
140  _mag_correction.y = (int32_t) (IMU_MAG_Y_CURRENT_COEF * (float) electrical.current); \
141  _mag_correction.z = (int32_t) (IMU_MAG_Z_CURRENT_COEF * (float) electrical.current); \
142  _imu.mag.x = (((_imu.mag_unscaled.x - _mag_correction.x) - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
143  _imu.mag.y = (((_imu.mag_unscaled.y - _mag_correction.y) - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
144  _imu.mag.z = (((_imu.mag_unscaled.z - _mag_correction.z) - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
145  }
146 #else
147 #define ImuScaleMag(_imu) { \
148  _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
149  _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
150  _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
151  }
152 #endif //IMU_MAG_45_HACK
153 #endif //ImuScaleMag
154 
155 
156 #endif /* IMU_H */
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:185
void reset(void)
void imu_SetBodyToImuTheta(float theta)
Definition: imu.c:171
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:66
angular rates
struct FloatRates gyro
Definition: imu.h:62
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
abstract IMU interface providing fixed point interface
Definition: imu.h:40
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:44
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:178
struct Int32Rates gyro_neutral
gyroscope bias
Definition: imu.h:46
void imu_ResetBodyToImu(float reset)
struct FloatVect3 accel
Definition: imu.h:63
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias)
Definition: imu.h:48
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
Paparazzi floating point algebra.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
struct FloatRates gyro_prev
Definition: imu.h:65
void imu_float_init(void)
Definition: imu.c:158
unsigned long uint32_t
Definition: types.h:18
struct Int32Vect3 accel_prev
previous accelerometer measurements
Definition: imu.h:45
angular rates
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
struct FloatVect3 mag
Definition: imu.h:64
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:164
struct Int32Vect3 accel_neutral
accelerometer bias
Definition: imu.h:47
bool_t b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:57
void imu_periodic(void)
Handle all the periodic tasks of the Navstik IMU components.
Definition: imu_apogee.c:87
struct ImuFloat imuf
Definition: imu.c:111
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
Generic orientation representation and conversion.
abstract IMU interface providing floating point interface
Definition: imu.h:61
void imu_init(void)
Definition: imu.c:113
void imu_impl_init(void)
Navstik IMU initializtion of the MPU-60x0 and HMC58xx.
Definition: imu_apogee.c:68
Paparazzi fixed point algebra.
uint32_t sample_count
Definition: imu.h:67