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_aspirin.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2010 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  */
23 
24 #ifndef IMU_ASPIRIN_H
25 #define IMU_ASPIRIN_H
26 
27 #include "generated/airframe.h"
28 #include "subsystems/imu.h"
29 
30 #include "mcu_periph/i2c.h"
31 #include "peripherals/itg3200.h"
32 #include "peripherals/hmc5843.h"
33 #include "peripherals/adxl345.h"
34 
35 #ifdef IMU_ASPIRIN_VERSION_1_0
36 #define IMU_MAG_X_CHAN 0
37 #define IMU_MAG_Y_CHAN 1
38 #define IMU_MAG_Z_CHAN 2
39 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
40 #define IMU_MAG_X_SIGN 1
41 #define IMU_MAG_Y_SIGN 1
42 #define IMU_MAG_Z_SIGN 1
43 #endif
44 #endif
45 
46 #ifdef IMU_ASPIRIN_VERSION_1_5
47 #define IMU_MAG_X_CHAN 2
48 #define IMU_MAG_Y_CHAN 0
49 #define IMU_MAG_Z_CHAN 1
50 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
51 #define IMU_MAG_X_SIGN 1
52 #define IMU_MAG_Y_SIGN -1
53 #define IMU_MAG_Z_SIGN 1
54 #endif
55 #endif
56 
57 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
58 #define IMU_GYRO_P_SIGN 1
59 #define IMU_GYRO_Q_SIGN 1
60 #define IMU_GYRO_R_SIGN 1
61 #endif
62 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
63 #define IMU_ACCEL_X_SIGN 1
64 #define IMU_ACCEL_Y_SIGN 1
65 #define IMU_ACCEL_Z_SIGN 1
66 #endif
67 
68 #if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
69 #ifdef IMU_ASPIRIN_VERSION_1_5
70 
75 #define IMU_GYRO_P_SENS 4.359
76 #define IMU_GYRO_P_SENS_NUM 4359
77 #define IMU_GYRO_P_SENS_DEN 1000
78 #define IMU_GYRO_Q_SENS 4.359
79 #define IMU_GYRO_Q_SENS_NUM 4359
80 #define IMU_GYRO_Q_SENS_DEN 1000
81 #define IMU_GYRO_R_SENS 4.359
82 #define IMU_GYRO_R_SENS_NUM 4359
83 #define IMU_GYRO_R_SENS_DEN 1000
84 #else
85 
90 #define IMU_GYRO_P_SENS 4.973
91 #define IMU_GYRO_P_SENS_NUM 4973
92 #define IMU_GYRO_P_SENS_DEN 1000
93 #define IMU_GYRO_Q_SENS 4.973
94 #define IMU_GYRO_Q_SENS_NUM 4973
95 #define IMU_GYRO_Q_SENS_DEN 1000
96 #define IMU_GYRO_R_SENS 4.973
97 #define IMU_GYRO_R_SENS_NUM 4973
98 #define IMU_GYRO_R_SENS_DEN 1000
99 #endif // IMU_ASPIRIN_VERSION_1_5
100 #endif
101 
102 
114 #if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
115 #define IMU_ACCEL_X_SENS 37.91
116 #define IMU_ACCEL_X_SENS_NUM 3791
117 #define IMU_ACCEL_X_SENS_DEN 100
118 #define IMU_ACCEL_Y_SENS 37.91
119 #define IMU_ACCEL_Y_SENS_NUM 3791
120 #define IMU_ACCEL_Y_SENS_DEN 100
121 #define IMU_ACCEL_Z_SENS 39.24
122 #define IMU_ACCEL_Z_SENS_NUM 3924
123 #define IMU_ACCEL_Z_SENS_DEN 100
124 #endif
125 
126 
132  };
133 
134 struct ImuAspirin {
135  volatile enum AspirinStatus status;
139  volatile uint8_t accel_tx_buf[7];
140  volatile uint8_t accel_rx_buf[7];
146 };
147 
148 extern struct ImuAspirin imu_aspirin;
149 #define ASPIRIN_GYRO_TIMEOUT 3
150 #define ASPIRIN_ACCEL_TIMEOUT 3
151 
152 #define foo_handler() {}
153 #define ImuMagEvent(_mag_handler) { \
154  MagEvent(foo_handler); \
155  if (hmc5843.data_available) { \
156  imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \
157  imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \
158  imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \
159  _mag_handler(); \
160  hmc5843.data_available = FALSE; \
161  } \
162 }
163 
164 /* underlying architecture */
165 #include "subsystems/imu/imu_aspirin_arch.h"
166 /* must be implemented by underlying architecture */
167 extern void imu_aspirin_arch_init(void);
168 
169 static inline void gyro_read_i2c(void)
170 {
177 }
178 
179 static inline void gyro_copy_i2c(void)
180 {
184  RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
185 }
186 
187 static inline void accel_copy_spi(void)
188 {
189  const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
190  const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
191  const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
192  VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
193 }
194 
195 
196 static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
197 {
198  if (imu_aspirin.status == AspirinStatusUninit) return;
199 
204  accel_copy_spi();
205  _accel_handler();
206  }
208 
209  // Reset everything if we've been waiting too long
211  // FIXME: there should be no arch specific code like that here!
212  i2c2_er_irq_handler();
213  gyro_read_i2c();
215  return;
216  }
217 
218  // Try again later if transaction is in progress
220  {
221  return;
222  }
223 
224  ImuMagEvent(_mag_handler);
225 
226  // Try back later if things are not idle
227  if ((i2c2.status != I2CIdle) || !i2c_idle(&i2c2)) {
228  return;
229  }
230 
233  gyro_copy_i2c();
236  _gyro_handler();
237  }
238  }
240  return;
241  }
242 
243  // If we're not already waiting for read, and conversion is complete, schedule a read
244  if (!imu_aspirin.reading_gyro && imu_aspirin_eoc() && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
247  }
248  gyro_read_i2c();
249  return;
250  }
251 
252 }
253 
254 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \
255  imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
256 } while(0);
257 
258 #endif /* IMU_ASPIRIN_H */
volatile uint8_t accel_tx_buf[7]
Definition: imu_aspirin.h:139
static int imu_aspirin_eoc(void)
uint8_t reading_gyro
Definition: imu_aspirin.h:144
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
bool_t i2c_idle(struct i2c_periph *p)
Definition: i2c_arch.c:317
static void imu_aspirin_event(void(*_gyro_handler)(void), void(*_accel_handler)(void), void(*_mag_handler)(void))
Definition: imu_aspirin.h:196
#define FALSE
Definition: imu_chimu.h:141
struct i2c_transaction i2c_trans_gyro
Definition: imu_aspirin.h:136
static void gyro_copy_i2c(void)
Definition: imu_aspirin.h:179
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
#define ImuMagEvent(_mag_handler)
Definition: imu_aspirin.h:153
Definition: i2c.h:23
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
Definition: i2c_arch.c:321
void imu_aspirin_arch_int_disable(void)
AspirinStatus
Definition: imu_aspirin.h:127
struct i2c_transaction i2c_trans_mag
Definition: imu_aspirin.h:137
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
static void accel_copy_spi(void)
Definition: imu_aspirin.h:187
uint8_t mag_available
Definition: imu_aspirin.h:143
static void gyro_read_i2c(void)
Definition: imu_aspirin.h:169
enum I2CTransactionStatus status
Definition: i2c.h:47
Inertial Measurement Unit interface.
struct ImuAspirin imu_aspirin
Definition: imu_aspirin.c:6
void imu_aspirin_arch_init(void)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
volatile uint8_t accel_rx_buf[7]
Definition: imu_aspirin.h:140
uint8_t len_w
Definition: i2c.h:45
uint16_t len_r
Definition: i2c.h:44
unsigned char uint8_t
Definition: types.h:14
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:296
volatile uint8_t accel_available
Definition: imu_aspirin.h:138
#define ITG3200_REG_GYRO_XOUT_H
Definition: itg3200.h:16
enum AspirinStatus status
Definition: imu_aspirin.h:135
void imu_aspirin_arch_int_enable(void)
uint32_t time_since_last_accel_reading
Definition: imu_aspirin.h:142
enum I2CTransactionType type
Definition: i2c.h:42
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t gyro_available_blaaa
Definition: imu_aspirin.h:145
uint32_t time_since_last_reading
Definition: imu_aspirin.h:141
#define ASPIRIN_GYRO_TIMEOUT
Definition: imu_aspirin.h:149