AP_BoardConfig: add min arming temp margin param and getter

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 @@ public:
     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 @@ private:
         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