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_b2.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2012 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 #ifndef IMU_B2_H
31 #define IMU_B2_H
32 
33 #include "subsystems/imu.h"
34 #include "generated/airframe.h"
35 
36 #include "peripherals/max1168.h"
37 
38 /* type of magnetometer */
39 #define IMU_B2_MAG_NONE 0
40 #define IMU_B2_MAG_MS2100 1
41 #define IMU_B2_MAG_AMI601 2
42 #define IMU_B2_MAG_HMC5843 3
43 #define IMU_B2_MAG_HMC58XX 4
44 
45 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
46 #include "peripherals/hmc58xx.h"
47 #endif
48 
49 #ifdef IMU_B2_VERSION_1_0
50 /* Default IMU b2 sensors connection */
51 #if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined IMU_GYRO_R_CHAN
52 #define IMU_GYRO_P_CHAN 1
53 #define IMU_GYRO_Q_CHAN 0
54 #define IMU_GYRO_R_CHAN 2
55 #endif
56 #if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined IMU_ACCEL_Z_CHAN
57 #define IMU_ACCEL_X_CHAN 3
58 #define IMU_ACCEL_Y_CHAN 5
59 #define IMU_ACCEL_Z_CHAN 6
60 #endif
61 #if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
62 #define IMU_MAG_X_CHAN 0
63 #define IMU_MAG_Y_CHAN 1
64 #define IMU_MAG_Z_CHAN 2
65 #endif
66 
67 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
68 #define IMU_GYRO_P_SIGN 1
69 #define IMU_GYRO_Q_SIGN -1
70 #define IMU_GYRO_R_SIGN -1
71 #endif
72 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
73 #define IMU_ACCEL_X_SIGN -1
74 #define IMU_ACCEL_Y_SIGN -1
75 #define IMU_ACCEL_Z_SIGN -1
76 #endif
77 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
78 #define IMU_MAG_X_SIGN 1
79 #define IMU_MAG_Y_SIGN -1
80 #define IMU_MAG_Z_SIGN -1
81 #endif
82 #endif /* IMU_B2_VERSION_1_0 */
83 
84 
85 #ifdef IMU_B2_VERSION_1_1
86 /* Default IMU b2 sensors connection */
87 #if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined IMU_GYRO_R_CHAN
88 #define IMU_GYRO_P_CHAN 1
89 #define IMU_GYRO_Q_CHAN 0
90 #define IMU_GYRO_R_CHAN 2
91 #endif
92 #if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined IMU_ACCEL_Z_CHAN
93 #define IMU_ACCEL_X_CHAN 5
94 #define IMU_ACCEL_Y_CHAN 3
95 #define IMU_ACCEL_Z_CHAN 4
96 #endif
97 #if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
98 #define IMU_MAG_X_CHAN 0
99 #define IMU_MAG_Y_CHAN 1
100 #define IMU_MAG_Z_CHAN 2
101 #endif
102 
103 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
104 #define IMU_GYRO_P_SIGN 1
105 #define IMU_GYRO_Q_SIGN -1
106 #define IMU_GYRO_R_SIGN -1
107 #endif
108 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
109 #define IMU_ACCEL_X_SIGN -1
110 #define IMU_ACCEL_Y_SIGN -1
111 #define IMU_ACCEL_Z_SIGN -1
112 #endif
113 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
114 #define IMU_MAG_X_SIGN 1
115 #define IMU_MAG_Y_SIGN -1
116 #define IMU_MAG_Z_SIGN -1
117 #endif
118 #endif /* IMU_B2_VERSION_1_1 */
119 
120 #ifdef IMU_B2_VERSION_1_2
121 /* Default IMU b2 sensors connection */
122 #if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined IMU_GYRO_R_CHAN
123 #define IMU_GYRO_P_CHAN 1
124 #define IMU_GYRO_Q_CHAN 0
125 #define IMU_GYRO_R_CHAN 2
126 #endif
127 #if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined IMU_ACCEL_Z_CHAN
128 #define IMU_ACCEL_X_CHAN 4
129 #define IMU_ACCEL_Y_CHAN 5
130 #define IMU_ACCEL_Z_CHAN 3
131 #endif
132 #if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
133 #define IMU_MAG_X_CHAN 0
134 #define IMU_MAG_Y_CHAN 1
135 #define IMU_MAG_Z_CHAN 2
136 #endif
137 
138 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
139 #define IMU_GYRO_P_SIGN 1
140 #define IMU_GYRO_Q_SIGN -1
141 #define IMU_GYRO_R_SIGN -1
142 #endif
143 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
144 #define IMU_ACCEL_X_SIGN 1
145 #define IMU_ACCEL_Y_SIGN -1
146 #define IMU_ACCEL_Z_SIGN 1
147 #endif
148 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
149 #define IMU_MAG_X_SIGN -1
150 #define IMU_MAG_Y_SIGN -1
151 #define IMU_MAG_Z_SIGN 1
152 #endif
153 #endif /* IMU_B2_VERSION_1_2 */
154 
155 
156 struct ImuBooz2 {
157 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
158  struct Hmc58xx mag_hmc;
159 #endif
160 };
161 
162 extern struct ImuBooz2 imu_b2;
163 
167 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
168 #include "peripherals/ms2100.h"
169 static inline void ImuMagEvent(void (* _mag_handler)(void)) {
172  imu.mag_unscaled.x = ms2100.data.value[IMU_MAG_X_CHAN];
173  imu.mag_unscaled.y = ms2100.data.value[IMU_MAG_Y_CHAN];
174  imu.mag_unscaled.z = ms2100.data.value[IMU_MAG_Z_CHAN];
176  _mag_handler();
177  }
178 }
179 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
180 #include "peripherals/ami601.h"
181 #define foo_handler() {}
182 #define ImuMagEvent(_mag_handler) { \
183  AMI601Event(foo_handler); \
184  if (ami601_status == AMI601_DATA_AVAILABLE) { \
185  imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
186  imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
187  imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
188  ami601_status = AMI601_IDLE; \
189  _mag_handler(); \
190  } \
191 }
192 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
193 #include "peripherals/hmc5843.h"
194 #define foo_handler() {}
195 #define ImuMagEvent(_mag_handler) { \
196  MagEvent(foo_handler); \
197  if (hmc5843.data_available) { \
198  imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \
199  imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \
200  imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \
201  _mag_handler(); \
202  hmc5843.data_available = FALSE; \
203  } \
204 }
205 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
206 static inline void ImuMagEvent(void (* _mag_handler)(void)) {
207  hmc58xx_event(&imu_b2.mag_hmc);
208  if (imu_b2.mag_hmc.data_available) {
209  imu.mag_unscaled.x = imu_b2.mag_hmc.data.value[IMU_MAG_X_CHAN];
210  imu.mag_unscaled.y = imu_b2.mag_hmc.data.value[IMU_MAG_Y_CHAN];
211  imu.mag_unscaled.z = imu_b2.mag_hmc.data.value[IMU_MAG_Z_CHAN];
212  _mag_handler();
213  imu_b2.mag_hmc.data_available = FALSE;
214  }
215 }
216 #else
217 #define ImuMagEvent(_mag_handler) {}
218 #define ImuScaleMag(_imu) {}
219 #endif
220 
221 static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
222 {
223  max1168_event();
225  imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
226  imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
227  imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
228  imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
229  imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
230  imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
232  _gyro_handler();
233  _accel_handler();
234  }
235  ImuMagEvent(_mag_handler);
236 }
237 
238 #endif /* IMU_B2_H */
void max1168_event(void)
Definition: max1168.c:98
enum Ms2100Status status
Definition: ms2100.h:50
int16_t value[3]
data values accessible by channel index
Definition: ms2100.h:54
uint16_t max1168_values[MAX1168_NB_CHAN]
Definition: max1168.c:28
int32_t p
in rad/s^2 with INT32_RATE_FRAC
union Ms2100::@27 data
Driver for the ms2100 magnetic sensor from PNI.
void ms2100_event(struct Ms2100 *ms)
Definition: ms2100.c:103
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
#define MAX1168_IDLE
Definition: max1168.h:59
#define FALSE
Definition: imu_chimu.h:141
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define ImuMagEvent(_mag_handler)
Event functions and macros for imu_b2.
Definition: imu_b2.h:217
struct Ms2100 ms2100
Definition: ms2100.c:44
Inertial Measurement Unit interface.
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
volatile uint8_t max1168_status
Definition: max1168.c:27
#define MAX1168_DATA_AVAILABLE
Definition: max1168.h:63
int32_t q
in rad/s^2 with INT32_RATE_FRAC
int32_t r
in rad/s^2 with INT32_RATE_FRAC
static void ImuEvent(void(*_gyro_handler)(void), void(*_accel_handler)(void), void(*_mag_handler)(void))
Definition: imu_b2.h:221
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:136
struct ImuBooz2 imu_b2
Definition: imu_b2.c:34