AP_Arming: check min heater temp

libraries/AP_Arming/AP_Arming.cpp

@@ -841,6 +841,26 @@ bool AP_Arming::board_voltage_checks(bool report)
     return true;
 }
 
+#if HAL_HAVE_IMU_HEATER
+bool AP_Arming::heater_min_temperature_checks(bool report)
+{
+    if (checks_to_perform & ARMING_CHECK_ALL) {
+        AP_BoardConfig *board = AP::boardConfig();
+        if (board) {
+            float temperature;
+            int8_t min_temperature;
+            if (board->get_board_heater_temperature(temperature) &&
+                board->get_board_heater_arming_temperature(min_temperature) &&
+                (temperature < min_temperature)) {
+                check_failed(ARMING_CHECK_SYSTEM, report, "heater temp low (%0.1f < %i)", temperature, min_temperature);
+                return false;
+            }
+        }
+    }
+    return true;
+}
+#endif // HAL_HAVE_IMU_HEATER
+
 /*
   check base system operations
  */
@@ -1231,6 +1251,9 @@ bool AP_Arming::pre_arm_checks(bool report)
 #endif
 
     return hardware_safety_check(report)
+#if HAL_HAVE_IMU_HEATER
+        &  heater_min_temperature_checks(report)
+#endif
         &  barometer_checks(report)
         &  ins_checks(report)
         &  compass_checks(report)

libraries/AP_Arming/AP_Arming.h

@@ -168,6 +168,10 @@ class AP_Arming {
 
     bool fence_checks(bool report);
 
+#if HAL_HAVE_IMU_HEATER
+    bool heater_min_temperature_checks(bool report);
+#endif
+
     bool camera_checks(bool display_failure);
 
     bool osd_checks(bool display_failure) const;

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -47,6 +47,10 @@
 #define HAL_IMU_TEMP_DEFAULT       -1 // disabled
 #endif
 
+#ifndef HAL_IMU_TEMP_MARGIN_LOW_DEFAULT
+#define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 0 // disabled
+#endif
+
 #ifndef BOARD_SAFETY_OPTION_DEFAULT
 #  define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
 #endif
@@ -306,6 +310,16 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     AP_GROUPINFO("ALT_CONFIG", 22, AP_BoardConfig, _alt_config, 0),
 #endif // HAL_PIN_ALT_CONFIG
 
+#if HAL_HAVE_IMU_HEATER
+    // @Param: TEMPMGN_LOW
+    // @DisplayName: hearter temp lower margin
+    // @Description: Arming check will fail if IMU temp is more than this value lower than BRD_IMU_TARGTEMP, 0 disables
+    // @Range: 0 20
+    // @Units: degC
+    // @User: Advanced
+    AP_GROUPINFO("TEMPMGN_LOW", 23, AP_BoardConfig, heater.imu_arming_temperature_margin_low, HAL_IMU_TEMP_MARGIN_LOW_DEFAULT),
+#endif
+
     AP_GROUPEND
 };
 

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -204,6 +204,10 @@ class AP_BoardConfig {
     float get_heater_duty_cycle(void) const {
         return heater.output;
     }
+
+    // getters for current temperature and min arming temperature, return false if heater disabled
+    bool get_board_heater_temperature(float &temperature) const;
+    bool get_board_heater_arming_temperature(int8_t &temperature) const;
 #endif
 
 private:
@@ -255,6 +259,8 @@ class AP_BoardConfig {
         float sum;
         float output;
         uint32_t last_log_ms;
+        float temperature;
+        AP_Int8 imu_arming_temperature_margin_low;
     } heater;
 #endif
 

libraries/AP_BoardConfig/IMU_heater.cpp

@@ -66,14 +66,14 @@ void AP_BoardConfig::set_imu_temp(float current)
 
     heater.last_update_ms = now;
 
-    float avg = heater.sum / heater.count;
+    heater.temperature = heater.sum / heater.count;
     heater.sum = 0;
     heater.count = 0;
 
     if (target < 0) {
         heater.output = 0;
     } else {
-        heater.output = heater.pi_controller.update(avg, target, dt);
+        heater.output = heater.pi_controller.update(heater.temperature, target, dt);
         heater.output = constrain_float(heater.output, 0, 100);
     }
 
@@ -88,7 +88,7 @@ void AP_BoardConfig::set_imu_temp(float current)
 // @Field: Out: Controller output to heating element
         AP::logger().WriteStreaming("HEAT", "TimeUS,Temp,Targ,P,I,Out", "Qfbfff",
                            AP_HAL::micros64(),
-                           avg, target,
+                           heater.temperature, target,
                            heater.pi_controller.get_P(),
                            heater.pi_controller.get_I(),
                            heater.output);
@@ -111,4 +111,24 @@ void AP_BoardConfig::set_imu_temp(float current)
 #endif
 }
 
+// getter for current temperature, return false if heater disabled
+bool AP_BoardConfig::get_board_heater_temperature(float &temperature) const
+{
+    if (heater.imu_target_temperature == -1) {
+        return false; // heater disabled
+    }
+    temperature = heater.temperature;
+    return true;
+}
+
+// getter for min arming temperature, return false if heater disabled or min check disabled
+bool AP_BoardConfig::get_board_heater_arming_temperature(int8_t &temperature) const
+{
+    if ((heater.imu_target_temperature == -1) || (heater.imu_arming_temperature_margin_low == 0)) {
+        return false; // heater or temperature check disabled
+    }
+    temperature = heater.imu_target_temperature - heater.imu_arming_temperature_margin_low;
+    return true;
+}
+
 #endif // HAL_HAVE_IMU_HEATER

libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat

@@ -96,3 +96,6 @@ PE3 VDD_3V3_SENSORS_EN OUTPUT LOW
 # in MAVLink2 or in PM message in logs
 # undef HAL_STORAGE_SIZE
 # define HAL_STORAGE_SIZE 32768
+
+
+define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat

@@ -303,6 +303,7 @@ define HAL_HAVE_IMU_HEATER 1
 define HAL_IMU_TEMP_DEFAULT 45
 define HAL_IMUHEAT_P_DEFAULT 50
 define HAL_IMUHEAT_I_DEFAULT 0.07
+define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
 
 # Enable FAT filesystem support (needs a microSD defined via SDMMC).
 define HAL_OS_FATFS_IO 1

libraries/AP_HAL_ChibiOS/hwdef/CubePurple/hwdef.dat

@@ -31,3 +31,5 @@ COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90
 
 # also probe for external compasses
 define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+
+define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5

libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat

@@ -48,3 +48,4 @@ COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90
 define HAL_PROBE_EXTERNAL_I2C_COMPASSES
 
 define HAL_OREO_LED_ENABLED 1
+define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5

libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat

@@ -441,6 +441,7 @@ define HAL_WITH_RAMTRON 1
 # setup for the possibility of an IMU heater as the The CUBE has
 # an IMU header
 define HAL_HAVE_IMU_HEATER 1
+define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
 
 # enable FAT filesystem support (needs a microSD defined via SDIO)
 define HAL_OS_FATFS_IO 1