AP_BoardConfig: moved heater PI to AP_BoardConfig

and made tunable

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -157,7 +157,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Range: -1 80
     // @Units: degC
     // @User: Advanced
-    AP_GROUPINFO("IMU_TARGTEMP", 8, AP_BoardConfig, _imu_target_temperature, HAL_IMU_TEMP_DEFAULT),
+    AP_GROUPINFO("IMU_TARGTEMP", 8, AP_BoardConfig, heater.imu_target_temperature, HAL_IMU_TEMP_DEFAULT),
 #endif
 
 #if AP_FEATURE_BOARD_DETECT
@@ -255,6 +255,30 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Units: ms
     // @User: Advanced
     AP_GROUPINFO("BOOT_DELAY", 20, AP_BoardConfig, _boot_delay_ms, HAL_DEFAULT_BOOT_DELAY),
+
+#if HAL_HAVE_IMU_HEATER
+    // @Param: IMUHEAT_P
+    // @DisplayName: IMU Heater P gain
+    // @Description: IMU Heater P gain
+    // @Range: 1 500
+    // @Increment: 1
+    // @User: Advanced
+
+    // @Param: IMUHEAT_I
+    // @DisplayName: IMU Heater I gain
+    // @Description: IMU Heater integrator gain
+    // @Range: 0 1
+    // @Increment: 0.1
+    // @User: Advanced
+
+    // @Param: IMUHEAT_IMAX
+    // @DisplayName: IMU Heater IMAX
+    // @Description: IMU Heater integrator maximum
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Advanced
+    AP_SUBGROUPINFO(heater.pi_controller, "IMUHEAT_",  21, AP_BoardConfig, AC_PI),
+#endif
     
     AP_GROUPEND
 };
@@ -263,13 +287,6 @@ void AP_BoardConfig::init()
 {
     board_setup();
 
-#if HAL_HAVE_IMU_HEATER
-    // let the HAL know the target temperature. We pass a pointer as
-    // we want the user to be able to change the parameter without
-    // rebooting
-    hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
-#endif
-
     AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW);
 
     if (_boot_delay_ms > 0) {
@@ -373,3 +390,9 @@ bool AP_BoardConfig::safety_button_handle_pressed(uint8_t press_count)
     }
     return true;
 }
+
+namespace AP {
+    AP_BoardConfig *boardConfig(void) {
+        return AP_BoardConfig::get_singleton();
+    }
+};

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -4,6 +4,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_RTC/AP_RTC.h>
+#include <AC_PID/AC_PI.h>
 
 #ifndef AP_FEATURE_BOARD_DETECT
 #if defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_CHIBIOS_ARCH_FMUV4PRO) || defined(HAL_CHIBIOS_ARCH_BRAINV51) || defined(HAL_CHIBIOS_ARCH_BRAINV52) || defined(HAL_CHIBIOS_ARCH_UBRAINV51) || defined(HAL_CHIBIOS_ARCH_COREV10) || defined(HAL_CHIBIOS_ARCH_BRAINV54)
@@ -173,6 +174,10 @@ public:
     // should be toggled
     bool safety_button_handle_pressed(uint8_t press_count);
 
+#if HAL_HAVE_IMU_HEATER
+    void set_imu_temp(float current_temp_c);
+#endif
+
 private:
     static AP_BoardConfig *_singleton;
     
@@ -211,8 +216,17 @@ private:
 
     static bool _in_sensor_config_error;
 
-    // target temperarure for IMU in Celsius, or -1 to disable
-    AP_Int8 _imu_target_temperature;
+#if HAL_HAVE_IMU_HEATER
+    struct {
+        AP_Int8 imu_target_temperature;
+        uint32_t last_update_ms;
+        AC_PI pi_controller{200, 0.3, 70};
+        uint16_t count;
+        float sum;
+        float output;
+        uint32_t last_log_ms;
+    } heater;
+#endif
 
 #if HAL_RCINPUT_WITH_AP_RADIO
     // direct attached radio
@@ -242,3 +256,7 @@ private:
 
     AP_Int32 _options;
 };
+
+namespace AP {
+    AP_BoardConfig *boardConfig(void);
+};

libraries/AP_BoardConfig/IMU_heater.cpp

@@ -0,0 +1,103 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  control IMU heater
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_IOMCU/AP_IOMCU.h>
+#include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
+#include "AP_BoardConfig.h"
+
+#if HAL_HAVE_IMU_HEATER
+
+extern const AP_HAL::HAL& hal;
+
+void AP_BoardConfig::set_imu_temp(float current)
+{
+    int8_t target = heater.imu_target_temperature.get();
+    // pass to HAL for Linux
+    hal.util->set_imu_target_temp((int8_t *)&heater.imu_target_temperature);
+    hal.util->set_imu_temp(current);
+
+    if (target == -1) {
+        // nothing to do
+        return;
+    }
+
+
+    // limit to 65 degrees to prevent damage
+    target = constrain_int16(target, -1, 65);
+    
+    // average over temperatures to remove noise
+    heater.count++;
+    heater.sum += current;
+
+    // update at 10Hz
+    uint32_t now = AP_HAL::millis();
+    if (now - heater.last_update_ms < 100) {
+#if defined(HAL_HEATER_GPIO_PIN)
+        // output as duty cycle to local pin. Use a random sequence to
+        // prevent a periodic change to magnetic field
+        bool heater_on = (get_random16() < uint32_t(heater.output) * 0xFFFFU / 100U);
+        hal.gpio->write(HAL_HEATER_GPIO_PIN, heater_on);
+#endif
+        return;
+    }
+    float dt = (now - heater.last_update_ms) * 0.001;
+    dt = constrain_float(dt, 0, 0.5);
+
+    heater.last_update_ms = now;
+
+    float avg = 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 = constrain_float(heater.output, 0, 100);
+    }
+
+    if (now - heater.last_log_ms >= 1000) {
+        AP::logger().Write("HEAT", "TimeUS,Temp,Targ,P,I,Out", "Qfbfff",
+                           AP_HAL::micros64(),
+                           avg, target,
+                           heater.pi_controller.get_P(),
+                           heater.pi_controller.get_I(),
+                           heater.output);
+        heater.last_log_ms = now;
+    }
+#if 0
+    gcs().send_text(MAV_SEVERITY_INFO, "Heater: P=%.1f I=%.1f Out=%.1f Temp=%.1f",
+                    heater.output,
+                    avg);
+#endif
+
+#if HAL_WITH_IO_MCU
+    if (io_enabled()) {
+        AP_IOMCU *iomcu = AP::iomcu();
+        if (iomcu) {
+            // tell IOMCU to setup heater
+            iomcu->set_heater_duty_cycle(heater.output);
+        }
+    }
+#endif
+}
+
+#endif // HAL_HAVE_IMU_HEATER

libraries/AP_BoardConfig/board_drivers.cpp

@@ -78,15 +78,17 @@ void AP_BoardConfig::board_setup_drivers(void)
     // run board auto-detection
     board_autodetect();
 
+#if HAL_HAVE_IMU_HEATER
     if (state.board_type == PX4_BOARD_PH2SLIM ||
         state.board_type == PX4_BOARD_PIXHAWK2) {
-        _imu_target_temperature.set_default(45);
-        if (_imu_target_temperature.get() < 0) {
+        heater.imu_target_temperature.set_default(45);
+        if (heater.imu_target_temperature.get() < 0) {
             // don't allow a value of -1 on the cube, or it could cook
             // the IMU
-            _imu_target_temperature.set(45);
+            heater.imu_target_temperature.set(45);
         }
     }
+#endif
 
     px4_configured_board = (enum px4_board_type)state.board_type.get();