mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Added second PX4 IMU, made it the primary one.
This commit is contained in:
kevindehecker
@@ -3,8 +3,8 @@
|
|||||||
<?xml version="1.0" encoding="UTF-8"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<!-- this is a quadrotor frame equiped with
|
<!-- this is a quadrotor frame equiped with
|
||||||
* Autopilot: 3dr Pixhawk 2.4
|
* Autopilot: 3dr Pixhawk 2.4
|
||||||
* IMU: MPU6000 + L3GD20 + LSM303D
|
* IMU: L3GD20 + LSM303D + MPU6000 + external HMC58XX
|
||||||
* Actuators: PWM motor controllers?
|
* Actuators: PWM motor controllers
|
||||||
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
|
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
|
||||||
* RC: PPM
|
* RC: PPM
|
||||||
-->
|
-->
|
||||||
@@ -21,14 +21,13 @@
|
|||||||
<subsystem name="udp" />
|
<subsystem name="udp" />
|
||||||
</target>
|
</target>
|
||||||
<subsystem name="telemetry" type="transparent" />
|
<subsystem name="telemetry" type="transparent" />
|
||||||
<!-- <subsystem name="imu" type="px4fmu_v2.4">-->
|
<subsystem name="imu" type="px4fmu_v2.4"/>
|
||||||
<subsystem name="imu" type="mpu6000">
|
|
||||||
<configure name="IMU_MPU_SPI_DEV" value="spi1" />
|
|
||||||
<configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE2" />
|
|
||||||
</subsystem>
|
|
||||||
<subsystem name="gps" type="ublox" />
|
<subsystem name="gps" type="ublox" />
|
||||||
<subsystem name="stabilization" type="indi" />
|
<subsystem name="stabilization" type="indi" />
|
||||||
<subsystem name="ahrs" type="int_cmpl_quat" />
|
<subsystem name="ahrs" type="int_cmpl_quat" >
|
||||||
|
<define name="AHRS_ICQ_IMU_ID" value="IMU_PX4_ID" /> <!-- Meaning the lsm303 and l3g -->
|
||||||
|
<define name="AHRS_ICQ_MAG_ID" value="IMU_MPU6000_HMC_ID" /> <!-- Meaning the external magnetometer -->
|
||||||
|
</subsystem>
|
||||||
<subsystem name="ins" type="extended" />
|
<subsystem name="ins" type="extended" />
|
||||||
<subsystem name="current_sensor">
|
<subsystem name="current_sensor">
|
||||||
<configure name="ADC_CURRENT_SENSOR" value="ADC_3" />
|
<configure name="ADC_CURRENT_SENSOR" value="ADC_3" />
|
||||||
|
|||||||
@@ -0,0 +1,73 @@
|
|||||||
|
# Hey Emacs, this is a -*- makefile -*-
|
||||||
|
#
|
||||||
|
# PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto
|
||||||
|
#
|
||||||
|
#MPU6000 + L3GD20 + LSM303D + HMC5883
|
||||||
|
|
||||||
|
include $(CFG_SHARED)/spi_master.makefile
|
||||||
|
|
||||||
|
IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\"
|
||||||
|
IMU_CFLAGS = -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\"
|
||||||
|
IMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||||
|
IMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c
|
||||||
|
|
||||||
|
# MPU
|
||||||
|
IMU_SRCS += peripherals/mpu60x0.c
|
||||||
|
IMU_SRCS += peripherals/mpu60x0_spi.c
|
||||||
|
|
||||||
|
#L3GD20 gyro
|
||||||
|
IMU_SRCS += peripherals/l3gd20_spi.c
|
||||||
|
|
||||||
|
#LSM303D accelero + magneto
|
||||||
|
IMU_SRCS += peripherals/lsm303dlhc_spi.c
|
||||||
|
|
||||||
|
# Magnetometer
|
||||||
|
IMU_SRCS += peripherals/hmc58xx.c
|
||||||
|
|
||||||
|
# for fixedwing firmware and ap only
|
||||||
|
ifeq ($(TARGET), ap)
|
||||||
|
IMU_CFLAGS += -DUSE_IMU
|
||||||
|
endif
|
||||||
|
|
||||||
|
# set default SPI device
|
||||||
|
IMU_SPI_DEV ?= spi1
|
||||||
|
# convert spix to upper/lower case
|
||||||
|
IMU_SPI_DEV_UPPER=$(shell echo $(IMU_SPI_DEV) | tr a-z A-Z)
|
||||||
|
IMU_SPI_DEV_LOWER=$(shell echo $(IMU_SPI_DEV) | tr A-Z a-z)
|
||||||
|
IMU_CFLAGS += -DIMU_SPI_DEV=$(IMU_SPI_DEV_LOWER)
|
||||||
|
IMU_CFLAGS += -DUSE_$(IMU_SPI_DEV_UPPER)
|
||||||
|
|
||||||
|
#********** MPU6000 ***********
|
||||||
|
IMU_MPU_SPI_SLAVE_IDX ?= SPI_SLAVE2
|
||||||
|
IMU_CFLAGS += -DIMU_MPU_SPI_SLAVE_IDX=$(IMU_MPU_SPI_SLAVE_IDX)
|
||||||
|
IMU_CFLAGS += -DUSE_$(IMU_MPU_SPI_SLAVE_IDX)
|
||||||
|
|
||||||
|
#********** L3GD20 ***********
|
||||||
|
IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0
|
||||||
|
IMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX)
|
||||||
|
IMU_CFLAGS += -DUSE_$(IMU_L3G_SPI_SLAVE_IDX)
|
||||||
|
|
||||||
|
#********** LSM303dlhc ***********
|
||||||
|
IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1
|
||||||
|
IMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX)
|
||||||
|
IMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX)
|
||||||
|
|
||||||
|
#********** HMC5883 ***********
|
||||||
|
IMU_HMC_I2C_DEV ?= i2c1
|
||||||
|
|
||||||
|
# convert i2cx to upper/lower case
|
||||||
|
IMU_HMC_I2C_DEV_UPPER=$(shell echo $(IMU_HMC_I2C_DEV) | tr a-z A-Z)
|
||||||
|
IMU_HMC_I2C_DEV_LOWER=$(shell echo $(IMU_HMC_I2C_DEV) | tr A-Z a-z)
|
||||||
|
IMU_CFLAGS += -DIMU_HMC_I2C_DEV=$(IMU_HMC_I2C_DEV_LOWER)
|
||||||
|
IMU_CFLAGS += -DUSE_$(IMU_HMC_I2C_DEV_UPPER)
|
||||||
|
|
||||||
|
# add it for all targets except sim, fbw and nps
|
||||||
|
ifeq (,$(findstring $(TARGET),sim fbw nps))
|
||||||
|
$(TARGET).CFLAGS += $(IMU_CFLAGS)
|
||||||
|
$(TARGET).srcs += $(IMU_SRCS)
|
||||||
|
endif
|
||||||
|
|
||||||
|
#
|
||||||
|
# NPS simulator
|
||||||
|
#
|
||||||
|
include $(CFG_SHARED)/imu_nps.makefile
|
||||||
@@ -51,7 +51,7 @@ struct L3gd20_Spi {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Functions
|
// Functions
|
||||||
extern void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t addr);
|
extern void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx);
|
||||||
extern void l3gd20_spi_start_configure(struct L3gd20_Spi *l3g);
|
extern void l3gd20_spi_start_configure(struct L3gd20_Spi *l3g);
|
||||||
extern void l3gd20_spi_read(struct L3gd20_Spi *l3g);
|
extern void l3gd20_spi_read(struct L3gd20_Spi *l3g);
|
||||||
extern void l3gd20_spi_event(struct L3gd20_Spi *l3g);
|
extern void l3gd20_spi_event(struct L3gd20_Spi *l3g);
|
||||||
|
|||||||
@@ -21,12 +21,12 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file peripherals/lsm303dlhc.c
|
* @file peripherals/lsm303dlhc_i2c.c
|
||||||
*
|
*
|
||||||
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "peripherals/lsm303dlhc.h"
|
#include "peripherals/lsm303dlhc_i2c.h"
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
|
|
||||||
/* LSM303DLHC default conf */
|
/* LSM303DLHC default conf */
|
||||||
@@ -79,7 +79,7 @@ static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c)
|
|||||||
* @param i2c_p I2C peripheral to use
|
* @param i2c_p I2C peripheral to use
|
||||||
* @param addr I2C address of Lsm303dlhc
|
* @param addr I2C address of Lsm303dlhc
|
||||||
*/
|
*/
|
||||||
void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr)
|
void lsm303dlhc_i2c_init(struct Lsm303dlhc_i2c *lsm, struct i2c_periph *i2c_p, uint8_t addr)
|
||||||
{
|
{
|
||||||
/* set i2c_peripheral */
|
/* set i2c_peripheral */
|
||||||
lsm->i2c_p = i2c_p;
|
lsm->i2c_p = i2c_p;
|
||||||
@@ -97,7 +97,7 @@ void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t a
|
|||||||
lsm->initialized = false;
|
lsm->initialized = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t val)
|
static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc_I2c *lsm, uint8_t reg, uint8_t val)
|
||||||
{
|
{
|
||||||
lsm->i2c_trans.type = I2CTransTx;
|
lsm->i2c_trans.type = I2CTransTx;
|
||||||
lsm->i2c_trans.buf[0] = reg;
|
lsm->i2c_trans.buf[0] = reg;
|
||||||
@@ -108,7 +108,7 @@ static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t v
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Configuration function called once before normal use
|
/// Configuration function called once before normal use
|
||||||
static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm)
|
static void lsm303dlhc_i2c_send_config(struct Lsm303dlhc_I2c *lsm)
|
||||||
{
|
{
|
||||||
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
||||||
switch (lsm->init_status.acc) {
|
switch (lsm->init_status.acc) {
|
||||||
@@ -132,7 +132,7 @@ static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm)
|
|||||||
case LSM_CONF_ACC_DONE:
|
case LSM_CONF_ACC_DONE:
|
||||||
lsm->initialized = true;
|
lsm->initialized = true;
|
||||||
lsm->i2c_trans.status = I2CTransDone;
|
lsm->i2c_trans.status = I2CTransDone;
|
||||||
lsm303dlhc_read(lsm);
|
lsm303dlhc_i2c_read(lsm);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
@@ -162,7 +162,7 @@ static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Configure
|
// Configure
|
||||||
void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm)
|
void lsm303dlhc_i2c_start_configure(struct Lsm303dlhc_I2c *lsm)
|
||||||
{
|
{
|
||||||
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
||||||
if (lsm->init_status.acc == LSM_CONF_ACC_UNINIT) {
|
if (lsm->init_status.acc == LSM_CONF_ACC_UNINIT) {
|
||||||
@@ -175,14 +175,14 @@ void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm)
|
|||||||
if (lsm->init_status.mag == LSM_CONF_MAG_UNINIT) {
|
if (lsm->init_status.mag == LSM_CONF_MAG_UNINIT) {
|
||||||
lsm->init_status.mag++;
|
lsm->init_status.mag++;
|
||||||
if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
|
if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
|
||||||
lsm303dlhc_send_config(lsm);
|
lsm303dlhc_i2c_send_config(lsm);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Normal reading
|
// Normal reading
|
||||||
void lsm303dlhc_read(struct Lsm303dlhc *lsm)
|
void lsm303dlhc_i2c_read(struct Lsm303dlhc *lsm)
|
||||||
{
|
{
|
||||||
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
|
||||||
//if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){
|
//if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){
|
||||||
@@ -206,7 +206,7 @@ void lsm303dlhc_read(struct Lsm303dlhc *lsm)
|
|||||||
|
|
||||||
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
|
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
|
||||||
|
|
||||||
void lsm303dlhc_event(struct Lsm303dlhc *lsm)
|
void lsm303dlhc_i2c_event(struct Lsm303dlhc *lsm)
|
||||||
{
|
{
|
||||||
if (lsm->initialized) {
|
if (lsm->initialized) {
|
||||||
if (lsm->i2c_trans.status == I2CTransFailed) {
|
if (lsm->i2c_trans.status == I2CTransFailed) {
|
||||||
@@ -79,7 +79,7 @@ struct Lsm303dlhc {
|
|||||||
union {
|
union {
|
||||||
struct Int16Vect3 vect; ///< data vector in acc coordinate system
|
struct Int16Vect3 vect; ///< data vector in acc coordinate system
|
||||||
int16_t value[3]; ///< data values accessible by channel index
|
int16_t value[3]; ///< data values accessible by channel index
|
||||||
} data;
|
} data_accel;
|
||||||
union {
|
union {
|
||||||
struct Lsm303dlhcAccConfig acc;
|
struct Lsm303dlhcAccConfig acc;
|
||||||
struct Lsm303dlhcMagConfig mag;
|
struct Lsm303dlhcMagConfig mag;
|
||||||
@@ -91,18 +91,18 @@ struct Lsm303dlhc {
|
|||||||
// TODO IRQ handling
|
// TODO IRQ handling
|
||||||
|
|
||||||
// Functions
|
// Functions
|
||||||
extern void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr);
|
extern void lsm303dlhc_i2c_init(struct Lsm303dlhc_i2c *lsm, struct i2c_periph *i2c_p, uint8_t addr);
|
||||||
extern void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm);
|
extern void lsm303dlhc_i2c_start_configure(struct Lsm303dlhc_i2c *lsm);
|
||||||
extern void lsm303dlhc_read(struct Lsm303dlhc *lsm);
|
extern void lsm303dlhc_i2c_read(struct Lsm303dlhc_i2c *lsm);
|
||||||
extern void lsm303dlhc_event(struct Lsm303dlhc *lsm);
|
extern void lsm303dlhc_i2c_event(struct Lsm303dlhc_i2c *lsm);
|
||||||
|
|
||||||
/// convenience function: read or start configuration if not already initialized
|
/// convenience function: read or start configuration if not already initialized
|
||||||
static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm)
|
static inline void lsm303dlhc_i2c_periodic(struct Lsm303dlhc_i2c *lsm)
|
||||||
{
|
{
|
||||||
if (lsm->initialized) {
|
if (lsm->initialized) {
|
||||||
lsm303dlhc_read(lsm);
|
lsm303dlhc_i2c_read(lsm);
|
||||||
} else {
|
} else {
|
||||||
lsm303dlhc_start_configure(lsm);
|
lsm303dlhc_i2c_start_configure(lsm);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -32,13 +32,43 @@
|
|||||||
#define LSM303DLHC_MAG_ADDR 0x3C
|
#define LSM303DLHC_MAG_ADDR 0x3C
|
||||||
|
|
||||||
/* Registers */
|
/* Registers */
|
||||||
#define LSM303DLHC_REG_CTRL_REG1_A 0x20
|
|
||||||
#define LSM303DLHC_REG_CTRL_REG2_A 0x21
|
//checked:
|
||||||
#define LSM303DLHC_REG_CTRL_REG3_A 0x22
|
#define LSM303DLHC_REG_TEMP_OUT_L_M 0x05
|
||||||
#define LSM303DLHC_REG_CTRL_REG4_A 0x23
|
#define LSM303DLHC_REG_TEMP_OUT_H_M 0x06
|
||||||
#define LSM303DLHC_REG_CTRL_REG5_A 0x24
|
|
||||||
#define LSM303DLHC_REG_CTRL_REG6_A 0x25
|
#define LSM303DLHC_REG_STATUS_REG_M 0x07
|
||||||
#define LSM303DLHC_REG_REF_DATA_CAP_A 0x26
|
#define LSM303DLHC_REG_OUT_X_L_M 0x08
|
||||||
|
#define LSM303DLHC_REG_OUT_X_H_M 0x09
|
||||||
|
#define LSM303DLHC_REG_OUT_Y_L_M 0x0A
|
||||||
|
#define LSM303DLHC_REG_OUT_Y_H_M 0x0B
|
||||||
|
#define LSM303DLHC_REG_OUT_Z_L_M 0x0C
|
||||||
|
#define LSM303DLHC_REG_OUT_Z_H_M 0x0D
|
||||||
|
|
||||||
|
#define LSM303DLHC_REG_WHO_AM_I 0x0F
|
||||||
|
|
||||||
|
#define LSM303DLHC_REG_INT_CTRL_M 0x12
|
||||||
|
#define LSM303DLHC_REG_INT_SRC_M 0x13
|
||||||
|
#define LSM303DLHC_REG_INT_THS_L_M 0x14
|
||||||
|
#define LSM303DLHC_REG_INT_THS_H_M 0x15
|
||||||
|
#define LSM303DLHC_REG_OFFSET_X_L_M 0x16
|
||||||
|
#define LSM303DLHC_REG_OFFSET_X_H_M 0x17
|
||||||
|
#define LSM303DLHC_REG_OFFSET_Y_L_M 0x18
|
||||||
|
#define LSM303DLHC_REG_OFFSET_Y_H_M 0x19
|
||||||
|
#define LSM303DLHC_REG_OFFSET_Z_L_M 0x1A
|
||||||
|
#define LSM303DLHC_REG_OFFSET_Z_H_M 0x1B
|
||||||
|
#define LSM303DLHC_REG_REFERENCE_X 0x1C
|
||||||
|
#define LSM303DLHC_REG_REFERENCE_Y 0x1D
|
||||||
|
#define LSM303DLHC_REG_REFERENCE_Z 0x1E
|
||||||
|
#define LSM303DLHC_REG_CTRL0 0x1F
|
||||||
|
#define LSM303DLHC_REG_CTRL1 0x20
|
||||||
|
#define LSM303DLHC_REG_CTRL2 0x21
|
||||||
|
#define LSM303DLHC_REG_CTRL3 0x22
|
||||||
|
#define LSM303DLHC_REG_CTRL4 0x23
|
||||||
|
#define LSM303DLHC_REG_CTRL5 0x24
|
||||||
|
#define LSM303DLHC_REG_CTRL6 0x25
|
||||||
|
#define LSM303DLHC_REG_CTRL7 0x26
|
||||||
|
|
||||||
#define LSM303DLHC_REG_STATUS_REG_A 0x27
|
#define LSM303DLHC_REG_STATUS_REG_A 0x27
|
||||||
#define LSM303DLHC_REG_OUT_X_L_A 0x28
|
#define LSM303DLHC_REG_OUT_X_L_A 0x28
|
||||||
#define LSM303DLHC_REG_OUT_X_H_A 0x29
|
#define LSM303DLHC_REG_OUT_X_H_A 0x29
|
||||||
@@ -46,55 +76,68 @@
|
|||||||
#define LSM303DLHC_REG_OUT_Y_H_A 0x2B
|
#define LSM303DLHC_REG_OUT_Y_H_A 0x2B
|
||||||
#define LSM303DLHC_REG_OUT_Z_L_A 0x2C
|
#define LSM303DLHC_REG_OUT_Z_L_A 0x2C
|
||||||
#define LSM303DLHC_REG_OUT_Z_H_A 0x2D
|
#define LSM303DLHC_REG_OUT_Z_H_A 0x2D
|
||||||
#define LSM303DLHC_REG_FIFO_CTRL_REG_A 0x2E
|
|
||||||
#define LSM303DLHC_REG_FIFO_SRC_REG_A 0x2F
|
#define LSM303DLHC_REG_FIFO_CTRL 0x2E
|
||||||
#define LSM303DLHC_REG_INT1_CFG_A 0x30
|
#define LSM303DLHC_REG_FIFO_SRC 0x2F
|
||||||
#define LSM303DLHC_REG_INT1_SRC_A 0x31
|
#define LSM303DLHC_REG_INT_CFG 0x30
|
||||||
#define LSM303DLHC_REG_INT1_THS_A 0x32
|
#define LSM303DLHC_REG_INT_SRC1 0x31
|
||||||
#define LSM303DLHC_REG_INT1_DURATION_A 0x33
|
#define LSM303DLHC_REG_INT_THS1 0x32
|
||||||
#define LSM303DLHC_REG_INT2_CFG_A 0x34
|
#define LSM303DLHC_REG_INT_DUR1 0x33
|
||||||
#define LSM303DLHC_REG_INT2_SRC_A 0x35
|
#define LSM303DLHC_REG_INT_CFG2 0x34
|
||||||
#define LSM303DLHC_REG_INT2_THS_A 0x36
|
#define LSM303DLHC_REG_INT_SRC2 0x35
|
||||||
#define LSM303DLHC_REG_INT2_DURATION_A 0x37
|
#define LSM303DLHC_REG_INT_THS2 0x36
|
||||||
#define LSM303DLHC_REG_CLICK_CFG_A 0x38
|
#define LSM303DLHC_REG_INT_DUR2 0x37
|
||||||
#define LSM303DLHC_REG_CLICK_SRC_A 0x39
|
#define LSM303DLHC_REG_CLICK_CFG 0x38
|
||||||
#define LSM303DLHC_REG_CLICK_THS_A 0x3A
|
#define LSM303DLHC_REG_CLICK_SRC 0x39
|
||||||
#define LSM303DLHC_REG_TIME_LIMIT_A 0x3B
|
#define LSM303DLHC_REG_CLICK_THS 0x3A
|
||||||
#define LSM303DLHC_REG_TIME_LATENCY_A 0x3C
|
#define LSM303DLHC_REG_TIME_LIMIT 0x3B
|
||||||
#define LSM303DLHC_REG_TIME_WINDOW_A 0x3D
|
#define LSM303DLHC_REG_TIME_LATENCY 0x3C
|
||||||
#define LSM303DLHC_REG_CRA_REG_M 0x00
|
#define LSM303DLHC_REG_TIME_WINDOW 0x3D
|
||||||
#define LSM303DLHC_REG_CRB_REG_M 0x01
|
#define LSM303DLHC_ACT_THS 0x3E
|
||||||
#define LSM303DLHC_REG_MR_REG_M 0x02
|
#define LSM303DLHC_ACT_DUR 0x3F
|
||||||
#define LSM303DLHC_REG_OUT_X_H_M 0x03
|
|
||||||
#define LSM303DLHC_REG_OUT_X_L_M 0x04
|
#define LSM303DLHC_REG_STATUS_ZYXADA 0x08
|
||||||
#define LSM303DLHC_REG_OUT_Z_H_M 0x05
|
#define LSM303DLHC_REG_STATUS_ZYXMDA 0x08
|
||||||
#define LSM303DLHC_REG_OUT_Z_L_M 0x06
|
|
||||||
#define LSM303DLHC_REG_OUT_Y_H_M 0x07
|
#define LSM303DLHC_WHO_I_AM 0x49
|
||||||
#define LSM303DLHC_REG_OUT_Y_L_M 0x08
|
|
||||||
#define LSM303DLHC_REG_SR_REG_M 0x09
|
|
||||||
#define LSM303DLHC_REG_IRA_REG_M 0x0A
|
|
||||||
#define LSM303DLHC_REG_IRB_REG_M 0x0B
|
|
||||||
#define LSM303DLHC_REG_IRC_REG_M 0x0C
|
|
||||||
#define LSM303DLHC_REG_TEMP_OUT_H_M 0x31
|
|
||||||
#define LSM303DLHC_REG_TEMP_OUT_L_M 0x32
|
|
||||||
|
|
||||||
/* Bit definitions */
|
/* Bit definitions */
|
||||||
#define LSM303DLHC_ODR_MASK 0xF0
|
//CTRL1
|
||||||
#define LSM303DLHC_LPen (1 << 3)
|
#define LSM303DLHC_Xen (0x01 << 0)
|
||||||
#define LSM303DLHC_Xen (1 << 0)
|
#define LSM303DLHC_Yen (0x01 << 1)
|
||||||
#define LSM303DLHC_Yen (1 << 1)
|
#define LSM303DLHC_Zen (0x01 << 2)
|
||||||
#define LSM303DLHC_Zen (1 << 2)
|
#define LSM303DLHC_BDU (0x01 << 3)
|
||||||
|
#define LSM303DLHC_AODR_MASK (0x0F << 4)
|
||||||
|
|
||||||
#define LSM303DLHC_FS_MASK 0x30
|
//CTRL2
|
||||||
#define LSM303DLHC_HR (1 << 3)
|
#define LSM303DLHC_SIM (0x01 << 0)
|
||||||
#define LSM303DLHC_BDU (1 << 7)
|
#define LSM303DLHC_AST (0x01 << 1)
|
||||||
|
#define LSM303DLHC_FS_MASK (0x07 << 3)
|
||||||
|
#define LSM303DLHC_ABW_MASK (0x03 << 6)
|
||||||
|
|
||||||
#define LSM303DLHC_I1_DRDY1 (1 << 4)
|
//CTRL3
|
||||||
|
#define LSM303DLHC_I1_DRDY_A (0x01 << 2)
|
||||||
|
//TODO: more CTRL3 regs
|
||||||
|
|
||||||
#define LSM303DLHC_DO0_MASK 0x1C
|
//CTRL4
|
||||||
#define LSM303DLHC_GN_MASK 0xE0
|
#define LSM303DLHC_I2_DRDY_A (0x01 << 3)
|
||||||
#define LSM303DLHC_MD_MASK 0x03
|
#define LSM303DLHC_I2_DRDY_M (0x01 << 2)
|
||||||
|
//TODO: more CTRL4 regs
|
||||||
|
|
||||||
|
//CTRL5
|
||||||
|
#define LSM303DLHC_TEMP_EN (0x01 << 7)
|
||||||
|
#define LSM303DLHC_M_RES (0x07 << 5) // only two modes, so no mask
|
||||||
|
#define LSM303DLHC_M_ODR_MASK (0x15 << 2)
|
||||||
|
#define LSM303DLHC_M_LIR_MASK (0x7 << 0)
|
||||||
|
|
||||||
|
//CTRL6
|
||||||
|
#define LSM303DLHC_MFS_MASK (0x07 << 5)
|
||||||
|
|
||||||
|
//CTRL7
|
||||||
|
#define LSM303DLHC_AHPM_MASK (0x07 << 6)
|
||||||
|
#define LSM303DLHC_AFDS (0x01 << 5)
|
||||||
|
#define LSM303DLHC_T_ONLY (0x01 << 4)
|
||||||
|
#define LSM303DLHC_MLP (0x01 << 2)
|
||||||
|
#define LSM303DLHC_MD_MASK (0x07 << 0)
|
||||||
|
|
||||||
#endif // LSM303DLHC_REGS_H
|
#endif // LSM303DLHC_REGS_H
|
||||||
|
|||||||
@@ -0,0 +1,256 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2011 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||||
|
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/lsm303dlhc_spi.c
|
||||||
|
*
|
||||||
|
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "peripherals/lsm303dlhc_spi.h"
|
||||||
|
#include "std.h"
|
||||||
|
|
||||||
|
/* LSM303DLHC default conf */
|
||||||
|
#ifndef LSM303DLHC_DEFAULT_AODR
|
||||||
|
#define LSM303DLHC_DEFAULT_AODR (0x01 << 4) //acc 3125 Hz
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_DEFAULT_AFS
|
||||||
|
#define LSM303DLHC_DEFAULT_AFS 0x00 // acc +- 2G
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_DEFAULT_MODR
|
||||||
|
#define LSM303DLHC_DEFAULT_MODR (0x5 << 2) // Magneto Data Output Rate (100Hz)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_DEFAULT_MFS
|
||||||
|
#define LSM303DLHC_DEFAULT_MFS (0x0 << 5) // Magneto gain configuration (+/- 2 Gauss)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_DEFAULT_MD
|
||||||
|
#define LSM303DLHC_DEFAULT_MD (0x00 << 0) // Magneto continious conversion mode
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c)
|
||||||
|
{
|
||||||
|
c->rate = LSM303DLHC_DEFAULT_AODR;
|
||||||
|
c->scale = LSM303DLHC_DEFAULT_AFS;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c)
|
||||||
|
{
|
||||||
|
c->rate = LSM303DLHC_DEFAULT_MODR;
|
||||||
|
c->scale = LSM303DLHC_DEFAULT_MFS;
|
||||||
|
c->mode = LSM303DLHC_DEFAULT_MD;
|
||||||
|
}
|
||||||
|
|
||||||
|
void lsm303dlhc_spi_init(struct Lsm303dlhc_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx,
|
||||||
|
enum Lsm303dlhcTarget target)
|
||||||
|
{
|
||||||
|
/* set spi_peripheral */
|
||||||
|
lsm->spi_p = spi_p;
|
||||||
|
|
||||||
|
/* set internal target mag/acc*/
|
||||||
|
lsm->target = target;
|
||||||
|
|
||||||
|
/* configure spi transaction */
|
||||||
|
lsm->spi_trans.cpol = SPICpolIdleHigh;
|
||||||
|
lsm->spi_trans.cpha = SPICphaEdge2;
|
||||||
|
lsm->spi_trans.dss = SPIDss8bit;
|
||||||
|
lsm->spi_trans.bitorder = SPIMSBFirst;
|
||||||
|
lsm->spi_trans.cdiv = SPIDiv64;
|
||||||
|
|
||||||
|
lsm->spi_trans.select = SPISelectUnselect;
|
||||||
|
lsm->spi_trans.slave_idx = slave_idx;
|
||||||
|
lsm->spi_trans.output_length = 2;
|
||||||
|
lsm->spi_trans.input_length = 8;
|
||||||
|
// callback currently unused
|
||||||
|
lsm->spi_trans.before_cb = NULL;
|
||||||
|
lsm->spi_trans.after_cb = NULL;
|
||||||
|
lsm->spi_trans.input_buf = &(lsm->rx_buf[0]);
|
||||||
|
lsm->spi_trans.output_buf = &(lsm->tx_buf[0]);
|
||||||
|
|
||||||
|
/* set inital status: Success or Done */
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
|
||||||
|
/* set default LSM303D config options */
|
||||||
|
lsm303dlhc_acc_set_default_config(&(lsm->config.acc));
|
||||||
|
lsm303dlhc_mag_set_default_config(&(lsm->config.mag));
|
||||||
|
lsm->init_status = LSM_CONF_UNINIT;
|
||||||
|
|
||||||
|
lsm->initialized = FALSE;
|
||||||
|
lsm->data_available_acc = FALSE;
|
||||||
|
lsm->data_available_mag = FALSE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void lsm303dlhc_spi_tx_reg(struct Lsm303dlhc_Spi *lsm, uint8_t reg, uint8_t val)
|
||||||
|
{
|
||||||
|
lsm->spi_trans.output_length = 2;
|
||||||
|
lsm->spi_trans.input_length = 0;
|
||||||
|
lsm->tx_buf[0] = reg;
|
||||||
|
lsm->tx_buf[1] = val;
|
||||||
|
spi_submit(lsm->spi_p, &(lsm->spi_trans));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Configuration function called once before normal use
|
||||||
|
static void lsm303dlhc_spi_send_config(struct Lsm303dlhc_Spi *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->target ==
|
||||||
|
LSM_TARGET_ACC) { // the complete config done below currently is one shot for both acc and mag. So, only do it for one of the devices.
|
||||||
|
switch (lsm->init_status) {
|
||||||
|
case LSM_CONF_WHO_AM_I:
|
||||||
|
/* query device id */
|
||||||
|
lsm->spi_trans.output_length = 1;
|
||||||
|
lsm->spi_trans.input_length = 2;
|
||||||
|
/* set read bit then reg address */
|
||||||
|
lsm->tx_buf[0] = (1 << 7 | LSM303DLHC_REG_WHO_AM_I);
|
||||||
|
if (spi_submit(lsm->spi_p, &(lsm->spi_trans))) {
|
||||||
|
if (lsm->rx_buf[1] == LSM303DLHC_WHO_I_AM) {
|
||||||
|
lsm->init_status++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG1:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL1,
|
||||||
|
(lsm->config.acc.rate & LSM303DLHC_AODR_MASK) |
|
||||||
|
LSM303DLHC_Xen | LSM303DLHC_Yen | LSM303DLHC_Zen);
|
||||||
|
lsm->init_status++;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG2:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL2, (lsm->config.acc.scale & LSM303DLHC_FS_MASK));
|
||||||
|
lsm->init_status++;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG3:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL3, LSM303DLHC_I1_DRDY_A);
|
||||||
|
lsm->init_status++;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG4:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL4, LSM303DLHC_I2_DRDY_M);
|
||||||
|
lsm->init_status++;
|
||||||
|
return;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG5:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL5,
|
||||||
|
(lsm->config.mag.rate & LSM303DLHC_M_ODR_MASK));
|
||||||
|
lsm->init_status++;
|
||||||
|
return;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG6:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL6,
|
||||||
|
(lsm->config.mag.scale & LSM303DLHC_MFS_MASK));
|
||||||
|
lsm->init_status++;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_CTRL_REG7:
|
||||||
|
lsm303dlhc_spi_tx_reg(lsm, LSM303DLHC_REG_CTRL7, (lsm->config.mag.mode & LSM303DLHC_AHPM_MASK));
|
||||||
|
lsm->init_status++;
|
||||||
|
break;
|
||||||
|
case LSM_CONF_DONE:
|
||||||
|
lsm->initialized = TRUE;
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
return;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
lsm->initialized = TRUE;
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Configure
|
||||||
|
void lsm303dlhc_spi_start_configure(struct Lsm303dlhc_Spi *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->init_status == LSM_CONF_UNINIT) {
|
||||||
|
lsm->init_status++;
|
||||||
|
if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) {
|
||||||
|
lsm303dlhc_spi_send_config(lsm);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Normal reading
|
||||||
|
void lsm303dlhc_spi_read(struct Lsm303dlhc_Spi *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->target == LSM_TARGET_ACC) {
|
||||||
|
if (!(lsm->initialized) || (lsm->initialized && lsm->spi_trans.status == SPITransDone)) {
|
||||||
|
lsm->spi_trans.output_length = 1;
|
||||||
|
lsm->spi_trans.input_length = 8;
|
||||||
|
/* set read bit and multiple byte bit, then address */
|
||||||
|
lsm->tx_buf[0] = LSM303DLHC_REG_STATUS_REG_A | 1 << 7 | 1 << 6;
|
||||||
|
spi_submit(lsm->spi_p, &(lsm->spi_trans));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (lsm->initialized && lsm->spi_trans.status == SPITransDone) {
|
||||||
|
lsm->spi_trans.output_length = 1;
|
||||||
|
lsm->spi_trans.input_length = 8;
|
||||||
|
/* set read bit and multiple byte bit, then address */
|
||||||
|
lsm->tx_buf[0] = LSM303DLHC_REG_STATUS_REG_M | 1 << 7 | 1 << 6;
|
||||||
|
spi_submit(lsm->spi_p, &(lsm->spi_trans));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
|
||||||
|
|
||||||
|
void lsm303dlhc_spi_event(struct Lsm303dlhc_Spi *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->initialized) {
|
||||||
|
if (lsm->spi_trans.status == SPITransFailed) {
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
} else if (lsm->spi_trans.status == SPITransSuccess) {
|
||||||
|
if (lsm->target == LSM_TARGET_ACC) {
|
||||||
|
if (!(lsm->rx_buf[1] & LSM303DLHC_REG_STATUS_ZYXADA)) {
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
lsm->data_accel.vect.x = Int16FromBuf(lsm->rx_buf, 2);
|
||||||
|
lsm->data_accel.vect.y = Int16FromBuf(lsm->rx_buf, 4);
|
||||||
|
lsm->data_accel.vect.z = Int16FromBuf(lsm->rx_buf, 6);
|
||||||
|
lsm->data_available_acc = TRUE;
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
} else { //magneto
|
||||||
|
if (!(lsm->rx_buf[1] & LSM303DLHC_REG_STATUS_ZYXMDA)) {
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
lsm->data_mag.vect.x = Int16FromBuf(lsm->rx_buf, 2);
|
||||||
|
lsm->data_mag.vect.y = Int16FromBuf(lsm->rx_buf, 4);
|
||||||
|
lsm->data_mag.vect.z = Int16FromBuf(lsm->rx_buf, 6);
|
||||||
|
lsm->data_available_mag = TRUE;
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (lsm->init_status != LSM_CONF_UNINIT) { // Configuring but not yet initialized
|
||||||
|
if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) {
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
lsm303dlhc_spi_send_config(lsm);
|
||||||
|
}
|
||||||
|
if (lsm->spi_trans.status == SPITransFailed) {
|
||||||
|
lsm->init_status--;
|
||||||
|
lsm->spi_trans.status = SPITransDone;
|
||||||
|
lsm303dlhc_spi_send_config(lsm); // Retry config (TODO max retry)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,114 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2011 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||||
|
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/lsm303dlhc.h
|
||||||
|
*
|
||||||
|
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_H
|
||||||
|
#define LSM303DLHC_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "mcu_periph/spi.h"
|
||||||
|
#include "math/pprz_algebra_int.h"
|
||||||
|
|
||||||
|
/* Address and register definitions */
|
||||||
|
#include "peripherals/lsm303dlhc_regs.h"
|
||||||
|
|
||||||
|
struct Lsm303dlhcAccConfig {
|
||||||
|
uint8_t rate; ///< Data Output Rate (Hz)
|
||||||
|
uint8_t scale; ///< full scale selection (m/s²)
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Lsm303dlhcMagConfig {
|
||||||
|
uint8_t rate; ///< Data Output Rate Bits (Hz)
|
||||||
|
uint8_t scale; ///< Full scale gain configuration (Gauss)
|
||||||
|
uint8_t mode; ///< Measurement mode
|
||||||
|
};
|
||||||
|
|
||||||
|
/** config status states */
|
||||||
|
enum Lsm303dlhcConfStatus {
|
||||||
|
LSM_CONF_UNINIT,
|
||||||
|
LSM_CONF_WHO_AM_I,
|
||||||
|
LSM_CONF_CTRL_REG1,
|
||||||
|
LSM_CONF_CTRL_REG2,
|
||||||
|
LSM_CONF_CTRL_REG3,
|
||||||
|
LSM_CONF_CTRL_REG4,
|
||||||
|
LSM_CONF_CTRL_REG5,
|
||||||
|
LSM_CONF_CTRL_REG6,
|
||||||
|
LSM_CONF_CTRL_REG7,
|
||||||
|
LSM_CONF_DONE
|
||||||
|
};
|
||||||
|
|
||||||
|
enum Lsm303dlhcTarget {
|
||||||
|
LSM_TARGET_ACC,
|
||||||
|
LSM_TARGET_MAG
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Lsm303dlhc_Spi {
|
||||||
|
struct spi_periph *spi_p;
|
||||||
|
struct spi_transaction spi_trans;
|
||||||
|
bool_t initialized; ///< config done flag
|
||||||
|
enum Lsm303dlhcTarget target;
|
||||||
|
volatile uint8_t tx_buf[2];
|
||||||
|
volatile uint8_t rx_buf[8];
|
||||||
|
enum Lsm303dlhcConfStatus init_status;
|
||||||
|
volatile bool_t data_available_acc; ///< data ready flag accelero
|
||||||
|
volatile bool_t data_available_mag; ///< data ready flag magneto
|
||||||
|
union {
|
||||||
|
struct Int16Vect3 vect; ///< data vector in acc coordinate system
|
||||||
|
int16_t value[3]; ///< data values accessible by channel index
|
||||||
|
} data_accel;
|
||||||
|
union {
|
||||||
|
struct Int16Vect3 vect; ///< data vector in mag coordinate system
|
||||||
|
int16_t value[3]; ///< data values accessible by channel index
|
||||||
|
} data_mag;
|
||||||
|
union {
|
||||||
|
struct Lsm303dlhcAccConfig acc;
|
||||||
|
struct Lsm303dlhcMagConfig mag;
|
||||||
|
} config;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// TODO IRQ handling
|
||||||
|
|
||||||
|
// Functions
|
||||||
|
extern void lsm303dlhc_spi_init(struct Lsm303dlhc_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx,
|
||||||
|
enum Lsm303dlhcTarget target);
|
||||||
|
extern void lsm303dlhc_spi_start_configure(struct Lsm303dlhc_Spi *lsm);
|
||||||
|
extern void lsm303dlhc_spi_read(struct Lsm303dlhc_Spi *lsm);
|
||||||
|
extern void lsm303dlhc_spi_event(struct Lsm303dlhc_Spi *lsm);
|
||||||
|
|
||||||
|
/// convenience function: read or start configuration if not already initialized
|
||||||
|
static inline void lsm303dlhc_spi_periodic(struct Lsm303dlhc_Spi *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->initialized) {
|
||||||
|
lsm303dlhc_spi_read(lsm);
|
||||||
|
} else {
|
||||||
|
lsm303dlhc_spi_start_configure(lsm);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* LSM303DLHC_H */
|
||||||
@@ -237,6 +237,10 @@
|
|||||||
#define PX4FLOW_VELOCITY_ID 17
|
#define PX4FLOW_VELOCITY_ID 17
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef IMU_PX4
|
||||||
|
#define IMU_PX4_ID 18
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* IDs of RSSI measurements (message 13)
|
* IDs of RSSI measurements (message 13)
|
||||||
*/
|
*/
|
||||||
|
|||||||
@@ -0,0 +1,170 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013-2015 Felix Ruess <felix.ruess@gmail.com>
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file subsystems/imu/imu_px4fmu_v2.4.h
|
||||||
|
* Driver for pixhawk IMU's.
|
||||||
|
* On with spi: L3GD20H + LSM303D and the MPU6000.
|
||||||
|
* On i2c: external HMC5883L (through 3dr gps).
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
#include "subsystems/abi.h"
|
||||||
|
#include "mcu_periph/spi.h"
|
||||||
|
#include "peripherals/hmc58xx_regs.h"
|
||||||
|
#include "peripherals/l3gd20_regs.h"
|
||||||
|
#include "peripherals/lsm303dlhc_regs.h"
|
||||||
|
#include "peripherals/lsm303dlhc_spi.h"
|
||||||
|
|
||||||
|
/************MPU6000*****************/
|
||||||
|
/* SPI defaults set in subsystem makefile, can be configured from airframe file */
|
||||||
|
PRINT_CONFIG_VAR(IMU_MPU_SPI_SLAVE_IDX)
|
||||||
|
PRINT_CONFIG_VAR(IMU_SPI_DEV)
|
||||||
|
|
||||||
|
/* MPU60x0 gyro/accel internal lowpass frequency */
|
||||||
|
#if !defined IMU_MPU_LOWPASS_FILTER && !defined IMU_MPU_SMPLRT_DIV
|
||||||
|
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
|
||||||
|
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
|
||||||
|
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
|
||||||
|
*/
|
||||||
|
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
|
||||||
|
#define IMU_MPU_SMPLRT_DIV 9
|
||||||
|
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
|
||||||
|
#elif PERIODIC_FREQUENCY == 512
|
||||||
|
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
|
||||||
|
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
|
||||||
|
*/
|
||||||
|
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_256HZ
|
||||||
|
#define IMU_MPU_SMPLRT_DIV 3
|
||||||
|
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
|
||||||
|
#else
|
||||||
|
#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV.
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
|
||||||
|
PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
|
||||||
|
|
||||||
|
#ifndef IMU_MPU_GYRO_RANGE
|
||||||
|
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
|
||||||
|
#endif
|
||||||
|
PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
|
||||||
|
|
||||||
|
#ifndef IMU_MPU_ACCEL_RANGE
|
||||||
|
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
|
||||||
|
#endif
|
||||||
|
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
|
||||||
|
|
||||||
|
/************HMC58XX*****************/
|
||||||
|
PRINT_CONFIG_VAR(IMU_HMC_I2C_DEV)
|
||||||
|
|
||||||
|
|
||||||
|
struct ImuPX4 imu_px4;
|
||||||
|
|
||||||
|
void imu_impl_init(void)
|
||||||
|
{
|
||||||
|
/* MPU6000 init */
|
||||||
|
mpu60x0_spi_init(&imu_px4.mpu, &IMU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
|
||||||
|
// change the default configuration
|
||||||
|
imu_px4.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
|
||||||
|
imu_px4.mpu.config.dlpf_cfg = IMU_MPU_LOWPASS_FILTER;
|
||||||
|
imu_px4.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
|
||||||
|
imu_px4.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
|
||||||
|
|
||||||
|
/* L3GD20 gyro init */
|
||||||
|
/* initialize gyro and set default options */
|
||||||
|
l3gd20_spi_init(&imu_px4.l3g, &IMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
|
||||||
|
|
||||||
|
/* LSM303dlhc acc + magneto init */
|
||||||
|
lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
|
||||||
|
lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
|
||||||
|
|
||||||
|
/* HMC58XX magneto init */
|
||||||
|
/* initialize mag and set default options */
|
||||||
|
hmc58xx_init(&imu_px4.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_periodic(void)
|
||||||
|
{
|
||||||
|
mpu60x0_spi_periodic(&imu_px4.mpu);
|
||||||
|
l3gd20_spi_periodic(&imu_px4.l3g);
|
||||||
|
lsm303dlhc_spi_periodic(&imu_px4.lsm_acc);
|
||||||
|
|
||||||
|
/* Read magneto's every 10 times of main freq
|
||||||
|
* at ~50Hz (main loop for rotorcraft: 512Hz)
|
||||||
|
*/
|
||||||
|
RunOnceEvery(10, hmc58xx_periodic(&imu_px4.hmc));
|
||||||
|
RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_px4_event(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
uint32_t now_ts = get_sys_time_usec();
|
||||||
|
|
||||||
|
/* MPU6000 event task */
|
||||||
|
mpu60x0_spi_event(&imu_px4.mpu);
|
||||||
|
if (imu_px4.mpu.data_available) {
|
||||||
|
RATES_COPY(imu.gyro_unscaled, imu_px4.mpu.data_rates.rates);
|
||||||
|
VECT3_COPY(imu.accel_unscaled, imu_px4.mpu.data_accel.vect);
|
||||||
|
imu_px4.mpu.data_available = FALSE;
|
||||||
|
imu_scale_gyro(&imu);
|
||||||
|
imu_scale_accel(&imu);
|
||||||
|
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
|
||||||
|
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* L3GD20 event task */
|
||||||
|
l3gd20_spi_event(&imu_px4.l3g);
|
||||||
|
if (imu_px4.l3g.data_available) {
|
||||||
|
RATES_COPY(imu.gyro_unscaled, imu_px4.l3g.data_rates.rates);
|
||||||
|
imu_px4.l3g.data_available = FALSE;
|
||||||
|
imu_scale_gyro(&imu);
|
||||||
|
AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* LSM303dlhc event task */
|
||||||
|
lsm303dlhc_spi_event(&imu_px4.lsm_acc);
|
||||||
|
if (imu_px4.lsm_acc.data_available_acc) {
|
||||||
|
VECT3_COPY(imu.accel_unscaled, imu_px4.lsm_acc.data_accel.vect);
|
||||||
|
imu_px4.lsm_acc.data_available_acc = FALSE;
|
||||||
|
imu_scale_accel(&imu);
|
||||||
|
AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
|
||||||
|
}
|
||||||
|
lsm303dlhc_spi_event(&imu_px4.lsm_mag);
|
||||||
|
if (imu_px4.lsm_mag.data_available_mag) {
|
||||||
|
VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect);
|
||||||
|
imu_px4.lsm_mag.data_available_mag = FALSE;
|
||||||
|
imu_scale_mag(&imu);
|
||||||
|
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* HMC58XX event task */
|
||||||
|
hmc58xx_event(&imu_px4.hmc);
|
||||||
|
if (imu_px4.hmc.data_available) {
|
||||||
|
/* mag rotated by 90deg around z axis relative to MPU */
|
||||||
|
VECT3_COPY(imu.mag_unscaled, imu_px4.hmc.data.vect);
|
||||||
|
imu_px4.hmc.data_available = FALSE;
|
||||||
|
imu_scale_mag(&imu);
|
||||||
|
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file subsystems/imu/imu_mpu6000.h
|
||||||
|
* Driver for pixhawk IMU's.
|
||||||
|
* On with spi: L3GD20H + LSM303D and the MPU6000.
|
||||||
|
* On i2c: external HMC5883L (through 3dr gps).
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IMU_PX4FMUV24_H
|
||||||
|
#define IMU_PX4FMUV24_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
|
||||||
|
#include "subsystems/imu/imu_mpu60x0_defaults.h"
|
||||||
|
#include "peripherals/mpu60x0_spi.h"
|
||||||
|
#include "peripherals/hmc58xx.h"
|
||||||
|
#include "peripherals/l3gd20_spi.h"
|
||||||
|
#include "peripherals/lsm303dlhc_spi.h"
|
||||||
|
|
||||||
|
|
||||||
|
struct ImuPX4 {
|
||||||
|
struct Mpu60x0_Spi mpu;
|
||||||
|
struct Hmc58xx hmc;
|
||||||
|
struct L3gd20_Spi l3g;
|
||||||
|
struct Lsm303dlhc_Spi lsm_acc;
|
||||||
|
struct Lsm303dlhc_Spi lsm_mag;
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct ImuPX4 imu_px4;
|
||||||
|
|
||||||
|
extern void imu_px4_event(void);
|
||||||
|
|
||||||
|
#define ImuEvent imu_px4_event
|
||||||
|
|
||||||
|
#endif /* IMU_PX4FMUV24_H */
|
||||||
Reference in New Issue
Block a user