AP_Compass: fix AK09916 hangup issue

ArduPilot · Oct 18, 2023 · cdf085a · cdf085a
1 parent cc799d3
commit cdf085a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git 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
@@ -97,6 +97,7 @@ class AP_Compass_AK09916 : public AP_Compass_Backend
     bool _initialized;
     enum Rotation _rotation;
     enum AP_Compass_Backend::DevTypes _devtype;
+    uint8_t no_data;
 };