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_krooz_memsic.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
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 
32 #include <math.h>
35 #include "mcu_periph/i2c.h"
36 #include "led.h"
37 #include "filters/median_filter.h"
38 #include "mcu_periph/sys_time.h"
39 
40 #if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
41 #define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
42 #define KROOZ_SMPLRT_DIV 1
43 #endif
46 
47 #ifndef KROOZ_GYRO_RANGE
48 #define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
49 #endif
51 
52 #ifndef KROOZ_ACCEL_RANGE
53 #define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
54 #endif
56 
58 
59 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
60 struct MedianFilter3Int median_accel;
61 #endif
63 
67 
68 void imu_impl_init( void )
69 {
71  // MPU-60X0
72  mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
73  // change the default configuration
78  imu_krooz.mpu.config.drdy_int_enable = TRUE;
79 
80  hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
81 
82  // Init median filters
83 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
84  InitMedianFilterVect3Int(median_accel);
85 #endif
87 
88  RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
89  VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
90  imu_krooz.meas_nb = 0;
91 
92  imu_krooz.gyr_valid = FALSE;
93  imu_krooz.acc_valid = FALSE;
94  imu_krooz.mag_valid = FALSE;
95 
96  imu_krooz.hmc_eoc = FALSE;
97  imu_krooz.mpu_eoc = FALSE;
98 
99  imu_krooz.ad7689_trans.slave_idx = IMU_KROOZ_SPI_SLAVE_IDX;
101  imu_krooz.ad7689_trans.cpol = SPICpolIdleLow;
102  imu_krooz.ad7689_trans.cpha = SPICphaEdge1;
103  imu_krooz.ad7689_trans.dss = SPIDss8bit;
104  imu_krooz.ad7689_trans.bitorder = SPIMSBFirst;
105  imu_krooz.ad7689_trans.cdiv = SPIDiv16;
106  imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer);
107  imu_krooz.ad7689_trans.output_buf = (uint8_t*) imu_krooz.ad7689_spi_tx_buffer;
108  imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer);
109  imu_krooz.ad7689_trans.input_buf = (uint8_t*) imu_krooz.ad7689_spi_rx_buffer;
110  imu_krooz.ad7689_trans.before_cb = NULL;
111  imu_krooz.ad7689_trans.after_cb = NULL;
112  axis_cnt = 0;
113  axis_nb = 2;
114 
116 }
117 
118 void imu_periodic( void )
119 {
120  // Start reading the latest gyroscope data
121  if (!imu_krooz.mpu.config.initialized)
122  mpu60x0_i2c_start_configure(&imu_krooz.mpu);
123 
124  if (!imu_krooz.hmc.initialized)
125  hmc58xx_start_configure(&imu_krooz.hmc);
126 
127  if (imu_krooz.meas_nb) {
128  RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
129  imu_krooz.rates_sum.p / imu_krooz.meas_nb,
130  imu_krooz.rates_sum.r / imu_krooz.meas_nb);
131 
132  RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
133  imu_krooz.meas_nb = 0;
134  imu_krooz.gyr_valid = TRUE;
135  }
136 
137  if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
138  imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
139  imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
140  imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
141 
142 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
144 #endif
149 
150  INT_VECT3_ZERO(imu_krooz.accel_sum);
151  INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
152  imu_krooz.acc_valid = TRUE;
153  }
154 
155  RunOnceEvery(128,{axis_nb = 5;});
156 }
157 
158 void imu_krooz_event( void )
159 {
160  if (imu_krooz.mpu_eoc) {
161  mpu60x0_i2c_read(&imu_krooz.mpu);
162  imu_krooz.mpu_eoc = FALSE;
163  }
164 
165  // If the MPU6050 I2C transaction has succeeded: convert the data
166  mpu60x0_i2c_event(&imu_krooz.mpu);
167  if (imu_krooz.mpu.data_available) {
168  RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
169  imu_krooz.meas_nb++;
170  imu_krooz.mpu.data_available = FALSE;
171  }
172 
173  if (SysTimeTimer(ad7689_event_timer) > 215) {
174  SysTimeTimerStart(ad7689_event_timer);
175  if (axis_cnt < axis_nb)
176  axis_cnt++;
177  else
178  axis_cnt = 0;
179  imu_krooz.ad7689_trans.output_buf[0] =
180  axis_cnt <= 2 ? 0xF0 | (axis_cnt << 1) : (axis_cnt >= 4 ? 0xF0 | ((axis_cnt - 3) << 1) : 0xB0);
181  imu_krooz.ad7689_trans.output_buf[1] = 0x44;
182  spi_submit(&(IMU_KROOZ_SPI_DEV), &imu_krooz.ad7689_trans);
183  }
184  if (imu_krooz.ad7689_trans.status == SPITransSuccess) {
185  imu_krooz.ad7689_trans.status = SPITransDone;
186  uint16_t buf = (imu_krooz.ad7689_trans.input_buf[0] << 8) | imu_krooz.ad7689_trans.input_buf[1];
187  switch(axis_cnt) {
188  case 0:
189  case 3:
190  imu_krooz.accel_sum.x += (int32_t)buf;
191  imu_krooz.meas_nb_acc.x++;
192  break;
193  case 1:
194  case 4:
195  imu_krooz.accel_sum.y += (int32_t)buf;
196  imu_krooz.meas_nb_acc.y++;
197  break;
198  case 2:
199  imu_krooz.accel_sum.z += (int32_t)buf;
200  imu_krooz.meas_nb_acc.z++;
201  break;
202  case 5:
203  imu_krooz.temperature = (imu_krooz.temperature * 4 + (int32_t)buf) / 5;
204  //imu.temperature = 33000 * imu_krooz.temp / 65536 - 2400;
205  axis_nb = 2;
206  break;
207  default:
208  axis_cnt = 0;
209  break;
210  }
211  }
212 
213  if (imu_krooz.hmc_eoc) {
214  hmc58xx_read(&imu_krooz.hmc);
215  imu_krooz.hmc_eoc = FALSE;
216  }
217 
218  // If the HMC5883 I2C transaction has succeeded: convert the data
219  hmc58xx_event(&imu_krooz.hmc);
220  if (imu_krooz.hmc.data_available) {
221  VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
223  imu_krooz.hmc.data_available = FALSE;
224  imu_krooz.mag_valid = TRUE;
225  }
226 }
struct Uint8Vect3 meas_nb_acc
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:152
unsigned short uint16_t
Definition: types.h:16
void imu_periodic(void)
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:149
volatile bool_t acc_valid
Definition: imu_krooz.h:109
uint8_t input_length
number of data words to read
Definition: spi.h:145
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:81
static uint8_t axis_cnt
enum Mpu60x0DLPF dlpf_cfg
Digital Low Pass Filter.
Definition: mpu60x0.h:75
#define KROOZ_LOWPASS_FILTER
void hmc58xx_start_configure(struct Hmc58xx *hmc)
Definition: hmc58xx.c:125
int32_t p
in rad/s with INT32_RATE_FRAC
void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
Definition: mpu60x0_i2c.c:31
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:150
volatile bool_t gyr_valid
Definition: imu_krooz.h:108
#define IMU_KROOZ_ACCEL_AVG_FILTER
Definition: imu_krooz.h:104
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
#define KROOZ_ACCEL_RANGE
void imu_krooz_sd_arch_init(void)
volatile bool_t mpu_eoc
Definition: imu_krooz.h:111
Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
#define InitMedianFilterVect3Int(_f)
Definition: median_filter.h:86
union Mpu60x0_I2c::@38 data_rates
struct ImuKrooz imu_krooz
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
uint8_t smplrt_div
Sample rate divider.
Definition: mpu60x0.h:74
union Hmc58xx::@29 data
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:70
#define FALSE
Definition: imu_chimu.h:141
enum SPITransactionStatus status
Definition: spi.h:156
int32_t temperature
Definition: imu_krooz.h:119
volatile uint8_t meas_nb
Definition: imu_krooz.h:117
bool_t drdy_int_enable
Enable Data Ready Interrupt.
Definition: mpu60x0.h:78
static uint32_t ad7689_event_timer
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
Definition: spi.h:84
void hmc58xx_read(struct Hmc58xx *hmc)
Definition: hmc58xx.c:138
enum Mpu60x0AccelRanges accel_range
g Range
Definition: mpu60x0.h:77
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.
volatile uint8_t ad7689_spi_tx_buffer[2]
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
bool_t initialized
config done flag
Definition: mpu60x0.h:82
unsigned long uint32_t
Definition: types.h:18
volatile uint8_t ad7689_spi_rx_buffer[2]
#define KROOZ_SMPLRT_DIV
uint8_t output_length
number of data words to write
Definition: spi.h:146
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
enum Mpu60x0GyroRanges gyro_range
deg/s Range
Definition: mpu60x0.h:76
CPOL = 0.
Definition: spi.h:77
struct Int32Vect3 accel_filtered
Definition: imu_krooz.h:118
#define INT_VECT3_ZERO(_v)
volatile bool_t data_available
data ready flag
Definition: mpu60x0_i2c.h:57
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
#define MPU60X0_ADDR
Definition: mpu60x0_regs.h:32
struct spi_transaction ad7689_trans
struct Int16Rates rates
rates data as angular rates in gyro coordinate system
Definition: mpu60x0_i2c.h:63
signed long int32_t
Definition: types.h:19
struct Int16Vect3 vect
data vector in mag coordinate system
Definition: hmc58xx.h:67
#define SysTimeTimer(_t)
Definition: sys_time.h:199
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 accel_sum
Definition: imu_krooz.h:116
#define UpdateMedianFilterVect3Int(_f, _v)
Definition: median_filter.h:95
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:136
volatile bool_t hmc_eoc
Definition: imu_krooz.h:112
struct adc_buf * buf
Definition: adc_arch.c:586
struct Mpu60x0Config config
Definition: mpu60x0_i2c.h:67
struct Hmc58xx hmc
Definition: imu_krooz.h:114
volatile bool_t mag_valid
Definition: imu_krooz.h:110
unsigned char uint8_t
Definition: types.h:14
void imu_impl_init(void)
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
CPHA = 0.
Definition: spi.h:68
Definition: spi.h:117
slave is selected before transaction and unselected after
Definition: spi.h:57
static uint8_t axis_nb
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:147
void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:60
struct MedianFilter3Int median_mag
int32_t q
in rad/s with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
void hmc58xx_init(struct Hmc58xx *hmc, struct i2c_periph *i2c_p, uint8_t addr)
Initialize Hmc58xx struct and set default config options.
Definition: hmc58xx.c:75
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
void imu_krooz_event(void)
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
#define SysTimeTimerStart(_t)
Definition: sys_time.h:198
int32_t r
in rad/s with INT32_RATE_FRAC
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:154
bool_t initialized
config done flag
Definition: hmc58xx.h:63
#define KROOZ_GYRO_RANGE
struct Mpu60x0_I2c mpu
Definition: imu_krooz.h:113
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:151
volatile bool_t data_available
data ready flag
Definition: hmc58xx.h:65
struct Int32Rates rates_sum
Definition: imu_krooz.h:115
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:155
Architecture independent I2C (Inter-Integrated Circuit Bus) API.