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.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 
31 #include <math.h>
32 #include "boards/krooz/imu_krooz.h"
34 #include "mcu_periph/i2c.h"
35 #include "led.h"
36 #include "filters/median_filter.h"
37 
38 // Downlink
39 #include "mcu_periph/uart.h"
40 #include "messages.h"
42 
43 #if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
44 #define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
45 #define KROOZ_SMPLRT_DIV 1
46 #endif
49 
50 #ifndef KROOZ_GYRO_RANGE
51 #define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
52 #endif
54 
55 #ifndef KROOZ_ACCEL_RANGE
56 #define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
57 #endif
59 
61 
62 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
63 struct MedianFilter3Int median_accel;
64 #endif
66 
67 void imu_impl_init( void )
68 {
70  // MPU-60X0
71  mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
72  // change the default configuration
77  imu_krooz.mpu.config.drdy_int_enable = TRUE;
78 
79  hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
80 
81  // Init median filters
82 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
83  InitMedianFilterVect3Int(median_accel);
84 #endif
86 
87  RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
88  VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
89  imu_krooz.meas_nb = 0;
90 
91  imu_krooz.gyr_valid = FALSE;
92  imu_krooz.acc_valid = FALSE;
93  imu_krooz.mag_valid = FALSE;
94 
95  imu_krooz.hmc_eoc = FALSE;
96  imu_krooz.mpu_eoc = FALSE;
97 
99 }
100 
101 void imu_periodic( void )
102 {
103  // Start reading the latest gyroscope data
104  if (!imu_krooz.mpu.config.initialized)
105  mpu60x0_i2c_start_configure(&imu_krooz.mpu);
106 
107  if (!imu_krooz.hmc.initialized)
108  hmc58xx_start_configure(&imu_krooz.hmc);
109 
110  if (imu_krooz.meas_nb) {
111  RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
112  VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
113 
114 #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
116 #endif
121 
122  RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
123  VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
124  imu_krooz.meas_nb = 0;
125 
126  imu_krooz.gyr_valid = TRUE;
127  imu_krooz.acc_valid = TRUE;
128  }
129 
130  //RunOnceEvery(10,imu_krooz_downlink_raw());
131 }
132 
134 {
137 }
138 
139 
140 void imu_krooz_event( void )
141 {
142  if (imu_krooz.mpu_eoc) {
143  mpu60x0_i2c_read(&imu_krooz.mpu);
144  imu_krooz.mpu_eoc = FALSE;
145  }
146 
147  // If the MPU6050 I2C transaction has succeeded: convert the data
148  mpu60x0_i2c_event(&imu_krooz.mpu);
149  if (imu_krooz.mpu.data_available) {
150  RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
151  VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
152  imu_krooz.meas_nb++;
153  imu_krooz.mpu.data_available = FALSE;
154  }
155 
156  if (imu_krooz.hmc_eoc) {
157  hmc58xx_read(&imu_krooz.hmc);
158  imu_krooz.hmc_eoc = FALSE;
159  }
160 
161  // If the HMC5883 I2C transaction has succeeded: convert the data
162  hmc58xx_event(&imu_krooz.hmc);
163  if (imu_krooz.hmc.data_available) {
164  VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
166  imu_krooz.hmc.data_available = FALSE;
167  imu_krooz.mag_valid = TRUE;
168  }
169 }
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
volatile bool_t acc_valid
Definition: imu_krooz.h:109
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:81
enum Mpu60x0DLPF dlpf_cfg
Digital Low Pass Filter.
Definition: mpu60x0.h:75
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
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_SMPLRT_DIV
Definition: imu_krooz.c:45
void imu_krooz_sd_arch_init(void)
volatile bool_t mpu_eoc
Definition: imu_krooz.h:111
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
uint8_t smplrt_div
Sample rate divider.
Definition: mpu60x0.h:74
#define KROOZ_LOWPASS_FILTER
Definition: imu_krooz.c:44
union Hmc58xx::@29 data
#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
void imu_krooz_event(void)
Definition: imu_krooz.c:140
volatile uint8_t meas_nb
Definition: imu_krooz.h:117
void imu_krooz_downlink_raw(void)
Definition: imu_krooz.c:133
bool_t drdy_int_enable
Enable Data Ready Interrupt.
Definition: mpu60x0.h:78
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
void hmc58xx_read(struct Hmc58xx *hmc)
Definition: hmc58xx.c:138
enum Mpu60x0AccelRanges accel_range
g Range
Definition: mpu60x0.h:77
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
bool_t initialized
config done flag
Definition: mpu60x0.h:82
#define KROOZ_GYRO_RANGE
Definition: imu_krooz.c:51
void imu_impl_init(void)
Definition: imu_krooz.c:67
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
enum Mpu60x0GyroRanges gyro_range
deg/s Range
Definition: mpu60x0.h:76
struct Int32Vect3 accel_filtered
Definition: imu_krooz.h:118
#define KROOZ_ACCEL_RANGE
Definition: imu_krooz.c:56
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 Int16Rates rates
rates data as angular rates in gyro coordinate system
Definition: mpu60x0_i2c.h:63
struct Int16Vect3 vect
data vector in mag coordinate system
Definition: hmc58xx.h:67
#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
Driver for the IMU on the KroozSD board.
void imu_periodic(void)
Definition: imu_krooz.c:101
struct Mpu60x0Config config
Definition: mpu60x0_i2c.h:67
struct ImuKrooz imu_krooz
Definition: imu_krooz.c:60
struct Hmc58xx hmc
Definition: imu_krooz.h:114
volatile bool_t mag_valid
Definition: imu_krooz.h:110
struct MedianFilter3Int median_mag
Definition: imu_krooz.c:65
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:60
struct Int16Vect3 vect
accel data vector in accel coordinate system
Definition: mpu60x0_i2c.h:59
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
int32_t r
in rad/s with INT32_RATE_FRAC
bool_t initialized
config done flag
Definition: hmc58xx.h:63
union Mpu60x0_I2c::@37 data_accel
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
Architecture independent I2C (Inter-Integrated Circuit Bus) API.