diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml
index 79385b5eb3..7c916edaa7 100644
--- a/conf/airframes/examples/Twinstar_energyadaptive.xml
+++ b/conf/airframes/examples/Twinstar_energyadaptive.xml
@@ -33,7 +33,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed
-
+
+
+
@@ -134,9 +136,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
-
-
-
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
index 7fa9186de7..a19bb4a2b8 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
@@ -49,15 +49,24 @@ IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_SLAVE_IDX=SPI_SLAVE0
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_DEV=spi1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
+ifndef ASPIRIN_I2C_DEV
+ASPIRIN_I2C_DEV=i2c0
+endif
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (ADXL345 CS)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
+ifndef ASPIRIN_I2C_DEV
+ASPIRIN_I2C_DEV=i2c2
endif
+endif
+
+# convert i2cx to upper case
+ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
+
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
+IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
#
# NPS simulator
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
index 16f6a05105..064ceb3d03 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
@@ -38,12 +38,19 @@ IMU_ASPIRIN_SRCS += peripherals/itg3200.c
# Magnetometer
IMU_ASPIRIN_SRCS += peripherals/hmc58xx.c
+# set default i2c bus
+ifndef ASPIRIN_I2C_DEV
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
-IMU_ASPIRIN_CFLAGS += -DI2C1_VIC_SLOT=12
+ASPIRIN_I2C_DEV=i2c0
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
+ASPIRIN_I2C_DEV=i2c2
+endif
endif
+# convert i2cx to upper case
+ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
+
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
+IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
+
include $(CFG_SHARED)/imu_nps.makefile
diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
deleted file mode 100644
index 384b92a726..0000000000
--- a/sw/airborne/peripherals/mpu60X0.h
+++ /dev/null
@@ -1,97 +0,0 @@
-#ifndef MPU60X0
-#define MPU60X0
-
-/* default I2C address */
-#define MPU60X0_ADDR 0xD0
-#define MPU60X0_ADDR_ALT 0xD2
-
-#define MPU60X0_SPI_READ 0x80
-
-// Power and Interface
-#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
-#define MPU60X0_REG_USER_CTRL 0x6A
-#define MPU60X0_REG_PWR_MGMT_1 0x6B
-#define MPU60X0_REG_PWR_MGMT_2 0x6C
-
-// FIFO
-#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
-
-// I2C Slave settings
-#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
-#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
-#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
-// Slave 1
-#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
-#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
-#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
-#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
-// Slave 2
-#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
-#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
-#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
-#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
-// Slave 3
-#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
-#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
-#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
-#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
-// Slave 4 - special
-#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
-#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
-#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
-#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
-#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
-
-// 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
-
-// Temperature
-#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
-
-// External Sensor Data
-#define MPU60X0_EXT_SENS_DATA 0X49
-#define MPU60X0_EXT_SENS_DATA_SIZE 24
-
-
-/////////////////////////////////////////////////
-// MPU60X0 Definitions
-
-#define MPU60X0_REG_WHO_AM_I 0X75
-#define MPU60X0_WHOAMI_REPLY 0x68
-
-
-
-#endif /* MPU60X0 */
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
new file mode 100644
index 0000000000..2647aee7f0
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0.h
+ *
+ * MPU-60X0 driver common interface (I2C and SPI).
+ */
+
+#ifndef MPU60X0_H
+#define MPU60X0_H
+
+#include "std.h"
+
+/* Include address and register definition */
+#include "peripherals/mpu60x0_regs.h"
+
+/// Default sample rate divider
+#define MPU60X0_DEFAULT_SMPLRT_DIV 0
+/// Default gyro full scale range +- 2000°/s
+#define MPU60X0_DEFAULT_FS_SEL MPU60X0_GYRO_RANGE_2000
+/// Default accel full scale range +- 16g
+#define MPU60X0_DEFAULT_AFS_SEL MPU60X0_ACCEL_RANGE_16G
+/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
+#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_42HZ
+/// Default interrupt config: RAW_RDY_EN
+#define MPU60X0_DEFAULT_INT_CFG 1
+/// Default clock: PLL with X gyro reference
+#define MPU60X0_DEFAULT_CLK_SEL 1
+
+enum Mpu60x0ConfStatus {
+ MPU60X0_CONF_UNINIT,
+ MPU60X0_CONF_SD,
+ MPU60X0_CONF_CONFIG,
+ MPU60X0_CONF_GYRO,
+ MPU60X0_CONF_ACCEL,
+ MPU60X0_CONF_INT_PIN,
+ MPU60X0_CONF_INT_ENABLE,
+ MPU60X0_CONF_PWR,
+ MPU60X0_CONF_DONE
+};
+
+struct Mpu60x0Config {
+ uint8_t smplrt_div; ///< Sample rate divider
+ enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
+ enum Adxl345GyroRanges gyro_range; ///< deg/s Range
+ enum Adxl345AccelRanges 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
+ enum Mpu60x0ConfStatus init_status; ///< init status
+ bool_t initialized; ///< config done flag
+};
+
+static inline void mpu60X0_set_default_config(struct Mpu60x0Config *c)
+{
+ 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;
+}
+
+/// Configuration function prototype
+typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
+
+/// Configuration sequence called once before normal use
+static inline void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
+{
+ switch (config->init_status) {
+ case MPU60X0_CONF_SD:
+ mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_CONFIG:
+ mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_GYRO:
+ mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_ACCEL:
+ mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_INT_PIN:
+ mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_INT_ENABLE:
+ mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_PWR:
+ mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->config.clk_sel)|(0<<6));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_DONE:
+ config->initialized = TRUE;
+ break;
+ default:
+ break;
+ }
+}
+
+
+#endif // MPU60X0_H
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
new file mode 100644
index 0000000000..a7f587fe99
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_i2c.c
+ *
+ * Driver for the MPU-60X0 using I2C.
+ *
+ */
+
+#include "peripherals/mpu60x0_i2c.h"
+
+void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
+{
+ /* set i2c_peripheral */
+ mpu->i2c_p = i2c_p;
+
+ /* slave address */
+ mpu->i2c_trans.slave_addr = addr;
+ /* set inital status: Success or Done */
+ mpu->i2c_trans.status = I2CTransDone;
+
+ /* set default MPU60X0 config options */
+ mpu60x0_set_default_config(&(mpu->config));
+
+ mpu->data_available = FALSE;
+ mpu->config.initialized = FALSE;
+ mpu->config.init_status = MPU60X0_CONF_UNINIT;
+}
+
+
+static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
+ struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
+ mpu_i2c->i2c_trans.buf[0] = _reg;
+ mpu_i2c->i2c_trans.buf[1] = _val;
+ i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu->i2c_trans.slave_addr, 2);
+}
+
+// Configuration function called once before normal use
+void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
+{
+ if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
+ mpu->config.init_status++;
+ if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) {
+ mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, mpu->config);
+ }
+ }
+}
+
+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);
+ }
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+
+void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
+{
+ if (mpu->config.initialized) {
+ if (mpu->i2c_trans.status == I2CTransFailed) {
+ mpu->i2c_trans.status = I2CTransDone;
+ }
+ else if (mpu->i2c_trans.status == I2CTransSuccess) {
+ // Successfull reading
+ 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_available = TRUE;
+ }
+ mpu->i2c_trans.status = I2CTransDone;
+ }
+ }
+ else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
+ switch (mpu->i2c_trans.status) {
+ case I2CTransFailed:
+ 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;
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h
new file mode 100644
index 0000000000..f7d95eff61
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_i2c.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_i2c.h
+ *
+ * Driver for the MPU-60X0 using I2C.
+ */
+
+#ifndef MPU60X0_I2C_H
+#define MPU60X0_I2C_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "mcu_periph/i2c.h"
+
+/* Include common MPU60X0 options and definitions */
+#include "peripherals/mpu60x0.h"
+
+
+struct Mpu60x0_I2c {
+ struct i2c_periph *i2c_p;
+ struct i2c_transaction i2c_trans;
+ volatile bool_t data_available; ///< data ready flag
+ union {
+ struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
+ int16_t value[3]; ///< accel data values accessible by channel index
+ } data_accel;
+ union {
+ 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;
+ struct Mpu60x0Config config;
+};
+
+// Functions
+extern void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr);
+extern void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu);
+extern void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu);
+extern void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) {
+ if (mpu->config.initialized)
+ mpu60x0_i2c_read(mpu);
+ else
+ mpu60x0_i2c_start_configure(mpu);
+}
+
+#endif // MPU60X0_I2C_H
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
new file mode 100644
index 0000000000..e1d460490c
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2010-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_regs.h
+ *
+ * Register and address definitions for MPU-6000 and MPU-6050.
+ */
+
+#ifndef MPU60X0_REGS_H
+#define MPU60X0_REGS_H
+
+/* default I2C address */
+#define MPU60X0_ADDR 0xD0
+#define MPU60X0_ADDR_ALT 0xD2
+
+#define MPU60X0_SPI_READ 0x80
+
+// Power and Interface
+#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
+#define MPU60X0_REG_USER_CTRL 0x6A
+#define MPU60X0_REG_PWR_MGMT_1 0x6B
+#define MPU60X0_REG_PWR_MGMT_2 0x6C
+
+// FIFO
+#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
+
+// I2C Slave settings
+#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
+#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
+#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
+// Slave 1
+#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
+#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
+#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
+#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
+// Slave 2
+#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
+#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
+#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
+#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
+// Slave 3
+#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
+#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
+#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
+#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
+// Slave 4 - special
+#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
+#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
+#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
+#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
+#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
+
+// 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
+
+// Temperature
+#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
+
+// External Sensor Data
+#define MPU60X0_EXT_SENS_DATA 0X49
+#define MPU60X0_EXT_SENS_DATA_SIZE 24
+
+
+/////////////////////////////////////////////////
+// MPU60X0 Definitions
+
+#define MPU60X0_REG_WHO_AM_I 0X75
+#define MPU60X0_WHOAMI_REPLY 0x68
+
+/** Digital Low Pass Filter Options
+ * DLFP is affecting both gyro and accels,
+ * with slightly different bandwidth
+ */
+enum Mpu60x0DLPF {
+ MPU60X0_DLPF_256HZ = 0x0, // internal sampling rate 8kHz
+ MPU60X0_DLPF_188HZ = 0x1, // internal sampling rate 1kHz
+ MPU60X0_DLPF_98HZ = 0x2,
+ MPU60X0_DLPF_42HZ = 0x3,
+ MPU60X0_DLPF_20HZ = 0x4,
+ MPU60X0_DLPF_10HZ = 0x5,
+ MPU60X0_DLPF_05HZ = 0x6
+};
+
+/**
+ * Selectable gyro range
+ */
+enum Mpu60x0GyroRanges {
+ MPU60X0_GYRO_RANGE_250 = 0x00,
+ MPU60X0_GYRO_RANGE_500 = 0x01,
+ MPU60X0_GYRO_RANGE_1000 = 0x02,
+ MPU60X0_GYRO_RANGE_2000 = 0x03
+};
+
+/**
+ * Selectable accel range
+ */
+enum Mpu60x0AccelRanges {
+ MPU60X0_ACCEL_RANGE_2G = 0x00,
+ MPU60X0_ACCEL_RANGE_4G = 0x01,
+ MPU60X0_ACCEL_RANGE_8G = 0x02,
+ MPU60X0_ACCEL_RANGE_16G = 0x03
+};
+
+#endif /* MPU60X0_REGS_H */
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
new file mode 100644
index 0000000000..2522d3ddf3
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_spi.c
+ *
+ * Driver for the MPU-60X0 using SPI.
+ *
+ */
+
+#include "peripherals/mpu60x0_spi.h"
+
+static void trans_cb( struct spi_transaction *trans );
+
+void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
+{
+ /* set spi_peripheral */
+ mpu->spi_p = spi_p;
+
+ /* configure spi transaction */
+ mpu->spi_trans.cpol = SPICpolIdleHigh;
+ mpu->spi_trans.cpha = SPICphaEdge2;
+ mpu->spi_trans.dss = SPIDss8bit;
+ mpu->spi_trans.bitorder = SPIMSBFirst;
+ mpu->spi_trans.cdiv = SPIDiv64;
+
+ mpu->spi_trans.select = SPISelectUnselect;
+ mpu->spi_trans.slave_idx = slave_idx;
+ mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
+ mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
+ mpu->spi_trans.before_cb = NULL;
+ mpu->spi_trans.after_cb = trans_cb;
+ mpu->spi_trans.input_buf = &(mpu->rx_buf[0]);
+ mpu->spi_trans.output_buf = &(mpu->tx_buf[0]);
+
+ /* set inital status: Success or Done */
+ mpu->spi_trans.status = SPITransDone;
+
+ /* set default MPU60X0 config options */
+ mpu60x0_set_default_config(&(mpu->config));
+
+ mpu->data_available = FALSE;
+ mpu->config.initialized = FALSE;
+ mpu->config.init_status = MPU60X0_CONF_UNINIT;
+}
+
+
+static void mpu60x0_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
+ struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu);
+ mpu_spi->spi_trans.output_length = 2;
+ mpu_spi->spi_trans.input_length = 0;
+ mpu_spi->tx_buf[0] = _reg;
+ mpu_spi->tx_buf[1] = _val;
+ spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans));
+}
+
+// Configuration function called once before normal use
+void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
+{
+ if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
+ mpu->config.init_status++;
+ if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
+ mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config);
+ }
+ }
+}
+
+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 = 15; // FIXME external data
+ /* set read bit and multiple byte bit, then address */
+ mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+ spi_submit(mpu->spi_p, &(mpu->spi_trans));
+ }
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+
+void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
+{
+ if (mpu->config.initialized) {
+ if (mpu->spi_trans.status == SPITransFailed) {
+ mpu->spi_trans.status = SPITransDone;
+ }
+ else if (mpu->spi_trans.status == SPITransSuccess) {
+ // Successfull reading
+ if (bit_is_set(mpu->rx_buf[0],0)) {
+ // new data
+ mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,1);
+ mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,3);
+ mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,5);
+ mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,9);
+ mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,11);
+ mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,13);
+ mpu->data_available = TRUE;
+ }
+ mpu->spi_trans.status = SPITransDone;
+ }
+ }
+ else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
+ switch (mpu->spi_trans.status) {
+ case SPITransFailed:
+ 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;
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h
new file mode 100644
index 0000000000..6eac22267c
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_spi.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_spi.h
+ *
+ * Driver for the MPU-60X0 using SPI.
+ */
+
+#ifndef MPU60X0_SPI_H
+#define MPU60X0_SPI_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "mcu_periph/spi.h"
+
+/* Include common MPU60X0 options and definitions */
+#include "peripherals/mpu60x0.h"
+
+
+#define MPU60X0_BUFFER_LEN 32
+
+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 bool_t data_available; ///< data ready flag
+ union {
+ struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
+ int16_t value[3]; ///< accel data values accessible by channel index
+ } data_accel;
+ union {
+ 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;
+ struct Mpu60x0Config config;
+};
+
+// Functions
+extern void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t addr);
+extern void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu);
+extern void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu);
+extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) {
+ if (mpu->config.initialized)
+ mpu60x0_spi_read(mpu);
+ else
+ mpu60x0_spi_start_configure(mpu);
+}
+
+#endif // MPU60X0_SPI_H
diff --git a/sw/ext/luftboot b/sw/ext/luftboot
index 3f40726c72..75c3107284 160000
--- a/sw/ext/luftboot
+++ b/sw/ext/luftboot
@@ -1 +1 @@
-Subproject commit 3f40726c7299a26fb9467466cd07459a373ee09b
+Subproject commit 75c3107284e6f02b27b21c2ae269824df58bb1ec