diff --git a/conf/airframes/TUDELFT/tudelft_iris_indi.xml b/conf/airframes/TUDELFT/tudelft_iris_indi.xml index 762b66977a..783465b35b 100644 --- a/conf/airframes/TUDELFT/tudelft_iris_indi.xml +++ b/conf/airframes/TUDELFT/tudelft_iris_indi.xml @@ -26,7 +26,11 @@ - + + + + + @@ -80,23 +84,33 @@ + + + + +
- - - - - - - - - + + + + + + + + + + + + + +
diff --git a/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile index a0a54993e2..9c3f6613ca 100644 --- a/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile +++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile @@ -2,7 +2,7 @@ # # PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto # -#MPU6000 + L3GD20 + LSM303D + HMC5883 +# L3GD20 + LSM303D include $(CFG_SHARED)/spi_master.makefile @@ -11,19 +11,12 @@ IMU_CFLAGS = -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\" IMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c IMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c -# MPU -IMU_SRCS += peripherals/mpu60x0.c -IMU_SRCS += peripherals/mpu60x0_spi.c - #L3GD20 gyro IMU_SRCS += peripherals/l3gd20_spi.c #LSM303D accelero + magneto IMU_SRCS += peripherals/lsm303dlhc_spi.c -# Magnetometer -IMU_SRCS += peripherals/hmc58xx.c - # for fixedwing firmware and ap only ifeq ($(TARGET), ap) IMU_CFLAGS += -DUSE_IMU @@ -37,11 +30,6 @@ IMU_SPI_DEV_LOWER=$(shell echo $(IMU_SPI_DEV) | tr A-Z a-z) IMU_CFLAGS += -DIMU_SPI_DEV=$(IMU_SPI_DEV_LOWER) IMU_CFLAGS += -DUSE_$(IMU_SPI_DEV_UPPER) -#********** MPU6000 *********** -IMU_MPU_SPI_SLAVE_IDX ?= SPI_SLAVE2 -IMU_CFLAGS += -DIMU_MPU_SPI_SLAVE_IDX=$(IMU_MPU_SPI_SLAVE_IDX) -IMU_CFLAGS += -DUSE_$(IMU_MPU_SPI_SLAVE_IDX) - #********** L3GD20 *********** IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0 IMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX) @@ -52,15 +40,6 @@ IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1 IMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX) IMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX) -#********** HMC5883 *********** -IMU_HMC_I2C_DEV ?= i2c1 - -# convert i2cx to upper/lower case -IMU_HMC_I2C_DEV_UPPER=$(shell echo $(IMU_HMC_I2C_DEV) | tr a-z A-Z) -IMU_HMC_I2C_DEV_LOWER=$(shell echo $(IMU_HMC_I2C_DEV) | tr A-Z a-z) -IMU_CFLAGS += -DIMU_HMC_I2C_DEV=$(IMU_HMC_I2C_DEV_LOWER) -IMU_CFLAGS += -DUSE_$(IMU_HMC_I2C_DEV_UPPER) - # add it for all targets except sim, fbw and nps ifeq (,$(findstring $(TARGET),sim fbw nps)) $(TARGET).CFLAGS += $(IMU_CFLAGS) diff --git a/sw/airborne/peripherals/l3gd20_regs.h b/sw/airborne/peripherals/l3gd20_regs.h index 6dd9a570a5..8247c58de6 100644 --- a/sw/airborne/peripherals/l3gd20_regs.h +++ b/sw/airborne/peripherals/l3gd20_regs.h @@ -80,7 +80,7 @@ enum L3gd20FullScale { L3GD20_FS_250dps = 0, L3GD20_FS_500dps = 1, L3GD20_FS_2000dps = 2, - L3GD20_FS_2000dps2 = 3, + L3GD20_FS_2000dps2 = 3, //yep, the same as L3GD20_FS_2000dps }; diff --git a/sw/airborne/peripherals/lsm303dlhc_spi.c b/sw/airborne/peripherals/lsm303dlhc_spi.c index 4bda0c929a..8a8f299cd2 100644 --- a/sw/airborne/peripherals/lsm303dlhc_spi.c +++ b/sw/airborne/peripherals/lsm303dlhc_spi.c @@ -35,7 +35,7 @@ #endif #ifndef LSM303DLHC_DEFAULT_AFS -#define LSM303DLHC_DEFAULT_AFS 0x00 // acc +- 2G +#define LSM303DLHC_DEFAULT_AFS (0x04 <<3) // acc +- 16G #endif #ifndef LSM303DLHC_DEFAULT_MODR diff --git a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c index 9641f32cf8..f9be617f1f 100644 --- a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c +++ b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013-2015 Felix Ruess + * Copyright (C) 2013-2016 the paparazzi team * * This file is part of paparazzi. * @@ -19,74 +19,29 @@ * Boston, MA 02111-1307, USA. */ + /** * @file subsystems/imu/imu_px4fmu_v2.4.h * Driver for pixhawk IMU's. - * On with spi: L3GD20H + LSM303D and the MPU6000. - * On i2c: external HMC5883L (through 3dr gps). + * L3GD20H + LSM303D (both on spi) */ #include "subsystems/imu.h" #include "subsystems/abi.h" #include "mcu_periph/spi.h" -#include "peripherals/hmc58xx_regs.h" #include "peripherals/l3gd20_regs.h" #include "peripherals/lsm303dlhc_regs.h" #include "peripherals/lsm303dlhc_spi.h" -/************MPU6000*****************/ /* SPI defaults set in subsystem makefile, can be configured from airframe file */ -PRINT_CONFIG_VAR(IMU_MPU_SPI_SLAVE_IDX) +PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX) +PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX) PRINT_CONFIG_VAR(IMU_SPI_DEV) -/* MPU60x0 gyro/accel internal lowpass frequency */ -#if !defined IMU_MPU_LOWPASS_FILTER && !defined IMU_MPU_SMPLRT_DIV -#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) -/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms - * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz - */ -#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ -#define IMU_MPU_SMPLRT_DIV 9 -PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") -#elif PERIODIC_FREQUENCY == 512 -/* Accelerometer: Bandwidth 260Hz, Delay 0ms - * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz - */ -#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_256HZ -#define IMU_MPU_SMPLRT_DIV 3 -PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") -#else -#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV. -#endif -#endif -PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER) -PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV) - -#ifndef IMU_MPU_GYRO_RANGE -#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 -#endif -PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE) - -#ifndef IMU_MPU_ACCEL_RANGE -#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G -#endif -PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE) - -/************HMC58XX*****************/ -PRINT_CONFIG_VAR(IMU_HMC_I2C_DEV) - - struct ImuPX4 imu_px4; void imu_impl_init(void) { - /* MPU6000 init */ - mpu60x0_spi_init(&imu_px4.mpu, &IMU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX); - // change the default configuration - imu_px4.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV; - imu_px4.mpu.config.dlpf_cfg = IMU_MPU_LOWPASS_FILTER; - imu_px4.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE; - imu_px4.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE; /* L3GD20 gyro init */ /* initialize gyro and set default options */ @@ -94,25 +49,23 @@ void imu_impl_init(void) /* LSM303dlhc acc + magneto init */ lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC); +#if MODULE_HMC58XX_UPDATE_AHRS lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG); - - /* HMC58XX magneto init */ - /* initialize mag and set default options */ - hmc58xx_init(&imu_px4.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR); +#endif } void imu_periodic(void) { - mpu60x0_spi_periodic(&imu_px4.mpu); l3gd20_spi_periodic(&imu_px4.l3g); lsm303dlhc_spi_periodic(&imu_px4.lsm_acc); +#if MODULE_HMC58XX_UPDATE_AHRS /* Read magneto's every 10 times of main freq * at ~50Hz (main loop for rotorcraft: 512Hz) */ - RunOnceEvery(10, hmc58xx_periodic(&imu_px4.hmc)); RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag)); +#endif } void imu_px4_event(void) @@ -120,22 +73,13 @@ void imu_px4_event(void) uint32_t now_ts = get_sys_time_usec(); - /* MPU6000 event task */ - mpu60x0_spi_event(&imu_px4.mpu); - if (imu_px4.mpu.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_px4.mpu.data_rates.rates); - VECT3_COPY(imu.accel_unscaled, imu_px4.mpu.data_accel.vect); - imu_px4.mpu.data_available = FALSE; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel); - } - /* L3GD20 event task */ l3gd20_spi_event(&imu_px4.l3g); if (imu_px4.l3g.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_px4.l3g.data_rates.rates); + //the p and q seem to be swapped on the Pixhawk board compared to the acc + imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q; + imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p; + imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r; imu_px4.l3g.data_available = FALSE; imu_scale_gyro(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro); @@ -143,28 +87,20 @@ void imu_px4_event(void) /* LSM303dlhc event task */ lsm303dlhc_spi_event(&imu_px4.lsm_acc); - if (imu_px4.lsm_acc.data_available_acc) { + if (imu_px4.lsm_acc.data_available_acc) { VECT3_COPY(imu.accel_unscaled, imu_px4.lsm_acc.data_accel.vect); imu_px4.lsm_acc.data_available_acc = FALSE; imu_scale_accel(&imu); AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel); } +#if MODULE_HMC58XX_UPDATE_AHRS lsm303dlhc_spi_event(&imu_px4.lsm_mag); if (imu_px4.lsm_mag.data_available_mag) { VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect); imu_px4.lsm_mag.data_available_mag = FALSE; imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag); - } - - /* HMC58XX event task */ - hmc58xx_event(&imu_px4.hmc); - if (imu_px4.hmc.data_available) { - /* mag rotated by 90deg around z axis relative to MPU */ - VECT3_COPY(imu.mag_unscaled, imu_px4.hmc.data.vect); - imu_px4.hmc.data_available = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag); } +#endif } diff --git a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h index 234995860d..135b5b3bfa 100644 --- a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h +++ b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Felix Ruess + * Copyright (C) 2013-2016 the paparazzi team * * This file is part of paparazzi. * @@ -20,10 +20,9 @@ */ /** - * @file subsystems/imu/imu_mpu6000.h + * @file subsystems/imu/imu_px4fmu_v2.4.h * Driver for pixhawk IMU's. - * On with spi: L3GD20H + LSM303D and the MPU6000. - * On i2c: external HMC5883L (through 3dr gps). + * L3GD20H + LSM303D (both on spi) */ #ifndef IMU_PX4FMUV24_H @@ -33,16 +32,12 @@ #include "generated/airframe.h" #include "subsystems/imu.h" -#include "subsystems/imu/imu_mpu60x0_defaults.h" -#include "peripherals/mpu60x0_spi.h" -#include "peripherals/hmc58xx.h" +#include "subsystems/imu/imu_px4_defaults.h" #include "peripherals/l3gd20_spi.h" #include "peripherals/lsm303dlhc_spi.h" struct ImuPX4 { - struct Mpu60x0_Spi mpu; - struct Hmc58xx hmc; struct L3gd20_Spi l3g; struct Lsm303dlhc_Spi lsm_acc; struct Lsm303dlhc_Spi lsm_mag;