From 5d98c23d89a95a6b0add0fcac3817ac81b0b7717 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 23 Nov 2023 15:52:27 +1100
Subject: [PATCH] AP_Compass: add RM3100 debug stuff

---
 libraries/AP_Compass/AP_Compass_RM3100.cpp | 9 ++++++++-
 libraries/AP_Compass/AP_Compass_RM3100.h   | 1 +
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp
index b191a6b52b7ca..e8102ff63f77e 100644
--- a/libraries/AP_Compass/AP_Compass_RM3100.cpp
+++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp
@@ -25,7 +25,7 @@
 #include <utility>
 #include <AP_Math/AP_Math.h>
 #include <stdio.h>
-
+#include <GCS_MAVLink/GCS.h>
 #define RM3100_POLL_REG        0x00
 
 #define RM3100_CMM_REG         0x01
@@ -139,6 +139,10 @@ bool AP_Compass_RM3100::init()
 
     _scaler = (1 / GAIN_CC200) * UTESLA_TO_MGAUSS; // has to be changed if using a different cycle count
 
+    // Do a BIST
+    dev->write_register(RM3100_BIST_REG, 0xFF); // BIST
+    hal.scheduler->delay(100);
+    dev->read_registers(RM3100_BIST_REG, &bist, 1);
     // lower retries for run
     dev->set_retries(3);
 
@@ -189,15 +193,18 @@ void AP_Compass_RM3100::timer()
     if (!dev->read_registers(RM3100_STATUS_REG, (uint8_t *)&status, 1)) {
         goto check_registers;
     }
+    hal.console->printf("RM3100: BIST %x\n", bist);
 
     if (!(status & 0x80)) {
         // data not available yet
         goto check_registers;
     }
+    // hal.console->printf("RM3100: status %x\n", status);
 
     if (!dev->read_registers(RM3100_MX2_REG, (uint8_t *)&data, sizeof(data))) {
         goto check_registers;
     }
+    // hal.console->printf("RM3100: read data %x\n", status);
 
     // the 24 bits of data for each axis are in 2s complement representation
     // each byte is shifted to its position in a 24-bit unsigned integer and from 8 more bits to be left-aligned in a 32-bit integer
diff --git a/libraries/AP_Compass/AP_Compass_RM3100.h b/libraries/AP_Compass/AP_Compass_RM3100.h
index 2b6419f03c95d..f479885ca9ec1 100644
--- a/libraries/AP_Compass/AP_Compass_RM3100.h
+++ b/libraries/AP_Compass/AP_Compass_RM3100.h
@@ -61,6 +61,7 @@ class AP_Compass_RM3100 : public AP_Compass_Backend
     bool force_external;
     enum Rotation rotation;
     float _scaler = 1.0;
+    uint8_t bist;
 };
 
 #endif  // AP_COMPASS_RM3100_ENABLED