diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
new file mode 100644
index 0000000000..807bd309d8
--- /dev/null
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -0,0 +1,249 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml
new file mode 100644
index 0000000000..7696df9c4a
--- /dev/null
+++ b/conf/airframes/CDW/yapa3_aspirin2.xml
@@ -0,0 +1,274 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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/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..103c70d277
--- /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:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+# 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..72a76cecf9
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -0,0 +1,191 @@
+#include "subsystems/imu.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "mcu_periph/spi.h"
+
+// 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) {
+ // 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_arch_int_disable(void) {
+ // 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;
+ 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_64;
+ 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);
+
+ // 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();
+}
+
+/*
+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 spi_clear_rx_buf(void) {
+ uint8_t __attribute__ ((unused)) ret = SPI_I2S_ReceiveData(SPI2);
+}
+*/
+
+struct spi_transaction* slave0;
+
+void spi_rw(struct spi_transaction * _trans)
+{
+ // 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) slave0->miso_buf,
+ .DMA_DIR = DMA_DIR_PeripheralSRC,
+ .DMA_BufferSize = slave0->length,
+ .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) slave0->mosi_buf,
+ .DMA_DIR = DMA_DIR_PeripheralDST,
+ .DMA_BufferSize = slave0->length,
+ .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)
+{
+ Spi2Slave0Unselect();
+
+ if (DMA_GetITStatus(DMA1_IT_TC4)) {
+ // clear int pending bit
+ DMA_ClearITPendingBit(DMA1_IT_GL4);
+
+ // mark as available
+ spi_message_received = 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);
+
+ slave0->status = SPITransSuccess;
+ *(slave0->ready) = 1;
+}
+
+
+
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..5af8cf63ca
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h
@@ -0,0 +1,166 @@
+/* $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
+
+extern void spi_arch_int_enable(void);
+extern void spi_arch_int_disable(void);
+
+extern void spi_clear_rx_buf(void);
+void spi_rw(struct spi_transaction * _trans);
+
+
+/*
+
+//////////
+// from aspirin_arch.h
+
+
+extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
+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..d7bc7268e3
--- /dev/null
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
@@ -0,0 +1,92 @@
+#include "subsystems/imu.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+
+
+
+// gyro interupt 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);
+*/
+
+}
+
+
+
+// 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
+
+
+
+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/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h
index c2fbf7c0c6..31580a537b 100644
--- a/sw/airborne/mcu_periph/spi.h
+++ b/sw/airborne/mcu_periph/spi.h
@@ -32,8 +32,25 @@
#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;
+ volatile uint8_t* ready;
+ 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/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c
new file mode 100644
index 0000000000..3290d7e0f9
--- /dev/null
+++ b/sw/airborne/modules/sensors/imu_aspirin2.c
@@ -0,0 +1,272 @@
+/*
+ * 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
+#include "imu_aspirin2.h"
+#include "mcu_periph/i2c.h"
+#include "led.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;
+
+// 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)
+{
+ /////////////////////////////////////////////////////////////////////
+ // MPU60X0
+ aspirin2_mpu60x0.type = I2CTransTx;
+ aspirin2_mpu60x0.slave_addr = MPU60X0_ADDR;
+ aspirin2_mpu60x0.len_r = 0;
+ aspirin2_mpu60x0.len_w = 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.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;
+ 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 = 21;
+ aspirin2_mpu60x0.len_w = 1;
+ aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS;
+ 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 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)
+ {
+ gyr_valid = TRUE;
+ acc_valid = TRUE;
+ }
+ else
+ {
+ }
+
+ 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..384b92a726
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60X0.h
@@ -0,0 +1,97 @@
+#ifndef MPU60X0
+#define MPU60X0
+
+/* default I2C address */
+#define MPU60X0_ADDR 0xD0
+#define MPU60X0_ADDR_ALT 0xD2
+
+#define MPU60X0_SPI_READ 0x80
+
+// Power and Interface
+#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
+#define MPU60X0_REG_USER_CTRL 0x6A
+#define MPU60X0_REG_PWR_MGMT_1 0x6B
+#define MPU60X0_REG_PWR_MGMT_2 0x6C
+
+// FIFO
+#define MPU60X0_REG_FIFO_EN 0X23
+#define MPU60X0_REG_FIFO_COUNT_H 0x72
+#define MPU60X0_REG_FIFO_COUNT_L 0x73
+#define MPU60X0_REG_FIFO_R_W 0x74
+
+// Measurement Settings
+#define MPU60X0_REG_SMPLRT_DIV 0X19
+#define MPU60X0_REG_CONFIG 0X1A
+#define MPU60X0_REG_GYRO_CONFIG 0X1B
+#define MPU60X0_REG_ACCEL_CONFIG 0X1C
+
+// I2C Slave settings
+#define MPU60X0_REG_I2C_MST_CTRL 0X24
+#define MPU60X0_REG_I2C_MST_STATUS 0X36
+#define MPU60X0_REG_I2C_MST_DELAY 0X67
+// Slave 0
+#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
+#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
+#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
+#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
+// Slave 1
+#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
+#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
+#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
+#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
+// Slave 2
+#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
+#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
+#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
+#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
+// Slave 3
+#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
+#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
+#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
+#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
+// Slave 4 - special
+#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
+#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
+#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
+#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
+#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
+
+// Interrupt
+#define MPU60X0_REG_INT_PIN_CFG 0X37
+#define MPU60X0_REG_INT_ENABLE 0X38
+#define MPU60X0_REG_INT_STATUS 0X3A
+
+// Accelero
+#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
+#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
+#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
+#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
+#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
+#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
+
+// Temperature
+#define MPU60X0_REG_TEMP_OUT_H 0X41
+#define MPU60X0_REG_TEMP_OUT_L 0X42
+
+// Gyro
+#define MPU60X0_REG_GYRO_XOUT_H 0X43
+#define MPU60X0_REG_GYRO_XOUT_L 0X44
+#define MPU60X0_REG_GYRO_YOUT_H 0X45
+#define MPU60X0_REG_GYRO_YOUT_L 0X46
+#define MPU60X0_REG_GYRO_ZOUT_H 0X47
+#define MPU60X0_REG_GYRO_ZOUT_L 0X48
+
+// External Sensor Data
+#define MPU60X0_EXT_SENS_DATA 0X49
+#define MPU60X0_EXT_SENS_DATA_SIZE 24
+
+
+/////////////////////////////////////////////////
+// MPU60X0 Definitions
+
+#define MPU60X0_REG_WHO_AM_I 0X75
+#define MPU60X0_WHOAMI_REPLY 0x68
+
+
+
+#endif /* MPU60X0 */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
new file mode 100644
index 0000000000..0da0ed9d31
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -0,0 +1,181 @@
+#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;
+
+struct spi_transaction aspirin2_mpu60x0;
+
+
+
+// initialize peripherals
+static void configure(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;
+
+ 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();
+
+}
+
+
+void imu_periodic(void)
+{
+
+ if (imu_aspirin2.status == Aspirin2StatusUninit)
+ {
+ configure();
+ // imu_aspirin_arch_int_enable();
+ imu_aspirin2.status = Aspirin2StatusIdle;
+
+ aspirin2_mpu60x0.length = 22;
+ aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+ {
+ for (int i=1;i 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);
+*/
+}
+
+
+/*
+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..14b64de6f9
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.h
@@ -0,0 +1,205 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 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.
+ */
+
+#ifndef IMU_ASPIRIN_H
+#define IMU_ASPIRIN_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.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
+
+/** 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,
+ 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 imu_from_buff(void)
+{
+ int32_t x, y, z, p, q, r;
+
+
+ // If the itg3200 I2C transaction has succeeded: convert the data
+#define MPU_OFFSET_GYRO 10
+ 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, -x, 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
+ if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01)
+ {
+ //gyr_valid = TRUE;
+ //acc_valid = TRUE;
+ }
+ else
+ {
+ }
+}
+
+
+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;
+ imu_from_buff();
+
+ _gyro_handler();
+ _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) { \
+ imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
+}
+
+#endif /* IMU_ASPIRIN_H */