[checks] Add IMU heater preflight check and fix minor bugs (#3193)

paparazzi · Dec 4, 2023 · 12c6524 · 12c6524
1 parent cd3c2af
commit 12c6524
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 14 deletions.
diff --git a/conf/modules/imu_heater.xml b/conf/modules/imu_heater.xml
@@ -5,13 +5,16 @@
     <description>
       IMU Heater through a resistor or IO co-processor
     </description>
-    <define name="IMU_HEATER_TARGET_TEMP" value="55." description="Target temperature" unit="Celcius"/>
-    <define name="IMU_HEATER_P_GAIN" value="200." description="kP gain for the heater" unit="%/degC"/>
-    <define name="IMU_HEATER_I_GAIN" value="0.3" description="kI gain for the heater"/>
-    <define name="IMU_HEATER_GYRO_ID" value="ABI_BROADCAST" description="Gyro ABI id for the temperature measurement"/>
-    <define name="IMU_HEATER_ACCEL_ID" description="Accel ABI id for the temperature measurement"/>
-    <define name="IMU_HEATER_GPIO" description="Heater GPIO port for resistor activation"/>
-    <define name="IMU_HEATER_GPIO_PIN" description="Heater GPIO pin for resistor activation"/>
+    <section name="IMU_HEATER"  prefix="IMU_HEATER_">
+      <define name="TARGET_TEMP" value="55." description="Target temperature" unit="Celcius"/>
+      <define name="P_GAIN" value="200." description="kP gain for the heater" unit="%/degC"/>
+      <define name="I_GAIN" value="0.3" description="kI gain for the heater"/>
+      <define name="GYRO_ID" value="ABI_BROADCAST" description="Gyro ABI id for the temperature measurement"/>
+      <define name="ACCEL_ID" description="Accel ABI id for the temperature measurement"/>
+      <define name="GPIO" description="Heater GPIO port for resistor activation"/>
+      <define name="GPIO_PIN" description="Heater GPIO pin for resistor activation"/>
+      <define name="MAX_ERROR" value="0.15" description="Maximum error for the heater preflight check" unit="percentile"/>
+    </section>
     <define name="INTERMCU_IOMCU" description="Heater IOMCU communication enabled"/>
   </doc>
   <settings>

diff --git a/sw/airborne/modules/checks/preflight_checks.c b/sw/airborne/modules/checks/preflight_checks.c
@@ -30,7 +30,7 @@
 
 /** Maximum combined message size for storing the errors */
 #ifndef PREFLIGHT_CHECK_MAX_MSGBUF
-#define PREFLIGHT_CHECK_MAX_MSGBUF 512
+#define PREFLIGHT_CHECK_MAX_MSGBUF 1024
 #endif
 
 /** Seperating character for storing the errors */
@@ -94,19 +94,19 @@ bool preflight_check(void)
       // Record the total
       int rc = 0;
       if (result.fail_cnt > 0) {
-        rc = snprintf(result.message, result.max_len, "Preflight fail [fail:%d warn:%d tot:%d]", result.fail_cnt,
+        rc = snprintf(result.message, result.max_len, "*Preflight fail [fail:%d warn:%d tot:%d]*", result.fail_cnt,
                       result.warning_cnt, (result.fail_cnt + result.warning_cnt + result.success_cnt));
       } else {
-        rc = snprintf(result.message, result.max_len, "Preflight success with warnings [%d/%d]", result.warning_cnt,
+        rc = snprintf(result.message, result.max_len, "*Preflight success with warnings [%d/%d]*", result.warning_cnt,
                       (result.fail_cnt + result.warning_cnt + result.success_cnt));
       }
       if (rc > 0) {
         result.max_len -= rc;
       }
 
       // Send the errors seperatly
-      uint8_t last_sendi = 0;
-      for (uint8_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
+      uint16_t last_sendi = 0;
+      for (uint16_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
         if (error_msg[i] == PREFLIGHT_CHECK_SEPERATOR || i == (PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len)) {
           DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, i - last_sendi, &error_msg[last_sendi]);
           last_sendi = i + 1;

diff --git a/sw/airborne/modules/imu/imu_heater.c b/sw/airborne/modules/imu/imu_heater.c
@@ -74,6 +74,32 @@ static void send_imu_heater(struct transport_tx *trans, struct link_device *dev)
 }
 #endif
 
+#ifdef PREFLIGHT_CHECKS
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t imu_heater_pfc;
+
+/** Maximum error in percentile */
+#ifndef IMU_HEATER_MAX_ERROR
+#define IMU_HEATER_MAX_ERROR 0.15f
+#endif
+
+/** ABI id from either guro or accel */
+#if defined(IMU_HEATER_GYRO_ID)
+#define IMU_HEATER_ABI_ID IMU_HEATER_GYRO_ID
+#elif defined(IMU_HEATER_ACCEL_ID) 
+#define IMU_HEATER_ABI_ID IMU_HEATER_ACCEL_ID
+#endif
+
+static void imu_heater_preflight(struct preflight_result_t *result) {
+  if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
+    preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
+  } else {
+    preflight_success(result, "IMU %d temperature ok", IMU_HEATER_ABI_ID);
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 #if defined(IMU_HEATER_GYRO_ID)
 static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Rates *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float rate __attribute__((unused)), float temp) {
   if(isnan(temp))
@@ -121,6 +147,11 @@ void imu_heater_init(void)
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_HEATER, send_imu_heater);
 #endif
+
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS
+  preflight_check_register(&imu_heater_pfc, imu_heater_preflight);
+#endif 
 }
 
 /**

diff --git a/sw/airborne/modules/loggers/sdlog_chibios.c b/sw/airborne/modules/loggers/sdlog_chibios.c
@@ -114,9 +114,9 @@ static struct preflight_check_t sdlog_pfc;
 static void sdlog_preflight(struct preflight_result_t *result) {
   if(chibios_sdlog_status != SDLOG_RUNNING) {
 #ifdef SDLOG_PREFLIGHT_ERROR
-    preflight_error(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
+    preflight_error(result, "SDLogger is not running [%d:%d]", chibios_sdlog_status, sdLogGetStorageStatus());
 #else
-    preflight_warning(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
+    preflight_warning(result, "SDLogger is not running [%d:%d]", chibios_sdlog_status, sdLogGetStorageStatus());
 #endif
   } else {
     preflight_success(result, "SDLogger running");