From 18a812ce0c71cbc7c0fce1ca5d942d98c63e71fe Mon Sep 17 00:00:00 2001
From: Daniel Cook <daniel.cook@advancednavigation.com>
Date: Mon, 26 Feb 2024 13:18:24 +1100
Subject: [PATCH] AP_ExternalAHRS: Fixed ANPP Msg Formatting issues

---
 .../AP_ExternalAHRS_AdvancedNavigation.cpp    |  8 ++++++--
 .../AP_ExternalAHRS_AdvancedNavigation.h      | 20 +++++++++++++++++--
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
index ef50dacc3f6170..fb4a0f14bcd4ab 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
@@ -531,9 +531,10 @@ bool AP_ExternalAHRS_AdvancedNavigation::sendPacketRequest()
     };
 
     AN_PACKET packet;
+   
     // load the AN_PACKETS_PERIOD Into the payload.
     packet.payload.packet_periods = periods;
-    packet.update_checks(packet_id_packet_periods, sizeof(packet.payload.packet_periods));
+    packet.update_checks(packet_id_packet_periods, packet.getPeriodsLength(periods));
 
     // Check for space in the tx buffer
     if (_uart->txspace() < packet.packet_size()) {
@@ -599,7 +600,10 @@ bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(bool gnss_en, vehicl
     options_packet.reversing_detection_enabled = false;
     options_packet.motion_analysis_enabled = false;
     options_packet.automatic_magnetic_calibration_enabled = true;
-
+    options_packet.dual_antenna_disabled = false;
+    // set reserved packets to 0
+    memset(options_packet.reserved, 0, sizeof(options_packet.reserved));
+    
     return set_filter_options(options_packet);
 }
 
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
index 7647b55b31cea0..a0b4ee5b3cb6b7 100644
--- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
+++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
@@ -412,6 +412,8 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend
         uint8_t reversing_detection_enabled;
         uint8_t motion_analysis_enabled;
         uint8_t automatic_magnetic_calibration_enabled;
+        uint8_t dual_antenna_disabled;
+        uint8_t reserved[7];
     };
 
     class PACKED AN_PACKET
@@ -434,10 +436,12 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend
             AN_FILTER_OPTIONS filter_options;
         } payload;
 
-        void update_checks(uint8_t header_id, uint8_t header_length)
+        void update_checks(uint8_t header_id, uint8_t payload_length)
         {
+            length = payload_length;
+            
             // Update the packet check and header id
-            crc = crc16_ccitt(payload.raw_packet, header_length, 0xFFFF);
+            crc = crc16_ccitt(payload.raw_packet, payload_length, 0xFFFF);
             id = header_id;
 
             // Update the header LRC
@@ -445,6 +449,18 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend
             lrc = ((id_ptr[0] + id_ptr[1] + id_ptr[2] + id_ptr[3]) ^ 0xFF) + 1;
         }
 
+        uint8_t getPeriodsLength(AN_PACKET_PERIODS packet_periods)
+        {            
+            // Find the utilized size of the packet and make it the payload length.
+            for (uint32_t idx = 0; idx < (AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD); idx++)
+            {
+                if (packet_periods.periods[idx].id == 0 && packet_periods.periods[idx].packet_period == 0) {
+                    return 2 + (sizeof(AN_PERIOD)*idx);
+                }
+            }
+            return (AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD);
+        }
+
         uint8_t* raw_pointer()
         {
             return &lrc;