Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[peripherals][imu] i2c slaves for the mpu60x0

Browse Source 
new IMU drivers using the mpu60x0 peripheral with slave support
- aspirin 2 (spi) driver with mag as real slave
- drotek 10dof v2 with mag as passthrough slave (untested and axes might be wrong)

closes #450
closes #190
This commit is contained in:
Felix Ruess
2013-05-10 01:04:06 +02:00
parent 65d0889ad0
commit b97d8c2236
18 changed files with 942 additions and 306 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+1 -1
View File
@@ -32,7 +32,7 @@
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="aspirin_v2.1_new"/>
<subsystem name="imu" type="aspirin_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
+17 -12
View File
@@ -39,32 +39,37 @@
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_ASPIRIN_CFLAGS = -DUSE_IMU
IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
endif
IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\"
IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
# Magnetometer
#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
endif
IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_SRCS)
ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_2_SRCS)
#
conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile → conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
+11 -18
View File
@@ -39,39 +39,32 @@
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
IMU_ASPIRIN_CFLAGS = -DUSE_IMU
endif
IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\"
IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
# Magnetometer
#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
endif
#IMU_ASPIRIN_2_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_2_SRCS)
ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_SRCS)
#
conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
+4 -35
View File
@@ -2,11 +2,13 @@
#
# Aspirin IMU v2.2
#
# actually identical with v2.1 since baro is not read in IMU driver
#
#
# required xml:
# <section name="IMU" prefix="IMU_">
#
# <!-- these gyro and accel calib values are the defaults for aspirin2.2 -->
# <!-- these gyro and accel calib values are the defaults for aspirin2.1 -->
# <define name="GYRO_X_NEUTRAL" value="0"/>
# <define name="GYRO_Y_NEUTRAL" value="0"/>
# <define name="GYRO_Z_NEUTRAL" value="0"/>
@@ -37,37 +39,4 @@
#
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_ASPIRIN_CFLAGS = -DUSE_IMU
endif
IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
endif
IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_2
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_SRCS)
#
# NPS simulator
#
include $(CFG_SHARED)/imu_nps.makefile
include $(CFG_SHARED)/imu_aspirin_v2.1.makefile
conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
+81
View File
@@ -0,0 +1,81 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Drotek 10DOF V2 IMU via I2C
#
#
# required xml:
# <section name="IMU" prefix="IMU_">
#
# <!-- these gyro and accel calib values are the defaults for aspirin2.1 -->
# <define name="GYRO_X_NEUTRAL" value="0"/>
# <define name="GYRO_Y_NEUTRAL" value="0"/>
# <define name="GYRO_Z_NEUTRAL" value="0"/>
#
# <define name="GYRO_X_SENS" value="4.359" integer="16"/>
# <define name="GYRO_Y_SENS" value="4.359" integer="16"/>
# <define name="GYRO_Z_SENS" value="4.359" integer="16"/>
#
# <define name="ACCEL_X_NEUTRAL" value="0"/>
# <define name="ACCEL_Y_NEUTRAL" value="0"/>
# <define name="ACCEL_Z_NEUTRAL" value="0"/>
#
# <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
# <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
# <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
#
# <!-- replace the mag calibration with your own-->
# <define name="MAG_X_NEUTRAL" value="-45"/>
# <define name="MAG_Y_NEUTRAL" value="334"/>
# <define name="MAG_Z_NEUTRAL" value="7"/>
#
# <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
# <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
# <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
#
# </section>
#
#
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_DROTEK_2_CFLAGS = -DUSE_IMU
endif
IMU_DROTEK_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_drotek_10dof_v2.h\"
IMU_DROTEK_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
IMU_DROTEK_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_drotek_10dof_v2.c
IMU_DROTEK_2_SRCS += peripherals/mpu60x0.c
IMU_DROTEK_2_SRCS += peripherals/mpu60x0_i2c.c
# Magnetometer
IMU_DROTEK_2_SRCS += peripherals/hmc58xx.c
# set default i2c bus
ifndef DROTEK_2_I2C_DEV
ifeq ($(ARCH), lpc21)
DROTEK_2_I2C_DEV=i2c0
else ifeq ($(ARCH), stm32)
DROTEK_2_I2C_DEV=i2c2
endif
endif
# convert i2cx to upper case
DROTEK_2_I2C_DEV_UPPER=$(shell echo $(DROTEK_2_I2C_DEV) | tr a-z A-Z)
IMU_DROTEK_2_CFLAGS += -DDROTEK_2_I2C_DEV=$(DROTEK_2_I2C_DEV)
IMU_DROTEK_2_CFLAGS += -DUSE_$(DROTEK_2_I2C_DEV_UPPER)
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
ap.CFLAGS += $(IMU_DROTEK_2_CFLAGS)
ap.srcs += $(IMU_DROTEK_2_SRCS)
#
# NPS simulator
#
include $(CFG_SHARED)/imu_nps.makefile
sw/airborne/peripherals/mpu60x0.c
+31 -5
View File
@@ -31,18 +31,38 @@
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
{
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
c->i2c_bypass = TRUE;
c->drdy_int_enable = FALSE;
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
/* Number of bytes to read starting with MPU60X0_REG_INT_STATUS
* By default read only gyro and accel data -> 15 bytes.
* Increase to include slave data.
*/
c->nb_bytes = 15;
c->nb_slaves = 0;
c->i2c_bypass = FALSE;
}
void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
{
switch (config->init_status) {
case MPU60X0_CONF_RESET:
/* device reset, set register values to defaults */
mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1<<6));
config->init_status++;
break;
case MPU60X0_CONF_USER_RESET:
/* trigger FIFO, I2C_MST and SIG_COND resets */
mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_FIFO_RESET) |
(1 << MPU60X0_I2C_MST_RESET) |
(1 << MPU60X0_SIG_COND_RESET)));
config->init_status++;
break;
case MPU60X0_CONF_PWR:
/* switch to gyroX clock by default */
mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
@@ -68,9 +88,15 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Conf
mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
config->init_status++;
break;
case MPU60X0_CONF_I2C_BYPASS:
mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
config->init_status++;
case MPU60X0_CONF_I2C_SLAVES:
/* if any, set MPU for I2C slaves and configure them*/
if (config->nb_slaves > 0) {
/* returns TRUE when all slaves are configured */
if (mpu60x0_configure_i2c_slaves(mpu_set, mpu))
config->init_status++;
}
else
config->init_status++;
break;
case MPU60X0_CONF_INT_ENABLE:
/* configure data ready interrupt */
sw/airborne/peripherals/mpu60x0.h
+33 -5
View File
@@ -48,34 +48,62 @@
enum Mpu60x0ConfStatus {
MPU60X0_CONF_UNINIT,
MPU60X0_CONF_RESET,
MPU60X0_CONF_USER_RESET,
MPU60X0_CONF_PWR,
MPU60X0_CONF_SD,
MPU60X0_CONF_DLPF,
MPU60X0_CONF_GYRO,
MPU60X0_CONF_ACCEL,
MPU60X0_CONF_I2C_BYPASS,
MPU60X0_CONF_I2C_SLAVES,
MPU60X0_CONF_INT_ENABLE,
MPU60X0_CONF_DONE
};
/// Configuration function prototype
typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
/// function prototype for configuration of a single I2C slave
typedef bool_t (*Mpu60x0I2cSlaveConfigure)(Mpu60x0ConfigSet mpu_set, void* mpu);
struct Mpu60x0I2cSlave {
Mpu60x0I2cSlaveConfigure configure;
};
struct Mpu60x0Config {
uint8_t smplrt_div; ///< Sample rate divider
enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range
enum Mpu60x0AccelRanges accel_range; ///< g Range
bool_t i2c_bypass; ///< bypass mpu i2c
bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
uint8_t clk_sel; ///< Clock select
uint8_t nb_bytes; ///< number of bytes to read starting with MPU60X0_REG_INT_STATUS
enum Mpu60x0ConfStatus init_status; ///< init status
bool_t initialized; ///< config done flag
/** Bypass MPU I2C.
* Only effective if using the I2C implementation.
*/
bool_t i2c_bypass;
uint8_t nb_slaves; ///< number of used I2C slaves
struct Mpu60x0I2cSlave slaves[5]; ///< I2C slaves
enum Mpu60x0MstClk i2c_mst_clk; ///< MPU I2C master clock speed
uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate
};
extern void mpu60x0_set_default_config(struct Mpu60x0Config *c);
/// Configuration function prototype
typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
/// Configuration sequence called once before normal use
extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config);
/**
* Configure I2C slaves of the MPU.
* This is I2C/SPI implementation specific.
* @param mpu_set configuration function
* @param mpu Mpu60x0Spi or Mpu60x0I2c peripheral
* @return TRUE when all slaves are configured
*/
extern bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu);
#endif // MPU60X0_H
sw/airborne/peripherals/mpu60x0_i2c.c
+80 -10
View File
@@ -44,6 +44,8 @@ void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU60X0_CONF_UNINIT;
mpu->slave_init_status = MPU60X0_I2C_CONF_UNINIT;
}
@@ -70,7 +72,7 @@ void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
/* set read bit and multiple byte bit, then address */
mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS;
i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15);
i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
}
}
@@ -84,14 +86,19 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
}
else if (mpu->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
if (bit_is_set(mpu->i2c_trans.buf[0],0)) {
if (bit_is_set(mpu->i2c_trans.buf[0], 0)) {
// new data
mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1);
mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3);
mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5);
mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9);
mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11);
mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13);
mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1);
mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3);
mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5);
mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9);
mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11);
mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13);
// if we are reading slaves, copy the ext_sens_data
if (mpu->config.nb_slaves > 0)
memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
mpu->data_available = TRUE;
}
mpu->i2c_trans.status = I2CTransDone;
@@ -103,12 +110,75 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
mpu->config.init_status--; // Retry config (TODO max retry)
case I2CTransSuccess:
case I2CTransDone:
mpu->i2c_trans.status = I2CTransDone;
mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config));
if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone;
if (mpu->config.initialized)
mpu->i2c_trans.status = I2CTransDone;
break;
default:
break;
}
}
}
/** @todo: only one slave so far. */
bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu)
{
struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT)
mpu_i2c->slave_init_status++;
switch (mpu_i2c->slave_init_status) {
case MPU60X0_I2C_CONF_I2C_MST_DIS:
mpu_set(mpu, MPU60X0_REG_USER_CTRL, 0);
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_I2C_BYPASS_EN:
/* switch to I2C passthrough */
mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1<<1));
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_SLAVES_CONFIGURE:
/* configure each slave. TODO: currently only one */
if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu))
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_I2C_BYPASS_DIS:
if (mpu_i2c->config.i2c_bypass) {
/* if bypassing I2C skip MPU I2C master setup */
mpu_i2c->slave_init_status = MPU60X0_I2C_CONF_DONE;
}
else {
/* disable I2C passthrough again */
mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0<<1));
mpu_i2c->slave_init_status++;
}
break;
case MPU60X0_I2C_CONF_I2C_MST_CLK:
/* configure MPU I2C master clock and stop/start between slave reads */
mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL,
((1<<4) | mpu_i2c->config.i2c_mst_clk));
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_I2C_MST_DELAY:
/* Set I2C slaves delayed sample rate */
mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay);
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_I2C_SMPLRT:
/* I2C slave0 sample rate/2 = 100/2 = 50Hz */
mpu_set(mpu, MPU60X0_REG_I2C_SLV4_CTRL, 0);
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_I2C_MST_EN:
/* enable internal I2C master */
mpu_set(mpu, MPU60X0_REG_USER_CTRL, (1 << MPU60X0_I2C_MST_EN));
mpu_i2c->slave_init_status++;
break;
case MPU60X0_I2C_CONF_DONE:
return TRUE;
default:
break;
}
return FALSE;
}
sw/airborne/peripherals/mpu60x0_i2c.h
+17
View File
@@ -36,6 +36,21 @@
#include "peripherals/mpu60x0.h"
#define MPU60X0_BUFFER_EXT_LEN 16
enum Mpu60x0I2cSlaveInitStatus {
MPU60X0_I2C_CONF_UNINIT,
MPU60X0_I2C_CONF_I2C_MST_DIS,
MPU60X0_I2C_CONF_I2C_BYPASS_EN,
MPU60X0_I2C_CONF_SLAVES_CONFIGURE,
MPU60X0_I2C_CONF_I2C_BYPASS_DIS,
MPU60X0_I2C_CONF_I2C_MST_CLK,
MPU60X0_I2C_CONF_I2C_MST_DELAY,
MPU60X0_I2C_CONF_I2C_SMPLRT,
MPU60X0_I2C_CONF_I2C_MST_EN,
MPU60X0_I2C_CONF_DONE
};
struct Mpu60x0_I2c {
struct i2c_periph *i2c_p;
struct i2c_transaction i2c_trans;
@@ -48,7 +63,9 @@ struct Mpu60x0_I2c {
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
int16_t value[3]; ///< rates data values accessible by channel index
} data_rates;
uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN];
struct Mpu60x0Config config;
enum Mpu60x0I2cSlaveInitStatus slave_init_status;
};
// Functions
sw/airborne/peripherals/mpu60x0_regs.h
+59 -30
View File
@@ -41,21 +41,21 @@
#define MPU60X0_REG_PWR_MGMT_2 0x6C
// FIFO
#define MPU60X0_REG_FIFO_EN 0X23
#define MPU60X0_REG_FIFO_EN 0x23
#define MPU60X0_REG_FIFO_COUNT_H 0x72
#define MPU60X0_REG_FIFO_COUNT_L 0x73
#define MPU60X0_REG_FIFO_R_W 0x74
// Measurement Settings
#define MPU60X0_REG_SMPLRT_DIV 0X19
#define MPU60X0_REG_CONFIG 0X1A
#define MPU60X0_REG_GYRO_CONFIG 0X1B
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
#define MPU60X0_REG_SMPLRT_DIV 0x19
#define MPU60X0_REG_CONFIG 0x1A
#define MPU60X0_REG_GYRO_CONFIG 0x1B
#define MPU60X0_REG_ACCEL_CONFIG 0x1C
// I2C Slave settings
#define MPU60X0_REG_I2C_MST_CTRL 0X24
#define MPU60X0_REG_I2C_MST_STATUS 0X36
#define MPU60X0_REG_I2C_MST_DELAY 0X67
#define MPU60X0_REG_I2C_MST_CTRL 0x24
#define MPU60X0_REG_I2C_MST_STATUS 0x36
#define MPU60X0_REG_I2C_MST_DELAY 0x67
// Slave 0
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
@@ -84,44 +84,51 @@
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
// Interrupt
#define MPU60X0_REG_INT_PIN_CFG 0X37
#define MPU60X0_REG_INT_ENABLE 0X38
#define MPU60X0_REG_INT_STATUS 0X3A
#define MPU60X0_REG_INT_PIN_CFG 0x37
#define MPU60X0_REG_INT_ENABLE 0x38
#define MPU60X0_REG_INT_STATUS 0x3A
// Accelero
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
#define MPU60X0_REG_ACCEL_XOUT_H 0x3B
#define MPU60X0_REG_ACCEL_XOUT_L 0x3C
#define MPU60X0_REG_ACCEL_YOUT_H 0x3D
#define MPU60X0_REG_ACCEL_YOUT_L 0x3E
#define MPU60X0_REG_ACCEL_ZOUT_H 0x3F
#define MPU60X0_REG_ACCEL_ZOUT_L 0x40
// Temperature
#define MPU60X0_REG_TEMP_OUT_H 0X41
#define MPU60X0_REG_TEMP_OUT_L 0X42
#define MPU60X0_REG_TEMP_OUT_H 0x41
#define MPU60X0_REG_TEMP_OUT_L 0x42
// Gyro
#define MPU60X0_REG_GYRO_XOUT_H 0X43
#define MPU60X0_REG_GYRO_XOUT_L 0X44
#define MPU60X0_REG_GYRO_YOUT_H 0X45
#define MPU60X0_REG_GYRO_YOUT_L 0X46
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
#define MPU60X0_REG_GYRO_XOUT_H 0x43
#define MPU60X0_REG_GYRO_XOUT_L 0x44
#define MPU60X0_REG_GYRO_YOUT_H 0x45
#define MPU60X0_REG_GYRO_YOUT_L 0x46
#define MPU60X0_REG_GYRO_ZOUT_H 0x47
#define MPU60X0_REG_GYRO_ZOUT_L 0x48
// External Sensor Data
#define MPU60X0_EXT_SENS_DATA 0X49
#define MPU60X0_EXT_SENS_DATA 0x49
#define MPU60X0_EXT_SENS_DATA_SIZE 24
/////////////////////////////////////////////////
// MPU60X0 Definitions
#define MPU60X0_REG_WHO_AM_I 0X75
#define MPU60X0_REG_WHO_AM_I 0x75
#define MPU60X0_WHOAMI_REPLY 0x68
// Bit positions
#define MPU60X0_I2C_BYPASS_EN 1
// in MPU60X0_REG_USER_CTRL
#define MPU60X0_SIG_COND_RESET 0
#define MPU60X0_I2C_MST_RESET 1
#define MPU60X0_FIFO_RESET 2
#define MPU60X0_I2C_IF_DIS 4
#define MPU60X0_I2C_MST_EN 5
#define MPU60X0_FIFO_EN 6
// in MPU60X0_REG_I2C_MST_STATUS
#define MPU60X0_I2C_SLV4_DONE 6
/** Digital Low Pass Filter Options
* DLFP is affecting both gyro and accels,
@@ -157,4 +164,26 @@ enum Mpu60x0AccelRanges {
MPU60X0_ACCEL_RANGE_16G = 0x03
};
/**
* I2C Master clock
*/
enum Mpu60x0MstClk {
MPU60X0_MST_CLK_500KHZ = 0x9,
MPU60X0_MST_CLK_471KHZ = 0xA,
MPU60X0_MST_CLK_444KHZ = 0xB,
MPU60X0_MST_CLK_421KHZ = 0xC,
MPU60X0_MST_CLK_400KHZ = 0xD,
MPU60X0_MST_CLK_381KHZ = 0xE,
MPU60X0_MST_CLK_364KHZ = 0xF,
MPU60X0_MST_CLK_348KHZ = 0x0,
MPU60X0_MST_CLK_333KHZ = 0x1,
MPU60X0_MST_CLK_320KHZ = 0x2,
MPU60X0_MST_CLK_308KHZ = 0x3,
MPU60X0_MST_CLK_296KHZ = 0x4,
MPU60X0_MST_CLK_286KHZ = 0x5,
MPU60X0_MST_CLK_276KHZ = 0x6,
MPU60X0_MST_CLK_267KHZ = 0x7,
MPU60X0_MST_CLK_254KHZ = 0x8
};
#endif /* MPU60X0_REGS_H */
sw/airborne/peripherals/mpu60x0_spi.c
+57 -12
View File
@@ -42,7 +42,7 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t
mpu->spi_trans.select = SPISelectUnselect;
mpu->spi_trans.slave_idx = slave_idx;
mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
mpu->spi_trans.output_length = 2;
mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
mpu->spi_trans.before_cb = NULL;
mpu->spi_trans.after_cb = NULL;
@@ -58,6 +58,8 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU60X0_CONF_UNINIT;
mpu->slave_init_status = MPU60X0_SPI_CONF_UNINIT;
}
@@ -85,9 +87,9 @@ void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
{
if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 16; // FIXME external data
mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes;
/* set read bit and multiple byte bit, then address */
mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS | MPU60X0_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
@@ -102,14 +104,19 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
}
else if (mpu->spi_trans.status == SPITransSuccess) {
// Successfull reading
if (bit_is_set(mpu->rx_buf[1],0)) {
if (bit_is_set(mpu->rx_buf[1], 0)) {
// new data
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,2);
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,4);
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,6);
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,10);
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,12);
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,14);
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2);
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4);
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6);
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10);
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12);
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
// if we are reading slaves, copy the ext_sens_data
if (mpu->config.nb_slaves > 0)
memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15);
mpu->data_available = TRUE;
}
mpu->spi_trans.status = SPITransDone;
@@ -121,12 +128,50 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
mpu->config.init_status--; // Retry config (TODO max retry)
case SPITransSuccess:
case SPITransDone:
mpu->spi_trans.status = SPITransDone;
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config));
if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone;
if (mpu->config.initialized)
mpu->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
}
/** @todo: only one slave so far. */
bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu)
{
struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu);
if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT)
mpu_spi->slave_init_status++;
switch (mpu_spi->slave_init_status) {
case MPU60X0_SPI_CONF_I2C_MST_CLK:
/* configure MPU I2C master clock and stop/start between slave reads */
mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk));
mpu_spi->slave_init_status++;
break;
case MPU60X0_SPI_CONF_I2C_MST_DELAY:
/* Set I2C slaves delayed sample rate */
mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay);
mpu_spi->slave_init_status++;
break;
case MPU60X0_SPI_CONF_I2C_MST_EN:
/* enable internal I2C master and disable primary I2C interface */
mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_I2C_IF_DIS) |
(1 << MPU60X0_I2C_MST_EN)));
mpu_spi->slave_init_status++;
break;
case MPU60X0_SPI_CONF_SLAVES_CONFIGURE:
/* configure first slave, only one slave supported so far */
if (mpu_spi->config.slaves[0].configure(mpu_set, mpu))
mpu_spi->slave_init_status++;
break;
case MPU60X0_SPI_CONF_DONE:
return TRUE;
default:
break;
}
return FALSE;
}
sw/airborne/peripherals/mpu60x0_spi.h
+14 -2
View File
@@ -37,12 +37,22 @@
#define MPU60X0_BUFFER_LEN 32
#define MPU60X0_BUFFER_EXT_LEN 16
enum Mpu60x0SpiSlaveInitStatus {
MPU60X0_SPI_CONF_UNINIT,
MPU60X0_SPI_CONF_I2C_MST_CLK,
MPU60X0_SPI_CONF_I2C_MST_DELAY,
MPU60X0_SPI_CONF_I2C_MST_EN,
MPU60X0_SPI_CONF_SLAVES_CONFIGURE,
MPU60X0_SPI_CONF_DONE
};
struct Mpu60x0_Spi {
struct spi_periph *spi_p;
struct spi_transaction spi_trans;
volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length
volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem
volatile uint8_t tx_buf[2];
volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN];
volatile bool_t data_available; ///< data ready flag
union {
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
@@ -52,7 +62,9 @@ struct Mpu60x0_Spi {
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
int16_t value[3]; ///< rates data values accessible by channel index
} data_rates;
uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN];
struct Mpu60x0Config config;
enum Mpu60x0SpiSlaveInitStatus slave_init_status;
};
// Functions
sw/airborne/subsystems/imu/imu_aspirin_2.c
-121
View File
@@ -1,121 +0,0 @@
/*
* 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_aspirin_2.c
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
*/
#include "subsystems/imu.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/spi.h"
/* defaults suitable for Lisa */
#ifndef ASPIRIN_2_SPI_SLAVE_IDX
#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
#ifndef ASPIRIN_2_SPI_DEV
#define ASPIRIN_2_SPI_DEV spi2
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
#ifndef ASPIRIN_2_I2C_DEV
#define ASPIRIN_2_I2C_DEV i2c2
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_I2C_DEV)
/* gyro internal lowpass frequency */
#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define ASPIRIN_2_SMPLRT_DIV 1
PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
#ifndef ASPIRIN_2_GYRO_RANGE
#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
#ifndef ASPIRIN_2_ACCEL_RANGE
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
struct ImuAspirin2 imu_aspirin2;
void imu_impl_init(void)
{
imu_aspirin2.accel_valid = FALSE;
imu_aspirin2.gyro_valid = FALSE;
imu_aspirin2.mag_valid = FALSE;
mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
// change the default configuration
imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
//imu_aspirin2.mpu.config.i2c_bypass = FALSE;
//imu_aspirin2.mpu.config.drdy_int_enable = TRUE;
//hmc58xx_init(&imu_aspirin2.mag_hmc, &(ASPIRIN_2_I2C_DEV), HMC58XX_ADDR);
}
void imu_periodic(void)
{
mpu60x0_spi_periodic(&imu_aspirin2.mpu);
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
//RunOnceEvery(10, hmc58xx_periodic(&imu_aspirin2.mag_hmc));
}
void imu_aspirin2_event(void)
{
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
imu_aspirin2.accel_valid = TRUE;
}
#if 0
/* HMC58XX event task */
hmc58xx_event(&imu_aspirin2.mag_hmc);
if (imu_aspirin2.mag_hmc.data_available) {
imu.mag_unscaled.x = imu_aspirin2.mag_hmc.data.vect.y;
imu.mag_unscaled.y = -imu_aspirin2.mag_hmc.data.vect.x;
imu.mag_unscaled.z = imu_aspirin2.mag_hmc.data.vect.z;
imu_aspirin2.mag_hmc.data_available = FALSE;
imu_aspirin2.mag_valid = TRUE;
}
#endif
}
sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
+232
View File
@@ -0,0 +1,232 @@
/*
* 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_aspirin_2_spi.c
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
*/
#include "subsystems/imu.h"
#include "mcu_periph/spi.h"
#include "peripherals/hmc58xx_regs.h"
/* defaults suitable for Lisa */
#ifndef ASPIRIN_2_SPI_SLAVE_IDX
#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
#ifndef ASPIRIN_2_SPI_DEV
#define ASPIRIN_2_SPI_DEV spi2
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
/* gyro internal lowpass frequency */
#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define ASPIRIN_2_SMPLRT_DIV 1
//PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
#ifndef ASPIRIN_2_GYRO_RANGE
#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
#ifndef ASPIRIN_2_ACCEL_RANGE
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
/* HMC58XX default conf */
#ifndef HMC58XX_DO
#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 75Hz with HMC5883)
#endif
#ifndef HMC58XX_MS
#define HMC58XX_MS 0x0 // Measurement configuration
#endif
#ifndef HMC58XX_GN
#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss)
#endif
#ifndef HMC58XX_MD
#define HMC58XX_MD 0x0 // Continious measurement mode
#endif
#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS))
#define HMC58XX_CRB (HMC58XX_GN<<5)
struct ImuAspirin2Spi imu_aspirin2;
void mpu_wait_slave4_ready(void);
void mpu_wait_slave4_ready_cb(struct spi_transaction * t);
bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu);
void imu_impl_init(void)
{
imu_aspirin2.accel_valid = FALSE;
imu_aspirin2.gyro_valid = FALSE;
imu_aspirin2.mag_valid = FALSE;
mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
// change the default configuration
imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
/* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */
imu_aspirin2.mpu.config.nb_bytes = 21;
/* HMC5883 magnetometer as I2C slave */
imu_aspirin2.mpu.config.nb_slaves = 1;
/* set function to configure mag */
imu_aspirin2.mpu.config.slaves[0].configure = &imu_aspirin2_configure_mag_slave;
/* Set MPU I2C master clock */
imu_aspirin2.mpu.config.i2c_mst_clk = MPU60X0_MST_CLK_400KHZ;
/* Enable I2C slave0 delayed sample rate */
imu_aspirin2.mpu.config.i2c_mst_delay = 1;
/* configure spi transaction for wait_slave4 */
imu_aspirin2.wait_slave4_trans.cpol = SPICpolIdleHigh;
imu_aspirin2.wait_slave4_trans.cpha = SPICphaEdge2;
imu_aspirin2.wait_slave4_trans.dss = SPIDss8bit;
imu_aspirin2.wait_slave4_trans.bitorder = SPIMSBFirst;
imu_aspirin2.wait_slave4_trans.cdiv = SPIDiv64;
imu_aspirin2.wait_slave4_trans.select = SPISelectUnselect;
imu_aspirin2.wait_slave4_trans.slave_idx = ASPIRIN_2_SPI_SLAVE_IDX;
imu_aspirin2.wait_slave4_trans.output_length = 1;
imu_aspirin2.wait_slave4_trans.input_length = 2;
imu_aspirin2.wait_slave4_trans.before_cb = NULL;
imu_aspirin2.wait_slave4_trans.after_cb = mpu_wait_slave4_ready_cb;
imu_aspirin2.wait_slave4_trans.input_buf = &(imu_aspirin2.wait_slave4_rx_buf[0]);
imu_aspirin2.wait_slave4_trans.output_buf = &(imu_aspirin2.wait_slave4_tx_buf[0]);
imu_aspirin2.wait_slave4_trans.status = SPITransDone;
imu_aspirin2.slave4_ready = FALSE;
}
void imu_periodic(void)
{
mpu60x0_spi_periodic(&imu_aspirin2.mpu);
}
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
void imu_aspirin2_event(void)
{
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y)
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
imu_aspirin2.accel_valid = TRUE;
imu_aspirin2.mag_valid = TRUE;
}
}
// hack with waiting to avoid creating another event loop to check the mag config status
static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t _reg, uint8_t _val)
{
mpu_set(mpu, _reg, _val);
while(imu_aspirin2.mpu.spi_trans.status != SPITransSuccess);
}
/** function to configure hmc5883 mag
* @return TRUE if mag configuration finished
*/
bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu)
{
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM);
// Put the enable command as last.
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_CTRL,
(1 << 7) | // Slave 0 enable
(6 << 0) ); // Read 6 bytes
return TRUE;
}
void mpu_wait_slave4_ready(void)
{
while (!imu_aspirin2.slave4_ready) {
if (imu_aspirin2.wait_slave4_trans.status == SPITransDone) {
imu_aspirin2.wait_slave4_tx_buf[0] = MPU60X0_REG_I2C_MST_STATUS | MPU60X0_SPI_READ;
spi_submit(imu_aspirin2.mpu.spi_p, &(imu_aspirin2.wait_slave4_trans));
}
}
}
void mpu_wait_slave4_ready_cb(struct spi_transaction * t)
{
if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE))
imu_aspirin2.slave4_ready = TRUE;
else
imu_aspirin2.slave4_ready = FALSE;
t->status = SPITransDone;
}
sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
+86
View File
@@ -0,0 +1,86 @@
/*
* 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_aspirin_2_spi.h
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
*/
#ifndef IMU_ASPIRIN_2_SPI_H
#define IMU_ASPIRIN_2_SPI_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
struct ImuAspirin2Spi {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_Spi mpu;
struct spi_transaction wait_slave4_trans;
volatile uint8_t wait_slave4_tx_buf[1];
volatile uint8_t wait_slave4_rx_buf[2];
volatile bool_t slave4_ready;
};
extern struct ImuAspirin2Spi imu_aspirin2;
extern void imu_aspirin2_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
imu_aspirin2_event();
if (imu_aspirin2.gyro_valid) {
imu_aspirin2.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_aspirin2.accel_valid) {
imu_aspirin2.accel_valid = FALSE;
_accel_handler();
}
if (imu_aspirin2.mag_valid) {
imu_aspirin2.mag_valid = FALSE;
_mag_handler();
}
}
#endif /* IMU_ASPIRIN_2_H */
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
+126
View File
@@ -0,0 +1,126 @@
/*
* 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_drotek_10dof_v2.c
*
* Driver for the Drotek 10DOF V2 IMU.
* MPU6050 + HMC5883 + MS5611
*
* @todo MS5611 baro not read yet
*/
#include "subsystems/imu.h"
#include "mcu_periph/i2c.h"
#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV
#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define DROTEK_2_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz")
#endif
PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV)
PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER)
#ifndef DROTEK_2_GYRO_RANGE
#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE)
#ifndef DROTEK_2_ACCEL_RANGE
#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE)
struct ImuDrotek2 imu_drotek2;
void imu_impl_init(void)
{
/* MPU-60X0 */
mpu60x0_i2c_init(&imu_drotek2.mpu, &(DROTEK_2_I2C_DEV), MPU60X0_ADDR_ALT);
// change the default configuration
imu_drotek2.mpu.config.smplrt_div = DROTEK_2_SMPLRT_DIV;
imu_drotek2.mpu.config.dlpf_cfg = DROTEK_2_LOWPASS_FILTER;
imu_drotek2.mpu.config.gyro_range = DROTEK_2_GYRO_RANGE;
imu_drotek2.mpu.config.accel_range = DROTEK_2_ACCEL_RANGE;
/* HMC5883 magnetometer */
hmc58xx_init(&imu_drotek2.hmc, &(DROTEK_2_I2C_DEV), HMC58XX_ADDR);
/* set callback function to configure mag */
imu_drotek2.mpu.config.slaves[0].configure = &imu_drotek2_configure_mag_slave;
#if DROTEK_2_MAG_SLAVE
/* Set MPU I2C master clock to 400kHz */
imu_drotek2.mpu.config.mst_clk = MPU60X0_MST_CLK_400KHZ;
/* Enable I2C slave0 delayed sample rate */
imu_drotek2.mpu.config.mst_delay = 1;
#else
// use hmc mag via passthrough
imu_drotek2.mpu.config.i2c_bypass = TRUE;
#endif
imu_drotek2.gyro_valid = FALSE;
imu_drotek2.accel_valid = FALSE;
imu_drotek2.mag_valid = FALSE;
}
void imu_periodic(void)
{
// Start reading the latest gyroscope data
mpu60x0_i2c_periodic(&imu_drotek2.mpu);
// Read HMC58XX at ~50Hz (main loop for rotorcraft: 512Hz)
RunOnceEvery(10, hmc58xx_periodic(&imu_drotek2.hmc));
}
void imu_drotek2_event(void)
{
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
memcpy(&imu.gyro_unscaled, &imu_drotek2.mpu.data_rates.rates, sizeof(struct Int32Rates));
memcpy(&imu.accel_unscaled, &imu_drotek2.mpu.data_accel.vect, sizeof(struct Int32Vect3));
imu_drotek2.mpu.data_available = FALSE;
imu_drotek2.gyro_valid = TRUE;
imu_drotek2.accel_valid = TRUE;
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
imu_drotek2.hmc.data_available = FALSE;
imu_drotek2.mag_valid = TRUE;
}
}
/** callback function to configure hmc5883 mag
* @return TRUE if mag configuration finished
*/
bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu)
{
hmc58xx_start_configure(&imu_drotek2.hmc);
if (imu_drotek2.hmc.initialized)
return TRUE;
else
return FALSE;
}
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
+88
View File
@@ -0,0 +1,88 @@
/*
* 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_drotek_10dof_v2.h
*
* Driver for the Drotek 10DOF V2 IMU.
* MPU6050 + HMC5883 + MS5611
*/
#ifndef IMU_DROTEK_10DOF_V2_H
#define IMU_DROTEK_10DOF_V2_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/hmc58xx.h"
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
struct ImuDrotek2 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
};
extern struct ImuDrotek2 imu_drotek2;
extern void imu_drotek2_event(void);
extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
imu_drotek2_event();
if (imu_drotek2.gyro_valid) {
imu_drotek2.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_drotek2.accel_valid) {
imu_drotek2.accel_valid = FALSE;
_accel_handler();
}
if (imu_drotek2.mag_valid) {
imu_drotek2.mag_valid = FALSE;
_mag_handler();
}
}
#endif /* IMU_DROTEK_10DOF_V2_H */
sw/airborne/subsystems/imu/imu_aspirin_2.h → sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
+5 -55
View File
@@ -20,35 +20,14 @@
*/
/**
* @file subsystems/imu/imu_aspirin_2.h
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
* @file subsystems/imu/imu_mpu60x0_defaults.h
* Default sensitivity definitions for an IMU using the MPU60x0.
*/
#ifndef IMU_ASPIRIN_2_H
#define IMU_ASPIRIN_2_H
#ifndef IMU_MPU60X0_DEFAULTS_H
#define IMU_MPU60X0_DEFAULTS_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/mpu60x0_spi.h"
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
@@ -94,33 +73,4 @@
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
struct ImuAspirin2 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_Spi mpu;
};
extern struct ImuAspirin2 imu_aspirin2;
extern void imu_aspirin2_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
imu_aspirin2_event();
if (imu_aspirin2.gyro_valid) {
imu_aspirin2.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_aspirin2.accel_valid) {
imu_aspirin2.accel_valid = FALSE;
_accel_handler();
}
if (imu_aspirin2.mag_valid) {
imu_aspirin2.mag_valid = FALSE;
_mag_handler();
}
}
#endif /* IMU_ASPIRIN_2_H */
#endif /* IMU_MPU60X0_DEFAULTS_H */