Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
imu.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
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 #else
128 #define ImuScaleMag(_imu) { \
129  _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; \
130  _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; \
131  _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; \
132  }
133 #endif //IMU_MAG_45_HACK
134 #endif //ImuScaleMag
135 
136 
137 #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_b2_arch.c:83
void imu_float_init(struct ImuFloat *imuf)
Definition: imu.c:64
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: imu.c:32
void imu_impl_init(void)
Definition: imu_navgo.c:59
Paparazzi fixed point algebra.
uint32_t sample_count
Definition: imu.h:64