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_px4fmu_v2.4.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013-2016 the paparazzi team
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 
29 #include "subsystems/imu.h"
30 #include "subsystems/abi.h"
31 #include "mcu_periph/spi.h"
35 
36 /* SPI defaults set in subsystem makefile, can be configured from airframe file */
37 PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
38 PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
39 PRINT_CONFIG_VAR(IMU_PX4FMU_SPI_DEV)
40 
41 struct ImuPX4 imu_px4;
42 
43 void imu_px4_init(void)
44 {
45 
46  /* L3GD20 gyro init */
47  /* initialize gyro and set default options */
48  l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
49 
50  /* LSM303dlhc acc + magneto init */
51  lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
52 #if !IMU_PX4_DISABLE_MAG
53  lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
54 #endif
55 
56 }
57 
58 void imu_px4_periodic(void)
59 {
62 
63 #if !IMU_PX4_DISABLE_MAG
64  /* Read magneto's every 10 times of main freq
65  * at ~50Hz (main loop for rotorcraft: 512Hz)
66  */
67  RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
68 #endif
69 }
70 
71 void imu_px4_event(void)
72 {
73 
74  uint32_t now_ts = get_sys_time_usec();
75 
76  /* L3GD20 event task */
79  //the p and q seem to be swapped on the Pixhawk board compared to the acc
85  AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
86  }
87 
88  /* LSM303dlhc event task */
94  AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
95  }
96 #if !IMU_PX4_DISABLE_MAG
101  imu_scale_mag(&imu);
102  AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag);
103  }
104 #endif
105 
106 }
void lsm303dlhc_spi_event(struct Lsm303dlhc_Spi *lsm)
#define IMU_PX4_ID
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:349
void l3gd20_spi_event(struct L3gd20_Spi *l3g)
Definition: l3gd20_spi.c:135
static void lsm303dlhc_spi_periodic(struct Lsm303dlhc_Spi *lsm)
convenience function: read or start configuration if not already initialized
void imu_scale_accel(struct Imu *_imu)
Definition: ahrs_gx3.c:350
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
Main include for ABI (AirBorneInterface).
void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: l3gd20_spi.c:30
struct Imu imu
global IMU state
Definition: imu.c:108
struct Lsm303dlhc_Spi lsm_acc
#define FALSE
Definition: std.h:5
Register defs for ST LSM303DLHC 3D accelerometer and magnetometer.
static void l3gd20_spi_periodic(struct L3gd20_Spi *l3g)
convenience function: read or start configuration if not already initialized
Definition: l3gd20_spi.h:60
Architecture independent SPI (Serial Peripheral Interface) API.
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
int32_t r
in rad/s with INT32_RATE_FRAC
void imu_px4_event(void)
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
ST L3GD20 3-axis gyroscope register definitions.
volatile bool data_available_acc
data ready flag accelero
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
union Lsm303dlhc_Spi::@310 data_mag
unsigned long uint32_t
Definition: types.h:18
union Lsm303dlhc_Spi::@309 data_accel
struct ImuPX4 imu_px4
struct L3gd20_Spi l3g
struct Lsm303dlhc_Spi lsm_mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
volatile bool data_available
data ready flag
Definition: l3gd20_spi.h:45
void imu_px4_periodic(void)
volatile bool data_available_mag
data ready flag magneto
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
int32_t p
in rad/s with INT32_RATE_FRAC
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
void lsm303dlhc_spi_init(struct Lsm303dlhc_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx, enum Lsm303dlhcTarget target)
union L3gd20_Spi::@304 data_rates
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t q
in rad/s with INT32_RATE_FRAC
void imu_px4_init(void)
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38