diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile deleted file mode 100644 index 446b84af59..0000000000 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile +++ /dev/null @@ -1,73 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Aspirin IMU v2.1 -# -# -# required xml: -#
-# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -#
-# -# - - -# for fixedwing firmware and ap only -ifeq ($(TARGET), ap) - IMU_ASPIRIN_CFLAGS = -DUSE_IMU -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c - -include $(CFG_SHARED)/spi_master.makefile - -ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 -else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 -# Slave select configuration -# SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 - -# 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) - - -# -# NPS simulator -# -include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c deleted file mode 100644 index b467e1bd35..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.c +++ /dev/null @@ -1,346 +0,0 @@ -/* - * 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" - -// Peripherials -#include "peripherals/mpu60x0_regs.h" -#include "peripherals/hmc58xx_regs.h" -#include "peripherals/ms5611.h" - -#ifndef MPU6000_SLAVE_IDX -#define MPU6000_SLAVE_IDX SPI_SLAVE2 -#endif - -#ifndef MPU6000_SPI_DEV -#define MPU6000_SPI_DEV spi2 -#endif - -/* HMC58XX default conf */ -#ifndef HMC58XX_DO -#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 50Hz with HMC5843, 75Hz with HMC5883) -#endif -#ifndef HMC58XX_MS -#define HMC58XX_MS 0x0 // Measurement configuration -#endif -#ifndef HMC58XX_GN -#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss) -#endif -#ifndef HMC58XX_MD -#define HMC58XX_MD 0x0 // Continious measurement mode -#endif - -#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS)) -#define HMC58XX_CRB (HMC58XX_GN<<5) - -struct ImuAspirin2 imu_aspirin2; - -struct spi_transaction aspirin2_mpu60x0; - -// initialize peripherals -static void mpu_configure(void); -static void trans_cb( struct spi_transaction *trans ); - -void imu_impl_init(void) { - aspirin2_mpu60x0.select = SPISelectUnselect; - aspirin2_mpu60x0.cpol = SPICpolIdleHigh; - aspirin2_mpu60x0.cpha = SPICphaEdge2; - aspirin2_mpu60x0.dss = SPIDss8bit; - aspirin2_mpu60x0.bitorder = SPIMSBFirst; - aspirin2_mpu60x0.cdiv = SPIDiv64; - aspirin2_mpu60x0.slave_idx = MPU6000_SLAVE_IDX; - aspirin2_mpu60x0.output_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.input_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.after_cb = trans_cb; - - imu_aspirin2.status = Aspirin2StatusUninit; - imu_aspirin2.imu_available = FALSE; - aspirin2_mpu60x0.input_buf = &imu_aspirin2.input_buf_p[0]; - aspirin2_mpu60x0.output_buf = &imu_aspirin2.output_buf_p[0]; -} - - -void imu_periodic(void) -{ - - if (imu_aspirin2.status == Aspirin2StatusUninit) { - mpu_configure(); - imu_aspirin2.status = Aspirin2StatusIdle; - - aspirin2_mpu60x0.output_length = 22; - aspirin2_mpu60x0.input_length = 22; - aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ; - for (int i=1; i6) - (MPU_DIG_FILTER << 0) ); // Low-Pass Filter - - // MPU60X0_REG_SMPLRT_DIV - mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV); - - // MPU60X0_REG_GYRO_CONFIG - mpu_set( MPU60X0_REG_GYRO_CONFIG, - (3 << 3) ); // -2000deg/sec - - // MPU60X0_REG_ACCEL_CONFIG - mpu_set( MPU60X0_REG_ACCEL_CONFIG, - (0 << 0) | // No HPFL - (3 << 3) ); // Full Scale = 16g - -#ifndef MPU6000_NO_SLAVES -PRINT_CONFIG_MSG("Reading MPU slaves") - - ///////////////////////////////////// - // SPI Slave Configuration Section - - // Power the Aux I2C Circuit: - // MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd - - // MPU60X0_REG_USER_CTRL: - mpu_set( MPU60X0_REG_USER_CTRL, - (1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode - (1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface - (0 << 1) ); // Trigger a I2C_MST_RESET - - // 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 - - mpu_set( MPU60X0_REG_I2C_MST_DELAY, - (0 << 2) | // No Delay Slave 2 - (1 << 3) ); // Delay Slave 3 - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE - - // MS5611 Send Reset - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, MS5611_REG_RESET); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Reg_Dis: do not write the register, just the data - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - // Wait at least 2.8ms - -#endif // read MS5611 as MPU slave - - // HMC5883 Magnetometer Configuration - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // From now on a delayed rate of 1/4 is defined... - - // HMC5883 Reading: - // a) write hmc-register to HMC - // b) read 6 bytes from HMC - - mpu_set( MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV0_CTRL, - (1 << 7) | // Slave 0 enable - (0 << 6) | // Byte Swap - (6 << 0) ); // Read 6 bytes - - // Slave 0 Control: - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE -PRINT_CONFIG_MSG("Reading the MS5611 as MPU slave") -/* - - - // Read MS5611 Calibration - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -*/ - - // Full Rate Request For Pressure - mpu_set( MPU60X0_REG_I2C_SLV2_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV2_DO, 0x48); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV2_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - // Reduced rate request For Temperature: Overwrites the Pressure Request - mpu_set( MPU60X0_REG_I2C_SLV3_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV3_DO, 0x58); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV3_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -#endif // read MS5611 as MPU slave - -#endif - -} - diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h deleted file mode 100644 index b55872f453..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * 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. - */ - -#ifndef IMU_ASPIRIN_2_H -#define IMU_ASPIRIN_2_H - -#include "generated/airframe.h" -#include "subsystems/imu.h" - - -#if defined IMU_ASPIRIN_VERSION_2_1 || defined IMU_ASPIRIN_VERSION_2_2 -#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 - }; - -#define IMU_ASPIRIN_BUFFER_LEN 32 - -struct ImuAspirin2 { - volatile enum Aspirin2Status status; - volatile uint8_t imu_available; - volatile uint8_t input_buf_p[IMU_ASPIRIN_BUFFER_LEN]; - volatile uint8_t output_buf_p[IMU_ASPIRIN_BUFFER_LEN]; -}; - -extern struct ImuAspirin2 imu_aspirin2; - - -static inline int imu_from_buff(volatile uint8_t *buf) -{ - int32_t x, y, z, p, q, r, Mx, My, Mz; - -#define MPU_OFFSET_STATUS 1 - if (!(buf[MPU_OFFSET_STATUS] & 0x01)) { - return 0; - } - -#define MPU_OFFSET_GYRO 10 - p = (int16_t) ((buf[0+MPU_OFFSET_GYRO] << 8) | buf[1+MPU_OFFSET_GYRO]); - q = (int16_t) ((buf[2+MPU_OFFSET_GYRO] << 8) | buf[3+MPU_OFFSET_GYRO]); - r = (int16_t) ((buf[4+MPU_OFFSET_GYRO] << 8) | buf[5+MPU_OFFSET_GYRO]); - -#define MPU_OFFSET_ACC 2 - x = (int16_t) ((buf[0+MPU_OFFSET_ACC] << 8) | buf[1+MPU_OFFSET_ACC]); - y = (int16_t) ((buf[2+MPU_OFFSET_ACC] << 8) | buf[3+MPU_OFFSET_ACC]); - z = (int16_t) ((buf[4+MPU_OFFSET_ACC] << 8) | buf[5+MPU_OFFSET_ACC]); - -#define MPU_OFFSET_MAG 16 - Mx = (int16_t) ((buf[0+MPU_OFFSET_MAG] << 8) | buf[1+MPU_OFFSET_MAG]); - My = (int16_t) ((buf[2+MPU_OFFSET_MAG] << 8) | buf[3+MPU_OFFSET_MAG]); - Mz = (int16_t) ((buf[4+MPU_OFFSET_MAG] << 8) | buf[5+MPU_OFFSET_MAG]); - -#ifdef LISA_M_LONGITUDINAL_X - RATES_ASSIGN(imu.gyro_unscaled, q, -p, r); - VECT3_ASSIGN(imu.accel_unscaled, y, -x, z); - VECT3_ASSIGN(imu.mag_unscaled, -Mx, -Mz, My); -#else - RATES_ASSIGN(imu.gyro_unscaled, p, q, r); - VECT3_ASSIGN(imu.accel_unscaled, x, y, z); - VECT3_ASSIGN(imu.mag_unscaled, Mz, -Mx, My); -#endif - - return 1; -} - - -static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) -{ - if (imu_aspirin2.status == Aspirin2StatusUninit) return; - - if (imu_aspirin2.imu_available) { - imu_aspirin2.imu_available = FALSE; - if (imu_from_buff(imu_aspirin2.input_buf_p)) { - _gyro_handler(); - _accel_handler(); - _mag_handler(); - } - } -} - -#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ - imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \ -} - -#endif /* IMU_ASPIRIN_2_H */