From 2b1a17cb335d2e000199219775f7db662a3e8031 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 29 Nov 2011 17:10:11 +0100
Subject: [PATCH 01/12] MPU60X0

---
 .../fixedwing/imu_aspirin2_i2c.makefile       |  23 ++
 sw/airborne/modules/sensors/imu_aspirin2.c    | 243 ++++++++++++++++++
 sw/airborne/modules/sensors/imu_aspirin2.h    |  57 ++++
 sw/airborne/peripherals/mpu60X0.h             |  65 +++++
 4 files changed, 388 insertions(+)
 create mode 100644 conf/autopilot/subsystems/fixedwing/imu_aspirin2_i2c.makefile
 create mode 100644 sw/airborne/modules/sensors/imu_aspirin2.c
 create mode 100644 sw/airborne/modules/sensors/imu_aspirin2.h
 create mode 100644 sw/airborne/peripherals/mpu60X0.h

diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin2_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin2_i2c.makefile
new file mode 100644
index 0000000000..2a8f4d6923
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin2_i2c.makefile
@@ -0,0 +1,23 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+IMU_ASPIRIN2_CFLAGS  = -DUSE_IMU
+IMU_ASPIRIN2_CFLAGS += -DIMU_TYPE_H=\"modules/sensors/imu_aspirin2.h\"
+
+IMU_ASPIRIN2_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+                      $(SRC_MODULES)/sensors/imu_aspirin2.c
+
+
+IMU_ASPIRIN2_CFLAGS += -DUSE_I2C
+ifeq ($(ARCH), stm32)
+	IMU_ASPIRIN2_CFLAGS += -DUSE_I2C2
+	IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2
+else ifeq ($(ARCH), lpc21)
+	IMU_ASPIRIN2_CFLAGS += -DUSE_I2C0
+	IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0
+endif
+
+ap.CFLAGS += $(IMU_ASPIRIN2_CFLAGS)
+ap.srcs   += $(IMU_ASPIRIN2_SRCS)
+
+ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
+
diff --git a/sw/airborne/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c
new file mode 100644
index 0000000000..2240aa9a6c
--- /dev/null
+++ b/sw/airborne/modules/sensors/imu_aspirin2.c
@@ -0,0 +1,243 @@
+/*
+ * 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.
+ *
+ */
+
+#include <math.h>
+#include "imu_ppzuav.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
+
+// Set SPI_CS High
+#include "mcu_periph/gpio_arch.h"
+
+// Downlink
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+// Peripherials
+#include "../../peripherals/mpu60X0.h"
+// #include "../../peripherals/hmc5843.h"
+
+// Results
+volatile bool_t mag_valid;
+volatile bool_t gyr_valid;
+volatile bool_t acc_valid;
+
+// Communication
+struct i2c_transaction aspirin2_mpu60x0;
+//struct i2c_transaction ppzuavimu_adxl345;
+//struct i2c_transaction ppzuavimu_hmc5843;
+
+// Standalone option: run module only
+#ifndef IMU_TYPE_H
+struct Imu imu;
+#endif
+
+#ifndef PERIODIC_FREQUENCY
+#define PERIODIC_FREQUENCY 60
+#endif
+
+void imu_impl_init(void)
+{
+  GPIO_ARCH_SET_SPI_CS_HIGH();
+
+  /////////////////////////////////////////////////////////////////////
+  // MPU60X0
+  aspirin2_mpu60x0.type = I2CTransTx;
+  aspirin2_mpu60x0.slave_addr = MPU60X0_ADDR;
+/*
+  aspirin2_mpu60x0.buf[0] = ITG3200_REG_DLPF_FS;
+#if PERIODIC_FREQUENCY == 60
+  // set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz 
+  aspirin2_mpu60x0.buf[1] = (0x03<<3) | (0x04<<0);
+#  warning Info: ITG3200 read at 50Hz
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  warning Info: ITG3200 read at 100Hz
+  // set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz
+  aspirin2_mpu60x0.buf[1] = (0x03<<3) | (0x03<<0);
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
+  aspirin2_mpu60x0.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little
+  aspirin2_mpu60x0.buf[0] = ITG3200_REG_SMPLRT_DIV;
+#if PERIODIC_FREQUENCY == 60
+  aspirin2_mpu60x0.buf[1] = 19;  // 50Hz
+#else
+  aspirin2_mpu60x0.buf[1] = 9;  // 100Hz
+#endif
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // switch to gyroX clock 
+  aspirin2_mpu60x0.buf[0] = ITG3200_REG_PWR_MGM;
+  aspirin2_mpu60x0.buf[1] = 0x01;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // no interrupts for now, but set data ready interrupt to enable reading status bits 
+  aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
+  aspirin2_mpu60x0.buf[1] = 0x01;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+*/
+
+/*
+  /////////////////////////////////////////////////////////////////////
+  // HMC5843
+  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA;  // set to rate to max speed: 50Hz no bias
+  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+  ppzuavimu_hmc5843.buf[1] = 0x00;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+*/
+}
+
+void imu_periodic( void )
+{
+  // Start reading the latest gyroscope data
+  aspirin2_mpu60x0.type = I2CTransTxRx;
+  aspirin2_mpu60x0.len_r = 1;
+  aspirin2_mpu60x0.len_w = 1;
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_WHO_AM_I;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &aspirin2_mpu60x0);
+
+/*
+  // Start reading the latest accelerometer data
+  ppzuavimu_adxl345.type = I2CTransTxRx;
+  ppzuavimu_adxl345.len_r = 6;
+  ppzuavimu_adxl345.len_w = 1;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
+*/
+  // Start reading the latest magnetometer data
+#if PERIODIC_FREQUENCY > 60
+  RunOnceEvery(2,{
+#endif
+/*  ppzuavimu_hmc5843.type = I2CTransTxRx;
+  ppzuavimu_hmc5843.len_r = 6;
+  ppzuavimu_hmc5843.len_w = 1;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+*/
+#if PERIODIC_FREQUENCY > 60
+  });
+#endif
+  //RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
+}
+
+void aspirin2_subsystem_downlink_raw( void )
+{
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+/*  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
+*/
+}
+
+void aspirin2_subsystem_event( void )
+{
+  int32_t x, y, z;
+
+  // If the itg3200 I2C transaction has succeeded: convert the data
+  if (aspirin2_mpu60x0.status == I2CTransSuccess)
+  {
+#define ITG_STA_DAT_OFFSET 3
+    x = (int16_t) ((aspirin2_mpu60x0.buf[0+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[1+ITG_STA_DAT_OFFSET]);
+    y = (int16_t) ((aspirin2_mpu60x0.buf[2+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[3+ITG_STA_DAT_OFFSET]);
+    z = (int16_t) ((aspirin2_mpu60x0.buf[4+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[5+ITG_STA_DAT_OFFSET]);
+
+    // Is this is new data
+    if (aspirin2_mpu60x0.buf[0] & 0x01)
+    {
+      //LED_ON(3);
+      gyr_valid = TRUE;
+      //LED_OFF(3);
+    }
+    else
+    {
+    }
+
+    RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
+
+    aspirin2_mpu60x0.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
+  }
+/*
+  // If the adxl345 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_adxl345.status == I2CTransSuccess)
+  {
+    x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
+    y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
+    z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
+#endif
+
+    acc_valid = TRUE;
+    ppzuavimu_adxl345.status = I2CTransDone;
+  }
+
+  // If the hmc5843 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
+  {
+    x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
+    y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
+    z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
+#endif
+
+    mag_valid = TRUE;
+    ppzuavimu_hmc5843.status = I2CTransDone;
+  }
+*/
+}
+
diff --git a/sw/airborne/modules/sensors/imu_aspirin2.h b/sw/airborne/modules/sensors/imu_aspirin2.h
new file mode 100644
index 0000000000..f60c7082b2
--- /dev/null
+++ b/sw/airborne/modules/sensors/imu_aspirin2.h
@@ -0,0 +1,57 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef PPZUAVIMU_H
+#define PPZUAVIMU_H
+
+#include "std.h"
+#include "subsystems/imu.h"
+
+extern volatile bool_t gyr_valid;
+extern volatile bool_t acc_valid;
+extern volatile bool_t mag_valid;
+
+/* must be defined in order to be IMU code: declared in imu.h
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+*/
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
+    aspirin2_subsystem_event();                \
+    if (gyr_valid) {                         \
+      gyr_valid = FALSE;                     \
+      _gyro_handler();                                  \
+    }                                                   \
+    if (acc_valid) {                         \
+      acc_valid = FALSE;                     \
+      _accel_handler();                                 \
+    }                                                   \
+    if (mag_valid) {                         \
+      mag_valid = FALSE;                     \
+      _mag_handler();                                 \
+    }                                                   \
+}
+
+/* Own Extra Functions */
+extern void aspirin2_subsystem_event( void );
+extern void aspirin2_subsystem_downlink_raw( void );
+
+
+#endif // PPZUAVIMU_H
diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
new file mode 100644
index 0000000000..2054015549
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60X0.h
@@ -0,0 +1,65 @@
+#ifndef MPU60X0
+#define MPU60X0
+
+/* default I2C address */
+#define MPU60X0_ADDR            0xD0
+#define MPU60X0_ADDR_ALT        0xD2
+
+/* Registers */
+
+#define MPU60X0_AUX_VDDIO		0x01
+// Must be set to 0 on MPU6000
+
+
+#define MPU60X0_REG_WHO_AM_I    	0X75
+#define MPU60X0_REG_SMPLRT_DIV  	0X19
+#define MPU60X0_REG_CONFIG      	0X1A
+#define MPU60X0_REG_GYRO_CONFIG 	0X1B
+#define MPU60X0_REG_ACC_CONFIG  	0X1C
+#define MPU60X0_REG_FIFO_EN     	0X23
+
+#define MPU60X0_REG_I2C_MSTR    	0X24
+#define MPU60X0_REG_I2C_MSTR_STATUS    	0X24
+// Slave 0
+#define MPU60X0_REG_I2C_SLV0_1  0X25	// i2c addr
+#define MPU60X0_REG_I2C_SLV0_2  0X26	// slave reg
+#define MPU60X0_REG_I2C_SLV0_3  0X27	// set-bits
+// Slave 1
+#define MPU60X0_REG_I2C_SLV1_1  0X28	// i2c addr
+#define MPU60X0_REG_I2C_SLV1_2  0X29	// slave reg
+#define MPU60X0_REG_I2C_SLV1_3  0X2A	// set-bits
+// Slave 2
+#define MPU60X0_REG_I2C_SLV2_1  0X2B	// i2c addr
+#define MPU60X0_REG_I2C_SLV2_2  0X2C	// slave reg
+#define MPU60X0_REG_I2C_SLV2_3  0X2D	// set-bits
+// Slave 3
+#define MPU60X0_REG_I2C_SLV3_1  0X2E	// i2c addr
+#define MPU60X0_REG_I2C_SLV3_2  0X2F	// slave reg
+#define MPU60X0_REG_I2C_SLV3_3  0X30	// set-bits
+// Slave 4 - special
+#define MPU60X0_REG_I2C_SLV4_1  0X31	// i2c addr
+#define MPU60X0_REG_I2C_SLV4_2  0X32	// slave reg
+#define MPU60X0_REG_I2C_SLV4_3  0X33	// DO
+#define MPU60X0_REG_I2C_SLV4_4  0X34	// set-bits
+#define MPU60X0_REG_I2C_SLV4_5  0X35	// DI
+
+//TODO
+#define MPU60X0_REG_TEMP_OUT_L  0X1C
+#define MPU60X0_REG_GYRO_XOUT_H 0X1D
+#define MPU60X0_REG_GYRO_XOUT_L 0X1E
+#define MPU60X0_REG_GYRO_YOUT_H 0X1F
+#define MPU60X0_REG_GYRO_YOUT_L 0X20
+#define MPU60X0_REG_GYRO_ZOUT_H 0X21
+#define MPU60X0_REG_GYRO_ZOUT_L 0X22
+#define MPU60X0_REG_PWR_MGM     0X3E
+
+
+
+/////////////////////////////////////////////////
+// MPU60X0 Definitions
+
+#define MPU60X0_WHOAMI_REPLY	0x68
+
+
+
+#endif /* MPU60X0 */

From fefca293e489ef16667cf91dd41848cb3eb99838 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 29 Nov 2011 17:56:50 +0100
Subject: [PATCH 02/12] MPU60X0 update

---
 sw/airborne/peripherals/mpu60X0.h | 93 ++++++++++++++++++++-----------
 1 file changed, 61 insertions(+), 32 deletions(-)

diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
index 2054015549..cdbedc427f 100644
--- a/sw/airborne/peripherals/mpu60X0.h
+++ b/sw/airborne/peripherals/mpu60X0.h
@@ -5,54 +5,83 @@
 #define MPU60X0_ADDR            0xD0
 #define MPU60X0_ADDR_ALT        0xD2
 
-/* Registers */
+// 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
 
-#define MPU60X0_AUX_VDDIO		0x01
-// Must be set to 0 on MPU6000
+// FIFO
+#define MPU60X0_REG_FIFO_COUNT_H	0x72
+#define MPU60X0_REG_FIFO_COUNT_L	0x73
+#define MPU60X0_REG_FIFO_R_W		0x74
 
-
-#define MPU60X0_REG_WHO_AM_I    	0X75
+// Measurement Settings
 #define MPU60X0_REG_SMPLRT_DIV  	0X19
 #define MPU60X0_REG_CONFIG      	0X1A
 #define MPU60X0_REG_GYRO_CONFIG 	0X1B
 #define MPU60X0_REG_ACC_CONFIG  	0X1C
 #define MPU60X0_REG_FIFO_EN     	0X23
 
+// I2C Slave settings
 #define MPU60X0_REG_I2C_MSTR    	0X24
-#define MPU60X0_REG_I2C_MSTR_STATUS    	0X24
+#define MPU60X0_REG_I2C_MSTR_STATUS    	0X36
+#define MPU60X0_REG_I2C_MSTR_DELAY    	0X67
 // Slave 0
-#define MPU60X0_REG_I2C_SLV0_1  0X25	// i2c addr
-#define MPU60X0_REG_I2C_SLV0_2  0X26	// slave reg
-#define MPU60X0_REG_I2C_SLV0_3  0X27	// set-bits
+#define MPU60X0_REG_I2C_SLV0_ADDR	0X25	// i2c addr
+#define MPU60X0_REG_I2C_SLV0_REG	0X26	// slave reg
+#define MPU60X0_REG_I2C_SLV0_SET	0X27	// set-bits
+#define MPU60X0_REG_I2C_SLV0_DO		0X63	// DO
 // Slave 1
-#define MPU60X0_REG_I2C_SLV1_1  0X28	// i2c addr
-#define MPU60X0_REG_I2C_SLV1_2  0X29	// slave reg
-#define MPU60X0_REG_I2C_SLV1_3  0X2A	// set-bits
+#define MPU60X0_REG_I2C_SLV1_ADDR	0X28	// i2c addr
+#define MPU60X0_REG_I2C_SLV1_REG	0X29	// slave reg
+#define MPU60X0_REG_I2C_SLV1_SET	0X2A	// set-bits
+#define MPU60X0_REG_I2C_SLV1_DO		0X64	// DO
 // Slave 2
-#define MPU60X0_REG_I2C_SLV2_1  0X2B	// i2c addr
-#define MPU60X0_REG_I2C_SLV2_2  0X2C	// slave reg
-#define MPU60X0_REG_I2C_SLV2_3  0X2D	// set-bits
+#define MPU60X0_REG_I2C_SLV2_ADDR	0X2B	// i2c addr
+#define MPU60X0_REG_I2C_SLV2_REG	0X2C	// slave reg
+#define MPU60X0_REG_I2C_SLV2_SET	0X2D	// set-bits
+#define MPU60X0_REG_I2C_SLV2_DO		0X65	// DO
 // Slave 3
-#define MPU60X0_REG_I2C_SLV3_1  0X2E	// i2c addr
-#define MPU60X0_REG_I2C_SLV3_2  0X2F	// slave reg
-#define MPU60X0_REG_I2C_SLV3_3  0X30	// set-bits
+#define MPU60X0_REG_I2C_SLV3_ADDR	0X2E	// i2c addr
+#define MPU60X0_REG_I2C_SLV3_REG	0X2F	// slave reg
+#define MPU60X0_REG_I2C_SLV3_SET	0X30	// set-bits
+#define MPU60X0_REG_I2C_SLV3_DO		0X66	// DO
 // Slave 4 - special
-#define MPU60X0_REG_I2C_SLV4_1  0X31	// i2c addr
-#define MPU60X0_REG_I2C_SLV4_2  0X32	// slave reg
-#define MPU60X0_REG_I2C_SLV4_3  0X33	// DO
-#define MPU60X0_REG_I2C_SLV4_4  0X34	// set-bits
-#define MPU60X0_REG_I2C_SLV4_5  0X35	// DI
+#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_SET	0X34	// set-bits
+#define MPU60X0_REG_I2C_SLV4_DI		0X35	// DI
 
-//TODO
-#define MPU60X0_REG_TEMP_OUT_L  0X1C
-#define MPU60X0_REG_GYRO_XOUT_H 0X1D
-#define MPU60X0_REG_GYRO_XOUT_L 0X1E
-#define MPU60X0_REG_GYRO_YOUT_H 0X1F
-#define MPU60X0_REG_GYRO_YOUT_L 0X20
-#define MPU60X0_REG_GYRO_ZOUT_H 0X21
-#define MPU60X0_REG_GYRO_ZOUT_L 0X22
-#define MPU60X0_REG_PWR_MGM     0X3E
+// Interrupt
+#define MPU60X0_REG_INT_PIN     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
 
 
 /////////////////////////////////////////////////

From b00b1b1c0fe26684e7323f86ab6c6a81a48ab357 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 30 Nov 2011 14:35:36 +0100
Subject: [PATCH 03/12] aspirin v2 works :-)

---
 conf/airframes/CDW/yapa3_aspirin2.xml      | 274 +++++++++++++++++++++
 sw/airborne/modules/sensors/imu_aspirin2.c | 125 ++++++----
 sw/airborne/peripherals/mpu60X0.h          |  61 ++---
 3 files changed, 382 insertions(+), 78 deletions(-)
 create mode 100644 conf/airframes/CDW/yapa3_aspirin2.xml

diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml
new file mode 100644
index 0000000000..e9b223d270
--- /dev/null
+++ b/conf/airframes/CDW/yapa3_aspirin2.xml
@@ -0,0 +1,274 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="9" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="8" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="7" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="6" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="5" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <command_laws>
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
+    <set servo="THROTTLE" value="@THROTTLE"/>
+    <set servo="ELEVATOR" value="@PITCH"/>
+  </command_laws>
+
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <!-- Calibration Neutral -->
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
+
+    <!-- SENS = 16.4 LSB/(deg/sec) * 57.6 deg/rad = 939.650 LSB/rad/sec / 12bit FRAC: 4096 / 939.65 -->
+    <define name="GYRO_P_SENS" value="4.359" integer="16"/>
+    <define name="GYRO_Q_SENS" value="4.359" integer="16"/>
+    <define name="GYRO_R_SENS" value="4.359" integer="16"/>
+
+    <define name="GYRO_P_Q" value="0."/>
+    <define name="GYRO_P_R" value="0"/>
+    <define name="GYRO_Q_P" value="0."/>
+    <define name="GYRO_Q_R" value="0."/>
+    <define name="GYRO_R_P" value="0."/>
+    <define name="GYRO_R_Q" value="0."/>
+
+    <define name="GYRO_P_SIGN" value="1"/>
+    <define name="GYRO_Q_SIGN" value="1"/>
+    <define name="GYRO_R_SIGN" value="1"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="-40"/>
+    <define name="ACCEL_Y_NEUTRAL" value="-15"/>
+    <define name="ACCEL_Z_NEUTRAL" value="375"/>
+
+    <!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
+    <define name="ACCEL_X_SENS" value="9.81" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="9.81" integer="16"/>
+
+<!--
+    <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
+-->
+    <define name="ACCEL_X_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Z_SIGN" value="1"/>
+
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+
+    <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_X_SIGN" value="1"/>
+    <define name="MAG_Y_SIGN" value="1"/>
+    <define name="MAG_Z_SIGN" value="1"/>
+
+    <define name="BODY_TO_IMU_PHI" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0"/>
+    <define name="BODY_TO_IMU_PSI" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+<!--
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
+    </load>
+    -->
+    <!--    <load name="digital_cam_i2c.xml"/>  -->
+
+ <!--    <load name="ins_ppzuavimu.xml" /> -->
+
+<!--
+    <load name="digital_cam.xml" >
+      <define name="DC_SHUTTER_LED" value="3"/>
+    </load>
+    -->
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="tiny_2.11"> 
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+
+      <configure name="PERIODIC_FREQUENCY" value="120"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
+
+      <configure name="AHRS_ALIGNER_LED" value="3"/>
+      <configure name="CPU_LED" value="3"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <define name="AGR_CLIMB"/>
+    <define name="LOITER_TRIM"/>
+    <define name="ALT_KALMAN"/>
+
+    <!-- Sensors -->
+    <!--
+    <subsystem name="ahrs" type="int_cmpl_quat">
+      <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
+    </subsystem>
+    <subsystem name="imu" type="aspirin_i2c"/>
+    -->
+    <subsystem name="imu" type="aspirin2_i2c"/>
+    <subsystem name="ahrs" type="float_dcm">
+<!--      <define name="USE_MAGNETOMETER" /> -->
+    </subsystem>
+
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B9600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="ublox_utm"/>
+
+
+</firmware>
+
+
+</airframe>
diff --git a/sw/airborne/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c
index 2240aa9a6c..e8f7ec42bb 100644
--- a/sw/airborne/modules/sensors/imu_aspirin2.c
+++ b/sw/airborne/modules/sensors/imu_aspirin2.c
@@ -19,13 +19,10 @@
  */
 
 #include <math.h>
-#include "imu_ppzuav.h"
+#include "imu_aspirin2.h"
 #include "mcu_periph/i2c.h"
 #include "led.h"
 
-// Set SPI_CS High
-#include "mcu_periph/gpio_arch.h"
-
 // Downlink
 #include "mcu_periph/uart.h"
 #include "messages.h"
@@ -47,8 +44,6 @@ volatile bool_t acc_valid;
 
 // Communication
 struct i2c_transaction aspirin2_mpu60x0;
-//struct i2c_transaction ppzuavimu_adxl345;
-//struct i2c_transaction ppzuavimu_hmc5843;
 
 // Standalone option: run module only
 #ifndef IMU_TYPE_H
@@ -61,47 +56,76 @@ struct Imu imu;
 
 void imu_impl_init(void)
 {
-  GPIO_ARCH_SET_SPI_CS_HIGH();
-
   /////////////////////////////////////////////////////////////////////
   // MPU60X0
   aspirin2_mpu60x0.type = I2CTransTx;
   aspirin2_mpu60x0.slave_addr = MPU60X0_ADDR;
-/*
-  aspirin2_mpu60x0.buf[0] = ITG3200_REG_DLPF_FS;
-#if PERIODIC_FREQUENCY == 60
-  // set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz 
-  aspirin2_mpu60x0.buf[1] = (0x03<<3) | (0x04<<0);
-#  warning Info: ITG3200 read at 50Hz
-#else
-#  if PERIODIC_FREQUENCY == 120
-#  warning Info: ITG3200 read at 100Hz
-  // set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz
-  aspirin2_mpu60x0.buf[1] = (0x03<<3) | (0x03<<0);
-#  else
-#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
-#  endif
-#endif
+  aspirin2_mpu60x0.len_r = 0;
   aspirin2_mpu60x0.len_w = 2;
-  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
-    while(aspirin2_mpu60x0.status == I2CTransPending);
 
-  // set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little
-  aspirin2_mpu60x0.buf[0] = ITG3200_REG_SMPLRT_DIV;
-#if PERIODIC_FREQUENCY == 60
-  aspirin2_mpu60x0.buf[1] = 19;  // 50Hz
-#else
-  aspirin2_mpu60x0.buf[1] = 9;  // 100Hz
-#endif
-  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
-    while(aspirin2_mpu60x0.status == I2CTransPending);
 
-  // switch to gyroX clock 
-  aspirin2_mpu60x0.buf[0] = ITG3200_REG_PWR_MGM;
+  ///////////////////
+  // Configure power:
+
+  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
+
+  // MPU60X0_REG_USER_CTRL: 
+  // -Enable Aux I2C Master Mode
+  // -Enable SPI
+
+  // MPU60X0_REG_PWR_MGMT_1
+  // -switch to gyroX clock 
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_PWR_MGMT_1;
   aspirin2_mpu60x0.buf[1] = 0x01;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
     while(aspirin2_mpu60x0.status == I2CTransPending);
 
+  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
+
+  /////////////////////////
+  // Measurement Settings
+
+  // MPU60X0_REG_CONFIG
+  // -ext sync on gyro X (bit 3->6)
+  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
+#if PERIODIC_FREQUENCY == 60
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_CONFIG;
+  aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // MPU60X0_REG_SMPLRT_DIV
+  // -100Hz output = 1kHz / (9 + 1)
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_SMPLRT_DIV;
+  aspirin2_mpu60x0.buf[1] = 9;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // MPU60X0_REG_GYRO_CONFIG
+  // -2000deg/sec
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_GYRO_CONFIG;
+  aspirin2_mpu60x0.buf[1] = (3<<3);
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+  // MPU60X0_REG_ACCEL_CONFIG
+  // 16g, no HPFL
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_ACCEL_CONFIG;
+  aspirin2_mpu60x0.buf[1] = (3<<3);
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status == I2CTransPending);
+
+
+
+  
+
+/*
   // no interrupts for now, but set data ready interrupt to enable reading status bits 
   aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
   aspirin2_mpu60x0.buf[1] = 0x01;
@@ -140,9 +164,9 @@ void imu_periodic( void )
 {
   // Start reading the latest gyroscope data
   aspirin2_mpu60x0.type = I2CTransTxRx;
-  aspirin2_mpu60x0.len_r = 1;
+  aspirin2_mpu60x0.len_r = 21;
   aspirin2_mpu60x0.len_w = 1;
-  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_WHO_AM_I;
+  aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &aspirin2_mpu60x0);
 
 /*
@@ -172,9 +196,8 @@ void imu_periodic( void )
 void aspirin2_subsystem_downlink_raw( void )
 {
   DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
-/*  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
   DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
-*/
 }
 
 void aspirin2_subsystem_event( void )
@@ -184,24 +207,30 @@ void aspirin2_subsystem_event( void )
   // If the itg3200 I2C transaction has succeeded: convert the data
   if (aspirin2_mpu60x0.status == I2CTransSuccess)
   {
-#define ITG_STA_DAT_OFFSET 3
-    x = (int16_t) ((aspirin2_mpu60x0.buf[0+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[1+ITG_STA_DAT_OFFSET]);
-    y = (int16_t) ((aspirin2_mpu60x0.buf[2+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[3+ITG_STA_DAT_OFFSET]);
-    z = (int16_t) ((aspirin2_mpu60x0.buf[4+ITG_STA_DAT_OFFSET] << 8) | aspirin2_mpu60x0.buf[5+ITG_STA_DAT_OFFSET]);
+#define MPU_OFFSET_GYRO 9
+    x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_GYRO]);
+    y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_GYRO]);
+    z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_GYRO]);
+
+    RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
+
+#define MPU_OFFSET_ACC 1
+    x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_ACC]);
+    y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_ACC]);
+    z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_ACC]);
+
+    VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
 
     // Is this is new data
     if (aspirin2_mpu60x0.buf[0] & 0x01)
     {
-      //LED_ON(3);
       gyr_valid = TRUE;
-      //LED_OFF(3);
+      acc_valid = TRUE;
     }
     else
     {
     }
 
-    RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
-
     aspirin2_mpu60x0.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
   }
 /*
diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
index cdbedc427f..2f186875b4 100644
--- a/sw/airborne/peripherals/mpu60X0.h
+++ b/sw/airborne/peripherals/mpu60X0.h
@@ -12,6 +12,7 @@
 #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
@@ -20,74 +21,74 @@
 #define MPU60X0_REG_SMPLRT_DIV  	0X19
 #define MPU60X0_REG_CONFIG      	0X1A
 #define MPU60X0_REG_GYRO_CONFIG 	0X1B
-#define MPU60X0_REG_ACC_CONFIG  	0X1C
-#define MPU60X0_REG_FIFO_EN     	0X23
+#define MPU60X0_REG_ACCEL_CONFIG  	0X1C
 
 // I2C Slave settings
-#define MPU60X0_REG_I2C_MSTR    	0X24
-#define MPU60X0_REG_I2C_MSTR_STATUS    	0X36
-#define MPU60X0_REG_I2C_MSTR_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
-#define MPU60X0_REG_I2C_SLV0_SET	0X27	// set-bits
+#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_SET	0X2A	// set-bits
+#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_SET	0X2D	// set-bits
+#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_SET	0X30	// set-bits
+#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_SET	0X34	// set-bits
+#define MPU60X0_REG_I2C_SLV4_CTRL	0X34	// set-bits
 #define MPU60X0_REG_I2C_SLV4_DI		0X35	// DI
 
 // Interrupt
-#define MPU60X0_REG_INT_PIN     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_SIZE 24
+#define MPU60X0_EXT_SENS_DATA    	0X49
+#define MPU60X0_EXT_SENS_DATA_SIZE 	24
 
 
 /////////////////////////////////////////////////
 // MPU60X0 Definitions
 
-#define MPU60X0_WHOAMI_REPLY	0x68
+#define MPU60X0_REG_WHO_AM_I    	0X75
+#define MPU60X0_WHOAMI_REPLY		0x68
 
 
 

From 8f9a39199d971564d4a942d3ccb5385368f7470b Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 9 Jan 2012 16:05:45 +0100
Subject: [PATCH 04/12] Empty structure for SPI / EINT / Aspirin2

---
 conf/airframes/CDW/LisaAspirin2.xml           | 273 ++++++++++++++++++
 conf/airframes/CDW/yapa3_aspirin2.xml         |   2 +-
 .../shared/imu_aspirin_v2.0.makefile          |  63 ++++
 sw/airborne/arch/stm32/mcu_periph/spi_arch.c  | 182 ++++++++++++
 sw/airborne/arch/stm32/mcu_periph/spi_arch.h  | 162 +++++++++++
 .../stm32/subsystems/imu/imu_aspirin2_arch.c  | 100 +++++++
 .../stm32/subsystems/imu/imu_aspirin2_arch.h  |  20 ++
 sw/airborne/modules/sensors/imu_aspirin2.c    |   8 +-
 sw/airborne/subsystems/imu/imu_aspirin2.c     |  94 ++++++
 sw/airborne/subsystems/imu/imu_aspirin2.h     | 145 ++++++++++
 10 files changed, 1044 insertions(+), 5 deletions(-)
 create mode 100644 conf/airframes/CDW/LisaAspirin2.xml
 create mode 100644 conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/spi_arch.c
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/spi_arch.h
 create mode 100644 sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
 create mode 100644 sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h
 create mode 100644 sw/airborne/subsystems/imu/imu_aspirin2.c
 create mode 100644 sw/airborne/subsystems/imu/imu_aspirin2.h

diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
new file mode 100644
index 0000000000..f754ff8e00
--- /dev/null
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -0,0 +1,273 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	Lisa + Aspirin v2 using SPI only
+-->
+
+<airframe name="LisaAspirin2">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <command_laws>
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
+    <set servo="THROTTLE" value="@THROTTLE"/>
+    <set servo="ELEVATOR" value="@PITCH"/>
+  </command_laws>
+
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <!-- Calibration Neutral -->
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
+
+    <!-- SENS = 16.4 LSB/(deg/sec) * 57.6 deg/rad = 939.650 LSB/rad/sec / 12bit FRAC: 4096 / 939.65 -->
+    <define name="GYRO_P_SENS" value="4.359" integer="16"/>
+    <define name="GYRO_Q_SENS" value="4.359" integer="16"/>
+    <define name="GYRO_R_SENS" value="4.359" integer="16"/>
+
+    <define name="GYRO_P_Q" value="0."/>
+    <define name="GYRO_P_R" value="0"/>
+    <define name="GYRO_Q_P" value="0."/>
+    <define name="GYRO_Q_R" value="0."/>
+    <define name="GYRO_R_P" value="0."/>
+    <define name="GYRO_R_Q" value="0."/>
+
+    <define name="GYRO_P_SIGN" value="1"/>
+    <define name="GYRO_Q_SIGN" value="1"/>
+    <define name="GYRO_R_SIGN" value="1"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="-40"/>
+    <define name="ACCEL_Y_NEUTRAL" value="-15"/>
+    <define name="ACCEL_Z_NEUTRAL" value="375"/>
+
+    <!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
+    <define name="ACCEL_X_SENS" value="9.81" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="9.81" integer="16"/>
+
+<!--
+    <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
+-->
+    <define name="ACCEL_X_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Z_SIGN" value="1"/>
+
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+
+    <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_X_SIGN" value="1"/>
+    <define name="MAG_Y_SIGN" value="1"/>
+    <define name="MAG_Z_SIGN" value="1"/>
+
+    <define name="BODY_TO_IMU_PHI" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0"/>
+    <define name="BODY_TO_IMU_PSI" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+<!--
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
+    </load>
+    -->
+    <!--    <load name="digital_cam_i2c.xml"/>  -->
+
+ <!--    <load name="ins_ppzuavimu.xml" /> -->
+
+<!--
+    <load name="digital_cam.xml" >
+      <define name="DC_SHUTTER_LED" value="3"/>
+    </load>
+    -->
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="lisa_m_1.0">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+
+      <configure name="PERIODIC_FREQUENCY" value="120"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
+
+      <configure name="AHRS_ALIGNER_LED" value="1"/>
+      <configure name="CPU_LED" value="1"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <define name="AGR_CLIMB"/>
+    <define name="LOITER_TRIM"/>
+    <define name="ALT_KALMAN"/>
+
+    <!-- Sensors -->
+    <!--
+    <subsystem name="ahrs" type="int_cmpl_quat">
+      <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
+    </subsystem>
+    -->
+    <subsystem name="imu" type="aspirin_v2.0"/>
+    <subsystem name="ahrs" type="float_dcm">
+<!--      <define name="USE_MAGNETOMETER" /> -->
+    </subsystem>
+
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B9600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="ublox_utm"/>
+
+
+</firmware>
+
+
+</airframe>
diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml
index e9b223d270..7696df9c4a 100644
--- a/conf/airframes/CDW/yapa3_aspirin2.xml
+++ b/conf/airframes/CDW/yapa3_aspirin2.xml
@@ -222,7 +222,7 @@
 
   <firmware name="fixedwing">
 
-    <target name="ap" board="tiny_2.11"> 
+    <target name="ap" board="tiny_2.11">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile b/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
new file mode 100644
index 0000000000..f96478e6a2
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
@@ -0,0 +1,63 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Aspirin IMU v2.0
+#
+#
+# required xml:
+#  <section name="IMU" prefix="IMU_">
+#
+#    <define name="GYRO_X_NEUTRAL" value="33924"/>
+#    <define name="GYRO_Y_NEUTRAL" value="33417"/>
+#    <define name="GYRO_Z_NEUTRAL" value="32809"/>
+#
+#    <define name="GYRO_X_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="32081"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
+#
+#    <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
+#
+#    <define name="MAG_X_NEUTRAL" value="2358"/>
+#    <define name="MAG_Y_NEUTRAL" value="2362"/>
+#    <define name="MAG_Z_NEUTRAL" value="2119"/>
+#
+#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
+#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
+#
+#  </section>
+#
+#
+
+# imu aspirin
+
+IMU_ASPIRIN_CFLAGS  = -DUSE_IMU
+IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -DIMU_OVERRIDE_CHANNELS
+IMU_ASPIRIN_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+                      $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c \
+                      $(SRC_ARCH)/subsystems/imu/imu_aspirin2_arch.c \
+                      $(SRC_ARCH)/mcu_periph/spi_arch.c \
+                      mcu_periph/spi.c
+
+IMU_ASPIRIN_CFLAGS += -DUSE_SPI
+
+ifeq ($(ARCH), lpc21)
+#TODO
+else ifeq ($(ARCH), stm32)
+# IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
+# IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
+endif
+
+IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_0
+
+# 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)
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
new file mode 100644
index 0000000000..30358483a6
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -0,0 +1,182 @@
+#include "subsystems/imu.h"
+
+#include <stm32/gpio.h>
+#include <stm32/misc.h>
+#include <stm32/rcc.h>
+#include <stm32/exti.h>
+#include <stm32/spi.h>
+#include <stm32/dma.h>
+
+#include "mcu_periph/spi.h"
+
+/*
+// accelerometer SPI selection 
+#define Adxl345Unselect() GPIOB->BSRR = GPIO_Pin_12
+#define Adxl345Select()   GPIOB->BRR = GPIO_Pin_12
+// accelerometer dma end of rx handler 
+void dma1_c4_irq_handler(void);
+
+void spi_arch_int_enable(void) {
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  // Enable DMA1 channel4 IRQ Channel ( SPI RX) 
+  NVIC_InitTypeDef NVIC_init_struct = {
+    .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+    .NVIC_IRQChannelPreemptionPriority = 0,
+    .NVIC_IRQChannelSubPriority = 0,
+    .NVIC_IRQChannelCmd = ENABLE
+  };
+  NVIC_Init(&NVIC_init_struct);
+
+}
+
+void spi_int_disable(void) {
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  // Enable DMA1 channel4 IRQ Channel ( SPI RX)
+  NVIC_InitTypeDef NVIC_init_struct = {
+    .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+    .NVIC_IRQChannelPreemptionPriority = 0,
+    .NVIC_IRQChannelSubPriority = 0,
+    .NVIC_IRQChannelCmd = DISABLE
+  };
+  NVIC_Init(&NVIC_init_struct);
+
+}
+*/
+
+void spi_init(void) {
+/*
+  GPIO_InitTypeDef GPIO_InitStructure;
+  EXTI_InitTypeDef EXTI_InitStructure;
+  SPI_InitTypeDef SPI_InitStructure;
+
+  // Enable SPI2 Periph clock -------------------------------------------------
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
+
+  // Configure GPIOs: SCK, MISO and MOSI  --------------------------------
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
+  SPI_Cmd(SPI2, ENABLE);
+
+  // configure SPI 
+  SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+  SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+  SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+  SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+  SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+  SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
+  SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+  SPI_InitStructure.SPI_CRCPolynomial = 7;
+  SPI_Init(SPI2, &SPI_InitStructure);
+
+
+  // Enable SPI_2 DMA clock ---------------------------------------------------
+  RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+
+*/
+}
+
+/*
+
+void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
+
+  Adxl345Select();
+  SPI_I2S_SendData(SPI2, addr);
+  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+  SPI_I2S_SendData(SPI2, val);
+  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
+  Adxl345Unselect();
+
+}
+
+void adxl345_clear_rx_buf(void) {
+  uint8_t __attribute__ ((unused)) ret = SPI_I2S_ReceiveData(SPI2);
+}
+
+void adxl345_start_reading_data(void) {
+   Adxl345Select();
+
+   imu_aspirin.accel_tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0);
+
+  // SPI2_Rx_DMA_Channel configuration ------------------------------------
+  DMA_DeInit(DMA1_Channel4);
+  DMA_InitTypeDef DMA_initStructure_4 = {
+    .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+    .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_rx_buf,
+    .DMA_DIR = DMA_DIR_PeripheralSRC,
+    .DMA_BufferSize = 7,
+    .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+    .DMA_MemoryInc = DMA_MemoryInc_Enable,
+    .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+    .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+    .DMA_Mode = DMA_Mode_Normal,
+    .DMA_Priority = DMA_Priority_VeryHigh,
+    .DMA_M2M = DMA_M2M_Disable
+  };
+  DMA_Init(DMA1_Channel4, &DMA_initStructure_4);
+
+  // SPI2_Tx_DMA_Channel configuration ------------------------------------
+  DMA_DeInit(DMA1_Channel5);
+  DMA_InitTypeDef DMA_initStructure_5 = {
+    .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+    .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_tx_buf,
+    .DMA_DIR = DMA_DIR_PeripheralDST,
+    .DMA_BufferSize = 7,
+    .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+    .DMA_MemoryInc = DMA_MemoryInc_Enable,
+    .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+    .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+    .DMA_Mode = DMA_Mode_Normal,
+    .DMA_Priority = DMA_Priority_Medium,
+    .DMA_M2M = DMA_M2M_Disable
+  };
+  DMA_Init(DMA1_Channel5, &DMA_initStructure_5);
+
+  // Enable SPI_2 Rx request 
+  SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE);
+  // Enable DMA1 Channel4 
+  DMA_Cmd(DMA1_Channel4, ENABLE);
+
+  // Enable SPI_2 Tx request 
+  SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE);
+  // Enable DMA1 Channel5 
+  DMA_Cmd(DMA1_Channel5, ENABLE);
+
+  // Enable DMA1 Channel4 Transfer Complete interrupt
+  DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
+}
+
+
+
+// Accel end of DMA transfert
+void dma1_c4_irq_handler(void) {
+  Adxl345Unselect();
+  if (DMA_GetITStatus(DMA1_IT_TC4)) {
+		// clear int pending bit
+		DMA_ClearITPendingBit(DMA1_IT_GL4);
+
+    // mark as available
+    imu_aspirin.accel_available = TRUE;
+	}
+
+  // disable DMA Channel
+  DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
+  // Disable SPI_2 Rx and TX request 
+  SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
+  SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE);
+  // Disable DMA1 Channel4 and 5 
+  DMA_Cmd(DMA1_Channel4, DISABLE);
+  DMA_Cmd(DMA1_Channel5, DISABLE);
+
+}
+
+*/
+
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
new file mode 100644
index 0000000000..a020355a82
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
@@ -0,0 +1,162 @@
+/*  $Id$
+ *
+ * Copyright (C) 2003-2005  Pascal Brisset, Antoine Drouin
+ *
+ * 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.
+ *
+ */
+
+/** \brief handling of stm32 SPI hardware
+ */
+
+#ifndef SPI_ARCH_H
+#define SPI_ARCH_H
+
+
+#include "mcu_periph/spi.h"
+#include <stm32/gpio.h>
+
+/*
+
+////////// 
+// from aspirin_arch.h
+
+extern void imu_aspirin_arch_int_enable(void);
+extern void imu_aspirin_arch_int_disable(void);
+
+extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
+extern void adxl345_clear_rx_buf(void);
+extern void adxl345_start_reading_data(void);
+
+*/
+
+//////////
+// from lpc spi_arch
+
+/*
+
+
+
+#define SpiTransmit() {                             \
+    while (spi_tx_idx < spi_buffer_length                       \
+       && bit_is_set(SSPSR, TNF)) {					\
+      SpiSend(spi_buffer_output[spi_tx_idx]);                           \
+      spi_tx_idx++;                             \
+    }                                                           \
+    if (spi_tx_idx == spi_buffer_length)                    \
+      SpiDisableTxi();                                                  \
+}
+
+#define SpiReceive() {                              \
+    while (bit_is_set(SSPSR, RNE)) {					\
+      if (spi_rx_idx < spi_buffer_length) {                             \
+          SpiRead(spi_buffer_input[spi_rx_idx])                         \
+          spi_rx_idx++;                             \
+      }                                                                 \
+      else {                                                            \
+         uint8_t foo;                                                   \
+     SpiRead(foo);                                                  \
+      }                                                                 \
+    }									\
+  }
+
+
+
+
+#ifdef SPI_MASTER
+
+
+// !!!!!!!!!!!!! Code for one single slave at a time !!!!!!!!!!!!!!!!! 
+#if defined SPI_SELECT_SLAVE1_PIN && defined SPI_SELECT_SLAVE0_PIN
+#error "SPI: one single slave, please"
+#endif
+
+
+#define SpiStart() {                                                    \
+   SpiEnable();                                                         \
+   SpiInitBuf();                                                        \
+   SpiEnableTxi();     // enable tx fifo half empty interrupt           \
+}
+
+*/
+
+/*
+ * Slave0 select : P0.20  PINSEL1 00 << 8
+ * Slave1 select : P1.20
+ *
+ */
+
+/*
+
+#define SPI_SELECT_SLAVE_IO__(port, reg) IO ## port ## reg
+#define SPI_SELECT_SLAVE_IO_(port, reg) SPI_SELECT_SLAVE_IO__(port, reg)
+
+#define SPI_SELECT_SLAVE0_IODIR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, DIR)
+#define SPI_SELECT_SLAVE0_IOCLR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, CLR)
+#define SPI_SELECT_SLAVE0_IOSET SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, SET)
+
+#define SPI_SELECT_SLAVE1_IODIR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, DIR)
+#define SPI_SELECT_SLAVE1_IOCLR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, CLR)
+#define SPI_SELECT_SLAVE1_IOSET SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, SET)
+
+
+#define SpiSelectSlave0() {	\
+    spi_cur_slave = SPI_SLAVE0;	\
+    SetBit(SPI_SELECT_SLAVE0_IOCLR, SPI_SELECT_SLAVE0_PIN);	\
+  }
+
+#define SpiUnselectSlave0() { \
+    spi_cur_slave = SPI_NONE;	\
+    SetBit(SPI_SELECT_SLAVE0_IOSET, SPI_SELECT_SLAVE0_PIN);	\
+  }
+
+
+#define SpiSelectSlave1() {	\
+    spi_cur_slave = SPI_SLAVE1;	\
+    SetBit(SPI_SELECT_SLAVE1_IOCLR, SPI_SELECT_SLAVE1_PIN);	\
+  }
+
+#define SpiUnselectSlave1() { \
+    spi_cur_slave = SPI_NONE;	\
+    SetBit(SPI_SELECT_SLAVE1_IOSET, SPI_SELECT_SLAVE1_PIN);	\
+  }
+
+#ifdef SPI_SELECT_SLAVE0_PIN
+#define SpiUnselectCurrentSlave() SpiUnselectSlave0()
+#endif
+
+#ifdef SPI_SELECT_SLAVE1_PIN
+#define SpiUnselectCurrentSlave() SpiUnselectSlave1()
+#endif
+
+#endif // SPI_MASTER
+
+
+#define SpiSetCPOL() (SSPCR0 |= _BV(6))
+#define SpiClrCPOL() (SSPCR0 &= ~(_BV(6)))
+
+#define SpiSetCPHA() (SSPCR0 |= _BV(7))
+#define SpiClrCPHA() (SSPCR0 &= ~(_BV(7)))
+
+*/
+
+#endif // SPI_ARCH_H 
+
+
+
+
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
new file mode 100644
index 0000000000..3120676324
--- /dev/null
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
@@ -0,0 +1,100 @@
+#include "subsystems/imu.h"
+
+#include <stm32/gpio.h>
+#include <stm32/misc.h>
+#include <stm32/rcc.h>
+#include <stm32/exti.h>
+#include <stm32/spi.h>
+#include <stm32/dma.h>
+
+
+
+
+// gyro int handler
+void exti15_10_irq_handler(void);
+
+void imu_aspirin2_arch_int_enable(void) {
+/*
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+*/
+}
+
+void imu_aspirin2_arch_int_disable(void) {
+/*
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
+  NVIC_Init(&NVIC_InitStructure);
+*/
+}
+
+void imu_aspirin2_arch_init(void) {
+/*
+  GPIO_InitTypeDef GPIO_InitStructure;
+  EXTI_InitTypeDef EXTI_InitStructure;
+  SPI_InitTypeDef SPI_InitStructure;
+
+  // Set "mag ss" and "mag reset" as floating inputs ------------------------
+  // "mag ss"    (PC12) is shorted to I2C2 SDA      
+  // "mag reset" (PC13) is shorted to I2C2 SCL       
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOC, &GPIO_InitStructure);
+
+  // Gyro --------------------------------------------------------------------
+  // set "eeprom ss" as floating input (on PC14) = gyro int          ---------
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOC, &GPIO_InitStructure);
+  // configure external interrupt exti15_10 on PC14( gyro int )
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOC, &GPIO_InitStructure);
+
+  // ifdef ASPIRIN_USE_GYRO_INT
+  GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
+  EXTI_InitStructure.EXTI_Line = EXTI_Line14;
+  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+  EXTI_Init(&EXTI_InitStructure);
+
+  // Accel 
+  // set accel slave select as output and assert it ( on PB12) 
+  Adxl345Unselect();
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+*/
+}
+
+
+
+// Gyro data ready
+void exti15_10_irq_handler(void) {
+  // clear EXTI 
+  if(EXTI_GetITStatus(EXTI_Line14) != RESET)
+    EXTI_ClearITPendingBit(EXTI_Line14);
+
+/*
+  imu_aspirin.gyro_eoc = TRUE;
+  imu_aspirin.status = AspirinStatusReadingGyro;
+*/
+}
+
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h
new file mode 100644
index 0000000000..b4456c4aa2
--- /dev/null
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h
@@ -0,0 +1,20 @@
+#ifndef IMU_ASPIRIN2_ARCH_H
+#define IMU_ASPIRIN2_ARCH_H
+
+#include "subsystems/imu.h"
+#include <stm32/gpio.h>
+
+
+
+extern void imu_aspirin2_arch_int_enable(void);
+extern void imu_aspirin2_arch_int_disable(void);
+
+/*
+static inline int imu_aspirin2_eoc(void)
+{
+  return !GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_14);
+}
+*/
+
+
+#endif
diff --git a/sw/airborne/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c
index e8f7ec42bb..3290d7e0f9 100644
--- a/sw/airborne/modules/sensors/imu_aspirin2.c
+++ b/sw/airborne/modules/sensors/imu_aspirin2.c
@@ -69,12 +69,12 @@ void imu_impl_init(void)
 
   // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
 
-  // MPU60X0_REG_USER_CTRL: 
+  // MPU60X0_REG_USER_CTRL:
   // -Enable Aux I2C Master Mode
   // -Enable SPI
 
   // MPU60X0_REG_PWR_MGMT_1
-  // -switch to gyroX clock 
+  // -switch to gyroX clock
   aspirin2_mpu60x0.buf[0] = MPU60X0_REG_PWR_MGMT_1;
   aspirin2_mpu60x0.buf[1] = 0x01;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
@@ -123,10 +123,10 @@ void imu_impl_init(void)
 
 
 
-  
+
 
 /*
-  // no interrupts for now, but set data ready interrupt to enable reading status bits 
+  // no interrupts for now, but set data ready interrupt to enable reading status bits
   aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
   aspirin2_mpu60x0.buf[1] = 0x01;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
new file mode 100644
index 0000000000..0067265f6c
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -0,0 +1,94 @@
+#include "subsystems/imu.h"
+
+#include "mcu_periph/spi.h"
+#include "mcu_periph/spi_arch.h"
+
+struct ImuAspirin2 imu_aspirin2;
+
+/*
+
+// initialize peripherals
+static void configure_gyro(void);
+static void configure_accel(void);
+//static void configure_mag(void);
+
+*/
+
+void imu_impl_init(void) {
+
+  imu_aspirin2.status = Aspirin2StatusUninit;
+  imu_aspirin2.imu_available = FALSE;
+
+  imu_aspirin2_arch_init();
+}
+
+
+void imu_periodic(void) {
+/*
+  hmc5843_periodic();
+  if (imu_aspirin.status == AspirinStatusUninit) {
+    configure_gyro();
+    configure_accel();
+    imu_aspirin_arch_int_enable();
+    imu_aspirin.status = AspirinStatusIdle;
+  } else {
+    imu_aspirin.gyro_available_blaaa = TRUE;
+    imu_aspirin.time_since_last_reading++;
+    imu_aspirin.time_since_last_accel_reading++;
+    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
+      configure_accel();
+      imu_aspirin.time_since_last_accel_reading=0;
+    }
+  }
+*/
+}
+
+
+/*
+// sends a serie of I2C commands to configure the ITG3200 gyro 
+static void configure_gyro(void) {
+  struct i2c_transaction t;
+  t.type = I2CTransTx;
+  t.slave_addr = ITG3200_ADDR;
+  // set gyro range to 2000deg/s and low pass at 256Hz
+  t.buf[0] = ITG3200_REG_DLPF_FS;
+  t.buf[1] = (0x03<<3);
+  t.len_w = 2;
+  send_i2c_msg_with_retry(&t);
+  // set sample rate to 533Hz 
+  t.buf[0] = ITG3200_REG_SMPLRT_DIV;
+  t.buf[1] = 0x0E;
+  send_i2c_msg_with_retry(&t);
+  // switch to gyroX clock 
+  t.buf[0] = ITG3200_REG_PWR_MGM;
+  t.buf[1] = 0x01;
+  send_i2c_msg_with_retry(&t);
+  // enable interrupt on data ready, idle high, latch until read any register
+  t.buf[0] = ITG3200_REG_INT_CFG;
+  t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
+  send_i2c_msg_with_retry(&t);
+
+}
+
+
+
+static void configure_accel(void) {
+
+  // set data rate to 800Hz
+  adxl345_write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
+  // switch to measurememnt mode 
+  adxl345_write_to_reg(ADXL345_REG_POWER_CTL, 1<<3);
+  // enable data ready interrupt 
+  adxl345_write_to_reg(ADXL345_REG_INT_ENABLE, 1<<7);
+  // Enable full res and interrupt active low 
+  adxl345_write_to_reg(ADXL345_REG_DATA_FORMAT, 1<<3|1<<5);
+  // clear spi rx reg to make DMA happy 
+  adxl345_clear_rx_buf();
+  // reads data once to bring interrupt line up 
+  adxl345_start_reading_data();
+
+}
+
+*/
+
+
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
new file mode 100644
index 0000000000..c1d054c791
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -0,0 +1,145 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef IMU_ASPIRIN_H
+#define IMU_ASPIRIN_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "mcu_periph/i2c.h"
+#include "peripherals/itg3200.h"
+#include "peripherals/hmc5843.h"
+#include "peripherals/adxl345.h"
+
+
+#ifdef IMU_ASPIRIN_VERSION_2_0
+#define IMU_MAG_X_CHAN 2
+#define IMU_MAG_Y_CHAN 0
+#define IMU_MAG_Z_CHAN 1
+#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
+#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
+
+enum Aspirin2Status
+  { Aspirin2StatusUninit,
+    Aspirin2StatusIdle,
+    Aspirin2StatusReading
+  };
+
+struct ImuAspirin2 {
+  volatile enum Aspirin2Status status;
+  volatile uint8_t imu_available;
+  volatile uint8_t imu_tx_buf[64];
+  volatile uint8_t imu_rx_buf[64];
+  uint32_t time_since_last_reading;
+};
+
+extern struct ImuAspirin2 imu_aspirin2;
+#define ASPIRIN2_TIMEOUT 3
+/*
+
+#define foo_handler() {}
+#define ImuMagEvent(_mag_handler) {					\
+      MagEvent(foo_handler); \
+}
+
+
+    if (hmc5843.data_available) {			\
+      imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN];		\
+      imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN];		\
+      imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN];		\
+      _mag_handler();							\
+      hmc5843.data_available = FALSE;		\
+    }									\
+*/
+
+/* underlying architecture */
+#include "subsystems/imu/imu_aspirin2_arch.h"
+/* must be implemented by underlying architecture */
+extern void imu_aspirin2_arch_init(void);
+
+/*
+static inline void gyro_copy_i2c(void)
+{
+  int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1];
+  int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3];
+  int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5];
+  RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
+}
+
+static inline void accel_copy_spi(void)
+{
+  const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
+  const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
+  const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
+  VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
+}
+
+
+static inline void imu_gyro_event(void (* _gyro_handler)(void))
+{
+
+}
+*/
+
+static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
+{
+  if (imu_aspirin2.status == Aspirin2StatusUninit) return;
+
+  imu_aspirin2_arch_int_disable();
+  if (imu_aspirin2.imu_available) {
+    imu_aspirin2.time_since_last_reading = 0;
+    imu_aspirin2.imu_available = FALSE;
+    //accel_copy_spi();
+    _accel_handler();
+  }
+  imu_aspirin2_arch_int_enable();
+
+  // Reset everything if we've been waiting too long
+  if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
+    imu_aspirin2.time_since_last_reading = 0;
+    return;
+  }
+
+}
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do {		\
+  imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
+} while(0);
+
+#endif /* IMU_ASPIRIN_H */

From 40d974bbac087236244b2a6922ad9f55089eca1f Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 9 Jan 2012 16:35:32 +0100
Subject: [PATCH 05/12] Basic Single Slave generic STM SPI Driver

---
 sw/airborne/arch/stm32/mcu_periph/spi_arch.c | 50 ++++++++------------
 sw/airborne/arch/stm32/mcu_periph/spi_arch.h | 11 +++--
 2 files changed, 28 insertions(+), 33 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index 30358483a6..4469e94ac4 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -9,16 +9,14 @@
 
 #include "mcu_periph/spi.h"
 
-/*
-// accelerometer SPI selection 
-#define Adxl345Unselect() GPIOB->BSRR = GPIO_Pin_12
-#define Adxl345Select()   GPIOB->BRR = GPIO_Pin_12
-// accelerometer dma end of rx handler 
+// SPI2 Slave Selection 
+#define Spi2Slave0Unselect() GPIOB->BSRR = GPIO_Pin_12
+#define Spi2Slave0Select()   GPIOB->BRR = GPIO_Pin_12
+
+// spi dma end of rx handler 
 void dma1_c4_irq_handler(void);
 
 void spi_arch_int_enable(void) {
-  NVIC_InitTypeDef NVIC_InitStructure;
-
   // Enable DMA1 channel4 IRQ Channel ( SPI RX) 
   NVIC_InitTypeDef NVIC_init_struct = {
     .NVIC_IRQChannel = DMA1_Channel4_IRQn,
@@ -30,9 +28,7 @@ void spi_arch_int_enable(void) {
 
 }
 
-void spi_int_disable(void) {
-  NVIC_InitTypeDef NVIC_InitStructure;
-
+void spi_arch_int_disable(void) {
   // Enable DMA1 channel4 IRQ Channel ( SPI RX)
   NVIC_InitTypeDef NVIC_init_struct = {
     .NVIC_IRQChannel = DMA1_Channel4_IRQn,
@@ -41,14 +37,11 @@ void spi_int_disable(void) {
     .NVIC_IRQChannelCmd = DISABLE
   };
   NVIC_Init(&NVIC_init_struct);
-
 }
-*/
 
 void spi_init(void) {
-/*
+
   GPIO_InitTypeDef GPIO_InitStructure;
-  EXTI_InitTypeDef EXTI_InitStructure;
   SPI_InitTypeDef SPI_InitStructure;
 
   // Enable SPI2 Periph clock -------------------------------------------------
@@ -75,15 +68,12 @@ void spi_init(void) {
   SPI_InitStructure.SPI_CRCPolynomial = 7;
   SPI_Init(SPI2, &SPI_InitStructure);
 
-
   // Enable SPI_2 DMA clock ---------------------------------------------------
   RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
 
-*/
 }
 
 /*
-
 void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
 
   Adxl345Select();
@@ -93,25 +83,26 @@ void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
   while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
   while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
   Adxl345Unselect();
-
 }
 
-void adxl345_clear_rx_buf(void) {
+void spi_clear_rx_buf(void) {
   uint8_t __attribute__ ((unused)) ret = SPI_I2S_ReceiveData(SPI2);
 }
+*/
 
-void adxl345_start_reading_data(void) {
-   Adxl345Select();
 
-   imu_aspirin.accel_tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0);
+
+void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len) 
+{
+   Spi2Slave0Select();
 
   // SPI2_Rx_DMA_Channel configuration ------------------------------------
   DMA_DeInit(DMA1_Channel4);
   DMA_InitTypeDef DMA_initStructure_4 = {
     .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
-    .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_rx_buf,
+    .DMA_MemoryBaseAddr = (uint32_t)_read,
     .DMA_DIR = DMA_DIR_PeripheralSRC,
-    .DMA_BufferSize = 7,
+    .DMA_BufferSize = _len,
     .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
     .DMA_MemoryInc = DMA_MemoryInc_Enable,
     .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
@@ -126,9 +117,9 @@ void adxl345_start_reading_data(void) {
   DMA_DeInit(DMA1_Channel5);
   DMA_InitTypeDef DMA_initStructure_5 = {
     .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
-    .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_tx_buf,
+    .DMA_MemoryBaseAddr = (uint32_t)_send,
     .DMA_DIR = DMA_DIR_PeripheralDST,
-    .DMA_BufferSize = 7,
+    .DMA_BufferSize = _len,
     .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
     .DMA_MemoryInc = DMA_MemoryInc_Enable,
     .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
@@ -157,14 +148,14 @@ void adxl345_start_reading_data(void) {
 
 // Accel end of DMA transfert
 void dma1_c4_irq_handler(void) {
-  Adxl345Unselect();
+  Spi2Slave0Unselect();
   if (DMA_GetITStatus(DMA1_IT_TC4)) {
 		// clear int pending bit
 		DMA_ClearITPendingBit(DMA1_IT_GL4);
 
     // mark as available
-    imu_aspirin.accel_available = TRUE;
-	}
+    spi_message_received = TRUE;
+  }
 
   // disable DMA Channel
   DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
@@ -177,6 +168,5 @@ void dma1_c4_irq_handler(void) {
 
 }
 
-*/
 
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
index a020355a82..7dfefa4b9a 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
@@ -31,16 +31,21 @@
 #include "mcu_periph/spi.h"
 #include <stm32/gpio.h>
 
+
+extern void spi_arch_int_enable(void);
+extern void spi_arch_int_disable(void);
+
+extern void spi_clear_rx_buf(void);
+void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len);
+
+
 /*
 
 ////////// 
 // from aspirin_arch.h
 
-extern void imu_aspirin_arch_int_enable(void);
-extern void imu_aspirin_arch_int_disable(void);
 
 extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
-extern void adxl345_clear_rx_buf(void);
 extern void adxl345_start_reading_data(void);
 
 */

From c6b8e0a2146ecafd529886ebf0a07e81204ed698 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 10 Jan 2012 17:56:15 +0100
Subject: [PATCH 06/12] Aspirin is talking to me over a generic re-usable DMA
 accelerated SPI interface

---
 conf/airframes/CDW/LisaAspirin2.xml           | 12 ++++-----
 .../shared/imu_aspirin_v2.0.makefile          |  2 +-
 sw/airborne/arch/stm32/mcu_periph/spi_arch.c  | 17 ++++++++++--
 sw/airborne/arch/stm32/mcu_periph/spi_arch.h  |  1 -
 .../stm32/subsystems/imu/imu_aspirin2_arch.c  | 12 ++-------
 sw/airborne/subsystems/imu/imu_aspirin2.c     | 26 ++++++++++++++++---
 sw/airborne/subsystems/imu/imu_aspirin2.h     |  1 +
 7 files changed, 48 insertions(+), 23 deletions(-)

diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index f754ff8e00..365f2e4406 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -203,13 +203,13 @@
   <modules>
 <!--
     <load name="light.xml">
-      <define name="LIGHT_LED_STROBE" value="3"/>
-      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="LIGHT_LED_STROBE" value="2"/>
+      <define name="LIGHT_LED_NAV" value="3"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
-    -->
-    <!--    <load name="digital_cam_i2c.xml"/>  -->
+
+        <load name="digital_cam_i2c.xml"/>  -->
 
  <!--    <load name="ins_ppzuavimu.xml" /> -->
 
@@ -256,8 +256,8 @@
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Communication -->
-    <subsystem name="telemetry" type="xbee_api">
-      <configure name="MODEM_BAUD" value="B9600"/>
+    <subsystem name="telemetry" type="transparent">
+      <configure name="MODEM_BAUD" value="B57600"/>
     </subsystem>
 
     <!-- Actuators -->
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile b/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
index f96478e6a2..103c70d277 100644
--- a/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
+++ b/conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
@@ -50,7 +50,7 @@ ifeq ($(ARCH), lpc21)
 #TODO
 else ifeq ($(ARCH), stm32)
 # IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
-# IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
+IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 endif
 
 IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_0
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index 4469e94ac4..fc17a26688 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -13,6 +13,7 @@
 #define Spi2Slave0Unselect() GPIOB->BSRR = GPIO_Pin_12
 #define Spi2Slave0Select()   GPIOB->BRR = GPIO_Pin_12
 
+
 // spi dma end of rx handler 
 void dma1_c4_irq_handler(void);
 
@@ -63,7 +64,7 @@ void spi_init(void) {
   SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
   SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
   SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
-  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
+  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_64;
   SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
   SPI_InitStructure.SPI_CRCPolynomial = 7;
   SPI_Init(SPI2, &SPI_InitStructure);
@@ -71,6 +72,16 @@ void spi_init(void) {
   // Enable SPI_2 DMA clock ---------------------------------------------------
   RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
 
+  // SLAVE 0
+  // set accel slave select as output and assert it ( on PB12) 
+  Spi2Slave0Unselect();
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  spi_arch_int_enable();
 }
 
 /*
@@ -147,8 +158,10 @@ void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len)
 
 
 // Accel end of DMA transfert
-void dma1_c4_irq_handler(void) {
+void dma1_c4_irq_handler(void) 
+{
   Spi2Slave0Unselect();
+
   if (DMA_GetITStatus(DMA1_IT_TC4)) {
 		// clear int pending bit
 		DMA_ClearITPendingBit(DMA1_IT_GL4);
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
index 7dfefa4b9a..8477924504 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
@@ -31,7 +31,6 @@
 #include "mcu_periph/spi.h"
 #include <stm32/gpio.h>
 
-
 extern void spi_arch_int_enable(void);
 extern void spi_arch_int_disable(void);
 
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
index 3120676324..d7bc7268e3 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
@@ -10,7 +10,7 @@
 
 
 
-// gyro int handler
+// gyro interupt handler
 void exti15_10_irq_handler(void);
 
 void imu_aspirin2_arch_int_enable(void) {
@@ -72,16 +72,8 @@ void imu_aspirin2_arch_init(void) {
   EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
   EXTI_InitStructure.EXTI_LineCmd = ENABLE;
   EXTI_Init(&EXTI_InitStructure);
-
-  // Accel 
-  // set accel slave select as output and assert it ( on PB12) 
-  Adxl345Unselect();
-  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
-  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_Init(GPIOB, &GPIO_InitStructure);
 */
+
 }
 
 
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
index 0067265f6c..3a226d74ec 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -1,8 +1,13 @@
 #include "subsystems/imu.h"
 
+#include "led.h"
 #include "mcu_periph/spi.h"
 #include "mcu_periph/spi_arch.h"
 
+// Peripherials
+#include "../../peripherals/mpu60X0.h"
+
+
 struct ImuAspirin2 imu_aspirin2;
 
 /*
@@ -19,11 +24,26 @@ void imu_impl_init(void) {
   imu_aspirin2.status = Aspirin2StatusUninit;
   imu_aspirin2.imu_available = FALSE;
 
-  imu_aspirin2_arch_init();
+//  imu_aspirin2_arch_init();
+//  spi_arch_int_enable();
+
+//  LED_TOGGLE(2);
+  LED_TOGGLE(3);
 }
 
 
-void imu_periodic(void) {
+void imu_periodic(void) 
+{
+
+//  LED_TOGGLE(2);
+//  LED_TOGGLE(3);
+
+  imu_aspirin2.imu_len = 2;
+  imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + 0x80;
+  imu_aspirin2.imu_tx_buf[1] = 0x00;
+
+  spi_rw(imu_aspirin2.imu_tx_buf, imu_aspirin2.imu_rx_buf, imu_aspirin2.imu_len);
+
 /*
   hmc5843_periodic();
   if (imu_aspirin.status == AspirinStatusUninit) {
@@ -39,7 +59,7 @@ void imu_periodic(void) {
       configure_accel();
       imu_aspirin.time_since_last_accel_reading=0;
     }
-  }
+  } 
 */
 }
 
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index c1d054c791..f386c36bf4 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -66,6 +66,7 @@ struct ImuAspirin2 {
   volatile uint8_t imu_available;
   volatile uint8_t imu_tx_buf[64];
   volatile uint8_t imu_rx_buf[64];
+  volatile uint8_t imu_len;
   uint32_t time_since_last_reading;
 };
 

From 35e2f6391b2c1a40290f23149283895e2bea8169 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 11 Jan 2012 14:00:11 +0100
Subject: [PATCH 07/12] SPI transaction struct

---
 conf/airframes/CDW/LisaAspirin2.xml          |  2 +-
 sw/airborne/arch/stm32/mcu_periph/spi_arch.c | 19 ++++++++++++-------
 sw/airborne/arch/stm32/mcu_periph/spi_arch.h |  2 +-
 sw/airborne/mcu_periph/spi.h                 | 16 ++++++++++++++++
 sw/airborne/subsystems/imu/imu_aspirin2.c    | 14 +++++++-------
 sw/airborne/subsystems/imu/imu_aspirin2.h    |  3 ++-
 6 files changed, 39 insertions(+), 17 deletions(-)

diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index 365f2e4406..3d0c1d0090 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -222,7 +222,7 @@
 
   <firmware name="fixedwing">
 
-    <target name="ap" board="lisa_m_1.0">
+    <target name="ap" board="lisa_l_1.0">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index fc17a26688..027969af79 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -101,19 +101,23 @@ void spi_clear_rx_buf(void) {
 }
 */
 
+struct spi_transaction* slave0;
 
-
-void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len) 
+void spi_rw(struct spi_transaction  * _trans) 
 {
-   Spi2Slave0Select();
+  // Store local copy to notify of the results
+  slave0 = _trans;
+  slave0->status = SPITransRunning;
+
+  Spi2Slave0Select();
 
   // SPI2_Rx_DMA_Channel configuration ------------------------------------
   DMA_DeInit(DMA1_Channel4);
   DMA_InitTypeDef DMA_initStructure_4 = {
     .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
-    .DMA_MemoryBaseAddr = (uint32_t)_read,
+    .DMA_MemoryBaseAddr = (uint32_t) slave0->miso_buf,
     .DMA_DIR = DMA_DIR_PeripheralSRC,
-    .DMA_BufferSize = _len,
+    .DMA_BufferSize = slave0->length,
     .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
     .DMA_MemoryInc = DMA_MemoryInc_Enable,
     .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
@@ -128,9 +132,9 @@ void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len)
   DMA_DeInit(DMA1_Channel5);
   DMA_InitTypeDef DMA_initStructure_5 = {
     .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
-    .DMA_MemoryBaseAddr = (uint32_t)_send,
+    .DMA_MemoryBaseAddr = (uint32_t) slave0->mosi_buf,
     .DMA_DIR = DMA_DIR_PeripheralDST,
-    .DMA_BufferSize = _len,
+    .DMA_BufferSize = slave0->length,
     .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
     .DMA_MemoryInc = DMA_MemoryInc_Enable,
     .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
@@ -179,6 +183,7 @@ void dma1_c4_irq_handler(void)
   DMA_Cmd(DMA1_Channel4, DISABLE);
   DMA_Cmd(DMA1_Channel5, DISABLE);
 
+  slave0->status = SPITransSuccess;
 }
 
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
index 8477924504..5af8cf63ca 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
@@ -35,7 +35,7 @@ extern void spi_arch_int_enable(void);
 extern void spi_arch_int_disable(void);
 
 extern void spi_clear_rx_buf(void);
-void spi_rw(volatile uint8_t* _send, volatile uint8_t* _read, volatile int _len);
+void spi_rw(struct spi_transaction  * _trans);
 
 
 /*
diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h
index 9cdd614a4b..ca296502d2 100644
--- a/sw/airborne/mcu_periph/spi.h
+++ b/sw/airborne/mcu_periph/spi.h
@@ -32,8 +32,24 @@
 #ifdef USE_SPI
 
 #include "std.h"
+
+enum SPITransactionStatus {
+  SPITransPending,
+  SPITransRunning,
+  SPITransSuccess,
+  SPITransFailed
+};
+
+struct spi_transaction {
+  volatile uint8_t* mosi_buf;
+  volatile uint8_t* miso_buf;
+  uint8_t length;
+  volatile enum SPITransactionStatus status;
+};
+
 #include "mcu_periph/spi_arch.h"
 
+
 extern uint8_t* spi_buffer_input;
 extern uint8_t* spi_buffer_output;
 extern uint8_t spi_buffer_length;
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
index 3a226d74ec..70ff314be1 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -10,6 +10,8 @@
 
 struct ImuAspirin2 imu_aspirin2;
 
+struct spi_transaction aspirin2_mpu60x0;
+
 /*
 
 // initialize peripherals
@@ -24,25 +26,23 @@ void imu_impl_init(void) {
   imu_aspirin2.status = Aspirin2StatusUninit;
   imu_aspirin2.imu_available = FALSE;
 
+  aspirin2_mpu60x0.mosi_buf = imu_aspirin2.imu_tx_buf;
+  aspirin2_mpu60x0.miso_buf = imu_aspirin2.imu_rx_buf;
+  aspirin2_mpu60x0.length = 2;
+
 //  imu_aspirin2_arch_init();
 //  spi_arch_int_enable();
 
-//  LED_TOGGLE(2);
-  LED_TOGGLE(3);
 }
 
 
 void imu_periodic(void) 
 {
 
-//  LED_TOGGLE(2);
-//  LED_TOGGLE(3);
-
-  imu_aspirin2.imu_len = 2;
   imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + 0x80;
   imu_aspirin2.imu_tx_buf[1] = 0x00;
 
-  spi_rw(imu_aspirin2.imu_tx_buf, imu_aspirin2.imu_rx_buf, imu_aspirin2.imu_len);
+  spi_rw(&aspirin2_mpu60x0);
 
 /*
   hmc5843_periodic();
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index f386c36bf4..928817166c 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -66,11 +66,12 @@ struct ImuAspirin2 {
   volatile uint8_t imu_available;
   volatile uint8_t imu_tx_buf[64];
   volatile uint8_t imu_rx_buf[64];
-  volatile uint8_t imu_len;
   uint32_t time_since_last_reading;
 };
 
 extern struct ImuAspirin2 imu_aspirin2;
+
+
 #define ASPIRIN2_TIMEOUT 3
 /*
 

From c61b44a1e323a501e36c12befee7acafb00005aa Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 11 Jan 2012 15:09:17 +0100
Subject: [PATCH 08/12] Aspirin2 Driver Via DMA accelerated SPI

---
 conf/airframes/CDW/LisaAspirin2.xml          |   2 +-
 sw/airborne/arch/stm32/mcu_periph/spi_arch.c |   1 +
 sw/airborne/mcu_periph/spi.h                 |   1 +
 sw/airborne/peripherals/mpu60X0.h            |   2 +
 sw/airborne/subsystems/imu/imu_aspirin2.c    | 157 +++++++++++++------
 sw/airborne/subsystems/imu/imu_aspirin2.h    |  82 +++++-----
 6 files changed, 164 insertions(+), 81 deletions(-)

diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index 3d0c1d0090..8abd75ab7a 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -230,7 +230,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
-      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
+<!--       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" /> -->
 
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="1"/>
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index 027969af79..72a76cecf9 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -184,6 +184,7 @@ void dma1_c4_irq_handler(void)
   DMA_Cmd(DMA1_Channel5, DISABLE);
 
   slave0->status = SPITransSuccess;
+  *(slave0->ready) = 1;
 }
 
 
diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h
index ca296502d2..a118cc493a 100644
--- a/sw/airborne/mcu_periph/spi.h
+++ b/sw/airborne/mcu_periph/spi.h
@@ -43,6 +43,7 @@ enum SPITransactionStatus {
 struct spi_transaction {
   volatile uint8_t* mosi_buf;
   volatile uint8_t* miso_buf;
+  volatile uint8_t* ready;
   uint8_t length;
   volatile enum SPITransactionStatus status;
 };
diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
index 2f186875b4..384b92a726 100644
--- a/sw/airborne/peripherals/mpu60X0.h
+++ b/sw/airborne/peripherals/mpu60X0.h
@@ -5,6 +5,8 @@
 #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
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
index 70ff314be1..0da0ed9d31 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -12,10 +12,11 @@ struct ImuAspirin2 imu_aspirin2;
 
 struct spi_transaction aspirin2_mpu60x0;
 
-/*
+
 
 // initialize peripherals
-static void configure_gyro(void);
+static void configure(void);
+/*
 static void configure_accel(void);
 //static void configure_mag(void);
 
@@ -28,10 +29,10 @@ void imu_impl_init(void) {
 
   aspirin2_mpu60x0.mosi_buf = imu_aspirin2.imu_tx_buf;
   aspirin2_mpu60x0.miso_buf = imu_aspirin2.imu_rx_buf;
+  aspirin2_mpu60x0.ready = &(imu_aspirin2.imu_available);
   aspirin2_mpu60x0.length = 2;
 
 //  imu_aspirin2_arch_init();
-//  spi_arch_int_enable();
 
 }
 
@@ -39,61 +40,128 @@ void imu_impl_init(void) {
 void imu_periodic(void) 
 {
 
-  imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + 0x80;
-  imu_aspirin2.imu_tx_buf[1] = 0x00;
+  if (imu_aspirin2.status == Aspirin2StatusUninit) 
+  {
+    configure();
+    // imu_aspirin_arch_int_enable();
+    imu_aspirin2.status = Aspirin2StatusIdle;
 
-  spi_rw(&aspirin2_mpu60x0);
+    aspirin2_mpu60x0.length = 22;
+    aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+    {
+      for (int i=1;i<aspirin2_mpu60x0.length;i++)
+        aspirin2_mpu60x0.mosi_buf[i] = 0;
+    }
+  }
+  else
+  {
+
+    // imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + MPU60X0_SPI_READ;
+    // imu_aspirin2.imu_tx_buf[1] = 0x00;
+
+    spi_rw(&aspirin2_mpu60x0);
 
 /*
-  hmc5843_periodic();
-  if (imu_aspirin.status == AspirinStatusUninit) {
-    configure_gyro();
-    configure_accel();
-    imu_aspirin_arch_int_enable();
-    imu_aspirin.status = AspirinStatusIdle;
-  } else {
-    imu_aspirin.gyro_available_blaaa = TRUE;
     imu_aspirin.time_since_last_reading++;
     imu_aspirin.time_since_last_accel_reading++;
-    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
+    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) 
+    {
       configure_accel();
       imu_aspirin.time_since_last_accel_reading=0;
     }
+*/
   } 
+}
+
+static void configure(void) 
+{
+  aspirin2_mpu60x0.length = 2;
+
+  ///////////////////
+  // Configure power:
+
+  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
+
+  // MPU60X0_REG_USER_CTRL:
+  // -Enable Aux I2C Master Mode
+  // -Enable SPI
+
+  // MPU60X0_REG_PWR_MGMT_1
+  // -switch to gyroX clock
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_PWR_MGMT_1;
+  aspirin2_mpu60x0.mosi_buf[1] = 0x01;
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
+
+  /////////////////////////
+  // Measurement Settings
+
+  // MPU60X0_REG_CONFIG
+  // -ext sync on gyro X (bit 3->6)
+  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
+#if PERIODIC_FREQUENCY == 60
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (2 << 3) | (3 << 0);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_SMPLRT_DIV
+  // -100Hz output = 1kHz / (9 + 1)
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_SMPLRT_DIV;
+  aspirin2_mpu60x0.mosi_buf[1] = 9;
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_GYRO_CONFIG
+  // -2000deg/sec
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_GYRO_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_ACCEL_CONFIG
+  // 16g, no HPFL
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_ACCEL_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+/*
+  struct i2c_transaction t;
+  t.type = I2CTransTx;
+  t.slave_addr = ITG3200_ADDR;
+  // set gyro range to 2000deg/s and low pass at 256Hz
+  t.mosi_buf[0] = ITG3200_REG_DLPF_FS;
+  t.mosi_buf[1] = (0x03<<3);
+  t.len_w = 2;
+  send_i2c_msg_with_retry(&t);
+  // set sample rate to 533Hz 
+  t.mosi_buf[0] = ITG3200_REG_SMPLRT_DIV;
+  t.mosi_buf[1] = 0x0E;
+  send_i2c_msg_with_retry(&t);
+  // switch to gyroX clock 
+  t.mosi_buf[0] = ITG3200_REG_PWR_MGM;
+  t.mosi_buf[1] = 0x01;
+  send_i2c_msg_with_retry(&t);
+  // enable interrupt on data ready, idle high, latch until read any register
+  t.mosi_buf[0] = ITG3200_REG_INT_CFG;
+  t.mosi_buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
+  send_i2c_msg_with_retry(&t);
 */
 }
 
 
 /*
-// sends a serie of I2C commands to configure the ITG3200 gyro 
-static void configure_gyro(void) {
-  struct i2c_transaction t;
-  t.type = I2CTransTx;
-  t.slave_addr = ITG3200_ADDR;
-  // set gyro range to 2000deg/s and low pass at 256Hz
-  t.buf[0] = ITG3200_REG_DLPF_FS;
-  t.buf[1] = (0x03<<3);
-  t.len_w = 2;
-  send_i2c_msg_with_retry(&t);
-  // set sample rate to 533Hz 
-  t.buf[0] = ITG3200_REG_SMPLRT_DIV;
-  t.buf[1] = 0x0E;
-  send_i2c_msg_with_retry(&t);
-  // switch to gyroX clock 
-  t.buf[0] = ITG3200_REG_PWR_MGM;
-  t.buf[1] = 0x01;
-  send_i2c_msg_with_retry(&t);
-  // enable interrupt on data ready, idle high, latch until read any register
-  t.buf[0] = ITG3200_REG_INT_CFG;
-  t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
-  send_i2c_msg_with_retry(&t);
-
-}
-
-
-
-static void configure_accel(void) {
-
+static void configure_accel(void) 
+{
   // set data rate to 800Hz
   adxl345_write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
   // switch to measurememnt mode 
@@ -108,7 +176,6 @@ static void configure_accel(void) {
   adxl345_start_reading_data();
 
 }
-
 */
 
 
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index 928817166c..f934e46512 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -26,11 +26,7 @@
 
 #include "generated/airframe.h"
 #include "subsystems/imu.h"
-
-#include "mcu_periph/i2c.h"
-#include "peripherals/itg3200.h"
-#include "peripherals/hmc5843.h"
-#include "peripherals/adxl345.h"
+#include "led.h"
 
 
 #ifdef IMU_ASPIRIN_VERSION_2_0
@@ -95,53 +91,69 @@ extern struct ImuAspirin2 imu_aspirin2;
 /* must be implemented by underlying architecture */
 extern void imu_aspirin2_arch_init(void);
 
-/*
-static inline void gyro_copy_i2c(void)
+
+static inline void imu_from_buff(void)
 {
-  int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1];
-  int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3];
-  int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5];
-  RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
+  int32_t x, y, z;
+
+
+  // If the itg3200 I2C transaction has succeeded: convert the data
+#define MPU_OFFSET_GYRO 10
+  x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_GYRO]);
+  y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_GYRO]);
+  z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_GYRO]);
+
+  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
+
+#define MPU_OFFSET_ACC 2
+  x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_ACC]);
+  y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_ACC]);
+  z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_ACC]);
+
+  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
+
+  // Is this is new data
+#define MPU_OFFSET_STATUS 1
+  if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01)
+  {
+    //gyr_valid = TRUE;
+    //acc_valid = TRUE;
+  }
+  else
+  {
+  }
 }
 
-static inline void accel_copy_spi(void)
-{
-  const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
-  const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
-  const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
-  VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
-}
-
-
-static inline void imu_gyro_event(void (* _gyro_handler)(void))
-{
-
-}
-*/
 
 static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
 {
+  LED_TOGGLE(4);
+
   if (imu_aspirin2.status == Aspirin2StatusUninit) return;
 
-  imu_aspirin2_arch_int_disable();
-  if (imu_aspirin2.imu_available) {
+  // imu_aspirin2_arch_int_disable();
+
+  if (imu_aspirin2.imu_available) 
+  {
     imu_aspirin2.time_since_last_reading = 0;
     imu_aspirin2.imu_available = FALSE;
-    //accel_copy_spi();
+    imu_from_buff();
+
+    _gyro_handler();
     _accel_handler();
   }
-  imu_aspirin2_arch_int_enable();
+  // imu_aspirin2_arch_int_enable();
 
   // Reset everything if we've been waiting too long
-  if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
-    imu_aspirin2.time_since_last_reading = 0;
-    return;
-  }
+  //if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
+  //  imu_aspirin2.time_since_last_reading = 0;
+  //  return;
+  //}
 
 }
 
-#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do {		\
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
   imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
-} while(0);
+}
 
 #endif /* IMU_ASPIRIN_H */

From 197a33fea19eafe1918226e940328e5a6e78d57b Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 Jan 2012 11:17:44 +0100
Subject: [PATCH 09/12] Lisa-M Aspirin2 Works too

---
 conf/airframes/CDW/LisaAspirin2.xml       | 44 ++++++-----------------
 sw/airborne/subsystems/imu/imu_aspirin2.h |  5 +--
 2 files changed, 11 insertions(+), 38 deletions(-)

diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index 8abd75ab7a..807bd309d8 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -7,9 +7,9 @@
 <airframe name="LisaAspirin2">
 
   <servos>
-    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="THROTTLE" no="2" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILEVON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILEVON_RIGHT" no="0" min="2000" neutral="1500" max="1000"/>
     <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
     <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
   </servos>
@@ -29,8 +29,8 @@
   </rc_commands>
 
   <command_laws>
-    <set servo="AILERON_LEFT" value="@ROLL"/>
-    <set servo="AILERON_RIGHT" value="@ROLL"/>
+    <set servo="AILEVON_LEFT" value="@ROLL + @PITCH"/>
+    <set servo="AILEVON_RIGHT" value="@ROLL - @PITCH"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
     <set servo="ELEVATOR" value="@PITCH"/>
   </command_laws>
@@ -53,35 +53,15 @@
     <define name="GYRO_Q_SENS" value="4.359" integer="16"/>
     <define name="GYRO_R_SENS" value="4.359" integer="16"/>
 
-    <define name="GYRO_P_Q" value="0."/>
-    <define name="GYRO_P_R" value="0"/>
-    <define name="GYRO_Q_P" value="0."/>
-    <define name="GYRO_Q_R" value="0."/>
-    <define name="GYRO_R_P" value="0."/>
-    <define name="GYRO_R_Q" value="0."/>
-
-    <define name="GYRO_P_SIGN" value="1"/>
-    <define name="GYRO_Q_SIGN" value="1"/>
-    <define name="GYRO_R_SIGN" value="1"/>
-
-    <define name="ACCEL_X_NEUTRAL" value="-40"/>
-    <define name="ACCEL_Y_NEUTRAL" value="-15"/>
-    <define name="ACCEL_Z_NEUTRAL" value="375"/>
+    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+    <define name="ACCEL_Z_NEUTRAL" value="0"/>
 
     <!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
     <define name="ACCEL_X_SENS" value="9.81" integer="16"/>
     <define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
     <define name="ACCEL_Z_SENS" value="9.81" integer="16"/>
 
-<!--
-    <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
--->
-    <define name="ACCEL_X_SIGN" value="1"/>
-    <define name="ACCEL_Y_SIGN" value="1"/>
-    <define name="ACCEL_Z_SIGN" value="1"/>
-
     <define name="MAG_X_NEUTRAL" value="0"/>
     <define name="MAG_Y_NEUTRAL" value="0"/>
     <define name="MAG_Z_NEUTRAL" value="0"/>
@@ -90,10 +70,6 @@
     <define name="MAG_Y_SENS" value="1" integer="16"/>
     <define name="MAG_Z_SENS" value="1" integer="16"/>
 
-    <define name="MAG_X_SIGN" value="1"/>
-    <define name="MAG_Y_SIGN" value="1"/>
-    <define name="MAG_Z_SIGN" value="1"/>
-
     <define name="BODY_TO_IMU_PHI" value="0"/>
     <define name="BODY_TO_IMU_THETA" value="0"/>
     <define name="BODY_TO_IMU_PSI" value="0"/>
@@ -222,7 +198,7 @@
 
   <firmware name="fixedwing">
 
-    <target name="ap" board="lisa_l_1.0">
+    <target name="ap" board="lisa_m_1.0">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
@@ -230,7 +206,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
-<!--       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" /> -->
+       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" /> 
 
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="1"/>
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index f934e46512..6f067aa453 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -26,7 +26,6 @@
 
 #include "generated/airframe.h"
 #include "subsystems/imu.h"
-#include "led.h"
 
 
 #ifdef IMU_ASPIRIN_VERSION_2_0
@@ -35,7 +34,7 @@
 #define IMU_MAG_Z_CHAN 1
 #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_Y_SIGN 1
 #define IMU_MAG_Z_SIGN 1
 #endif
 #endif
@@ -127,8 +126,6 @@ static inline void imu_from_buff(void)
 
 static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
 {
-  LED_TOGGLE(4);
-
   if (imu_aspirin2.status == Aspirin2StatusUninit) return;
 
   // imu_aspirin2_arch_int_disable();

From 33e481062ff343241c898875a37c59062f4f0401 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 Jan 2012 12:13:55 +0100
Subject: [PATCH 10/12] Ability to turn the aspirin2

---
 sw/airborne/subsystems/imu/imu_aspirin2.h | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index 6f067aa453..4c4093b60d 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -93,23 +93,27 @@ extern void imu_aspirin2_arch_init(void);
 
 static inline void imu_from_buff(void)
 {
-  int32_t x, y, z;
+  int32_t x, y, z, p, q, r;
 
 
   // If the itg3200 I2C transaction has succeeded: convert the data
 #define MPU_OFFSET_GYRO 10
-  x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_GYRO]);
-  y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_GYRO]);
-  z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_GYRO]);
-
-  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
+  p = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_GYRO]);
+  q = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_GYRO]);
+  r = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_GYRO]);
 
 #define MPU_OFFSET_ACC 2
   x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_ACC]);
   y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_ACC]);
   z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_ACC]);
 
+#ifdef LISA_M_LONGITUDINAL_X
+  RATES_ASSIGN(imu.gyro_unscaled, q, -p, r);
+  VECT3_ASSIGN(imu.accel_unscaled, y, -y, z);
+#else
+  RATES_ASSIGN(imu.gyro_unscaled, p, q, r);
   VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
+#endif
 
   // Is this is new data
 #define MPU_OFFSET_STATUS 1

From fc27a8263854b38dcd4fab14196c455d41e82db6 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 Jan 2012 12:34:24 +0100
Subject: [PATCH 11/12] Typo

---
 sw/airborne/subsystems/imu/imu_aspirin2.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index 4c4093b60d..9a5e53eba1 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -109,7 +109,7 @@ static inline void imu_from_buff(void)
 
 #ifdef LISA_M_LONGITUDINAL_X
   RATES_ASSIGN(imu.gyro_unscaled, q, -p, r);
-  VECT3_ASSIGN(imu.accel_unscaled, y, -y, z);
+  VECT3_ASSIGN(imu.accel_unscaled, y, -x, z);
 #else
   RATES_ASSIGN(imu.gyro_unscaled, p, q, r);
   VECT3_ASSIGN(imu.accel_unscaled, x, y, z);

From 16c2bbf176f7ae6285e498296935a1bc8ace8247 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 1 Feb 2012 14:47:14 +0100
Subject: [PATCH 12/12] default sens values for aspirin2

---
 sw/airborne/subsystems/imu/imu_aspirin2.h | 45 +++++++++++++++++++++++
 1 file changed, 45 insertions(+)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h
index 9a5e53eba1..14b64de6f9 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -50,6 +50,51 @@
 #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
+ * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/16.4 * pi/180 * 4096 = 4.359066229
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 4.359
+#define IMU_GYRO_P_SENS_NUM 4359
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS 4.359
+#define IMU_GYRO_Q_SENS_NUM 4359
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS 4.359
+#define IMU_GYRO_R_SENS_NUM 4359
+#define IMU_GYRO_R_SENS_DEN 1000
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+/** default accel sensitivy from the datasheet
+ * MPU60X0 has 2048 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * sens = 9.81 / 2048 * 1024 = 4.905
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 4.905
+#define IMU_ACCEL_X_SENS_NUM 4905
+#define IMU_ACCEL_X_SENS_DEN 1000
+#define IMU_ACCEL_Y_SENS 4.905
+#define IMU_ACCEL_Y_SENS_NUM 4905
+#define IMU_ACCEL_Y_SENS_DEN 1000
+#define IMU_ACCEL_Z_SENS 4.905
+#define IMU_ACCEL_Z_SENS_NUM 4905
+#define IMU_ACCEL_Z_SENS_DEN 1000
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
+
 enum Aspirin2Status
   { Aspirin2StatusUninit,
     Aspirin2StatusIdle,