From 7ddb582569f13035347806ad58c024de8374adb6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 5 Jun 2012 14:21:36 +0200 Subject: [PATCH] [imu aspirin 2.1] removed unneeded stm32 arch implementation and a bit more cleanup --- .../shared/imu_aspirin_v2.1.makefile | 33 +++++-- .../stm32/subsystems/imu/imu_aspirin2_arch.c | 92 ------------------- .../stm32/subsystems/imu/imu_aspirin2_arch.h | 20 ---- sw/airborne/subsystems/imu/imu_aspirin2.c | 76 +++++++-------- sw/airborne/subsystems/imu/imu_aspirin2.h | 58 ++---------- 5 files changed, 72 insertions(+), 207 deletions(-) delete mode 100644 sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c delete mode 100644 sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile index 345f75eba4..c4957a0b58 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile @@ -1,17 +1,32 @@ # Hey Emacs, this is a -*- makefile -*- # -# Aspirin IMU v2.0 +# Aspirin IMU v2.1 # # -# if ACCEL and GYRO SENS/NEUTRAL are not defined, -# the defaults from the datasheet will be used -# # required xml: #
# -# -# -# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# # # # @@ -25,7 +40,6 @@ 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 @@ -33,9 +47,8 @@ IMU_ASPIRIN_CFLAGS += -DUSE_SPI ifeq ($(ARCH), lpc21) #TODO -$(error Not implemented for the LCP21x yet. Not hard, just needs to be done. Patches welcome!) +$(error Not implemented for the LCP21x yet. Needs the new SPI mcu_periph. See issue 147!) 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 diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c deleted file mode 100644 index ca55e514a8..0000000000 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c +++ /dev/null @@ -1,92 +0,0 @@ -#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 deleted file mode 100644 index b4456c4aa2..0000000000 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h +++ /dev/null @@ -1,20 +0,0 @@ -#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/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c index 990861a69e..2a47cfd73a 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin2.c +++ b/sw/airborne/subsystems/imu/imu_aspirin2.c @@ -1,12 +1,32 @@ +/* + * Copyright (C) 2012 Christophe DeWagter + * + * 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 "subsystems/imu.h" #include "led.h" #include "mcu_periph/spi.h" -#include "mcu_periph/spi_arch.h" // Peripherials -#include "../../peripherals/mpu60X0.h" -#include "../../peripherals/hmc58xx.h" +#include "peripherals/mpu60X0.h" +#include "peripherals/hmc58xx.h" struct ImuAspirin2 imu_aspirin2; @@ -14,14 +34,9 @@ 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); +static void mpu_configure(void); -*/ void imu_impl_init(void) { @@ -33,44 +48,30 @@ void imu_impl_init(void) { 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(); + if (imu_aspirin2.status == Aspirin2StatusUninit) { + mpu_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; - } -*/ } } @@ -79,7 +80,8 @@ static inline void mpu_set(uint8_t _reg, uint8_t _val) aspirin2_mpu60x0.mosi_buf[0] = _reg; aspirin2_mpu60x0.mosi_buf[1] = _val; spi_rw(&aspirin2_mpu60x0); - while(aspirin2_mpu60x0.status != SPITransSuccess); + + while(aspirin2_mpu60x0.status != SPITransSuccess); } static inline void mpu_wait_slave4_ready(void) @@ -90,7 +92,7 @@ static inline void mpu_wait_slave4_ready(void) aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_I2C_SLV4_CTRL | MPU60X0_SPI_READ ; aspirin2_mpu60x0.mosi_buf[1] = 0; spi_rw(&aspirin2_mpu60x0); - while(aspirin2_mpu60x0.status != SPITransSuccess); + while(aspirin2_mpu60x0.status != SPITransSuccess); ret = aspirin2_mpu60x0.miso_buf[1]; } @@ -98,7 +100,7 @@ static inline void mpu_wait_slave4_ready(void) -static void configure(void) +static void mpu_configure(void) { aspirin2_mpu60x0.length = 2; @@ -174,10 +176,10 @@ static void configure(void) // Enable the aux i2c mpu_set( MPU60X0_REG_I2C_MST_CTRL, (0 << 7) | // no multimaster - (0 << 6) | // do not delay IRQ waiting for all external slaves - (0 << 5) | // no slave 3 FIFO - (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start - (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz + (0 << 6) | // do not delay IRQ waiting for all external slaves + (0 << 5) | // no slave 3 FIFO + (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start + (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz // HMC5883 Magnetometer Configuration @@ -223,7 +225,7 @@ static void configure(void) (0 << 6) | // Byte Swap (6 << 0) ); // Read 6 bytes - // Slave 0 Control: + // Slave 0 Control: #endif diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h index d7ac3076cd..b00f37ca89 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin2.h +++ b/sw/airborne/subsystems/imu/imu_aspirin2.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2010 Antoine Drouin + * Copyright (C) 2012 Christophe DeWagter * * This file is part of paparazzi. * @@ -21,8 +19,8 @@ * Boston, MA 02111-1307, USA. */ -#ifndef IMU_ASPIRIN_H -#define IMU_ASPIRIN_H +#ifndef IMU_ASPIRIN_2_H +#define IMU_ASPIRIN_2_H #include "generated/airframe.h" #include "subsystems/imu.h" @@ -103,36 +101,11 @@ struct ImuAspirin2 { 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, Mx, My, Mz; @@ -165,16 +138,17 @@ static inline void imu_from_buff(void) #endif + //FIXME, remove it or use it... +#if 0 // Is this is new data #define MPU_OFFSET_STATUS 1 - if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01) - { + if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01) { //gyr_valid = TRUE; //acc_valid = TRUE; } - else - { + else { } +#endif } @@ -182,11 +156,7 @@ static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _acc { if (imu_aspirin2.status == Aspirin2StatusUninit) return; - // imu_aspirin2_arch_int_disable(); - - if (imu_aspirin2.imu_available) - { - imu_aspirin2.time_since_last_reading = 0; + if (imu_aspirin2.imu_available) { imu_aspirin2.imu_available = FALSE; imu_from_buff(); @@ -194,18 +164,10 @@ static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _acc _accel_handler(); _mag_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 */ +#endif /* IMU_ASPIRIN_2_H */