AP_ExternalAHRS_VectorNav: Address review comments

Remove unnecessary header Switch parameters to default initialization Change pointer casting to prevent a const_cast, and remove doubled sync byte when echoing ASCII messages Fix Valgrind report by preventing use of nmea_printf on buffers which may not be null-terminated
ArduPilot · Jul 19, 2024 · 23c72e6 · 23c72e6
1 parent c76a64b
commit 23c72e6
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 25 deletions.
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
@@ -16,7 +16,6 @@
   support for serial connected AHRS systems
  */
 
-#include <cstdint>
 #define ALLOW_DOUBLE_MATH_FUNCTIONS
 
 #include "AP_ExternalAHRS_config.h"

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
@@ -73,13 +73,13 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
     uint16_t pktoffset;
     uint16_t bufsize;
 
-    struct VN_imu_packet *latest_imu_packet = nullptr;
-    struct VN_INS_ekf_packet *latest_ins_ekf_packet = nullptr;
-    struct VN_INS_gnss_packet *latest_ins_gnss_packet = nullptr;
+    struct VN_imu_packet *latest_imu_packet;
+    struct VN_INS_ekf_packet *latest_ins_ekf_packet;
+    struct VN_INS_gnss_packet *latest_ins_gnss_packet;
 
-    uint32_t last_pkt1_ms = UINT32_MAX;
-    uint32_t last_pkt2_ms = UINT32_MAX;
-    uint32_t last_pkt3_ms = UINT32_MAX;
+    uint32_t last_pkt1_ms;
+    uint32_t last_pkt2_ms;
+    uint32_t last_pkt3_ms;
 
     enum class TYPE {
         VN_INS,  // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0

diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp
@@ -129,16 +129,16 @@ void VectorNav::send_imu_packet(void)
     pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
 
     const uint8_t sync_byte = 0xFA;
-    write_to_autopilot((char *)&sync_byte, 1);
-    write_to_autopilot((char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header));
-    write_to_autopilot((char *)&pkt, sizeof(pkt));
+    write_to_autopilot((const char *)&sync_byte, 1);
+    write_to_autopilot((const char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header));
+    write_to_autopilot((const char *)&pkt, sizeof(pkt));
 
     uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0);
     crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
     uint16_t crc2;
     swab(&crc, &crc2, 2);
 
-    write_to_autopilot((char *)&crc2, sizeof(crc2));
+    write_to_autopilot((const char *)&crc2, sizeof(crc2));
 }
 
 void VectorNav::send_ins_ekf_packet(void)
@@ -174,17 +174,17 @@ void VectorNav::send_ins_ekf_packet(void)
     pkt.velU = 0.25;
 
     const uint8_t sync_byte = 0xFA;
-    write_to_autopilot((char *)&sync_byte, 1);
-    write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header));
-    write_to_autopilot((char *)&pkt, sizeof(pkt));
+    write_to_autopilot((const char *)&sync_byte, 1);
+    write_to_autopilot((const char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header));
+    write_to_autopilot((const char *)&pkt, sizeof(pkt));
 
     uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0);
     crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
 
     uint16_t crc2;
     swab(&crc, &crc2, 2);
 
-    write_to_autopilot((char *)&crc2, sizeof(crc2));
+    write_to_autopilot((const char *)&crc2, sizeof(crc2));
 }
 
 void VectorNav::send_ins_gnss_packet(void)
@@ -221,17 +221,17 @@ void VectorNav::send_ins_gnss_packet(void)
     pkt.fix2 = 3;
 
     const uint8_t sync_byte = 0xFA;
-    write_to_autopilot((char *)&sync_byte, 1);
-    write_to_autopilot((char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header));
-    write_to_autopilot((char *)&pkt, sizeof(pkt));
+    write_to_autopilot((const char *)&sync_byte, 1);
+    write_to_autopilot((const char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header));
+    write_to_autopilot((const char *)&pkt, sizeof(pkt));
 
     uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0);
     crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
 
     uint16_t crc2;
     swab(&crc, &crc2, 2);
 
-    write_to_autopilot((char *)&crc2, sizeof(crc2));
+    write_to_autopilot((const char *)&crc2, sizeof(crc2));
 }
 
 void VectorNav::nmea_printf(const char *fmt, ...)
@@ -273,14 +273,24 @@ void VectorNav::update(void)
     }
 
     char receive_buf[50];
-    const ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf));
-    if (n > 0) {
-        if (strncmp(receive_buf, "$VNRRG,01", 9) == 0) {
+    ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf));
+    if (n <= 0) {
+        return;
+    }
+
+    // avoid parsing the NMEA stream here by making assumptions about
+    // how we receive configuration strings.  Generally we can just
+    // echo back the configuration string to make the driver happy.
+    if (n >= 9) {
+        // intercept device-version query, respond with simulated version:
+        const char *ver_query_string = "$VNRRG,01";
+        if (strncmp(receive_buf, ver_query_string, strlen(ver_query_string)) == 0) {
             nmea_printf("$VNRRG,01,VN-300-SITL");
-        } else {
-            nmea_printf("$%s", receive_buf);
+            // consume the query so we don't "respond" twice:
+            memmove(&receive_buf[0], &receive_buf[strlen(ver_query_string)], n - strlen(ver_query_string));
+            n -= 9;
         }
     }
-
+    write_to_autopilot(receive_buf, n);
 }