Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_gl1.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
3  * 2013 Felix Ruess <felix.ruess@gmail.com>
4  * 2013 Eduardo Lavratti <agressiva@hotmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
29 #include "subsystems/imu.h"
30 #include "subsystems/abi.h"
31 #include "mcu_periph/i2c.h"
32 
33 PRINT_CONFIG_VAR(GL1_I2C_DEV)
34 
35 
36 #ifndef GL1_ACCEL_RATE
37 # if PERIODIC_FREQUENCY <= 60
38 # define GL1_ACCEL_RATE ADXL345_RATE_50HZ
39 # elif PERIODIC_FREQUENCY <= 120
40 # define GL1_ACCEL_RATE ADXL345_RATE_100HZ
41 # else
42 # define GL1_ACCEL_RATE ADXL345_RATE_200HZ
43 # endif
44 #endif
45 PRINT_CONFIG_VAR(GL1_ACCEL_RATE)
46 
47 
48 #ifndef GL1_GYRO_LOWPASS
49 # if PERIODIC_FREQUENCY <= 60
50 # define GL1_GYRO_LOWPASS L3G4200_DLPF_1
51 # elif PERIODIC_FREQUENCY <= 120
52 # define GL1_GYRO_LOWPASS L3G4200_DLPF_2
53 # else
54 # define GL1_GYRO_LOWPASS L3G4200_DLPF_3
55 # endif
56 #endif
57 PRINT_CONFIG_VAR(GL1_GYRO_LOWPASS)
58 
59 
60 #ifndef GL1_GYRO_SMPLRT
61 # if PERIODIC_FREQUENCY <= 60
62 # define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
63 PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
64 # else
65 # define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
66 PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
67 # endif
68 #endif
69 PRINT_CONFIG_VAR(GL1_GYRO_SMPLRT)
70 
71 #ifdef GL1_GYRO_SCALE
72 # define L3G4200_SCALE GL1_GYRO_SCALE
73 # else
74 # define L3G4200_SCALE L3G4200_SCALE_2000
75 #endif
76 PRINT_CONFIG_VAR(L3G4200_SCALE)
77 
79 
80 void imu_gl1_init(void)
81 {
82  /* Set accel configuration */
83  adxl345_i2c_init(&imu_gl1.acc_adxl, &(GL1_I2C_DEV), ADXL345_ADDR);
84  // set the data rate
87  //imu_aspirin.acc_adxl.config.drdy_int_enable = true;
88 
89 
90  /* Gyro configuration and initalization */
91  l3g4200_init(&imu_gl1.gyro_l3g, &(GL1_I2C_DEV), L3G4200_ADDR_ALT);
92  /* change the default config */
93  // output data rate, bandwidth, enable axis (0x1f = 100 ODR, 25hz) (0x5f = 200hz ODR, 25hz)
94  imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT << 6) | (GL1_GYRO_LOWPASS << 4) | 0xf);
95  // senstivity
96  imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE << 4) | 0x00;
97  // filter config
98  imu_gl1.gyro_l3g.config.ctrl_reg5 = 0x00; // only first LPF active
99 
100 
101  /* initialize mag and set default options */
102  hmc58xx_init(&imu_gl1.mag_hmc, &(GL1_I2C_DEV), HMC58XX_ADDR);
103  imu_gl1.mag_hmc.type = HMC_TYPE_5883;
104 }
105 
106 
108 {
110 
111  // Start reading the latest gyroscope data
113 
114  // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
115  RunOnceEvery(10, hmc58xx_periodic(&imu_gl1.mag_hmc));
116 }
117 
118 void imu_gl1_event(void)
119 {
120  uint32_t now_ts = get_sys_time_usec();
121 
130  AbiSendMsgIMU_ACCEL_INT32(IMU_GL1_ID, now_ts, &imu.accel);
131  }
132 
133  /* If the lg34200 I2C transaction has succeeded: convert the data */
139  imu.gyro_unscaled.r = -imu_gl1.gyro_l3g.data.rates.r;
142  AbiSendMsgIMU_GYRO_INT32(IMU_GL1_ID, now_ts, &imu.gyro);
143  }
144 
145  /* HMC58XX event task */
148  // VECT3_COPY(imu.mag_unscaled, imu_gl1.mag_hmc.data.vect);
151  imu.mag_unscaled.z = -imu_gl1.mag_hmc.data.vect.z;
153  imu_scale_mag(&imu);
154  AbiSendMsgIMU_MAG_INT32(IMU_GL1_ID, now_ts, &imu.mag);
155  }
156 }
void imu_gl1_event(void)
Definition: imu_gl1.c:118
void l3g4200_event(struct L3g4200 *l3g)
Definition: l3g4200.c:114
volatile bool data_available
data ready flag
Definition: hmc58xx.h:65
struct L3g4200 gyro_l3g
Definition: imu_gl1.h:45
enum Hmc58xxType type
Definition: hmc58xx.h:71
union Hmc58xx::@301 data
#define GL1_GYRO_LOWPASS
gyro internal lowpass frequency
Definition: imu_gl1.c:50
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:349
#define L3G4200_SCALE
Definition: imu_gl1.c:74
void imu_scale_accel(struct Imu *_imu)
Definition: ahrs_gx3.c:350
#define GL1_ACCEL_RATE
adxl345 accelerometer output rate, lowpass is set to half of rate
Definition: imu_gl1.c:38
enum Adxl345Rates rate
Data Output Rate.
Definition: adxl345.h:53
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
struct Adxl345_I2c acc_adxl
Definition: imu_gl1.h:44
Main include for ABI (AirBorneInterface).
struct Imu imu
global IMU state
Definition: imu.c:108
struct L3g4200Config config
Definition: l3g4200.h:77
void imu_gl1_periodic(void)
Definition: imu_gl1.c:107
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
static void l3g4200_periodic(struct L3g4200 *l3g)
convenience function: read or start configuration if not already initialized
Definition: l3g4200.h:88
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
struct Hmc58xx mag_hmc
Definition: imu_gl1.h:46
int32_t r
in rad/s with INT32_RATE_FRAC
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
struct Adxl345Config config
Definition: adxl345_i2c.h:49
static void adxl345_i2c_periodic(struct Adxl345_I2c *adxl)
convenience function: read or start configuration if not already initialized
Definition: adxl345_i2c.h:59
void adxl345_i2c_event(struct Adxl345_I2c *adxl)
Definition: adxl345_i2c.c:114
unsigned long uint32_t
Definition: types.h:18
struct ImuGL1I2c imu_gl1
Definition: imu_gl1.c:78
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define IMU_GL1_ID
uint8_t ctrl_reg5
Definition: l3g4200.h:55
void imu_gl1_init(void)
Definition: imu_gl1.c:80
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
void adxl345_i2c_init(struct Adxl345_I2c *adxl, struct i2c_periph *i2c_p, uint8_t addr)
Definition: adxl345_i2c.c:36
void l3g4200_init(struct L3g4200 *l3g, struct i2c_periph *i2c_p, uint8_t addr)
Initialize L3g4200 struct and set default config options.
Definition: l3g4200.c:47
uint8_t ctrl_reg4
Definition: l3g4200.h:54
union L3g4200::@303 data
#define L3G4200_ADDR_ALT
Definition: l3g4200_regs.h:32
union Adxl345_I2c::@291 data
uint8_t ctrl_reg1
Definition: l3g4200.h:53
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
static void hmc58xx_periodic(struct Hmc58xx *hmc)
convenience function: read or start configuration if not already initialized
Definition: hmc58xx.h:85
int32_t p
in rad/s with INT32_RATE_FRAC
#define ADXL345_ADDR
default I2C address
Definition: adxl345_regs.h:32
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
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
#define GL1_GYRO_SMPLRT
gyro sample rate divider
Definition: imu_gl1.c:62
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
volatile bool data_available
data ready flag
Definition: l3g4200.h:72
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:152
int32_t q
in rad/s with INT32_RATE_FRAC
volatile bool data_available
data ready flag
Definition: adxl345_i2c.h:44
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
Architecture independent I2C (Inter-Integrated Circuit Bus) API.