mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Merge pull request #1569 from kevindehecker/PX4_IMU
Pixhawk board IMU driver - SPI driver for the lsm303dlhc and refactor I2C driver. - Created a PX4 imu, which consists lsm303dlhc (acc) + l3g (gyro) IMU, both on spi and the lsm303dlhc mag - Changed the ms5611 baro to spi - configurable signs for hmc58xx (to rotate sensor output in 90deg units)
This commit is contained in:
Felix Ruess
@@ -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,17 @@
|
|||||||
<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="MAG_HMC58XX_SENDER_ID" /> <!-- Meaning the external hmc-->
|
||||||
|
|
||||||
|
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||||
|
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
|
||||||
|
|
||||||
|
</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" />
|
||||||
@@ -81,23 +84,38 @@
|
|||||||
<load name="air_data.xml" />
|
<load name="air_data.xml" />
|
||||||
<load name="send_imu_mag_current.xml" />
|
<load name="send_imu_mag_current.xml" />
|
||||||
<load name="gps_ubx_ucenter.xml" />
|
<load name="gps_ubx_ucenter.xml" />
|
||||||
|
<load name="mag_hmc58xx.xml">
|
||||||
|
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
|
||||||
|
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
|
||||||
|
<define name="HMC58XX_CHAN_X" value="1"/>
|
||||||
|
<define name="HMC58XX_CHAN_Y" value="0"/>
|
||||||
|
<define name="HMC58XX_CHAN_Z" value="2"/>
|
||||||
|
<define name="HMC58XX_CHAN_X_SIGN" value="-"/>
|
||||||
|
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
|
||||||
|
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
|
||||||
|
</load>
|
||||||
<!-- <load name="spektrum_soft_bind.xml"/>-->
|
<!-- <load name="spektrum_soft_bind.xml"/>-->
|
||||||
</modules>
|
</modules>
|
||||||
|
|
||||||
<section name="MISC">
|
<section name="MISC">
|
||||||
<define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
|
<define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
|
||||||
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
|
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
|
||||||
</section>
|
</section>
|
||||||
<section name="IMU" prefix="IMU_">
|
<section name="IMU" prefix="IMU_">
|
||||||
<!-- replace this with your own calibration -->
|
<!-- replace this with your own calibration -->
|
||||||
<define name="MAG_X_NEUTRAL" value="14" />
|
<define name="ACCEL_X_NEUTRAL" value="248"/>
|
||||||
<define name="MAG_Y_NEUTRAL" value="116" />
|
<define name="ACCEL_Y_NEUTRAL" value="-21"/>
|
||||||
<define name="MAG_Z_NEUTRAL" value="119" />
|
<define name="ACCEL_Z_NEUTRAL" value="1123"/>
|
||||||
<define name="MAG_X_SENS" value="5.09245681612" integer="16" />
|
<define name="ACCEL_X_SENS" value="0.62584356013" integer="16"/>
|
||||||
<define name="MAG_Y_SENS" value="5.29702744632" integer="16" />
|
<define name="ACCEL_Y_SENS" value="0.609921478775" integer="16"/>
|
||||||
<define name="MAG_Z_SENS" value="5.65287938992" integer="16" />
|
<define name="ACCEL_Z_SENS" value="0.619923942578" integer="16"/>
|
||||||
<define name="BODY_TO_IMU_PHI" value="180." unit="deg" />
|
|
||||||
<define name="BODY_TO_IMU_THETA" value="180." unit="deg" />
|
<define name="MAG_X_NEUTRAL" value="92"/>
|
||||||
<define name="BODY_TO_IMU_PSI" value="90." unit="deg" />
|
<define name="MAG_Y_NEUTRAL" value="35"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-15"/>
|
||||||
|
<define name="MAG_X_SENS" value="5.52543569751" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="4.91233764159" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="4.99453775296" integer="16"/>
|
||||||
</section>
|
</section>
|
||||||
<commands>
|
<commands>
|
||||||
<axis name="PITCH" failsafe_value="0" />
|
<axis name="PITCH" failsafe_value="0" />
|
||||||
|
|||||||
@@ -168,11 +168,14 @@ else ifeq ($(BOARD), krooz)
|
|||||||
|
|
||||||
# PX4FMU
|
# PX4FMU
|
||||||
else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
|
else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
|
||||||
BARO_BOARD_CFLAGS += -DUSE_I2C2
|
|
||||||
BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2
|
BARO_BOARD_CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE3
|
||||||
|
BARO_BOARD_CFLAGS += -DBB_MS5611_SPI_DEV=spi1
|
||||||
|
BARO_BOARD_CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3
|
||||||
|
|
||||||
BARO_BOARD_SRCS += peripherals/ms5611.c
|
BARO_BOARD_SRCS += peripherals/ms5611.c
|
||||||
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
|
BARO_BOARD_SRCS += peripherals/ms5611_spi.c
|
||||||
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
|
BARO_BOARD_SRCS += boards/baro_board_ms5611_spi.c
|
||||||
|
|
||||||
# apogee baro
|
# apogee baro
|
||||||
else ifeq ($(BOARD), apogee)
|
else ifeq ($(BOARD), apogee)
|
||||||
|
|||||||
@@ -0,0 +1,51 @@
|
|||||||
|
# Hey Emacs, this is a -*- makefile -*-
|
||||||
|
#
|
||||||
|
# PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto
|
||||||
|
#
|
||||||
|
# L3GD20 + LSM303D
|
||||||
|
|
||||||
|
include $(CFG_SHARED)/spi_master.makefile
|
||||||
|
|
||||||
|
IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\"
|
||||||
|
IMU_PX4FMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||||
|
IMU_PX4FMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c
|
||||||
|
|
||||||
|
#L3GD20 gyro
|
||||||
|
IMU_PX4FMU_SRCS += peripherals/l3gd20_spi.c
|
||||||
|
|
||||||
|
#LSM303D accelero + magneto
|
||||||
|
IMU_PX4FMU_SRCS += peripherals/lsm303dlhc_spi.c
|
||||||
|
|
||||||
|
# for fixedwing firmware and ap only
|
||||||
|
ifeq ($(TARGET), ap)
|
||||||
|
IMU_PX4FMU_CFLAGS += -DUSE_IMU
|
||||||
|
endif
|
||||||
|
|
||||||
|
# set default SPI device
|
||||||
|
IMU_PX4FMU_SPI_DEV ?= spi1
|
||||||
|
# convert spix to upper/lower case
|
||||||
|
IMU_PX4FMU_SPI_DEV_UPPER=$(shell echo $(IMU_PX4FMU_SPI_DEV) | tr a-z A-Z)
|
||||||
|
IMU_PX4FMU_SPI_DEV_LOWER=$(shell echo $(IMU_PX4FMU_SPI_DEV) | tr A-Z a-z)
|
||||||
|
IMU_PX4FMU_CFLAGS += -DIMU_PX4FMU_SPI_DEV=$(IMU_PX4FMU_SPI_DEV_LOWER)
|
||||||
|
IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_PX4FMU_SPI_DEV_UPPER)
|
||||||
|
|
||||||
|
#********** L3GD20 ***********
|
||||||
|
IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0
|
||||||
|
IMU_PX4FMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX)
|
||||||
|
IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_L3G_SPI_SLAVE_IDX)
|
||||||
|
|
||||||
|
#********** LSM303dlhc ***********
|
||||||
|
IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1
|
||||||
|
IMU_PX4FMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX)
|
||||||
|
IMU_PX4FMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX)
|
||||||
|
|
||||||
|
# add it for all targets except sim, fbw and nps
|
||||||
|
ifeq (,$(findstring $(TARGET),sim fbw nps))
|
||||||
|
$(TARGET).CFLAGS += $(IMU_PX4FMU_CFLAGS)
|
||||||
|
$(TARGET).srcs += $(IMU_PX4FMU_SRCS)
|
||||||
|
endif
|
||||||
|
|
||||||
|
#
|
||||||
|
# NPS simulator
|
||||||
|
#
|
||||||
|
include $(CFG_SHARED)/imu_nps.makefile
|
||||||
@@ -9,6 +9,12 @@
|
|||||||
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
|
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
|
||||||
<define name="MODULE_HMC58XX_SYNC_SEND" value="TRUE|FALSE" description="Send IMU_RAW message with each new measurement (default: FALSE)"/>
|
<define name="MODULE_HMC58XX_SYNC_SEND" value="TRUE|FALSE" description="Send IMU_RAW message with each new measurement (default: FALSE)"/>
|
||||||
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE|FALSE" description="Copy measurements to imu and send as ABI message (default: FALSE)"/>
|
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE|FALSE" description="Copy measurements to imu and send as ABI message (default: FALSE)"/>
|
||||||
|
<define name="HMC58XX_CHAN_X_SIGN" value="+|-" description="Reverse polarity of x axis (default: +)"/>
|
||||||
|
<define name="HMC58XX_CHAN_Y_SIGN" value="+|-" description="Reverse polarity of y axis (default: +)"/>
|
||||||
|
<define name="HMC58XX_CHAN_Z_SIGN" value="+|-" description="Reverse polarity of z axis (default: +)"/>
|
||||||
|
<define name="HMC58XX_CHAN_X" value="0|1|2" description="Channel id of x axis (default: 0)"/>
|
||||||
|
<define name="HMC58XX_CHAN_Y" value="0|1|2" description="Channel id of y axis (default: 1)"/>
|
||||||
|
<define name="HMC58XX_CHAN_Z" value="0|1|2" description="Channel id of z axis (default: 2)"/>
|
||||||
</doc>
|
</doc>
|
||||||
<header>
|
<header>
|
||||||
<file name="mag_hmc58xx.h"/>
|
<file name="mag_hmc58xx.h"/>
|
||||||
|
|||||||
@@ -8,7 +8,7 @@
|
|||||||
#define BOARDS_PX4FMU_BARO_H
|
#define BOARDS_PX4FMU_BARO_H
|
||||||
|
|
||||||
// only for printing the baro type during compilation
|
// only for printing the baro type during compilation
|
||||||
#define BARO_BOARD BARO_BOARD_MS5611_I2C
|
#define BARO_BOARD BARO_BOARD_MS5611_SPI
|
||||||
|
|
||||||
extern void baro_event(void);
|
extern void baro_event(void);
|
||||||
|
|
||||||
|
|||||||
@@ -39,6 +39,15 @@
|
|||||||
#ifndef HMC58XX_CHAN_Z
|
#ifndef HMC58XX_CHAN_Z
|
||||||
#define HMC58XX_CHAN_Z 2
|
#define HMC58XX_CHAN_Z 2
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef HMC58XX_CHAN_X_SIGN
|
||||||
|
#define HMC58XX_CHAN_X_SIGN +
|
||||||
|
#endif
|
||||||
|
#ifndef HMC58XX_CHAN_Y_SIGN
|
||||||
|
#define HMC58XX_CHAN_Y_SIGN +
|
||||||
|
#endif
|
||||||
|
#ifndef HMC58XX_CHAN_Z_SIGN
|
||||||
|
#define HMC58XX_CHAN_Z_SIGN +
|
||||||
|
#endif
|
||||||
|
|
||||||
#if MODULE_HMC58XX_UPDATE_AHRS
|
#if MODULE_HMC58XX_UPDATE_AHRS
|
||||||
#include "subsystems/imu.h"
|
#include "subsystems/imu.h"
|
||||||
@@ -68,9 +77,9 @@ void mag_hmc58xx_module_event(void)
|
|||||||
|
|
||||||
// set channel order
|
// set channel order
|
||||||
struct Int32Vect3 mag = {
|
struct Int32Vect3 mag = {
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
|
HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
|
HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
|
HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
|
||||||
};
|
};
|
||||||
// unscaled vector
|
// unscaled vector
|
||||||
VECT3_COPY(imu.mag_unscaled, mag);
|
VECT3_COPY(imu.mag_unscaled, mag);
|
||||||
@@ -91,9 +100,9 @@ void mag_hmc58xx_module_event(void)
|
|||||||
void mag_hmc58xx_report(void)
|
void mag_hmc58xx_report(void)
|
||||||
{
|
{
|
||||||
struct Int32Vect3 mag = {
|
struct Int32Vect3 mag = {
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
|
HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
|
HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
|
||||||
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
|
HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
|
||||||
};
|
};
|
||||||
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
|
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -80,7 +80,7 @@ enum L3gd20FullScale {
|
|||||||
L3GD20_FS_250dps = 0,
|
L3GD20_FS_250dps = 0,
|
||||||
L3GD20_FS_500dps = 1,
|
L3GD20_FS_500dps = 1,
|
||||||
L3GD20_FS_2000dps = 2,
|
L3GD20_FS_2000dps = 2,
|
||||||
L3GD20_FS_2000dps2 = 3,
|
L3GD20_FS_2000dps2 = 3, //yep, the same as L3GD20_FS_2000dps
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -25,85 +25,74 @@
|
|||||||
*
|
*
|
||||||
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
* Driver for ST LSM303DLHC 3D accelerometer and magnetometer.
|
||||||
*/
|
*/
|
||||||
|
#ifndef LSM303_H
|
||||||
#ifndef LSM303DLHC_H
|
#define LSM303_H
|
||||||
#define LSM303DLHC_H
|
|
||||||
|
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
#include "mcu_periph/i2c.h"
|
|
||||||
#include "math/pprz_algebra_int.h"
|
|
||||||
|
|
||||||
/* Address and register definitions */
|
/* Address and register definitions */
|
||||||
#include "peripherals/lsm303dlhc_regs.h"
|
#include "peripherals/lsm303dlhc_regs.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 (0x04 <<3) // acc +- 16G
|
||||||
|
#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
|
||||||
|
|
||||||
struct Lsm303dlhcAccConfig {
|
struct Lsm303dlhcAccConfig {
|
||||||
uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883)
|
uint8_t rate; ///< Data Output Rate (Hz)
|
||||||
uint8_t lp_mode; ///< Low power mode
|
uint8_t scale; ///< full scale selection (m/s²)
|
||||||
uint8_t scale; ///< full scale selection
|
|
||||||
uint8_t hres; ///< high resolution output mode
|
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Lsm303dlhcMagConfig {
|
struct Lsm303dlhcMagConfig {
|
||||||
uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883)
|
uint8_t rate; ///< Data Output Rate Bits (Hz)
|
||||||
uint8_t gain; ///< Gain configuration (1 -> +- 1 Gauss)
|
uint8_t scale; ///< Full scale gain configuration (Gauss)
|
||||||
uint8_t mode; ///< Measurement mode
|
uint8_t mode; ///< Measurement mode
|
||||||
};
|
};
|
||||||
|
|
||||||
/** config status states */
|
/** config status states */
|
||||||
enum Lsm303dlhcAccConfStatus {
|
enum Lsm303dlhcConfStatus {
|
||||||
LSM_CONF_ACC_UNINIT,
|
LSM_CONF_UNINIT,
|
||||||
LSM_CONF_ACC_CTRL_REG4_A,
|
LSM_CONF_WHO_AM_I,
|
||||||
LSM_CONF_ACC_CTRL_REG1_A,
|
LSM_CONF_CTRL_REG1,
|
||||||
LSM_CONF_ACC_CTRL_REG3_A,
|
LSM_CONF_CTRL_REG2,
|
||||||
LSM_CONF_ACC_DONE
|
LSM_CONF_CTRL_REG3,
|
||||||
|
LSM_CONF_CTRL_REG4,
|
||||||
|
LSM_CONF_CTRL_REG5,
|
||||||
|
LSM_CONF_CTRL_REG6,
|
||||||
|
LSM_CONF_CTRL_REG7,
|
||||||
|
LSM_CONF_DONE
|
||||||
};
|
};
|
||||||
|
|
||||||
/** config status states */
|
enum Lsm303dlhcTarget {
|
||||||
enum Lsm303dlhcMagConfStatus {
|
LSM_TARGET_ACC,
|
||||||
LSM_CONF_MAG_UNINIT,
|
LSM_TARGET_MAG
|
||||||
LSM_CONF_MAG_CRA_REG_M,
|
|
||||||
LSM_CONF_MAG_CRB_REG_M,
|
|
||||||
LSM_CONF_MAG_MR_REG_M,
|
|
||||||
LSM_CONF_MAG_DONE
|
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Lsm303dlhc {
|
static inline void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c)
|
||||||
struct i2c_periph *i2c_p;
|
|
||||||
struct i2c_transaction i2c_trans;
|
|
||||||
bool initialized; ///< config done flag
|
|
||||||
union {
|
|
||||||
enum Lsm303dlhcAccConfStatus acc; ///< init status
|
|
||||||
enum Lsm303dlhcMagConfStatus mag; ///< init status
|
|
||||||
} init_status;
|
|
||||||
volatile bool data_available; ///< data ready flag
|
|
||||||
union {
|
|
||||||
struct Int16Vect3 vect; ///< data vector in acc coordinate system
|
|
||||||
int16_t value[3]; ///< data values accessible by channel index
|
|
||||||
} data;
|
|
||||||
union {
|
|
||||||
struct Lsm303dlhcAccConfig acc;
|
|
||||||
struct Lsm303dlhcMagConfig mag;
|
|
||||||
} config;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// TODO IRQ handling
|
|
||||||
|
|
||||||
// Functions
|
|
||||||
extern void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr);
|
|
||||||
extern void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm);
|
|
||||||
extern void lsm303dlhc_read(struct Lsm303dlhc *lsm);
|
|
||||||
extern void lsm303dlhc_event(struct Lsm303dlhc *lsm);
|
|
||||||
|
|
||||||
/// convenience function: read or start configuration if not already initialized
|
|
||||||
static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm)
|
|
||||||
{
|
{
|
||||||
if (lsm->initialized) {
|
c->rate = LSM303DLHC_DEFAULT_AODR;
|
||||||
lsm303dlhc_read(lsm);
|
c->scale = LSM303DLHC_DEFAULT_AFS;
|
||||||
} else {
|
|
||||||
lsm303dlhc_start_configure(lsm);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif /* LSM303DLHC_H */
|
static inline void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c)
|
||||||
|
{
|
||||||
|
c->rate = LSM303DLHC_DEFAULT_MODR;
|
||||||
|
c->scale = LSM303DLHC_DEFAULT_MFS;
|
||||||
|
c->mode = LSM303DLHC_DEFAULT_MD;
|
||||||
|
}
|
||||||
|
#endif // LSM303_H
|
||||||
|
|||||||
@@ -21,65 +21,22 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @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.
|
||||||
|
* UNTESTED
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "peripherals/lsm303dlhc.h"
|
#include "peripherals/lsm303dlhc_i2c.h"
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
|
|
||||||
/* LSM303DLHC default conf */
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_ODR
|
|
||||||
#define LSM303DLHC_DEFAULT_ODR 0x90 //90 //normal 1.344khz, low power 5.376khz
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_LP
|
|
||||||
#define LSM303DLHC_DEFAULT_LP 0x00 //low power disabled
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_FS
|
|
||||||
#define LSM303DLHC_DEFAULT_FS 0x00 // +- 2G
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_HR
|
|
||||||
#define LSM303DLHC_DEFAULT_HR 0x04 // high res enabled
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_DO
|
|
||||||
#define LSM303DLHC_DEFAULT_DO (0x6 << 2) // Data Output Rate (75Hz)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_GN
|
|
||||||
#define LSM303DLHC_DEFAULT_GN (0x1 << 5) // Gain configuration (1 -> +- 1.3 Gauss)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef LSM303DLHC_DEFAULT_MD
|
|
||||||
#define LSM303DLHC_DEFAULT_MD 0x00 // Continious conversion mode
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c)
|
|
||||||
{
|
|
||||||
c->rate = LSM303DLHC_DEFAULT_ODR;
|
|
||||||
c->lp_mode = LSM303DLHC_DEFAULT_LP;
|
|
||||||
c->scale = LSM303DLHC_DEFAULT_FS;
|
|
||||||
c->hres = LSM303DLHC_DEFAULT_HR;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c)
|
|
||||||
{
|
|
||||||
c->rate = (LSM303DLHC_DEFAULT_DO & LSM303DLHC_DO0_MASK);
|
|
||||||
c->gain = (LSM303DLHC_DEFAULT_GN & LSM303DLHC_GN_MASK);
|
|
||||||
c->mode = (LSM303DLHC_DEFAULT_MD & LSM303DLHC_MD_MASK);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initialize Lsm303dlhc struct and set default config options.
|
* Initialize Lsm303dlhc struct and set default config options.
|
||||||
* @param lsm Lsm303dlhc struct
|
* @param lsm Lsm303dlhc struct
|
||||||
* @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 +54,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 +65,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 +89,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 +119,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 +132,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 +163,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) {
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
/*
|
||||||
|
* 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.
|
||||||
|
* UNTESTED
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef LSM303DLHC_H
|
||||||
|
#define LSM303DLHC_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "mcu_periph/i2c.h"
|
||||||
|
#include "math/pprz_algebra_int.h"
|
||||||
|
|
||||||
|
#include "peripherals/lsm303dlhc.h"
|
||||||
|
|
||||||
|
#warning "The Lsm303dlhc I2C driver has not been tested. Use with caution."
|
||||||
|
|
||||||
|
struct Lsm303dlhc_i2c {
|
||||||
|
struct i2c_periph *i2c_p;
|
||||||
|
struct i2c_transaction i2c_trans;
|
||||||
|
bool initialized; ///< config done flag
|
||||||
|
union {
|
||||||
|
enum Lsm303dlhcAccConfStatus acc; ///< init status
|
||||||
|
enum Lsm303dlhcMagConfStatus mag; ///< init status
|
||||||
|
} init_status;
|
||||||
|
volatile bool data_available; ///< data ready flag
|
||||||
|
union {
|
||||||
|
struct Int16Vect3 vect; ///< data vector in acc coordinate system
|
||||||
|
int16_t value[3]; ///< data values accessible by channel index
|
||||||
|
} data_accel;
|
||||||
|
union {
|
||||||
|
struct Lsm303dlhcAccConfig acc;
|
||||||
|
struct Lsm303dlhcMagConfig mag;
|
||||||
|
} config;
|
||||||
|
};
|
||||||
|
|
||||||
|
// TODO IRQ handling
|
||||||
|
|
||||||
|
// Functions
|
||||||
|
extern void lsm303dlhc_i2c_init(struct Lsm303dlhc_i2c *lsm, struct i2c_periph *i2c_p, uint8_t addr);
|
||||||
|
extern void lsm303dlhc_i2c_start_configure(struct Lsm303dlhc_i2c *lsm);
|
||||||
|
extern void lsm303dlhc_i2c_read(struct Lsm303dlhc_i2c *lsm);
|
||||||
|
extern void lsm303dlhc_i2c_event(struct Lsm303dlhc_i2c *lsm);
|
||||||
|
|
||||||
|
/// convenience function: read or start configuration if not already initialized
|
||||||
|
static inline void lsm303dlhc_i2c_periodic(struct Lsm303dlhc_i2c *lsm)
|
||||||
|
{
|
||||||
|
if (lsm->initialized) {
|
||||||
|
lsm303dlhc_i2c_read(lsm);
|
||||||
|
} else {
|
||||||
|
lsm303dlhc_i2c_start_configure(lsm);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* LSM303DLHC_H */
|
||||||
@@ -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,222 @@
|
|||||||
|
/*
|
||||||
|
* 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"
|
||||||
|
|
||||||
|
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,80 @@
|
|||||||
|
/*
|
||||||
|
* 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"
|
||||||
|
#include "peripherals/lsm303dlhc.h"
|
||||||
|
|
||||||
|
struct Lsm303dlhc_Spi {
|
||||||
|
struct spi_periph *spi_p;
|
||||||
|
struct spi_transaction spi_trans;
|
||||||
|
bool 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 data_available_acc; ///< data ready flag accelero
|
||||||
|
volatile bool 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,64 @@
|
|||||||
|
/*
|
||||||
|
* 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_px4_defaults.h
|
||||||
|
* Default sensitivity definitions for the Pixhawk IMU using the l3d20 gyro and lsm303dlc acc.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IMU_PX4_DEFAULTS_H
|
||||||
|
#define IMU_PX4_DEFAULTS_H
|
||||||
|
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
|
||||||
|
/** default gyro sensitivy and neutral from the datasheet
|
||||||
|
* L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range
|
||||||
|
* sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC
|
||||||
|
* sens = (70e-3 / 180.0f) * pi * 4096
|
||||||
|
*/
|
||||||
|
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
|
||||||
|
#define IMU_GYRO_P_SENS_NUM 5004
|
||||||
|
#define IMU_GYRO_P_SENS_DEN 1000
|
||||||
|
#define IMU_GYRO_Q_SENS_NUM 5004
|
||||||
|
#define IMU_GYRO_Q_SENS_DEN 1000
|
||||||
|
#define IMU_GYRO_R_SENS_NUM 5004
|
||||||
|
#define IMU_GYRO_R_SENS_DEN 1000
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/** default accel sensitivy from the datasheet
|
||||||
|
* LSM303DLHC has 732 LSB/g
|
||||||
|
* fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC
|
||||||
|
* sens = 9.81 / 732 * 1024 = 13.72
|
||||||
|
*/
|
||||||
|
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
|
||||||
|
|
||||||
|
#define IMU_ACCEL_X_SENS 13.723
|
||||||
|
#define IMU_ACCEL_X_SENS_NUM 13723
|
||||||
|
#define IMU_ACCEL_X_SENS_DEN 1000
|
||||||
|
#define IMU_ACCEL_Y_SENS 13.723
|
||||||
|
#define IMU_ACCEL_Y_SENS_NUM 13723
|
||||||
|
#define IMU_ACCEL_Y_SENS_DEN 1000
|
||||||
|
#define IMU_ACCEL_Z_SENS 13.723
|
||||||
|
#define IMU_ACCEL_Z_SENS_NUM 13723
|
||||||
|
#define IMU_ACCEL_Z_SENS_DEN 1000
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* IMU_PX4_DEFAULTS_H */
|
||||||
@@ -0,0 +1,106 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013-2016 the paparazzi team
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
* L3GD20H + LSM303D (both on spi)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
#include "subsystems/abi.h"
|
||||||
|
#include "mcu_periph/spi.h"
|
||||||
|
#include "peripherals/l3gd20_regs.h"
|
||||||
|
#include "peripherals/lsm303dlhc_regs.h"
|
||||||
|
#include "peripherals/lsm303dlhc_spi.h"
|
||||||
|
|
||||||
|
/* SPI defaults set in subsystem makefile, can be configured from airframe file */
|
||||||
|
PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
|
||||||
|
PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
|
||||||
|
PRINT_CONFIG_VAR(IMU_PX4FMU_SPI_DEV)
|
||||||
|
|
||||||
|
struct ImuPX4 imu_px4;
|
||||||
|
|
||||||
|
void imu_impl_init(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
/* L3GD20 gyro init */
|
||||||
|
/* initialize gyro and set default options */
|
||||||
|
l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
|
||||||
|
|
||||||
|
/* LSM303dlhc acc + magneto init */
|
||||||
|
lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
|
||||||
|
#if !IMU_PX4_DISABLE_MAG
|
||||||
|
lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_periodic(void)
|
||||||
|
{
|
||||||
|
l3gd20_spi_periodic(&imu_px4.l3g);
|
||||||
|
lsm303dlhc_spi_periodic(&imu_px4.lsm_acc);
|
||||||
|
|
||||||
|
#if !IMU_PX4_DISABLE_MAG
|
||||||
|
/* Read magneto's every 10 times of main freq
|
||||||
|
* at ~50Hz (main loop for rotorcraft: 512Hz)
|
||||||
|
*/
|
||||||
|
RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_px4_event(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
uint32_t now_ts = get_sys_time_usec();
|
||||||
|
|
||||||
|
/* L3GD20 event task */
|
||||||
|
l3gd20_spi_event(&imu_px4.l3g);
|
||||||
|
if (imu_px4.l3g.data_available) {
|
||||||
|
//the p and q seem to be swapped on the Pixhawk board compared to the acc
|
||||||
|
imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q;
|
||||||
|
imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p;
|
||||||
|
imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r;
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
#if !IMU_PX4_DISABLE_MAG
|
||||||
|
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_PX4_ID, now_ts, &imu.mag);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013-2016 the paparazzi team
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
* L3GD20H + LSM303D (both on spi)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IMU_PX4FMUV24_H
|
||||||
|
#define IMU_PX4FMUV24_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
|
||||||
|
#include "subsystems/imu/imu_px4_defaults.h"
|
||||||
|
#include "peripherals/l3gd20_spi.h"
|
||||||
|
#include "peripherals/lsm303dlhc_spi.h"
|
||||||
|
|
||||||
|
#ifndef IMU_PX4_DISABLE_MAG
|
||||||
|
#if MODULE_HMC58XX_UPDATE_AHRS
|
||||||
|
#define IMU_PX4_DISABLE_MAG TRUE
|
||||||
|
#else
|
||||||
|
#define IMU_PX4_DISABLE_MAG FALSE
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct ImuPX4 {
|
||||||
|
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 */
|
||||||
@@ -81,8 +81,9 @@ void disable_inter_comm(bool value)
|
|||||||
void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused)))
|
void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused)))
|
||||||
{
|
{
|
||||||
if (!disable_comm) {
|
if (!disable_comm) {
|
||||||
|
uint8_t autopilot_motors_on_tmp = autopilot_motors_on;
|
||||||
pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device,
|
pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device,
|
||||||
INTERMCU_AP, &autopilot_motors_on, COMMANDS_NB, command_values); //TODO: Append more status
|
INTERMCU_AP, &autopilot_motors_on_tmp, COMMANDS_NB, command_values); //TODO: Append more status
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user