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_ppzuav.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_ppzuav.h"
23 #include "mcu_periph/i2c.h"
24 #include "led.h"
25 
26 // Set SPI_CS High
27 #include "mcu_periph/gpio_arch.h"
28 
29 // Downlink
30 #include "mcu_periph/uart.h"
31 #include "messages.h"
33 
34 #ifndef DOWNLINK_DEVICE
35 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
36 #endif
37 
38 
39 // Peripherials
40 #define HMC5843_NO_IRQ
41 #include "../../peripherals/itg3200.h"
42 #include "../../peripherals/adxl345.h"
43 #include "../../peripherals/hmc5843.h"
44 
45 // Results
46 volatile bool_t mag_valid;
47 volatile bool_t gyr_valid;
48 volatile bool_t acc_valid;
49 
50 // Communication
54 
55 // Standalone option: run module only
56 #ifndef IMU_TYPE_H
57 struct Imu imu;
58 #endif
59 
60 #ifndef PERIODIC_FREQUENCY
61 #define PERIODIC_FREQUENCY 60
62 #endif
63 
64 void imu_impl_init(void)
65 {
67 
69  // ITG3200
73 #if PERIODIC_FREQUENCY == 60
74  /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
75  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
76 # pragma message "ITG3200 read at 50Hz."
77 #else
78 # if PERIODIC_FREQUENCY == 120
79 # pragma message "ITG3200 read at 100Hz."
80  /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
81  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
82 # else
83 # error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
84 # endif
85 #endif
87  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
89 
90  /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
92 #if PERIODIC_FREQUENCY == 60
93  ppzuavimu_itg3200.buf[1] = 19; // 50Hz
94 #else
95  ppzuavimu_itg3200.buf[1] = 9; // 100Hz
96 #endif
97  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
99 
100  /* switch to gyroX clock */
102  ppzuavimu_itg3200.buf[1] = 0x01;
103  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
105 
106  /* no interrupts for now, but set data ready interrupt to enable reading status bits */
108  ppzuavimu_itg3200.buf[1] = 0x01;
109  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
111 
113  // ADXL345
114 
115  /* set data rate to 100Hz */
119 #if PERIODIC_FREQUENCY == 60
120  ppzuavimu_adxl345.buf[1] = ADXL345_RATE_50; // normal power and 50Hz sampling, 25Hz BW
121 #else
122  ppzuavimu_adxl345.buf[1] = ADXL345_RATE_100; // normal power and 100Hz sampling, 50Hz BW
123 #endif
125  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
127 
128  /* switch to measurement mode */
131  ppzuavimu_adxl345.buf[1] = 1<<3;
133  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
135 
136  /* Set range to 16g but keeping full resolution of 3.9 mV/g */
140  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
142 
144  // HMC5843
147  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
148  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
150  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
152 
154  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
155  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
157  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
159 
161  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
162  ppzuavimu_hmc5843.buf[1] = 0x00;
164  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
166 
167 }
168 
169 void imu_periodic( void )
170 {
171  // Start reading the latest gyroscope data
176  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
177 
178  // Start reading the latest accelerometer data
183  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
184 
185  // Start reading the latest magnetometer data
186 #if PERIODIC_FREQUENCY > 60
187  RunOnceEvery(2,{
188 #endif
193  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
194 #if PERIODIC_FREQUENCY > 60
195  });
196 #endif
197  //RunOnceEvery(10,ppzuavimu_module_downlink_raw());
198 }
199 
201 {
205 }
206 
208 {
209  int32_t x, y, z;
210 
211  // If the itg3200 I2C transaction has succeeded: convert the data
213  {
214 #define ITG_STA_DAT_OFFSET 3
218 
219  // Is this is new data
220  if (ppzuavimu_itg3200.buf[0] & 0x01)
221  {
222  //LED_ON(3);
223  gyr_valid = TRUE;
224  //LED_OFF(3);
225  }
226  else
227  {
228  }
229 
230  // Signs depend on the way sensors are soldered on the board: so they are hardcoded
231 #ifdef ASPIRIN_IMU
232  RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
233 #else // PPZIMU
234  RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
235 #endif
236 
237  ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
238  }
239 
240  // If the adxl345 I2C transaction has succeeded: convert the data
242  {
243  x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
244  y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
245  z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
246 
247 #ifdef ASPIRIN_IMU
248  VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
249 #else // PPZIMU
250  VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
251 #endif
252 
253  acc_valid = TRUE;
255  }
256 
257  // If the hmc5843 I2C transaction has succeeded: convert the data
259  {
260  x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
261  y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
262  z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
263 
264 #ifdef ASPIRIN_IMU
265  VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
266 #else // PPZIMU
267  VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
268 #endif
269 
270  mag_valid = TRUE;
272  }
273 }
#define ITG3200_REG_INT_CFG
Definition: itg3200.h:12
#define ADXL345_RATE_100
Definition: adxl345.h:28
#define HMC5843_REG_CFGA
Definition: hmc5843.h:60
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define ITG3200_REG_SMPLRT_DIV
Definition: itg3200.h:10
#define ITG3200_REG_INT_STATUS
Definition: itg3200.h:13
#define ADXL345_REG_DATA_FORMAT
Definition: adxl345.h:12
#define ADXL345_RATE_50
Definition: adxl345.h:29
int32_t p
in rad/s^2 with INT32_RATE_FRAC
struct i2c_transaction ppzuavimu_itg3200
Definition: imu_ppzuav.c:52
#define HMC5843_ADDR
Definition: hmc5843.h:57
#define ITG3200_ADDR
Definition: itg3200.h:5
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
abstract IMU interface providing fixed point interface
Definition: imu.h:39
#define HMC5843_REG_CFGB
Definition: hmc5843.h:61
volatile bool_t acc_valid
Definition: imu_ppzuav.c:48
struct i2c_transaction ppzuavimu_hmc5843
Definition: imu_ppzuav.c:51
#define ADXL345_REG_POWER_CTL
Definition: adxl345.h:10
uint8_t slave_addr
Definition: i2c.h:43
#define HMC5843_REG_MODE
Definition: hmc5843.h:62
#define ITG_STA_DAT_OFFSET
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
#define ADXL345_ADDR
Definition: adxl345.h:5
void ppzuavimu_module_downlink_raw(void)
Definition: imu_ppzuav.c:200
volatile bool_t gyr_valid
Definition: imu_ppzuav.c:47
#define HMC5843_REG_DATXM
Definition: hmc5843.h:63
signed short int16_t
Definition: types.h:17
#define GPIO_ARCH_SET_SPI_CS_HIGH()
Definition: gpio_arch.h:5
#define ADXL345_REG_BW_RATE
Definition: adxl345.h:9
enum I2CTransactionStatus status
Definition: i2c.h:47
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
#define ADXL345_FULL_RES
Definition: adxl345.h:33
signed long int32_t
Definition: types.h:19
#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
void imu_periodic(void)
Definition: imu_ppzuav.c:169
uint8_t len_w
Definition: i2c.h:45
uint16_t len_r
Definition: i2c.h:44
volatile bool_t mag_valid
Definition: imu_ppzuav.c:46
#define ADXL345_REG_DATA_X0
Definition: adxl345.h:13
#define ADXL345_RANGE_16G
Definition: adxl345.h:35
#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
struct i2c_transaction ppzuavimu_adxl345
Definition: imu_ppzuav.c:53
struct Imu imu
global IMU state
Definition: imu_ppzuav.c:57
int32_t r
in rad/s^2 with INT32_RATE_FRAC
#define ITG3200_REG_DLPF_FS
Definition: itg3200.h:11
Definition: i2c.h:9
void ppzuavimu_module_event(void)
Definition: imu_ppzuav.c:207
#define ITG3200_REG_PWR_MGM
Definition: itg3200.h:22
void imu_impl_init(void)
Definition: imu_ppzuav.c:64