[imu] Multi IMU support

paparazzi · Sep 20, 2022 · 7c7cfbc · 7c7cfbc
1 parent ad4654b
commit 7c7cfbc
Show file tree

Hide file tree

Showing 14 changed files with 478 additions and 217 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -22,17 +22,17 @@
       <field name="temp" type="float" unit="deg Celcius"/>
     </message>
 
-    <message name="IMU_GYRO_INT32" id="4">
+    <message name="IMU_GYRO" id="4">
       <field name="stamp" type="uint32_t" unit="us"/>
       <field name="gyro" type="struct Int32Rates *"/>
     </message>
 
-    <message name="IMU_ACCEL_INT32" id="5">
+    <message name="IMU_ACCEL" id="5">
       <field name="stamp" type="uint32_t" unit="us"/>
       <field name="accel" type="struct Int32Vect3 *"/>
     </message>
 
-    <message name="IMU_MAG_INT32" id="6">
+    <message name="IMU_MAG" id="6">
       <field name="stamp" type="uint32_t" unit="us"/>
       <field name="mag" type="struct Int32Vect3 *"/>
     </message>
@@ -215,6 +215,35 @@
       <field name="rc" type="struct RadioControl *">Pointer to a radio control structure</field>
     </message>
 
+    <message name="IMU_GYRO_RAW" id="31">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="gyro" type="struct Int32Rates *"/>
+      <field name="samples" type="uint8_t"/>
+    </message>
+
+    <message name="IMU_ACCEL_RAW" id="32">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="accel" type="struct Int32Vect3 *"/>
+      <field name="samples" type="uint8_t"/>
+    </message>
+
+    <message name="IMU_MAG_RAW" id="33">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="mag" type="struct Int32Vect3 *"/>
+    </message>
+
+    <message name="IMU_GYRO_INT" id="34">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="delta_gyro" type="struct FloatRates *"/>
+      <field name="delta_dt" type="uint16_t" unit="us"/>
+    </message>
+
+    <message name="IMU_ACCEL_INT" id="35">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="delta_accel" type="struct FloatVect3 *"/>
+      <field name="delta_dt" type="uint16_t" unit="us"/>
+    </message>
+
   </msg_class>
 
 </protocol>
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
@@ -87,7 +87,7 @@
     <module name="ins"           type="ekf2" />
 
     <module name="air_data"/>
-    <module name="send_imu_mag_current"/>
+    <!--module name="send_imu_mag_current"/-->
 
     <!-- Internal MAG -->
     <!--module name="mag_ist8310">

diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml
@@ -19,22 +19,31 @@
   <periodic fun="imu_cube_periodic()"/>
   <event fun="imu_cube_event()"/>
   <makefile target="!sim|nps|fbw">
+    <define name="IMU_TYPE_H" value="modules/imu/imu_cube.h" type="string"/>
+
     <!-- ICM20602 (isolated) -->
-    <!--configure name="CUBE_IMU1_SPI_DEV" default="spi4" case="lower|upper"/>
-    <configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE3"/-->
-    <!-- ICM20649 (isolated) -->
-    <!--configure name="CUBE_IMU1_SPI_DEV" default="spi4" case="lower|upper"/>
-    <configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE8"/-->
-    <!-- ICM20948 (non-isolated) -->
-    <configure name="CUBE_IMU1_SPI_DEV" default="spi1" case="lower|upper"/>
-    <configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE2"/>
-
+    <configure name="CUBE_IMU1_SPI_DEV" default="spi4" case="lower|upper"/>
+    <configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE3"/>
     <define name="CUBE_IMU1_SPI_DEV" value="$(CUBE_IMU1_SPI_DEV_LOWER)"/>
     <define name="USE_$(CUBE_IMU1_SPI_DEV_UPPER)"/>
     <define name="CUBE_IMU1_SPI_SLAVE_IDX" value="$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
     <define name="USE_$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
 
-    <define name="IMU_TYPE_H" value="modules/imu/imu_cube.h" type="string"/>
+    <!-- ICM20649 (isolated) -->
+    <configure name="CUBE_IMU2_SPI_DEV" default="spi4" case="lower|upper"/>
+    <configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE8"/>
+    <define name="CUBE_IMU2_SPI_DEV" value="$(CUBE_IMU2_SPI_DEV_LOWER)"/>
+    <define name="USE_$(CUBE_IMU2_SPI_DEV_UPPER)"/>
+    <define name="CUBE_IMU2_SPI_SLAVE_IDX" value="$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
+    <define name="USE_$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
+
+    <!-- ICM20948 (non-isolated) -->
+    <configure name="CUBE_IMU3_SPI_DEV" default="spi1" case="lower|upper"/>
+    <configure name="CUBE_IMU3_SPI_SLAVE_IDX" default="SPI_SLAVE2"/>
+    <define name="CUBE_IMU3_SPI_DEV" value="$(CUBE_IMU3_SPI_DEV_LOWER)"/>
+    <define name="USE_$(CUBE_IMU3_SPI_DEV_UPPER)"/>
+    <define name="CUBE_IMU3_SPI_SLAVE_IDX" value="$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
+    <define name="USE_$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
 
     <file name="invensense2.c" dir="peripherals"/>
     <file name="imu_cube.c"/>

diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h
@@ -372,7 +372,7 @@
 #endif
 
 #ifndef IMU_CUBE3_ID
-#define IMU_CUBE4_ID 22
+#define IMU_CUBE3_ID 22
 #endif
 
 // prefiltering with OneEuro filter