From 1288810c709fe3c342ffcf0459cc9197234e5b5a Mon Sep 17 00:00:00 2001
From: BLash <lash@vectornav.com>
Date: Mon, 8 Jul 2024 18:38:49 -0500
Subject: [PATCH] AP_ExternalAHRS_VectorNav: Handle VNERR responses

Print received VNERR response to console and continually retry sending the configuration message, instead of panicing
---
 .../AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp     | 11 ++++++++++-
 libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h |  1 +
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
index c8fbeed119..a815a53de7 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
@@ -305,6 +305,10 @@ void AP_ExternalAHRS_VectorNav::send_command_blocking()
         while (nbytes-- > 0) {
             char c = uart->read();
             if (decode(c)) {
+                if (nmea.error_response) {
+                    // Received a valid VNERR. Try to resend after the timeout length
+                    break;
+                }
                 return;
             }
         }
@@ -353,6 +357,7 @@ bool AP_ExternalAHRS_VectorNav::decode(char c)
         nmea.checksum = 0;
         nmea.term_is_checksum = false;
         nmea.sentence_done = false;
+        nmea.error_response = false;
         return false;
     }
 
@@ -378,10 +383,14 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term()
             if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) {  // ignore sync byte in comparison
                 return false;
             } else if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
-                AP_HAL::panic("VectorNav received VN error response");
+                nmea.error_response = true;  // Message will be printed on next term 
             }
             break;
         case 1: 
+            if (nmea.error_response) {
+                GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
+                break;
+            }
             if (strncmp(nmea.term, message_to_send + 6,
                         nmea.term_offset) != 0) {  // Start after "VNXXX,"
                 return false;
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
index b6f4216b55..d3b9ab26dd 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
@@ -99,6 +99,7 @@ private:
         bool term_is_checksum;    // current term is the checksum
         bool sentence_valid;      // is current sentence valid so far
         bool sentence_done;       // true if this sentence has already been decoded
+        bool error_response;      // true if received a VNERR response
     } nmea;
 };