From 1fd0ed4e1f43bb55c86abe866b6c4d95f7b638ea Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 1 Aug 2022 18:07:10 +1000
Subject: [PATCH] AP_Compass: switch to a general method of heater compensation

this allows any board to easily setup heater compensation for an
internal compass. The offsets are in body frame (previously in sensor
frame) and are sensor specific using bus device IDs
---
 libraries/AP_Compass/AP_Compass_AK09916.cpp | 13 -----------
 libraries/AP_Compass/AP_Compass_Backend.cpp | 25 +++++++++++++++++++++
 libraries/AP_Compass/AP_Compass_IST8310.cpp | 13 -----------
 3 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp
index 3be3053a58..fec2e90232 100644
--- a/libraries/AP_Compass/AP_Compass_AK09916.cpp
+++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp
@@ -324,19 +324,6 @@ void AP_Compass_AK09916::_update()
     _make_adc_sensitivity_adjustment(raw_field);
     raw_field *= AK09916_MILLIGAUSS_SCALE;
 
-#ifdef HAL_AK09916_HEATER_OFFSET
-    /*
-      the internal AK09916 can be impacted by the magnetic field from
-      a heater. We use the heater duty cycle to correct for the error
-     */
-    if (AP_HAL::Device::devid_get_bus_type(_bus->get_bus_id()) == AP_HAL::Device::BUS_TYPE_SPI) {
-        auto *bc = AP::boardConfig();
-        if (bc) {
-            raw_field += HAL_AK09916_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01;
-        }
-    }
-#endif
-
     accumulate_sample(raw_field, _compass_instance, 10);
 
 check_registers:
diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index ae41108221..9b9f5611cc 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -5,6 +5,7 @@
 
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -21,6 +22,30 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
     }
     mag.rotate(state.rotation);
 
+#ifdef HAL_HEATER_MAG_OFFSET
+    /*
+      apply compass compensations for boards that have a heater which
+      interferes with an internal compass. This needs to be applied
+      before the board orientation so it is independent of
+      AHRS_ORIENTATION
+     */
+    if (!is_external(instance)) {
+        const uint32_t dev_id = uint32_t(_compass._state[Compass::StateIndex(instance)].dev_id);
+        static const struct offset {
+            uint32_t dev_id;
+            Vector3f ofs;
+        } offsets[] = HAL_HEATER_MAG_OFFSET;
+        const auto *bc = AP::boardConfig();
+        if (bc) {
+            for (const auto &o : offsets) {
+                if (o.dev_id == dev_id) {
+                    mag += o.ofs * bc->get_heater_duty_cycle() * 0.01;
+                }
+            }
+        }
+    }
+#endif
+
     if (!state.external) {
         // and add in AHRS_ORIENTATION setting if not an external compass
         if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) {
diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp
index cd4fb05cb3..7a0769761d 100644
--- a/libraries/AP_Compass/AP_Compass_IST8310.cpp
+++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp
@@ -229,19 +229,6 @@ 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);
 }