Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 
32 
33 // Peripherials
34 #include "../../peripherals/mpu60x0.h"
35 // #include "../../peripherals/hmc5843.h"
36 
37 // Results
38 volatile bool_t mag_valid;
39 volatile bool_t gyr_valid;
40 volatile bool_t acc_valid;
41 
42 // Communication
44 
45 // Standalone option: run module only
46 #ifndef IMU_TYPE_H
47 struct Imu imu;
48 #endif
49 
50 #ifndef PERIODIC_FREQUENCY
51 #define PERIODIC_FREQUENCY 60
52 #endif
53 
54 void imu_impl_init(void)
55 {
56 
58  // Configure power:
59 
60  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
61 
62  // MPU60X0_REG_USER_CTRL:
63  // -Enable Aux I2C Master Mode
64  // -Enable SPI
65 
66  // MPU60X0_REG_PWR_MGMT_1
67  // -switch to gyroX clock
69  aspirin2_mpu60x0.buf[1] = 0x01;
70  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
72 
73  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
74 
76  // Measurement Settings
77 
78  // MPU60X0_REG_CONFIG
79  // -ext sync on gyro X (bit 3->6)
80  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
81 #if PERIODIC_FREQUENCY == 60
82 #else
83 # if PERIODIC_FREQUENCY == 120
84 # else
85 # error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
86 # endif
87 #endif
89  aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
90  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
92 
93  // MPU60X0_REG_SMPLRT_DIV
94  // -100Hz output = 1kHz / (9 + 1)
96  aspirin2_mpu60x0.buf[1] = 9;
97  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
99 
100  // MPU60X0_REG_GYRO_CONFIG
101  // -2000deg/sec
103  aspirin2_mpu60x0.buf[1] = (3<<3);
104  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
106 
107  // MPU60X0_REG_ACCEL_CONFIG
108  // 16g, no HPFL
110  aspirin2_mpu60x0.buf[1] = (3<<3);
111  i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
113 
114 
115 
116 
117 
118 /*
119  // no interrupts for now, but set data ready interrupt to enable reading status bits
120  aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
121  aspirin2_mpu60x0.buf[1] = 0x01;
122  i2c_submit(&PPZUAVIMU_I2C_DEV,&aspirin2_mpu60x0);
123  while(aspirin2_mpu60x0.status == I2CTransPending);
124 */
125 
126 /*
128  // HMC5843
129  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
130  ppzuavimu_hmc5843.type = I2CTransTx;
131  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
132  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
133  ppzuavimu_hmc5843.len_w = 2;
134  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
135  while(ppzuavimu_hmc5843.status == I2CTransPending);
136 
137  ppzuavimu_hmc5843.type = I2CTransTx;
138  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
139  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
140  ppzuavimu_hmc5843.len_w = 2;
141  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
142  while(ppzuavimu_hmc5843.status == I2CTransPending);
143 
144  ppzuavimu_hmc5843.type = I2CTransTx;
145  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
146  ppzuavimu_hmc5843.buf[1] = 0x00;
147  ppzuavimu_hmc5843.len_w = 2;
148  i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
149  while(ppzuavimu_hmc5843.status == I2CTransPending);
150 */
151 }
152 
153 void imu_periodic( void )
154 {
155  // Start reading the latest gyroscope data
157  i2c_transceive(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 1, 21);
158 
159 /*
160  // Start reading the latest accelerometer data
161  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
162  i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6);
163 */
164  // Start reading the latest magnetometer data
165 #if PERIODIC_FREQUENCY > 60
166  RunOnceEvery(2,{
167 #endif
168 /* ppzuavimu_hmc5843.type = I2CTransTxRx;
169  ppzuavimu_hmc5843.len_r = 6;
170  ppzuavimu_hmc5843.len_w = 1;
171  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
172  i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843);
173 */
174 #if PERIODIC_FREQUENCY > 60
175  });
176 #endif
177  //RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
178 }
179 
181 {
185 }
186 
188 {
189  int32_t x, y, z;
190 
191  // If the itg3200 I2C transaction has succeeded: convert the data
193  {
194 #define MPU_OFFSET_GYRO 9
198 
199  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
200 
201 #define MPU_OFFSET_ACC 1
205 
206  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
207 
208  // Is this is new data
209  if (aspirin2_mpu60x0.buf[0] & 0x01)
210  {
211  gyr_valid = TRUE;
212  acc_valid = TRUE;
213  }
214  else
215  {
216  }
217 
218  aspirin2_mpu60x0.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
219  }
220 /*
221  // If the adxl345 I2C transaction has succeeded: convert the data
222  if (ppzuavimu_adxl345.status == I2CTransSuccess)
223  {
224  x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
225  y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
226  z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
227 
228 #ifdef ASPIRIN_IMU
229  VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
230 #else // PPZIMU
231  VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
232 #endif
233 
234  acc_valid = TRUE;
235  ppzuavimu_adxl345.status = I2CTransDone;
236  }
237 
238  // If the hmc5843 I2C transaction has succeeded: convert the data
239  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
240  {
241  x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
242  y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
243  z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
244 
245 #ifdef ASPIRIN_IMU
246  VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
247 #else // PPZIMU
248  VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
249 #endif
250 
251  mag_valid = TRUE;
252  ppzuavimu_hmc5843.status = I2CTransDone;
253  }
254 */
255 }
256 
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void imu_impl_init(void)
Definition: imu_aspirin2.c:54
#define MPU60X0_REG_PWR_MGMT_1
Definition: mpu60x0_regs.h:40
transaction successfully finished by I2C driver
Definition: i2c.h:57
int32_t p
in rad/s with INT32_RATE_FRAC
I2C transaction structure.
Definition: i2c.h:93
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
abstract IMU interface providing fixed point interface
Definition: imu.h:40
#define MPU60X0_REG_SMPLRT_DIV
Definition: mpu60x0_regs.h:50
void aspirin2_subsystem_event(void)
Definition: imu_aspirin2.c:187
#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)
Submit a write/read transaction.
Definition: i2c.c:258
void aspirin2_subsystem_downlink_raw(void)
Definition: imu_aspirin2.c:180
volatile bool_t gyr_valid
Definition: imu_aspirin2.c:39
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
transaction set to done by user level
Definition: i2c.h:59
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
#define MPU_OFFSET_ACC
bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:238
signed short int16_t
Definition: types.h:17
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
#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:38
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
#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:319
struct i2c_transaction aspirin2_mpu60x0
Definition: imu_aspirin2.c:43
int32_t q
in rad/s with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
void imu_periodic(void)
Definition: imu_aspirin2.c:153
int32_t r
in rad/s with INT32_RATE_FRAC
volatile bool_t acc_valid
Definition: imu_aspirin2.c:40
#define MPU60X0_REG_GYRO_CONFIG
Definition: mpu60x0_regs.h:52
transaction is pending in queue
Definition: i2c.h:55
#define MPU60X0_REG_ACCEL_CONFIG
Definition: mpu60x0_regs.h:53
Architecture independent I2C (Inter-Integrated Circuit Bus) API.