From ed531977275014c4c1e86543c558d22deac082bc Mon Sep 17 00:00:00 2001
From: Daniel Cook <daniel.cook@advancednavigation.com>
Date: Mon, 20 May 2024 14:34:44 +1000
Subject: [PATCH] AP_ExternalAHRS_AdvancedNavigation: Rearranged the way
 initialization is handled to prevent locking of thread.

---
 .../AP_ExternalAHRS_AdvancedNavigation.cpp    | 45 ++++++++++++-------
 .../AP_ExternalAHRS_AdvancedNavigation.h      |  1 +
 2 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
index 30b0c84d8e46a9..c13a641f78ad3b 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
@@ -162,31 +162,43 @@ AP_ExternalAHRS_AdvancedNavigation::AP_ExternalAHRS_AdvancedNavigation(AP_Extern
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_AdvancedNavigation::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
         AP_HAL::panic("Failed to start ExternalAHRS update thread");
     }
+}
+
+void AP_ExternalAHRS_AdvancedNavigation::update()
+{
+    get_packets();
+}
+
+void AP_ExternalAHRS_AdvancedNavigation::update_thread(void)
+{
+    _uart->begin(_baudrate);
 
     uint32_t tstart = AP_HAL::millis();
 
+    (void) request_device_information();
+
+    // Ensure device is responsive by requesting its information.
     while (!_last_device_info_pkt_ms)
     {
         const uint32_t tnow = AP_HAL::millis();
         if (tnow - tstart >= AN_TIMEOUT)
         {
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Advanced Navigation Device Unresponsive");
+            if (!request_device_information())
+            {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Request Data Error");
+            }
             tstart = tnow;
         }
-        hal.scheduler->delay(50);
+
+        // Collect the requested packets from the UART manager
+        // This will still process all received packets like normal and feed data out. This ensures that it won't fail completely if the TX fails.
+        if (!get_packets()) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Error Receiving Initialization Packets");
+        }
     }
 
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised: %s", get_name());
-}
-
-void AP_ExternalAHRS_AdvancedNavigation::update()
-{
-    get_packets();
-}
-
-void AP_ExternalAHRS_AdvancedNavigation::update_thread(void)
-{
-    _uart->begin(_baudrate);
 
     while (true) {
         // Request data. If error occurs notify.
@@ -240,15 +252,18 @@ bool AP_ExternalAHRS_AdvancedNavigation::get_packets(void)
     return true;
 }
 
-bool AP_ExternalAHRS_AdvancedNavigation::request_data(void)
+bool AP_ExternalAHRS_AdvancedNavigation::request_device_information(void)
 {
+    return !(_uart->txspace() < sizeof(request_an_info) || _uart->write(request_an_info, sizeof(request_an_info)) != sizeof(request_an_info));
+}
 
+bool AP_ExternalAHRS_AdvancedNavigation::request_data(void)
+{
     // Update device info every 20 secs
-    if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || (_last_device_info_pkt_ms == 0)) {
-        if (_uart->txspace() < sizeof(request_an_info)) {
+    if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || !_last_device_info_pkt_ms) {
+        if (!request_device_information()) {
             return false;
         }
-        _uart->write(request_an_info, sizeof(request_an_info));
     }
 
     // Don't send a packet request unless the device is healthy
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
index 3f004367e7eb3a..2b31c8ac0c8585 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
@@ -522,6 +522,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend
 
     void update_thread();
     bool get_packets(void);
+    bool request_device_information(void);
     bool request_data(void);
     bool sendPacketRequest(void);
     bool get_gnss_capability(void) const;