AP_ExternalAHRS: Fixed ANPP Msg Formatting issues

ArduPilot · Apr 17, 2024 · 07bd022 · 07bd022
1 parent cd2f23e
commit 07bd022
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 4 deletions.
diff --git 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
@@ -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,17 +436,31 @@ 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
             uint8_t* id_ptr = &id;
             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;