diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index 1f4331d59a..e9e9aa9dba 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -32,7 +32,7 @@
-
+
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
index 446b84af59..1e16703269 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
@@ -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)
#
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
similarity index 69%
rename from conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
rename to conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
index 3ad6e9a953..446b84af59 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
@@ -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)
#
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
index 8dfb93613a..99968ac5a2 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
@@ -2,11 +2,13 @@
#
# Aspirin IMU v2.2
#
+# actually identical with v2.1 since baro is not read in IMU driver
+#
#
# required xml:
#
#
-#
+#
#
#
#
@@ -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
diff --git a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
new file mode 100644
index 0000000000..b41ec9b741
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
@@ -0,0 +1,81 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Drotek 10DOF V2 IMU via I2C
+#
+#
+# required xml:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+
+# 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
diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
index 34598ac5c9..ea926b0125 100644
--- a/sw/airborne/peripherals/mpu60x0.c
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -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 */
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index 60334d8bd2..00df171767 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -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
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
index ada9ee4b55..4d25af6585 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.c
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -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;
+}
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h
index f7d95eff61..581ad60b45 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.h
+++ b/sw/airborne/peripherals/mpu60x0_i2c.h
@@ -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
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
index 211d1729f0..ac48968ab6 100644
--- a/sw/airborne/peripherals/mpu60x0_regs.h
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -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 */
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
index 97993f322e..0689e972ad 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.c
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -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;
+}
diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h
index 6eac22267c..75975f851b 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.h
+++ b/sw/airborne/peripherals/mpu60x0_spi.h
@@ -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
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.c b/sw/airborne/subsystems/imu/imu_aspirin_2.c
deleted file mode 100644
index 33a444456e..0000000000
--- a/sw/airborne/subsystems/imu/imu_aspirin_2.c
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * Copyright (C) 2013 Felix Ruess
- *
- * 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
-}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
new file mode 100644
index 0000000000..84918649e4
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
@@ -0,0 +1,232 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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;
+}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
new file mode 100644
index 0000000000..9ab98631ca
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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 */
diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
new file mode 100644
index 0000000000..76c3d84f77
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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;
+}
diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
new file mode 100644
index 0000000000..0cb89ca681
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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 */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.h b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
similarity index 62%
rename from sw/airborne/subsystems/imu/imu_aspirin_2.h
rename to sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
index 33b91686e4..b53faef232 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin_2.h
+++ b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
@@ -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 */