Paparazzi UAS  v7.0_unstable
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 
29 #include "modules/imu/imu.h"
30 #include "modules/core/abi.h"
31 #include "mcu_periph/spi.h"
32 #include "generated/modules.h"
33 
34 /* SPI defaults set in subsystem makefile, can be configured from airframe file */
35 PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
36 PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
37 PRINT_CONFIG_VAR(IMU_PX4FMU_SPI_DEV)
38 
39 struct ImuPX4 imu_px4;
40 
41 void imu_px4_init(void) {
42  /* L3GD20 gyro init */
43  /* initialize gyro and set default options */
44  l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
45 
46  /* LSM303d acc init */
47  lsm303d_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_ACC);
48 
49  // Set the default scaling
50  const struct Int32Rates gyro_scale[2] = {
53  };
54  const struct Int32Vect3 accel_scale[2] = {
57  };
60 
61 #if !IMU_PX4_DISABLE_MAG
62  /* LSM303d mag init */
63  lsm303d_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_MAG);
64 #endif
65 
66 }
67 
68 void imu_px4_periodic(void) {
71 
72 #if !IMU_PX4_DISABLE_MAG
73  /* Read magneto's every 10 times of main freq
74  * at ~50Hz (main loop for rotorcraft: 512Hz)
75  */
76  RunOnceEvery(10, lsm303d_spi_periodic(&imu_px4.lsm_mag));
77 #endif
78 }
79 
80 void imu_px4_event(void) {
81 
82  uint32_t now_ts = get_sys_time_usec();
83 
84  /* L3GD20 event task */
87  //the p and q seem to be swapped on the Pixhawk board compared to the acc
88  struct Int32Rates gyro = {
89  imu_px4.l3g.data_rates.rates.q,
90  -imu_px4.l3g.data_rates.rates.p,
91  imu_px4.l3g.data_rates.rates.r
92  };
94  AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1, IMU_PX4_PERIODIC_FREQ, NAN);
95  }
96 
97  /* LSM303d event task */
100  struct Int32Vect3 accel;
101  VECT3_COPY(accel, imu_px4.lsm_acc.data_accel.vect);
102  AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1, IMU_PX4_PERIODIC_FREQ, NAN);
104  }
105 #if !IMU_PX4_DISABLE_MAG
108  struct Int32Vect3 mag;
109  VECT3_ASSIGN(mag,
110  imu_px4.lsm_mag.data_mag.vect.x,
111  imu_px4.lsm_mag.data_mag.vect.y,
112  imu_px4.lsm_mag.data_mag.vect.z);
113  AbiSendMsgIMU_MAG_RAW(IMU_PX4_ID, now_ts, &mag);
115  }
116 #endif
117 
118 }
Main include for ABI (AirBorneInterface).
#define IMU_PX4_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
angular rates
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition: imu.c:521
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition: imu.c:491
Inertial Measurement Unit interface.
struct ImuPX4 imu_px4
void imu_px4_init(void)
void imu_px4_periodic(void)
void imu_px4_event(void)
Driver for pixhawk IMU's.
struct Lsm303d_Spi lsm_acc
struct L3gd20_Spi l3g
struct Lsm303d_Spi lsm_mag
#define L3GD20_SENS_2000_DEN
Definition: l3gd20.h:41
#define L3GD20_SENS_2000_NUM
default gyro sensitivy and neutral from the datasheet L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range...
Definition: l3gd20.h:40
void l3gd20_spi_event(struct L3gd20_Spi *l3g)
Definition: l3gd20_spi.c:135
void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: l3gd20_spi.c:30
static void l3gd20_spi_periodic(struct L3gd20_Spi *l3g)
convenience function: read or start configuration if not already initialized
Definition: l3gd20_spi.h:60
volatile bool data_available
data ready flag
Definition: l3gd20_spi.h:45
union L3gd20_Spi::@329 data_rates
@ LSM303D_TARGET_MAG
Definition: lsm303d.h:90
@ LSM303D_TARGET_ACC
Definition: lsm303d.h:89
#define LSM303D_ACCEL_SENS_16G_NUM
default accel sensitivy from the datasheet LSM303DLHC has 732 LSB/g fixed point sens: 9....
Definition: lsm303d.h:61
#define LSM303D_ACCEL_SENS_16G_DEN
Definition: lsm303d.h:62
void lsm303d_spi_event(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:176
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
static void lsm303d_spi_periodic(struct Lsm303d_Spi *lsm)
convenience function: read or start configuration if not already initialized
Definition: lsm303d_spi.h:68
volatile bool data_available_acc
data ready flag accelero
Definition: lsm303d_spi.h:45
union Lsm303d_Spi::@333 data_mag
volatile bool data_available_mag
data ready flag magneto
Definition: lsm303d_spi.h:46
union Lsm303d_Spi::@332 data_accel
static const struct Int32Rates gyro_scale[2]
Default gyro scale.
Definition: navdata.c:92
static const struct Int32Vect3 accel_scale[2]
Default accel scale/neutral.
Definition: navdata.c:100
Architecture independent SPI (Serial Peripheral Interface) API.
#define FALSE
Definition: std.h:5
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78