Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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"
32 #include "generated/airframe.h"
33 
34 /* must be defined by underlying hardware */
35 extern void imu_impl_init(void);
36 extern void imu_periodic(void);
37 
39 struct Imu {
40  struct Int32Rates gyro;
41  struct Int32Vect3 accel;
42  struct Int32Vect3 mag;
53 };
54 
56 struct ImuFloat {
57  struct FloatRates gyro;
58  struct FloatVect3 accel;
59  struct FloatVect3 mag;
65 };
66 
67 extern void imu_float_init(struct ImuFloat* imuf);
68 
70 extern struct Imu imu;
71 
72 /* underlying hardware */
73 #ifdef IMU_TYPE_H
74 #include IMU_TYPE_H
75 #endif
76 
77 extern void imu_init(void);
78 
79 
80 #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
81 #define IMU_BODY_TO_IMU_PHI 0
82 #define IMU_BODY_TO_IMU_THETA 0
83 #define IMU_BODY_TO_IMU_PSI 0
84 #endif
85 
86 #if !defined IMU_GYRO_P_NEUTRAL && !defined IMU_GYRO_Q_NEUTRAL && !defined IMU_GYRO_R_NEUTRAL
87 #define IMU_GYRO_P_NEUTRAL 0
88 #define IMU_GYRO_Q_NEUTRAL 0
89 #define IMU_GYRO_R_NEUTRAL 0
90 #endif
91 
92 #if !defined IMU_ACCEL_X_NEUTRAL && !defined IMU_ACCEL_Y_NEUTRAL && !defined IMU_ACCEL_Z_NEUTRAL
93 #define IMU_ACCEL_X_NEUTRAL 0
94 #define IMU_ACCEL_Y_NEUTRAL 0
95 #define IMU_ACCEL_Z_NEUTRAL 0
96 #endif
97 
98 
99 #ifndef ImuScaleGyro
100 #define ImuScaleGyro(_imu) { \
101  RATES_COPY(_imu.gyro_prev, _imu.gyro); \
102  _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; \
103  _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; \
104  _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; \
105  }
106 #endif
107 
108 
109 #ifndef ImuScaleAccel
110 #define ImuScaleAccel(_imu) { \
111  VECT3_COPY(_imu.accel_prev, _imu.accel); \
112  _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; \
113  _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; \
114  _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; \
115  }
116 #endif
117 
118 #ifndef ImuScaleMag
119 #if defined IMU_MAG_45_HACK
120 #define ImuScaleMag(_imu) { \
121  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; \
122  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; \
123  _imu.mag.x = msx - msy; \
124  _imu.mag.y = msx + msy; \
125  _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; \
126  }
127 #elif defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
128 #define ImuScaleMag(_imu) { \
129  struct Int32Vect3 _mag_correction; \
130  _mag_correction.x = (int32_t) (IMU_MAG_X_CURRENT_COEF * (float) electrical.current); \
131  _mag_correction.y = (int32_t) (IMU_MAG_Y_CURRENT_COEF * (float) electrical.current); \
132  _mag_correction.z = (int32_t) (IMU_MAG_Z_CURRENT_COEF * (float) electrical.current); \
133  _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; \
134  _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; \
135  _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; \
136  }
137 #else
138 #define ImuScaleMag(_imu) { \
139  _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; \
140  _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; \
141  _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; \
142  }
143 #endif //IMU_MAG_45_HACK
144 #endif //ImuScaleMag
145 
146 
147 #endif /* IMU_H */
struct FloatEulers body_to_imu_eulers
Definition: imu.h:61
rotation matrix
angular rates
Rotation quaternion.
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct FloatRates gyro
Definition: imu.h:57
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
abstract IMU interface providing fixed point interface
Definition: imu.h:39
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:43
struct FloatRMat body_to_imu_rmat
Definition: imu.h:63
euler angles
struct Int32Rates gyro_neutral
gyroscope bias
Definition: imu.h:45
Roation quaternion.
struct FloatVect3 accel
Definition: imu.h:58
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias)
Definition: imu.h:47
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
Paparazzi floating point algebra.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
struct FloatRates gyro_prev
Definition: imu.h:60
unsigned long uint32_t
Definition: types.h:18
struct Int32Vect3 accel_prev
previous accelerometer measurements
Definition: imu.h:44
angular rates
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
struct FloatVect3 mag
Definition: imu.h:59
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
struct Int32Quat body_to_imu_quat
rotation from body to imu frame as a unit quaternion
Definition: imu.h:51
struct Int32Vect3 accel_neutral
accelerometer bias
Definition: imu.h:46
void imu_periodic(void)
Definition: imu_apogee.c:79
void imu_float_init(struct ImuFloat *imuf)
Definition: imu.c:63
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
rotation matrix
struct FloatQuat body_to_imu_quat
Definition: imu.h:62
abstract IMU interface providing floating point interface
Definition: imu.h:56
void imu_init(void)
Definition: ins_xsens700.c:86
void imu_impl_init(void)
Definition: imu_apogee.c:64
Paparazzi fixed point algebra.
uint32_t sample_count
Definition: imu.h:64