From 3ec0e27e4f2e6a94d05467f39bb4a1da16042d99 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 3 May 2022 11:50:34 +1000
Subject: [PATCH] AP_Compass: move logging of compass ERR flags into AP_Compass

---
 libraries/AP_Compass/AP_Compass.cpp | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 46f9141cae..492027b96b 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -1580,6 +1580,10 @@ Compass::read(void)
         return false;
     }
 
+#if HAL_LOGGING_ENABLED
+    const bool old_healthy = healthy();
+#endif
+
 #ifndef HAL_BUILD_AP_PERIPH
     if (!_initial_location_set) {
         try_set_initial_location();
@@ -1620,7 +1624,21 @@ Compass::read(void)
             break;
         }
     }
-    return healthy();
+    const bool new_healthy = healthy();
+
+#if HAL_LOGGING_ENABLED
+
+    #define MASK_LOG_ANY                    0xFFFF
+
+    if (new_healthy != old_healthy) {
+        if (AP::logger().should_log(MASK_LOG_ANY)) {
+            const LogErrorCode code = new_healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY;
+            AP::logger().Write_Error(LogErrorSubsystem::COMPASS, code);
+        }
+    }
+#endif
+
+    return new_healthy;
 }
 
 uint8_t