From d5dccaa7348b893e758f0357ef09ec2332e32b68 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 31 May 2021 10:56:07 +1000
Subject: [PATCH] AP_Compass: added heater compensation to internal mag for
 Pix32v5

---
 libraries/AP_Compass/AP_Compass_IST8310.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp
index 52716e4c71..018572a539 100644
--- a/libraries/AP_Compass/AP_Compass_IST8310.cpp
+++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp
@@ -24,6 +24,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/utility/sparse-endian.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 #define WAI_REG 0x0
 #define DEVICE_ID 0x10
@@ -234,6 +235,19 @@ void AP_Compass_IST8310::timer()
     /* Resolution: 0.3 µT/LSB - already convert to milligauss */
     Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f};
 
+#ifdef HAL_IST8310_I2C_HEATER_OFFSET
+    /*
+      the internal IST8310 can be impacted by the magnetic field from
+      a heater. We use the heater duty cycle to correct for the error
+     */
+    if (!is_external(_instance) && AP_HAL::Device::devid_get_bus_type(_dev->get_bus_id()) == AP_HAL::Device::BUS_TYPE_I2C) {
+        const auto *bc = AP::boardConfig();
+        if (bc) {
+            field += HAL_IST8310_I2C_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01;
+        }
+    }
+#endif
+    
     accumulate_sample(field, _instance);
 }