From 49f74e82385073c386940d91505c3214fedc81a7 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Fri, 29 Aug 2014 19:14:56 -0700 Subject: [PATCH] [Lisa/M] [Lisa/MX] Added IMU subsystems. The Lisa/M and Lisa/MX V2.1 have an integrated Aspirin like IMU. The orientation of the sensors is bit different than the Aspirin so for backwards compatibility we need to assign the axis appropriately, to do that we add IMU subsystem makefiles and one additianal raw sensor data field assignement. --- conf/airframes/examples/quadrotor_lisa_mx.xml | 2 +- .../shared/imu_lisa_m_or_mx_v2.1.makefile | 75 +++++++++++++++++++ .../shared/imu_lisa_m_v2.1.makefile | 6 ++ .../shared/imu_lisa_mx_v2.1.makefile | 6 ++ .../subsystems/imu/imu_aspirin_2_spi.c | 43 ++++++++--- 5 files changed, 120 insertions(+), 12 deletions(-) create mode 100644 conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile create mode 100644 conf/firmwares/subsystems/shared/imu_lisa_m_v2.1.makefile create mode 100644 conf/firmwares/subsystems/shared/imu_lisa_mx_v2.1.makefile diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml index 6f566f181d4..b391cca192b 100644 --- a/conf/airframes/examples/quadrotor_lisa_mx.xml +++ b/conf/airframes/examples/quadrotor_lisa_mx.xml @@ -30,7 +30,7 @@ - + diff --git a/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile new file mode 100644 index 00000000000..2dc6b1b3208 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile @@ -0,0 +1,75 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# The IMU system integrated into Lisa/MX V2.1 based on Aspirin V2.2. Major +# difference is that the orientation of the chips is bit different and we need +# to compensate for that. +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU +endif + +IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\" +IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c + +include $(CFG_SHARED)/spi_master.makefile + +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE2 +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi2 + +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 -DLISA_M_OR_MX_21 +# Slave select configuration +# SLAVE2 is on PB12 (NSS) (MPU600 CS) +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2 + +# SLAVE3 is on PC13, which is the baro CS +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE3 + +ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_2_SRCS) + +test_imu.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +test_imu.srcs += $(IMU_ASPIRIN_2_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/firmwares/subsystems/shared/imu_lisa_m_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_m_v2.1.makefile new file mode 100644 index 00000000000..90d4c5ca36e --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_lisa_m_v2.1.makefile @@ -0,0 +1,6 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# As Lisa/M and Lisa/MX have the same configuration of the IMU this file +# serves as an alias to make the airframe file look consistant. + +include $(CFG_SHARED)/imu_lisa_m_or_mx_v2.1.makefile diff --git a/conf/firmwares/subsystems/shared/imu_lisa_mx_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_mx_v2.1.makefile new file mode 100644 index 00000000000..90d4c5ca36e --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_lisa_mx_v2.1.makefile @@ -0,0 +1,6 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# As Lisa/M and Lisa/MX have the same configuration of the IMU this file +# serves as an alias to make the airframe file look consistant. + +include $(CFG_SHARED)/imu_lisa_m_or_mx_v2.1.makefile diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index cdc2e2eef49..18fff6b991d 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -168,17 +168,8 @@ void imu_aspirin2_event(void) mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); -#ifdef LISA_M_LONGITUDINAL_X - RATES_ASSIGN(imu.gyro_unscaled, - imu_aspirin2.mpu.data_rates.rates.q, - -imu_aspirin2.mpu.data_rates.rates.p, - imu_aspirin2.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, - imu_aspirin2.mpu.data_accel.vect.y, - -imu_aspirin2.mpu.data_accel.vect.x, - imu_aspirin2.mpu.data_accel.vect.z); - VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); -#else + +/* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */ #ifdef LISA_S #ifdef LISA_S_UPSIDE_DOWN RATES_ASSIGN(imu.gyro_unscaled, @@ -195,12 +186,42 @@ void imu_aspirin2_event(void) VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_COPY(imu.mag_unscaled, mag); #endif +#else + +/* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like + * IMU. + */ +#ifdef LISA_M_OR_MX_21 + RATES_ASSIGN(imu.gyro_unscaled, + -imu_aspirin2.mpu.data_rates.rates.q, + imu_aspirin2.mpu.data_rates.rates.p, + imu_aspirin2.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + -imu_aspirin2.mpu.data_accel.vect.y, + imu_aspirin2.mpu.data_accel.vect.x, + imu_aspirin2.mpu.data_accel.vect.z); + VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z); +#else + +/* Handle real Aspirin IMU axis assignement. */ +#ifdef LISA_M_LONGITUDINAL_X + RATES_ASSIGN(imu.gyro_unscaled, + imu_aspirin2.mpu.data_rates.rates.q, + -imu_aspirin2.mpu.data_rates.rates.p, + imu_aspirin2.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + imu_aspirin2.mpu.data_accel.vect.y, + -imu_aspirin2.mpu.data_accel.vect.x, + imu_aspirin2.mpu.data_accel.vect.z); + VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) #endif #endif +#endif + imu_aspirin2.mpu.data_available = FALSE; imu_aspirin2.gyro_valid = TRUE; imu_aspirin2.accel_valid = TRUE;