From 12a70d00c582dab2d17439908b8593cdff8e68ed Mon Sep 17 00:00:00 2001
From: Hayden <haydendonald@gmail.com>
Date: Fri, 7 Jul 2023 15:03:49 +1000
Subject: [PATCH] AP_Compass: fix AK09916 hangup issue

---
 libraries/AP_Compass/AP_Compass_AK09916.cpp | 7 +++++++
 libraries/AP_Compass/AP_Compass_AK09916.h   | 1 +
 2 files changed, 8 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp
index a7d959d4f0..bb6732365a 100644
--- a/libraries/AP_Compass/AP_Compass_AK09916.cpp
+++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp
@@ -311,8 +311,15 @@ void AP_Compass_AK09916::_update()
     }
 
     if (!(regs.st1 & 0x01)) {
+        no_data++;
+        if (no_data == 5) {
+            _reset();
+            _setup_mode();
+            no_data = 0;
+        }
         goto check_registers;
     }
+    no_data = 0;
 
     /* Check for overflow. See AK09916's datasheet*/
     if ((regs.st2 & 0x08)) {
diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h
index f83550e64a..b4cca6e973 100644
--- a/libraries/AP_Compass/AP_Compass_AK09916.h
+++ b/libraries/AP_Compass/AP_Compass_AK09916.h
@@ -97,6 +97,7 @@ private:
     bool _initialized;
     enum Rotation _rotation;
     enum AP_Compass_Backend::DevTypes _devtype;
+    uint8_t no_data;
 };