From 42c82ac319336dd97ea7e83fa7500bb8a35d523e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 Nov 2019 13:32:30 +1100
Subject: [PATCH] HAL_ChibiOS: removed heater control

---
 libraries/AP_HAL_ChibiOS/Util.cpp | 61 -------------------------------
 libraries/AP_HAL_ChibiOS/Util.h   | 15 --------
 2 files changed, 76 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 82cfdc1e0f..dfe42a39bd 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -123,67 +123,6 @@ Util::safety_state Util::safety_switch_state(void)
 #endif
 }
 
-void Util::set_imu_temp(float current)
-{
-#if HAL_HAVE_IMU_HEATER
-    if (!heater.target || *heater.target == -1) {
-        return;
-    }
-
-    // average over temperatures to remove noise
-    heater.count++;
-    heater.sum += current;
-
-    // update once a second
-    uint32_t now = AP_HAL::millis();
-    if (now - heater.last_update_ms < 1000) {
-#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;
-    }
-    heater.last_update_ms = now;
-
-    current = heater.sum / heater.count;
-    heater.sum = 0;
-    heater.count = 0;
-
-    // experimentally tweaked for Pixhawk2
-    const float kI = 0.3f;
-    const float kP = 200.0f;
-    float target = (float)(*heater.target);
-
-    // limit to 65 degrees to prevent damage
-    target = constrain_float(target, 0, 65);
-
-    float err = target - current;
-
-    heater.integrator += kI * err;
-    heater.integrator = constrain_float(heater.integrator, 0, 70);
-
-    heater.output = constrain_float(kP * err + heater.integrator, 0, 100);
-
-    //hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", heater.integrator, heater.output, current, err);
-
-#if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
-        // tell IOMCU to setup heater
-        iomcu.set_heater_duty_cycle(heater.output);
-    }
-#endif
-#endif // HAL_HAVE_IMU_HEATER
-}
-
-void Util::set_imu_target_temp(int8_t *target)
-{
-#if HAL_HAVE_IMU_HEATER
-    heater.target = target;
-#endif
-}
-
 #ifdef HAL_PWM_ALARM
 struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM;
 
diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h
index 5e44b851c7..9222057d52 100644
--- a/libraries/AP_HAL_ChibiOS/Util.h
+++ b/libraries/AP_HAL_ChibiOS/Util.h
@@ -45,10 +45,6 @@ public:
      */
     enum safety_state safety_switch_state(void) override;
 
-    // IMU temperature control
-    void set_imu_temp(float current) override;
-    void set_imu_target_temp(int8_t *target) override;
-
     // get system ID as a string
     bool get_system_id(char buf[40]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
@@ -79,17 +75,6 @@ private:
     static ToneAlarmPwmGroup _toneAlarm_pwm_group;
 #endif
 
-#if HAL_HAVE_IMU_HEATER
-    struct {
-        int8_t *target;
-        float integrator;
-        uint16_t count;
-        float sum;
-        uint32_t last_update_ms;
-        float output;
-    } heater;
-#endif
-
     /*
       set HW RTC in UTC microseconds
      */