Merge remote-tracking branch 'paparazzi/mpu60x0' into stm32f4

conf/airframes/examples/Twinstar_energyadaptive.xml

@@ -33,7 +33,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed
     <subsystem name="radio_control" type="ppm"/>          <!-- Radio -->
     <subsystem name="gps" type="ublox"/>                  <!-- GPS -->
     <subsystem name="telemetry"     type="transparent"/>  <!-- Communication -->
-    <subsystem name="imu"	type="aspirin_i2c_v1.5"/>      <!-- IMU -->
+    <subsystem name="imu"	type="aspirin_i2c_v1.5">      <!-- IMU -->
+      <configure name="ASPIRIN_I2C_DEV" value="i2c0"/>
+    </subsystem>
     <subsystem name="ahrs"      type="int_cmpl_quat"/>
     <subsystem name="ins"	type="alt_float"/>
     <subsystem name="control" type="energyadaptive"/>
@@ -134,9 +136,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
     <define name="MAG_X_SENS" value="1." integer="16"/>
     <define name="MAG_Y_SENS" value="1." integer="16"/>
     <define name="MAG_Z_SENS" value="1." integer="16"/>
-    <define name="MAG_XY_SENS" value="0.0" integer="16"/>
-    <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
-    <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
 
     <define name="BODY_TO_IMU_PHI" value="-2" unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="2" unit="deg"/>

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
+ifndef ASPIRIN_I2C_DEV
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
+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

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
+ASPIRIN_I2C_DEV=i2c0
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
-IMU_ASPIRIN_CFLAGS += -DI2C1_VIC_SLOT=12
 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

sw/airborne/peripherals/mpu60X0.h

@@ -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 */

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

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;
+    }
+  }
+}

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

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 */

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;
+    }
+  }
+}

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