From 7c2e4d0419b97ae1bbaba281b2537109d4f4f834 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 15 Jun 2016 18:01:38 +1000
Subject: [PATCH] HAL_PX4: implement IMU heater in hal.util for Pixhawk2

---
 libraries/AP_HAL_PX4/Util.cpp | 50 +++++++++++++++++++++++++++++++++++
 libraries/AP_HAL_PX4/Util.h   | 12 +++++++++
 2 files changed, 62 insertions(+)

diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index 7a132db5e8..b496cda24e 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -12,6 +12,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <systemlib/board_serial.h>
+#include <drivers/drv_gpio.h>
+#include <AP_Math/AP_Math.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -170,4 +172,52 @@ void PX4Util::perf_count(perf_counter_t h)
     ::perf_count((::perf_counter_t)h);
 }
 
+void PX4Util::set_imu_temp(float current)
+{
+    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) {
+        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 err = ((float)*_heater.target) - current;
+
+    _heater.integrator += kI * err;
+    _heater.integrator = constrain_float(_heater.integrator, 0, 70);
+
+    float output = constrain_float(kP * err + _heater.integrator, 0, 100);
+    
+    // hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err);
+
+    if (_heater.fd == -1) {
+        _heater.fd = open("/dev/px4io", O_RDWR);
+    }
+    if (_heater.fd != -1) {
+        ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output);
+    }
+   
+}
+
+void PX4Util::set_imu_target_temp(int8_t *target)
+{
+    _heater.target = target;
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h
index 801858347e..ae29fff8b7 100644
--- a/libraries/AP_HAL_PX4/Util.h
+++ b/libraries/AP_HAL_PX4/Util.h
@@ -58,7 +58,19 @@ public:
     // create a new semaphore
     AP_HAL::Semaphore *new_semaphore(void) override { return new PX4::Semaphore; }
 
+    void set_imu_temp(float current) override;
+    void set_imu_target_temp(int8_t *target) override;
+    
 private:
     int _safety_handle;
     PX4::NSHShellStream _shell_stream;
+
+    struct {
+        int8_t *target;
+        float integrator;
+        uint16_t count;
+        float sum;
+        uint32_t last_update_ms;
+        int fd = -1;
+    } _heater;
 };