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_aspirin_2_spi.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
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 
27 #include "subsystems/imu.h"
28 #include "mcu_periph/sys_time.h"
29 #include "mcu_periph/spi.h"
31 
32 /* defaults suitable for Lisa */
33 #ifndef ASPIRIN_2_SPI_SLAVE_IDX
34 #define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
35 #endif
37 
38 #ifndef ASPIRIN_2_SPI_DEV
39 #define ASPIRIN_2_SPI_DEV spi2
40 #endif
42 
43 /* MPU60x0 gyro/accel internal lowpass frequency */
44 #if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
45 #if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
46 /* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
47  * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
48  */
49 #define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
50 #define ASPIRIN_2_SMPLRT_DIV 9
51 PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
52 #elif PERIODIC_FREQUENCY == 512
53 /* Accelerometer: Bandwidth 260Hz, Delay 0ms
54  * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
55  */
56 #define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
57 #define ASPIRIN_2_SMPLRT_DIV 3
58 PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
59 #else
60 #error Non-default PERIODIC_FREQUENCY: please define ASPIRIN_2_LOWPASS_FILTER and ASPIRIN_2_SMPLRT_DIV.
61 #endif
62 #endif
63 PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
64 PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
65 
66 #ifndef ASPIRIN_2_GYRO_RANGE
67 #define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
68 #endif
70 
71 #ifndef ASPIRIN_2_ACCEL_RANGE
72 #define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
73 #endif
75 
76 
77 /* HMC58XX default conf */
78 #ifndef HMC58XX_DO
79 #define HMC58XX_DO 0x6 // Data Output Rate (6 -> 75Hz with HMC5883)
80 #endif
81 #ifndef HMC58XX_MS
82 #define HMC58XX_MS 0x0 // Measurement configuration
83 #endif
84 #ifndef HMC58XX_GN
85 #define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss)
86 #endif
87 #ifndef HMC58XX_MD
88 #define HMC58XX_MD 0x0 // Continious measurement mode
89 #endif
90 
91 #define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS))
92 #define HMC58XX_CRB (HMC58XX_GN<<5)
93 
95 #ifndef ASPIRIN_2_MAG_STARTUP_DELAY
96 #define ASPIRIN_2_MAG_STARTUP_DELAY 1.5
97 #endif
98 
100 
101 void mpu_wait_slave4_ready(void);
104 
105 void imu_impl_init(void)
106 {
110 
112  // change the default configuration
113  imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
114  imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
117 
118  /* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */
120 
121  /* HMC5883 magnetometer as I2C slave */
123 
124  /* set function to configure mag */
126 
127 
128  /* Set MPU I2C master clock */
130  /* Enable I2C slave0 delayed sample rate */
132 
133 
134  /* configure spi transaction for wait_slave4 */
140 
149 
152 }
153 
154 
155 void imu_periodic(void)
156 {
158 }
159 
160 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
161 
163 {
166  /* HMC5883 has xzy order of axes in returned data */
167  struct Int32Vect3 mag;
171 
172 /* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */
173 #ifdef LISA_S
174 #ifdef LISA_S_UPSIDE_DOWN
183  VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
184 #else
188 #endif
189 #else
190 
191 /* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like
192  * IMU.
193  */
194 #ifdef LISA_M_OR_MX_21
203  VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z);
204 #else
205 
206 /* Handle real Aspirin IMU axis assignement. */
207 #ifdef LISA_M_LONGITUDINAL_X
216  VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
217 #else
220  VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
221 #endif
222 #endif
223 #endif
224 
229  }
230 }
231 
232 // hack with waiting to avoid creating another event loop to check the mag config status
233 static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t _reg, uint8_t _val)
234 {
235  mpu_set(mpu, _reg, _val);
237 }
238 
243 {
244  // wait before starting the configuration of the HMC58xx mag
245  // doing to early may void the mode configuration
247  return FALSE;
248 
252  mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
253 
255 
259  mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
260 
262 
266  mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
267 
270  // Put the enable command as last.
272  (1 << 7) | // Slave 0 enable
273  (6 << 0) ); // Read 6 bytes
274 
275  return TRUE;
276 }
277 
279 {
280  while (!imu_aspirin2.slave4_ready) {
284  }
285  }
286 }
287 
289 {
290  if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE))
292  else
294  t->status = SPITransDone;
295 }
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:152
void imu_periodic(void)
#define HMC58XX_MD
void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: mpu60x0_spi.c:31
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:149
struct Int16Rates rates
rates data as angular rates in gyro coordinate system
Definition: mpu60x0_spi.h:62
#define MPU60X0_REG_I2C_SLV0_REG
Definition: mpu60x0_regs.h:61
struct ImuAspirin2Spi imu_aspirin2
uint8_t input_length
number of data words to read
Definition: spi.h:145
#define MPU60X0_I2C_SLV4_DONE
Definition: mpu60x0_regs.h:131
enum Mpu60x0DLPF dlpf_cfg
Digital Low Pass Filter.
Definition: mpu60x0.h:75
#define MPU60X0_REG_I2C_SLV0_CTRL
Definition: mpu60x0_regs.h:62
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:150
CPHA = 1.
Definition: spi.h:69
void mpu_wait_slave4_ready(void)
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
Definition: mpu60x0_spi.c:99
uint8_t nb_slaves
number of used I2C slaves
Definition: mpu60x0.h:89
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
volatile bool_t accel_valid
volatile uint8_t wait_slave4_tx_buf[1]
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
#define ASPIRIN_2_ACCEL_RANGE
union Mpu60x0_Spi::@39 data_accel
#define HMC58XX_REG_CFGA
Definition: hmc58xx_regs.h:34
volatile bool_t slave4_ready
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
CPOL = 1.
Definition: spi.h:78
uint8_t smplrt_div
Sample rate divider.
Definition: mpu60x0.h:74
#define ASPIRIN_2_GYRO_RANGE
#define ASPIRIN_2_SPI_DEV
bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu)
function to configure hmc5883 mag
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:123
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
#define FALSE
Definition: imu_chimu.h:141
enum SPITransactionStatus status
Definition: spi.h:156
Architecture independent SPI (Serial Peripheral Interface) API.
Mpu60x0I2cSlaveConfigure configure
Definition: mpu60x0.h:70
uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN]
Definition: mpu60x0_spi.h:65
#define MPU60X0_SPI_READ
Definition: mpu60x0_regs.h:35
struct Mpu60x0Config config
Definition: mpu60x0_spi.h:66
static void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu)
convenience function: read or start configuration if not already initialized
Definition: mpu60x0_spi.h:77
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
Definition: spi.h:84
enum Mpu60x0AccelRanges accel_range
g Range
Definition: mpu60x0.h:77
union Mpu60x0_Spi::@40 data_rates
volatile bool_t gyro_valid
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition: spi.h:153
bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit a spi transaction.
Definition: spi_arch.c:470
Architecture independent timing functions.
uint8_t nb_bytes
number of bytes to read starting with MPU60X0_REG_INT_STATUS
Definition: mpu60x0.h:80
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
void imu_aspirin2_event(void)
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
uint8_t output_length
number of data words to write
Definition: spi.h:146
volatile uint8_t wait_slave4_rx_buf[2]
#define MPU60X0_REG_I2C_SLV4_REG
Definition: mpu60x0_regs.h:81
#define HMC58XX_REG_DATXM
Definition: hmc58xx_regs.h:37
enum Mpu60x0GyroRanges gyro_range
deg/s Range
Definition: mpu60x0.h:76
Inertial Measurement Unit interface.
struct Mpu60x0I2cSlave slaves[5]
I2C slaves.
Definition: mpu60x0.h:90
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
#define TRUE
Definition: imu_chimu.h:144
static void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void *mpu, uint8_t _reg, uint8_t _val)
uint8_t i2c_mst_delay
MPU I2C slaves delayed sample rate.
Definition: mpu60x0.h:92
#define MPU60X0_REG_I2C_SLV4_ADDR
Definition: mpu60x0_regs.h:80
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
Register defs for Honeywell HMC5843 and HMC5883 magnetometers.
enum Mpu60x0MstClk i2c_mst_clk
MPU I2C master clock speed.
Definition: mpu60x0.h:91
#define MPU60X0_REG_I2C_SLV0_ADDR
Definition: mpu60x0_regs.h:60
#define Int16FromBuf(_buf, _idx)
#define MPU60X0_REG_I2C_SLV4_CTRL
Definition: mpu60x0_regs.h:83
#define HMC58XX_CRA
volatile bool_t data_available
data ready flag
Definition: mpu60x0_spi.h:56
unsigned char uint8_t
Definition: types.h:14
#define ASPIRIN_2_SPI_SLAVE_IDX
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
struct spi_transaction wait_slave4_trans
slave is selected before transaction and unselected after
Definition: spi.h:57
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:147
struct Mpu60x0_Spi mpu
volatile bool_t mag_valid
#define MPU60X0_REG_I2C_MST_STATUS
Definition: mpu60x0_regs.h:57
Definition: spi.h:119
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
struct spi_periph * spi_p
Definition: mpu60x0_spi.h:52
#define HMC58XX_REG_CFGB
Definition: hmc58xx_regs.h:35
SPI transaction structure.
Definition: spi.h:142
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
#define MPU60X0_REG_I2C_SLV4_DO
Definition: mpu60x0_regs.h:82
#define HMC58XX_CRB
void mpu_wait_slave4_ready_cb(struct spi_transaction *t)
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:154
#define ASPIRIN_2_MAG_STARTUP_DELAY
delay in seconds before starting to configure HMC58xx mag slave
struct Int16Vect3 vect
accel data vector in accel coordinate system
Definition: mpu60x0_spi.h:58
void(* Mpu60x0ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val)
Configuration function prototype.
Definition: mpu60x0.h:64
#define HMC58XX_REG_MODE
Definition: hmc58xx_regs.h:36
struct spi_transaction spi_trans
Definition: mpu60x0_spi.h:53
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:155
void imu_impl_init(void)