Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
imu_aspirin2.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Christophe DeWagter
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 
22 #ifndef IMU_ASPIRIN_2_H
23 #define IMU_ASPIRIN_2_H
24 
25 #include "generated/airframe.h"
26 #include "subsystems/imu.h"
27 
28 
29 #ifdef IMU_ASPIRIN_VERSION_2_1
30 #if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
31 #define IMU_MAG_X_SIGN 1
32 #define IMU_MAG_Y_SIGN 1
33 #define IMU_MAG_Z_SIGN 1
34 #endif
35 #endif
36 
37 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
38 #define IMU_GYRO_P_SIGN 1
39 #define IMU_GYRO_Q_SIGN 1
40 #define IMU_GYRO_R_SIGN 1
41 #endif
42 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
43 #define IMU_ACCEL_X_SIGN 1
44 #define IMU_ACCEL_Y_SIGN 1
45 #define IMU_ACCEL_Z_SIGN 1
46 #endif
47 
53 #if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
54 #define IMU_GYRO_P_SENS 4.359
55 #define IMU_GYRO_P_SENS_NUM 4359
56 #define IMU_GYRO_P_SENS_DEN 1000
57 #define IMU_GYRO_Q_SENS 4.359
58 #define IMU_GYRO_Q_SENS_NUM 4359
59 #define IMU_GYRO_Q_SENS_DEN 1000
60 #define IMU_GYRO_R_SENS 4.359
61 #define IMU_GYRO_R_SENS_NUM 4359
62 #define IMU_GYRO_R_SENS_DEN 1000
63 #endif
64 #if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
65 #define IMU_GYRO_P_NEUTRAL 0
66 #define IMU_GYRO_Q_NEUTRAL 0
67 #define IMU_GYRO_R_NEUTRAL 0
68 #endif
69 
75 #if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
76 #define IMU_ACCEL_X_SENS 4.905
77 #define IMU_ACCEL_X_SENS_NUM 4905
78 #define IMU_ACCEL_X_SENS_DEN 1000
79 #define IMU_ACCEL_Y_SENS 4.905
80 #define IMU_ACCEL_Y_SENS_NUM 4905
81 #define IMU_ACCEL_Y_SENS_DEN 1000
82 #define IMU_ACCEL_Z_SENS 4.905
83 #define IMU_ACCEL_Z_SENS_NUM 4905
84 #define IMU_ACCEL_Z_SENS_DEN 1000
85 #endif
86 #if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
87 #define IMU_ACCEL_X_NEUTRAL 0
88 #define IMU_ACCEL_Y_NEUTRAL 0
89 #define IMU_ACCEL_Z_NEUTRAL 0
90 #endif
91 
92 
97  };
98 
99 struct ImuAspirin2 {
100  volatile enum Aspirin2Status status;
102  volatile uint8_t imu_tx_buf[64];
103  volatile uint8_t imu_rx_buf[64];
104 };
105 
106 extern struct ImuAspirin2 imu_aspirin2;
107 
108 
109 static inline uint8_t imu_from_buff(void)
110 {
111  int32_t x, y, z, p, q, r, Mx, My, Mz;
112 
113 #define MPU_OFFSET_STATUS 1
114  if (!(imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01)) {
115  return 0;
116  }
117 
118  // If the itg3200 I2C transaction has succeeded: convert the data
119 #define MPU_OFFSET_GYRO 10
123 
124 #define MPU_OFFSET_ACC 2
128 
129 #define MPU_OFFSET_MAG 16
133 
134 #ifdef LISA_M_LONGITUDINAL_X
135  RATES_ASSIGN(imu.gyro_unscaled, q, -p, r);
136  VECT3_ASSIGN(imu.accel_unscaled, y, -x, z);
137  VECT3_ASSIGN(imu.mag_unscaled, -Mx, -Mz, My);
138 #else
139  RATES_ASSIGN(imu.gyro_unscaled, p, q, r);
140  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
141  VECT3_ASSIGN(imu.mag_unscaled, Mz, -Mx, My);
142 #endif
143 
144  return 1;
145 }
146 
147 
148 static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
149 {
151 
154  if (imu_from_buff())
155  {
156  _gyro_handler();
157  _accel_handler();
158  _mag_handler();
159  }
160  }
161 }
162 
163 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
164  imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
165 }
166 
167 /* underlying architecture */
169 /* must be implemented by underlying architecture */
170 extern void imu_aspirin2_arch_init(void);
171 
172 #endif /* IMU_ASPIRIN_2_H */
#define MPU_OFFSET_ACC
#define MPU_OFFSET_STATUS
enum Aspirin2Status status
Definition: imu_aspirin2.h:100
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
struct ImuAspirin2 imu_aspirin2
Definition: imu_aspirin2.c:32
#define FALSE
Definition: imu_chimu.h:141
volatile uint8_t imu_rx_buf[64]
Definition: imu_aspirin2.h:103
volatile uint8_t imu_tx_buf[64]
Definition: imu_aspirin2.h:102
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
Aspirin2Status
Definition: imu_aspirin2.h:93
void imu_aspirin2_arch_init(void)
signed short int16_t
Definition: types.h:17
static void imu_aspirin2_event(void(*_gyro_handler)(void), void(*_accel_handler)(void), void(*_mag_handler)(void))
Definition: imu_aspirin2.h:148
#define MPU_OFFSET_MAG
Inertial Measurement Unit interface.
#define MPU_OFFSET_GYRO
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
signed long int32_t
Definition: types.h:19
volatile uint8_t imu_spi_data_received
Definition: imu_aspirin2.h:101
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
unsigned char uint8_t
Definition: types.h:14
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:296
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
static uint8_t imu_from_buff(void)
Definition: imu_aspirin2.h:109