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_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 {
60  // MPU60X0
65 
66 
68  // Configure power:
69 
70  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
71 
72  // MPU60X0_REG_USER_CTRL:
73  // -Enable Aux I2C Master Mode
74  // -Enable SPI
75 
76  // MPU60X0_REG_PWR_MGMT_1
77  // -switch to gyroX clock
79  aspirin2_mpu60x0.buf[1] = 0x01;
80  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
82 
83  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
84 
86  // Measurement Settings
87 
88  // MPU60X0_REG_CONFIG
89  // -ext sync on gyro X (bit 3->6)
90  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
91 #if PERIODIC_FREQUENCY == 60
92 #else
93 # if PERIODIC_FREQUENCY == 120
94 # else
95 # error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
96 # endif
97 #endif
99  aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
100  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
102 
103  // MPU60X0_REG_SMPLRT_DIV
104  // -100Hz output = 1kHz / (9 + 1)
106  aspirin2_mpu60x0.buf[1] = 9;
107  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
109 
110  // MPU60X0_REG_GYRO_CONFIG
111  // -2000deg/sec
113  aspirin2_mpu60x0.buf[1] = (3<<3);
114  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
116 
117  // MPU60X0_REG_ACCEL_CONFIG
118  // 16g, no HPFL
120  aspirin2_mpu60x0.buf[1] = (3<<3);
121  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
123 
124 
125 
126 
127 
128 /*
129  // no interrupts for now, but set data ready interrupt to enable reading status bits
130  aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
131  aspirin2_mpu60x0.buf[1] = 0x01;
132  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
133  while(aspirin2_mpu60x0.status == I2CTransPending);
134 */
135 
136 /*
138  // HMC5843
139  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
140  ppzuavimu_hmc5843.type = I2CTransTx;
141  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
142  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
143  ppzuavimu_hmc5843.len_w = 2;
144  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
145  while(ppzuavimu_hmc5843.status == I2CTransPending);
146 
147  ppzuavimu_hmc5843.type = I2CTransTx;
148  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
149  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
150  ppzuavimu_hmc5843.len_w = 2;
151  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
152  while(ppzuavimu_hmc5843.status == I2CTransPending);
153 
154  ppzuavimu_hmc5843.type = I2CTransTx;
155  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
156  ppzuavimu_hmc5843.buf[1] = 0x00;
157  ppzuavimu_hmc5843.len_w = 2;
158  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
159  while(ppzuavimu_hmc5843.status == I2CTransPending);
160 */
161 }
162 
163 void imu_periodic( void )
164 {
165  // Start reading the latest gyroscope data
167  aspirin2_mpu60x0.len_r = 21;
170  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &aspirin2_mpu60x0);
171 
172 /*
173  // Start reading the latest accelerometer data
174  ppzuavimu_adxl345.type = I2CTransTxRx;
175  ppzuavimu_adxl345.len_r = 6;
176  ppzuavimu_adxl345.len_w = 1;
177  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
178  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
179 */
180  // Start reading the latest magnetometer data
181 #if PERIODIC_FREQUENCY > 60
182  RunOnceEvery(2,{
183 #endif
184 /* ppzuavimu_hmc5843.type = I2CTransTxRx;
185  ppzuavimu_hmc5843.len_r = 6;
186  ppzuavimu_hmc5843.len_w = 1;
187  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
188  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
189 */
190 #if PERIODIC_FREQUENCY > 60
191  });
192 #endif
193  //RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
194 }
195 
197 {
201 }
202 
204 {
205  int32_t x, y, z;
206 
207  // If the itg3200 I2C transaction has succeeded: convert the data
209  {
210 #define MPU_OFFSET_GYRO 9
214 
215  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
216 
217 #define MPU_OFFSET_ACC 1
221 
222  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
223 
224  // Is this is new data
225  if (aspirin2_mpu60x0.buf[0] & 0x01)
226  {
227  gyr_valid = TRUE;
228  acc_valid = TRUE;
229  }
230  else
231  {
232  }
233 
234  aspirin2_mpu60x0.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
235  }
236 /*
237  // If the adxl345 I2C transaction has succeeded: convert the data
238  if (ppzuavimu_adxl345.status == I2CTransSuccess)
239  {
240  x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
241  y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
242  z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
243 
244 #ifdef ASPIRIN_IMU
245  VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
246 #else // PPZIMU
247  VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
248 #endif
249 
250  acc_valid = TRUE;
251  ppzuavimu_adxl345.status = I2CTransDone;
252  }
253 
254  // If the hmc5843 I2C transaction has succeeded: convert the data
255  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
256  {
257  x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
258  y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
259  z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
260 
261 #ifdef ASPIRIN_IMU
262  VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
263 #else // PPZIMU
264  VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
265 #endif
266 
267  mag_valid = TRUE;
268  ppzuavimu_hmc5843.status = I2CTransDone;
269  }
270 */
271 }
272 
volatile bool_t gyr_valid
Definition: imu_aspirin2.c:42
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t p
in rad/s^2 with INT32_RATE_FRAC
void aspirin2_subsystem_downlink_raw(void)
Definition: imu_aspirin2.c:196
#define MPU60X0_REG_SMPLRT_DIV
Definition: mpu60X0.h:23
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
abstract IMU interface providing fixed point interface
Definition: imu.h:39
void imu_impl_init(void)
Definition: imu_aspirin2.c:57
uint8_t slave_addr
Definition: i2c.h:43
#define MPU_OFFSET_ACC
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
Definition: i2c_arch.c:321
volatile bool_t mag_valid
Definition: imu_aspirin2.c:41
#define MPU60X0_REG_PWR_MGMT_1
Definition: mpu60X0.h:13
#define MPU60X0_ADDR
Definition: mpu60X0.h:5
signed short int16_t
Definition: types.h:17
struct i2c_transaction aspirin2_mpu60x0
Definition: imu_aspirin2.c:46
#define MPU60X0_REG_INT_STATUS
Definition: mpu60X0.h:62
enum I2CTransactionStatus status
Definition: i2c.h:47
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
signed long int32_t
Definition: types.h:19
void imu_periodic(void)
Definition: imu_aspirin2.c:163
#define TRUE
Definition: imu_chimu.h:144
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
uint8_t len_w
Definition: i2c.h:45
uint16_t len_r
Definition: i2c.h:44
volatile bool_t acc_valid
Definition: imu_aspirin2.c:43
#define MPU_OFFSET_GYRO
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:296
enum I2CTransactionType type
Definition: i2c.h:42
int32_t q
in rad/s^2 with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
void aspirin2_subsystem_event(void)
Definition: imu_aspirin2.c:203
#define MPU60X0_REG_CONFIG
Definition: mpu60X0.h:24
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
int32_t r
in rad/s^2 with INT32_RATE_FRAC
Definition: i2c.h:9
#define MPU60X0_REG_GYRO_CONFIG
Definition: mpu60X0.h:25
#define MPU60X0_REG_ACCEL_CONFIG
Definition: mpu60X0.h:26