[imu] Enable the sending of specific IMU sensors (#3084)

paparazzi · Sep 21, 2023 · b11dcf7 · b11dcf7
1 parent f835b95
commit b11dcf7
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 12 deletions.
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
@@ -7,7 +7,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
    gui_color="blue"
   />
   <aircraft
@@ -18,7 +18,7 @@
    telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/versatile.xml"
    settings="settings/fixedwing_basic.xml"
-   settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/digital_cam.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/light.xml modules/nav_basic_fw.xml modules/nav_catapult.xml modules/stabilization_attitude_fw.xml"
+   settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/digital_cam.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/light.xml modules/nav_basic_fw.xml modules/nav_catapult.xml modules/stabilization_attitude_fw.xml"
    gui_color="blue"
   />
   <aircraft
@@ -29,7 +29,7 @@
    telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/versatile.xml"
    settings="settings/fixedwing_basic.xml settings/control/ctl_dash_loiter_trim.xml"
-   settings_modules="modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_attitude_fw.xml"
+   settings_modules="modules/electrical.xml modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_attitude_fw.xml"
    gui_color="blue"
   />
   <aircraft
@@ -40,7 +40,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/ahrs_float_mlkf.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
+   settings_modules="modules/ahrs_float_mlkf.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml"
    gui_color="white"
   />
   <aircraft
@@ -51,7 +51,7 @@
    telemetry="telemetry/fixedwing_flight_recorder.xml"
    flight_plan="flight_plans/basic.xml"
    settings="settings/fixedwing_basic.xml"
-   settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_attitude_fw.xml"
+   settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/logger_sd_chibios.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_attitude_fw.xml"
    gui_color="blue"
   />
   <aircraft
@@ -62,7 +62,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_optitrack.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/cv_colorfilter.xml modules/cv_opticflow.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/cv_colorfilter.xml modules/cv_opticflow.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
    gui_color="red"
   />
   <aircraft
@@ -73,7 +73,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
    gui_color="blue"
   />
   <aircraft

diff --git a/conf/modules/imu_common.xml b/conf/modules/imu_common.xml
@@ -7,14 +7,26 @@
 
       This takes the IMU_GYRO_RAW, IMU_ACCEL_RAW and IMU_MAG_RAW ABI messages as input.
     </description>
+    <section name="IMU" prefix="IMU_">
+      <define name="INTEGRATION" value="FALSE" description="Enable gyro/accel integration calculations (enabled for the ekf2)"/>
+      <define name="GYRO_CALIB" value="{}" description="Gyroscope calibration structures list (see struct imu_gyro_t)"/>
+      <define name="ACCEL_CALIB" value="{}" description="Accelerometer calibration structures list (see struct imu_accel_t)"/>
+      <define name="MAG_CALIB" value="{}" description="Magnetometer calibration structures list (see struct imu_mag_t)"/>
+      <define name="GYRO_ABI_SEND_ID" value="ABI_BROADCAST" description="The gyro ABI ID which is send over telemetry/logging"/>
+      <define name="ACCEL_ABI_SEND_ID" value="ABI_BROADCAST" description="The accel ABI ID which is send over telemetry/logging"/>
+      <define name="MAG_ABI_SEND_ID" value="ABI_BROADCAST" description="The mag ABI ID which is send over telemetry/logging"/>
+    </section>
   </doc>
   <settings>
     <dl_settings>
-      <dl_settings NAME="body2imu">
-        <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.phi" shortname="b2i phi" module="modules/imu/imu" param="IMU_BODY_TO_IMU_PHI" unit="rad" alt_unit="deg" handler="SetBodyToImuPhi" type="float" persistent="true"/>
-        <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.theta" shortname="b2i theta" module="modules/imu/imu" param="IMU_BODY_TO_IMU_THETA" unit="rad" alt_unit="deg" handler="SetBodyToImuTheta" type="float" persistent="true"/>
-        <dl_setting MAX="180" MIN="-180" STEP="0.5" VAR="imu.body_to_imu.eulers_f.psi" shortname="b2i psi" module="modules/imu/imu" param="IMU_BODY_TO_IMU_PSI" unit="rad" alt_unit="deg" handler="SetBodyToImuPsi" type="float" persistent="true"/>
-        <dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_set_current" values="FALSE|TRUE" shortname="b2i cur roll/pitch" module="modules/imu/imu" handler="SetBodyToImuCurrent"/>
+      <dl_settings name="IMU">
+        <dl_setting var="imu.body_to_imu.eulers_f.phi" shortname="b2i phi" min="-90" max="90" step="0.5"  module="modules/imu/imu" param="IMU_BODY_TO_IMU_PHI" unit="rad" alt_unit="deg" handler="SetBodyToImuPhi" type="float" persistent="true"/>
+        <dl_setting var="imu.body_to_imu.eulers_f.theta" shortname="b2i theta" min="-90" max="90" step="0.5"  module="modules/imu/imu" param="IMU_BODY_TO_IMU_THETA" unit="rad" alt_unit="deg" handler="SetBodyToImuTheta" type="float" persistent="true"/>
+        <dl_setting var="imu.body_to_imu.eulers_f.psi" shortname="b2i psi" min="-90" max="90" step="0.5" module="modules/imu/imu" param="IMU_BODY_TO_IMU_PSI" unit="rad" alt_unit="deg" handler="SetBodyToImuPsi" type="float" persistent="true"/>
+        <dl_setting var="imu.b2i_set_current" shortname="b2i cur roll/pitch" min="0" max="1" step="1" values="FALSE|TRUE" module="modules/imu/imu" handler="SetBodyToImuCurrent"/>
+        <dl_setting var="imu.gyro_abi_send_id" shortname="gyro abi send" min="0" max="255" step="1" param="IMU_GYRO_ABI_SEND_ID"/>
+        <dl_setting var="imu.accel_abi_send_id" shortname="accel abi send" min="0" max="255" step="1" param="IMU_ACCEL_ABI_SEND_ID"/>
+        <dl_setting var="imu.mag_abi_send_id" shortname="mag abi send" min="0" max="255" step="1" param="IMU_MAG_ABI_SEND_ID"/>
       </dl_settings>
     </dl_settings>
   </settings>

diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c
@@ -155,115 +155,173 @@ PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
 PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
 PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
 
+/** Which gyro measurements to send over telemetry/logging */
+#ifndef IMU_GYRO_ABI_SEND_ID
+#define IMU_GYRO_ABI_SEND_ID  ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(IMU_GYRO_ABI_SEND_ID)
+
+/** Which accel measurements to send over telemetry/logging */
+#ifndef IMU_ACCEL_ABI_SEND_ID
+#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)
+
+/** Which mag measurements to send over telemetry/logging */
+#ifndef IMU_MAG_ABI_SEND_ID
+#define IMU_MAG_ABI_SEND_ID   ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)
+
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
 
 static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.accel_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
                               &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
+  if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.accel_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
                                  &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
+  if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_accel(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.accel_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   struct FloatVect3 accel_float;
   ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
   pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
                           &accel_float.x, &accel_float.y, &accel_float.z);
+  if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.gyro_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
                              &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
+  if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.gyro_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
                                 &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
+  if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.gyro_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   struct FloatRates gyro_float;
   RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
   pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
                          &gyro_float.p, &gyro_float.q, &gyro_float.r);
+  if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.mag_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
                             &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
+  if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.mag_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
                                &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
+  if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_mag(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.mag_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   struct FloatVect3 mag_float;
   MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
   pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
                         &mag_float.x, &mag_float.y, &mag_float.z);
+  if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
     id = 0;
 }
 
 static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
 {
+  if(imu.mag_abi_send_id == ABI_DISABLE)
+    return;
   static uint8_t id = 0;
   pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
       &imu.mags[id].abi_id,
       &imu.mags[id].unscaled.x,
       &imu.mags[id].unscaled.y,
       &imu.mags[id].unscaled.z,
       &electrical.current);
+  if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
+    return;
   id++;
   if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
     id = 0;
@@ -396,6 +454,10 @@ void imu_init(void)
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
 #endif // DOWNLINK
 
+  // Set defaults
+  imu.gyro_abi_send_id = IMU_GYRO_ABI_SEND_ID;
+  imu.accel_abi_send_id = IMU_ACCEL_ABI_SEND_ID;
+  imu.mag_abi_send_id = IMU_MAG_ABI_SEND_ID;
   imu.initialized = true;
 }
 

diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h
@@ -87,6 +87,9 @@ struct Imu {
   struct imu_accel_t accels[IMU_MAX_SENSORS]; ///< The accelerometer sensors
   struct imu_mag_t mags[IMU_MAX_SENSORS];     ///< The magnetometer sensors
   struct OrientationReps body_to_imu;         ///< Rotation from body to imu (all sensors) frame
+  uint8_t gyro_abi_send_id;                   ///< Filter out and send only a specific ABI id in telemetry for the gyro
+  uint8_t accel_abi_send_id;                  ///< Filter out and send only a specific ABI id in telemetry for the accelerometer
+  uint8_t mag_abi_send_id;                    ///< Filter out and send only a specific ABI id in telemetry for the magnetometer
 
   /** flag for adjusting body_to_imu via settings.
    * if FALSE, reset to airframe values, if TRUE set current roll/pitch