Paparazzi UAS  v4.2.2_stable-4-gcc32f65
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  /* to pull PC13 high */
53 }
54 
55 
56 void imu_periodic(void)
57 {
58 
60  mpu_configure();
61  // imu_aspirin_arch_int_enable();
63 
64  aspirin2_mpu60x0.length = 22;
66 
67  for (int i=1;i<aspirin2_mpu60x0.length;i++) {
68  aspirin2_mpu60x0.mosi_buf[i] = 0;
69  }
70  }
71  else {
72 
73  // imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + MPU60X0_SPI_READ;
74  // imu_aspirin2.imu_tx_buf[1] = 0x00;
75 
77  }
78 }
79 
80 static inline void mpu_set(uint8_t _reg, uint8_t _val)
81 {
82  aspirin2_mpu60x0.mosi_buf[0] = _reg;
83  aspirin2_mpu60x0.mosi_buf[1] = _val;
85 
86  while(aspirin2_mpu60x0.status != SPITransSuccess);
87 }
88 
89 static inline void mpu_wait_slave4_ready(void)
90 {
91  uint8_t ret = 0x80;
92  while (ret & 0x80)
93  {
95  aspirin2_mpu60x0.mosi_buf[1] = 0;
97  while(aspirin2_mpu60x0.status != SPITransSuccess);
98 
99  ret = aspirin2_mpu60x0.miso_buf[1];
100  }
101 }
102 
103 
104 
105 static void mpu_configure(void)
106 {
107  aspirin2_mpu60x0.length = 2;
108 
110  // Configure power:
111 
112  // MPU60X0_REG_PWR_MGMT_1
114  0x01); // -switch to gyroX clock
115 
116  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
117  // -No standby and no wake timer
118 
120  // Measurement Settings
121 
122 #if PERIODIC_FREQUENCY == 60
123 // Accelerometer: Bandwidth 44Hz, Delay 4.9ms
124 // Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
125 # define MPU_DIG_FILTER 3
126 // -100Hz output = 1kHz / (9 + 1)
127 # define MPU_SMPLRT_DIV 9
128 #else
129 # if PERIODIC_FREQUENCY == 120
130 // Accelerometer: Bandwidth 44Hz, Delay 4.9ms
131 // Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
132 # define MPU_DIG_FILTER 3
133 // -100Hz output = 1kHz / (9 + 1)
134 # define MPU_SMPLRT_DIV 9
135 # else
136 # if PERIODIC_FREQUENCY == 512
137 // Accelerometer: Bandwidth 260Hz, Delay 0ms
138 // Gyroscope: Bandwidth 256Hz, Delay 0.89ms sampling 8Khz
139 # define MPU_DIG_FILTER 0
140 // -500Hz output = 1kHz / (1 + 1)
141 # define MPU_SMPLRT_DIV 1
142 # else
143 # error PERIODIC_FREQUENCY should be either 60Hz, 120Hz or 512Hz. Otherwise manually fix the sensor rates
144 # endif
145 # endif
146 #endif
147  // MPU60X0_REG_CONFIG
149  (2 << 3) | // Fsync / ext sync on gyro X (bit 3->6)
150  (MPU_DIG_FILTER << 0) ); // Low-Pass Filter
151 
152  // MPU60X0_REG_SMPLRT_DIV
153  mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV);
154 
155  // MPU60X0_REG_GYRO_CONFIG
157  (3 << 3) ); // -2000deg/sec
158 
159  // MPU60X0_REG_ACCEL_CONFIG
161  (0 << 0) | // No HPFL
162  (3 << 3) ); // Full Scale = 16g
163 
164 #ifndef MPU6000_NO_SLAVES
165 
167  // SPI Slave Configuration Section
168 
169  // Power the Aux I2C Circuit:
170  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd
171 
172  // MPU60X0_REG_USER_CTRL:
174  (1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode
175  (1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface
176  (0 << 1) ); // Trigger a I2C_MST_RESET
177 
178  // Enable the aux i2c
180  (0 << 7) | // no multimaster
181  (0 << 6) | // do not delay IRQ waiting for all external slaves
182  (0 << 5) | // no slave 3 FIFO
183  (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start
184  (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz
185 
186 
187  // HMC5883 Magnetometer Configuration
188 
193  (1 << 7) | // Slave 4 enable
194  (0 << 6) | // Byte Swap
195  (0 << 0) ); // Full Speed
196 
198 
203  (1 << 7) | // Slave 4 enable
204  (0 << 6) | // Byte Swap
205  (0 << 0) ); // Full Speed
206 
208 
213  (1 << 7) | // Slave 4 enable
214  (0 << 6) | // Byte Swap
215  (0 << 0) ); // Full Speed
216 
217 
218  // HMC5883 Reading:
219  // a) write hmc-register to HMC
220  // b) read 6 bytes from HMC
221 
224  // Put the enable command as last.
226  (1 << 7) | // Slave 0 enable
227  (0 << 6) | // Byte Swap
228  (6 << 0) ); // Read 6 bytes
229 
230  // Slave 0 Control:
231 
232 
233 #endif
234 
235 }
236 
#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:89
struct i2c_transaction aspirin2_mpu60x0
Definition: imu_aspirin2.c:46
static void mpu_set(uint8_t _reg, uint8_t _val)
Definition: imu_aspirin2.c:80
#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
void imu_aspirin2_arch_init(void)
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:105
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