[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.
paparazzi · Aug 31, 2014 · 49f74e8 · 49f74e8
1 parent 06b06a1
commit 49f74e8
Show file tree

Hide file tree

Showing 5 changed files with 120 additions and 12 deletions.
diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml
@@ -30,7 +30,7 @@
     </subsystem>
 
     <subsystem name="telemetry"     type="transparent"/>
-    <subsystem name="imu"           type="aspirin_v2.2"/>
+    <subsystem name="imu"           type="lisa_mx_v2.1"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization" type="int_quat"/>
     <subsystem name="ahrs"          type="float_mlkf"/>

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
@@ -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:
+#  <section name="IMU" prefix="IMU_">
+#
+#    <!-- these gyro and accel calib values are the defaults for aspirin2.1/2.2 -->
+#    <define name="GYRO_X_NEUTRAL" value="0"/>
+#    <define name="GYRO_Y_NEUTRAL" value="0"/>
+#    <define name="GYRO_Z_NEUTRAL" value="0"/>
+#
+#    <define name="GYRO_X_SENS" value="4.359" integer="16"/>
+#    <define name="GYRO_Y_SENS" value="4.359" integer="16"/>
+#    <define name="GYRO_Z_SENS" value="4.359" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="0"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="0"/>
+#
+#    <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
+#
+#    <!-- replace the mag calibration with your own-->
+#    <define name="MAG_X_NEUTRAL" value="-45"/>
+#    <define name="MAG_Y_NEUTRAL" value="334"/>
+#    <define name="MAG_Z_NEUTRAL" value="7"/>
+#
+#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
+#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
+#
+#  </section>
+#
+#
+
+
+# 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
@@ -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
@@ -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
@@ -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;