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_aspirin2.c
Go to the documentation of this file.
1 /*
2  * This file is part of paparazzi.
3  *
4  * paparazzi is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; either version 2, or (at your option)
7  * any later version.
8  *
9  * paparazzi is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with paparazzi; see the file COPYING. If not, write to
16  * the Free Software Foundation, 59 Temple Place - Suite 330,
17  * Boston, MA 02111-1307, USA.
18  *
19  */
20 
21 #include <math.h>
22 #include "imu_aspirin2.h"
23 #include "mcu_periph/i2c.h"
24 #include "led.h"
25 
26 // Downlink
27 #include "mcu_periph/uart.h"
28 #include "messages.h"
30 
31 #ifndef DOWNLINK_DEVICE
32 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
33 #endif
34 
35 
36 // Peripherials
37 #include "../../peripherals/mpu60x0.h"
38 // #include "../../peripherals/hmc5843.h"
39 
40 // Results
41 volatile bool_t mag_valid;
42 volatile bool_t gyr_valid;
43 volatile bool_t acc_valid;
44 
45 // Communication
47 
48 // Standalone option: run module only
49 #ifndef IMU_TYPE_H
50 struct Imu imu;
51 #endif
52 
53 #ifndef PERIODIC_FREQUENCY
54 #define PERIODIC_FREQUENCY 60
55 #endif
56 
57 void imu_impl_init(void)
58 {
59 
61  // Configure power:
62 
63  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
64 
65  // MPU60X0_REG_USER_CTRL:
66  // -Enable Aux I2C Master Mode
67  // -Enable SPI
68 
69  // MPU60X0_REG_PWR_MGMT_1
70  // -switch to gyroX clock
72  aspirin2_mpu60x0.buf[1] = 0x01;
73  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
75 
76  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
77 
79  // Measurement Settings
80 
81  // MPU60X0_REG_CONFIG
82  // -ext sync on gyro X (bit 3->6)
83  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
84 #if PERIODIC_FREQUENCY == 60
85 #else
86 # if PERIODIC_FREQUENCY == 120
87 # else
88 # error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
89 # endif
90 #endif
92  aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
93  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
95 
96  // MPU60X0_REG_SMPLRT_DIV
97  // -100Hz output = 1kHz / (9 + 1)
99  aspirin2_mpu60x0.buf[1] = 9;
100  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
102 
103  // MPU60X0_REG_GYRO_CONFIG
104  // -2000deg/sec
106  aspirin2_mpu60x0.buf[1] = (3<<3);
107  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
109 
110  // MPU60X0_REG_ACCEL_CONFIG
111  // 16g, no HPFL
113  aspirin2_mpu60x0.buf[1] = (3<<3);
114  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
116 
117 
118 
119 
120 
121 /*
122  // no interrupts for now, but set data ready interrupt to enable reading status bits
123  aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
124  aspirin2_mpu60x0.buf[1] = 0x01;
125  i2c_submit(&PPZUAVIMU_I2C_DEV,&aspirin2_mpu60x0);
126  while(aspirin2_mpu60x0.status == I2CTransPending);
127 */
128 
129 /*
131  // HMC5843
132  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
133  ppzuavimu_hmc5843.type = I2CTransTx;
134  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
135  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
136  ppzuavimu_hmc5843.len_w = 2;
137  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
138  while(ppzuavimu_hmc5843.status == I2CTransPending);
139 
140  ppzuavimu_hmc5843.type = I2CTransTx;
141  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
142  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
143  ppzuavimu_hmc5843.len_w = 2;
144  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
145  while(ppzuavimu_hmc5843.status == I2CTransPending);
146 
147  ppzuavimu_hmc5843.type = I2CTransTx;
148  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
149  ppzuavimu_hmc5843.buf[1] = 0x00;
150  ppzuavimu_hmc5843.len_w = 2;
151  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
152  while(ppzuavimu_hmc5843.status == I2CTransPending);
153 */
154 }
155 
156 void imu_periodic( void )
157 {
158  // Start reading the latest gyroscope data
160  i2c_transceive(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 1, 21);
161 
162 /*
163  // Start reading the latest accelerometer data
164  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
165  i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6);
166 */
167  // Start reading the latest magnetometer data
168 #if PERIODIC_FREQUENCY > 60
169  RunOnceEvery(2,{
170 #endif
171 /* ppzuavimu_hmc5843.type = I2CTransTxRx;
172  ppzuavimu_hmc5843.len_r = 6;
173  ppzuavimu_hmc5843.len_w = 1;
174  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
175  i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843);
176 */
177 #if PERIODIC_FREQUENCY > 60
178  });
179 #endif
180  //RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
181 }
182 
184 {
188 }
189 
191 {
192  int32_t x, y, z;
193 
194  // If the itg3200 I2C transaction has succeeded: convert the data
196  {
197 #define MPU_OFFSET_GYRO 9
201 
202  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
203 
204 #define MPU_OFFSET_ACC 1
208 
209  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
210 
211  // Is this is new data
212  if (aspirin2_mpu60x0.buf[0] & 0x01)
213  {
214  gyr_valid = TRUE;
215  acc_valid = TRUE;
216  }
217  else
218  {
219  }
220 
221  aspirin2_mpu60x0.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
222  }
223 /*
224  // If the adxl345 I2C transaction has succeeded: convert the data
225  if (ppzuavimu_adxl345.status == I2CTransSuccess)
226  {
227  x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
228  y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
229  z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
230 
231 #ifdef ASPIRIN_IMU
232  VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
233 #else // PPZIMU
234  VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
235 #endif
236 
237  acc_valid = TRUE;
238  ppzuavimu_adxl345.status = I2CTransDone;
239  }
240 
241  // If the hmc5843 I2C transaction has succeeded: convert the data
242  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
243  {
244  x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
245  y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
246  z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
247 
248 #ifdef ASPIRIN_IMU
249  VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
250 #else // PPZIMU
251  VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
252 #endif
253 
254  mag_valid = TRUE;
255  ppzuavimu_hmc5843.status = I2CTransDone;
256  }
257 */
258 }
259 
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void imu_impl_init(void)
Definition: imu_aspirin2.c:57
#define MPU60X0_REG_PWR_MGMT_1
Definition: mpu60x0_regs.h:40
int32_t p
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
abstract IMU interface providing fixed point interface
Definition: imu.h:39
#define MPU60X0_REG_SMPLRT_DIV
Definition: mpu60x0_regs.h:50
void aspirin2_subsystem_event(void)
Definition: imu_aspirin2.c:190
#define MPU60X0_REG_CONFIG
Definition: mpu60x0_regs.h:51
bool_t i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Definition: i2c.c:105
void aspirin2_subsystem_downlink_raw(void)
Definition: imu_aspirin2.c:183
volatile bool_t gyr_valid
Definition: imu_aspirin2.c:42
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define MPU_OFFSET_ACC
bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Definition: i2c.c:85
signed short int16_t
Definition: types.h:17
enum I2CTransactionStatus status
Definition: i2c.h:83
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
#define MPU60X0_ADDR
Definition: mpu60x0_regs.h:32
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
volatile bool_t mag_valid
Definition: imu_aspirin2.c:41
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:97
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:82
#define MPU60X0_REG_INT_STATUS
Definition: mpu60x0_regs.h:89
#define MPU_OFFSET_GYRO
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
struct i2c_transaction aspirin2_mpu60x0
Definition: imu_aspirin2.c:46
int32_t q
in rad/s^2 with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
void imu_periodic(void)
Definition: imu_aspirin2.c:156
int32_t r
in rad/s^2 with INT32_RATE_FRAC
volatile bool_t acc_valid
Definition: imu_aspirin2.c:43
#define MPU60X0_REG_GYRO_CONFIG
Definition: mpu60x0_regs.h:52
#define MPU60X0_REG_ACCEL_CONFIG
Definition: mpu60x0_regs.h:53
Architecture independent I2C (Inter-Integrated Circuit Bus) API.