Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
Imu::gyro_unscaled
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
Lsm303d_Spi::data_available_acc
volatile bool data_available_acc
data ready flag accelero
Definition: lsm303d_spi.h:45
L3gd20_Spi::data_rates
union L3gd20_Spi::@321 data_rates
lsm303d_spi_event
void lsm303d_spi_event(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:176
abi.h
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
l3gd20_spi_event
void l3gd20_spi_event(struct L3gd20_Spi *l3g)
Definition: l3gd20_spi.c:135
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
spi.h
imu_px4
struct ImuPX4 imu_px4
Definition: imu_px4fmu_v2.4.c:40
Lsm303d_Spi::data_accel
union Lsm303d_Spi::@324 data_accel
L3gd20_Spi::data_available
volatile bool data_available
data ready flag
Definition: l3gd20_spi.h:45
IMU_PX4_ID
#define IMU_PX4_ID
Definition: abi_sender_ids.h:339
uint32_t
unsigned long uint32_t
Definition: types.h:18
imu_px4_init
void imu_px4_init(void)
Definition: imu_px4fmu_v2.4.c:42
Imu::accel_unscaled
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
l3gd20_spi_periodic
static void l3gd20_spi_periodic(struct L3gd20_Spi *l3g)
convenience function: read or start configuration if not already initialized
Definition: l3gd20_spi.h:60
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
imu.h
lsm303d_spi_periodic
static void lsm303d_spi_periodic(struct Lsm303d_Spi *lsm)
convenience function: read or start configuration if not already initialized
Definition: lsm303d_spi.h:68
ImuPX4::lsm_mag
struct Lsm303d_Spi lsm_mag
Definition: imu_px4fmu_v2.4.h:50
imu_scale_gyro
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
LSM303D_TARGET_ACC
@ LSM303D_TARGET_ACC
Definition: lsm303d.h:81
l3gd20_regs.h
imu
struct Imu imu
global IMU state
Definition: imu.c:108
Lsm303d_Spi::data_mag
union Lsm303d_Spi::@325 data_mag
imu_px4_event
void imu_px4_event(void)
Definition: imu_px4fmu_v2.4.c:67
ImuPX4::lsm_acc
struct Lsm303d_Spi lsm_acc
Definition: imu_px4fmu_v2.4.h:49
l3gd20_spi_init
void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: l3gd20_spi.c:30
LSM303D_TARGET_MAG
@ LSM303D_TARGET_MAG
Definition: lsm303d.h:82
imu_scale_mag
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
lsm303d_spi_init
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
Lsm303d_Spi::data_available_mag
volatile bool data_available_mag
data ready flag magneto
Definition: lsm303d_spi.h:46
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Imu::mag_unscaled
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
FALSE
#define FALSE
Definition: std.h:5
imu_px4_periodic
void imu_px4_periodic(void)
Definition: imu_px4fmu_v2.4.c:55
ImuPX4::l3g
struct L3gd20_Spi l3g
Definition: imu_px4fmu_v2.4.h:48
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
imu_scale_accel
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
ImuPX4
Definition: imu_px4fmu_v2.4.h:47
lsm303d_regs.h
Imu::gyro
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38