From 5c67e8a0abaa834728e1af249785e328e969ca35 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 27 Nov 2024 15:40:39 +1100
Subject: [PATCH] AP_Periph: add support for ak09916 backup compass

---
 AP_Periph/AP_Periph.h        |  4 ++--
 AP_Periph/sensor_compass.cpp | 34 ++++++++++++++++++++--------------
 Here4/hwdef.dat              | 11 ++++++++++-
 3 files changed, 32 insertions(+), 17 deletions(-)

diff --git a/AP_Periph/AP_Periph.h b/AP_Periph/AP_Periph.h
index 9bad860..f36c6fb 100644
--- a/AP_Periph/AP_Periph.h
+++ b/AP_Periph/AP_Periph.h
@@ -168,7 +168,7 @@ class AP_Periph_FW {
 
     static const AP_Param::Info var_info[];
 
-    uint32_t last_mag_update_ms;
+    uint32_t last_mag_update_ms[COMPASS_MAX_INSTANCES];
     uint32_t last_gps_update_ms;
     uint32_t last_baro_update_ms;
     uint64_t last_time_sync_usec;
@@ -269,7 +269,7 @@ class AP_Periph_DroneCAN {
     Canard::Publisher<dronecan_protocol_CanStats> can_stats_pub{canard_iface};
 
     void can_mag_update();
-    Canard::Publisher<uavcan_equipment_ahrs_MagneticFieldStrength> mag_pub{canard_iface};
+    Canard::Publisher<uavcan_equipment_ahrs_MagneticFieldStrength2> mag_pub{canard_iface};
 
     void can_gps_init();
     void can_gps_update();
diff --git a/AP_Periph/sensor_compass.cpp b/AP_Periph/sensor_compass.cpp
index be8467a..b481495 100644
--- a/AP_Periph/sensor_compass.cpp
+++ b/AP_Periph/sensor_compass.cpp
@@ -12,22 +12,28 @@ void AP_Periph_DroneCAN::can_mag_update(void)
     }
     compass.read();
 
-    if (periph.last_mag_update_ms == compass.last_update_ms()) {
-        return;
-    }
-    if (!compass.healthy()) {
-        return;
+    // estimate scale and offset from first compass
+    if (compass.healthy(0) && compass.healthy(1) && compass.get_offsets(1).is_zero()) {
+        const Vector3f &reference = compass.get_field(0);
+        const Vector3f &field = compass.get_field(1);
+        compass.set_and_save_offsets(1, reference - field);
     }
 
-    periph.last_mag_update_ms = compass.last_update_ms();
-    const Vector3f &field = compass.get_field();
-    uavcan_equipment_ahrs_MagneticFieldStrength pkt {};
-
-    // the canard dsdl compiler doesn't understand float16
-    for (uint8_t i=0; i<3; i++) {
-        pkt.magnetic_field_ga[i] = field[i] * 0.001;
+    uavcan_equipment_ahrs_MagneticFieldStrength2 pkt {};
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if (!compass.healthy(i)) {
+            continue;
+        }
+        if (periph.last_mag_update_ms[i] == compass.last_update_ms(i)) {
+            continue;
+        }
+        periph.last_mag_update_ms[i] = compass.last_update_ms(i);
+        const Vector3f &field = compass.get_field(i);
+        pkt.sensor_id = i;
+        for (uint8_t j=0; j<3; j++) {
+            pkt.magnetic_field_ga[j] = field[j] * 0.001;
+        }
+        mag_pub.broadcast(pkt);
     }
-
-    mag_pub.broadcast(pkt);
 #endif // HAL_PERIPH_ENABLE_MAG
 }
diff --git a/Here4/hwdef.dat b/Here4/hwdef.dat
index de0702a..7e109df 100644
--- a/Here4/hwdef.dat
+++ b/Here4/hwdef.dat
@@ -85,6 +85,12 @@ PB4 SPI3_MISO SPI3
 PB2 SPI3_MOSI SPI3
 PB1 MAG_CS CS
 
+
+PF14 I2C4_SCL I2C4
+PF15 I2C4_SDA I2C4
+
+I2C_ORDER I2C4
+
 # analog input
 define HAL_USE_ADC FALSE
 define STM32_ADC_USE_ADC1 FALSE
@@ -93,8 +99,11 @@ define HAL_DISABLE_ADC_DRIVER TRUE
 define HAL_INS_ACCELCAL_ENABLED FALSE
 define HAL_PERIPH_ENABLE_BARO TRUE
 
+
 SPIDEV rm3100 SPI3 DEVID1    MAG_CS MODE0  1*MHZ  1*MHZ
 COMPASS RM3100 SPI:rm3100 false ROTATION_YAW_180
+COMPASS AK09916 I2C:0:0x0C false ROTATION_NONE
+
 define AP_RM3100_REVERSAL_MASK 4
 
 PA8 ICM_CS CS
@@ -124,7 +133,7 @@ define SRV_LED_DATA_FUNCTION SRV_Channel::k_ProfiLED_1
 
 define GPS_MAX_RECEIVERS 1
 define GPS_MAX_INSTANCES 1
-define HAL_COMPASS_MAX_SENSORS 1
+define HAL_COMPASS_MAX_SENSORS 2
 
 
 define HAL_PERIPH_ENABLE_GPS 1