Paparazzi UAS  v5.15_devel-109-gee85905
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 
28 #include "subsystems/imu.h"
29 #include "subsystems/abi.h"
30 #include "mcu_periph/spi.h"
33 //#include "peripherals/lsm303d_spi.h"
34 
35 /* SPI defaults set in subsystem makefile, can be configured from airframe file */
36 PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
37 PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
38 PRINT_CONFIG_VAR(IMU_PX4FMU_SPI_DEV)
39 
40 struct ImuPX4 imu_px4;
41 
42 void imu_px4_init(void) {
43  /* L3GD20 gyro init */
44  /* initialize gyro and set default options */
45  l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
46 
47  /* LSM303d acc + magneto init */
48  lsm303d_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_ACC);
49 #if !IMU_PX4_DISABLE_MAG
50  lsm303d_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_MAG);
51 #endif
52 
53 }
54 
55 void imu_px4_periodic(void) {
58 
59 #if !IMU_PX4_DISABLE_MAG
60  /* Read magneto's every 10 times of main freq
61  * at ~50Hz (main loop for rotorcraft: 512Hz)
62  */
63  RunOnceEvery(10, lsm303d_spi_periodic(&imu_px4.lsm_mag));
64 #endif
65 }
66 
67 void imu_px4_event(void) {
68 
69  uint32_t now_ts = get_sys_time_usec();
70 
71  /* L3GD20 event task */
74  //the p and q seem to be swapped on the Pixhawk board compared to the acc
80  AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
81  }
82 
83  /* LSM303d event task */
89  AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
90  }
91 #if !IMU_PX4_DISABLE_MAG
97  AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag);
98  }
99 #endif
100 
101 }
#define IMU_PX4_ID
void l3gd20_spi_event(struct L3gd20_Spi *l3g)
Definition: l3gd20_spi.c:135
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
union Lsm303d_Spi::@316 data_mag
Main include for ABI (AirBorneInterface).
Register defs for ST LSM303D 3D accelerometer and magnetometer.
union Lsm303d_Spi::@315 data_accel
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
#define FALSE
Definition: std.h:5
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 Lsm303d_Spi lsm_acc
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
ST L3GD20 3-axis gyroscope register definitions.
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
volatile bool data_available_mag
data ready flag magneto
Definition: lsm303d_spi.h:46
volatile bool data_available_acc
data ready flag accelero
Definition: lsm303d_spi.h:45
unsigned long uint32_t
Definition: types.h:18
struct ImuPX4 imu_px4
struct L3gd20_Spi l3g
void lsm303d_spi_init(struct Lsm303d_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx, enum Lsm303dTarget target)
Definition: lsm303d_spi.c:32
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
volatile bool data_available
data ready flag
Definition: l3gd20_spi.h:45
void lsm303d_spi_event(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:176
void imu_px4_periodic(void)
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
struct Lsm303d_Spi lsm_mag
int32_t p
in rad/s with INT32_RATE_FRAC
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
union L3gd20_Spi::@312 data_rates
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
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)
static void lsm303d_spi_periodic(struct Lsm303d_Spi *lsm)
convenience function: read or start configuration if not already initialized
Definition: lsm303d_spi.h:68
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38