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  * 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 #include "subsystems/imu.h"
23 
24 #include "led.h"
25 #include "mcu_periph/spi.h"
26 
27 // Peripherials
28 #include "peripherals/mpu60X0.h"
29 #include "peripherals/hmc58xx.h"
30 
31 
33 
34 struct spi_transaction aspirin2_mpu60x0;
35 
36 
37 // initialize peripherals
38 static void mpu_configure(void);
39 
40 
41 void imu_impl_init(void) {
42 
45 
49  aspirin2_mpu60x0.length = 2;
50 
51 }
52 
53 
54 void imu_periodic(void)
55 {
56 
58  mpu_configure();
59  // imu_aspirin_arch_int_enable();
61 
62  aspirin2_mpu60x0.length = 22;
64 
65  for (int i=1;i<aspirin2_mpu60x0.length;i++) {
66  aspirin2_mpu60x0.mosi_buf[i] = 0;
67  }
68  }
69  else {
70 
71  // imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + MPU60X0_SPI_READ;
72  // imu_aspirin2.imu_tx_buf[1] = 0x00;
73 
75  }
76 }
77 
78 static inline void mpu_set(uint8_t _reg, uint8_t _val)
79 {
80  aspirin2_mpu60x0.mosi_buf[0] = _reg;
81  aspirin2_mpu60x0.mosi_buf[1] = _val;
83 
84  while(aspirin2_mpu60x0.status != SPITransSuccess);
85 }
86 
87 static inline void mpu_wait_slave4_ready(void)
88 {
89  uint8_t ret = 0x80;
90  while (ret & 0x80)
91  {
93  aspirin2_mpu60x0.mosi_buf[1] = 0;
95  while(aspirin2_mpu60x0.status != SPITransSuccess);
96 
97  ret = aspirin2_mpu60x0.miso_buf[1];
98  }
99 }
100 
101 
102 
103 static void mpu_configure(void)
104 {
105  aspirin2_mpu60x0.length = 2;
106 
108  // Configure power:
109 
110  // MPU60X0_REG_PWR_MGMT_1
112  0x01); // -switch to gyroX clock
113 
114  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
115  // -No standby and no wake timer
116 
118  // Measurement Settings
119 
120 #if PERIODIC_FREQUENCY == 60
121 // Accelerometer: Bandwidth 44Hz, Delay 4.9ms
122 // Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
123 # define MPU_DIG_FILTER 3
124 // -100Hz output = 1kHz / (9 + 1)
125 # define MPU_SMPLRT_DIV 9
126 #else
127 # if PERIODIC_FREQUENCY == 120
128 // Accelerometer: Bandwidth 44Hz, Delay 4.9ms
129 // Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
130 # define MPU_DIG_FILTER 3
131 // -100Hz output = 1kHz / (9 + 1)
132 # define MPU_SMPLRT_DIV 9
133 # else
134 # if PERIODIC_FREQUENCY == 512
135 // Accelerometer: Bandwidth 260Hz, Delay 0ms
136 // Gyroscope: Bandwidth 256Hz, Delay 0.89ms sampling 8Khz
137 # define MPU_DIG_FILTER 0
138 // -500Hz output = 1kHz / (1 + 1)
139 # define MPU_SMPLRT_DIV 1
140 # else
141 # error PERIODIC_FREQUENCY should be either 60Hz, 120Hz or 512Hz. Otherwise manually fix the sensor rates
142 # endif
143 # endif
144 #endif
145  // MPU60X0_REG_CONFIG
147  (2 << 3) | // Fsync / ext sync on gyro X (bit 3->6)
148  (MPU_DIG_FILTER << 0) ); // Low-Pass Filter
149 
150  // MPU60X0_REG_SMPLRT_DIV
151  mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV);
152 
153  // MPU60X0_REG_GYRO_CONFIG
155  (3 << 3) ); // -2000deg/sec
156 
157  // MPU60X0_REG_ACCEL_CONFIG
159  (0 << 0) | // No HPFL
160  (3 << 3) ); // Full Scale = 16g
161 
162 #ifndef MPU6000_NO_SLAVES
163 
165  // SPI Slave Configuration Section
166 
167  // Power the Aux I2C Circuit:
168  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd
169 
170  // MPU60X0_REG_USER_CTRL:
172  (1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode
173  (1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface
174  (0 << 1) ); // Trigger a I2C_MST_RESET
175 
176  // Enable the aux i2c
178  (0 << 7) | // no multimaster
179  (0 << 6) | // do not delay IRQ waiting for all external slaves
180  (0 << 5) | // no slave 3 FIFO
181  (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start
182  (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz
183 
184 
185  // HMC5883 Magnetometer Configuration
186 
191  (1 << 7) | // Slave 4 enable
192  (0 << 6) | // Byte Swap
193  (0 << 0) ); // Full Speed
194 
196 
201  (1 << 7) | // Slave 4 enable
202  (0 << 6) | // Byte Swap
203  (0 << 0) ); // Full Speed
204 
206 
211  (1 << 7) | // Slave 4 enable
212  (0 << 6) | // Byte Swap
213  (0 << 0) ); // Full Speed
214 
215 
216  // HMC5883 Reading:
217  // a) write hmc-register to HMC
218  // b) read 6 bytes from HMC
219 
222  // Put the enable command as last.
224  (1 << 7) | // Slave 0 enable
225  (0 << 6) | // Byte Swap
226  (6 << 0) ); // Read 6 bytes
227 
228  // Slave 0 Control:
229 
230 
231 #endif
232 
233 }
234 
#define MPU60X0_REG_I2C_SLV4_CTRL
Definition: mpu60X0.h:56
#define MPU60X0_REG_I2C_SLV0_REG
Definition: mpu60X0.h:34
enum Aspirin2Status status
Definition: imu_aspirin2.h:100
#define MPU60X0_REG_SMPLRT_DIV
Definition: mpu60X0.h:23
#define HMC58XX_ADDR
Definition: hmc58xx.h:34
#define HMC58XX_CRB
Definition: hmc58xx.h:66
#define HMC58XX_MD
Definition: hmc58xx.h:62
#define MPU60X0_SPI_READ
Definition: mpu60X0.h:8
#define MPU60X0_REG_USER_CTRL
Definition: mpu60X0.h:12
void imu_impl_init(void)
Definition: imu_aspirin2.c:57
void spi_rw(struct spi_transaction *_trans)
Definition: spi_arch.c:106
#define FALSE
Definition: imu_chimu.h:141
volatile uint8_t imu_rx_buf[64]
Definition: imu_aspirin2.h:103
#define HMC58XX_CRA
Definition: hmc58xx.h:65
arch independent SPI (Serial Peripheral Interface) API
volatile uint8_t imu_tx_buf[64]
Definition: imu_aspirin2.h:102
struct ImuAspirin2 imu_aspirin2
Definition: imu_aspirin2.c:32
#define MPU60X0_REG_I2C_SLV0_ADDR
Definition: mpu60X0.h:33
#define MPU60X0_REG_PWR_MGMT_1
Definition: mpu60X0.h:13
static void mpu_wait_slave4_ready(void)
Definition: imu_aspirin2.c:87
struct i2c_transaction aspirin2_mpu60x0
Definition: imu_aspirin2.c:46
static void mpu_set(uint8_t _reg, uint8_t _val)
Definition: imu_aspirin2.c:78
#define MPU60X0_REG_I2C_MST_CTRL
Definition: mpu60X0.h:29
#define MPU60X0_REG_INT_STATUS
Definition: mpu60X0.h:62
#define MPU60X0_REG_I2C_SLV4_REG
Definition: mpu60X0.h:54
enum I2CTransactionStatus status
Definition: i2c.h:47
Inertial Measurement Unit interface.
#define HMC58XX_REG_CFGA
Definition: hmc58xx.h:37
#define MPU60X0_REG_I2C_SLV4_DO
Definition: mpu60X0.h:55
void imu_periodic(void)
Definition: imu_aspirin2.c:163
volatile uint8_t imu_spi_data_received
Definition: imu_aspirin2.h:101
unsigned char uint8_t
Definition: types.h:14
#define HMC58XX_REG_DATXM
Definition: hmc58xx.h:40
static void mpu_configure(void)
Definition: imu_aspirin2.c:103
arch independent LED (Light Emitting Diodes) API
#define MPU60X0_REG_CONFIG
Definition: mpu60X0.h:24
#define MPU60X0_REG_I2C_SLV4_ADDR
Definition: mpu60X0.h:53
#define HMC58XX_REG_MODE
Definition: hmc58xx.h:39
#define MPU60X0_REG_I2C_SLV0_CTRL
Definition: mpu60X0.h:35
#define MPU60X0_REG_GYRO_CONFIG
Definition: mpu60X0.h:25
#define MPU60X0_REG_ACCEL_CONFIG
Definition: mpu60X0.h:26
#define HMC58XX_REG_CFGB
Definition: hmc58xx.h:38