From 23c72e6e4cd9cbb4b4433656ca8f7f1adf258462 Mon Sep 17 00:00:00 2001 From: BLash Date: Thu, 18 Jul 2024 21:02:40 -0500 Subject: [PATCH] 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 --- .../AP_ExternalAHRS_VectorNav.cpp | 1 - .../AP_ExternalAHRS_VectorNav.h | 12 ++--- libraries/SITL/SIM_VectorNav.cpp | 46 +++++++++++-------- 3 files changed, 34 insertions(+), 25 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 3e58710799d031..38f44ed1501118 100644 --- 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 #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 index 456090d205f00a..9c1639640a1e00 100644 --- 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 index d51e500e58a54d..d8a659f6d26a9e 100644 --- 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,9 +174,9 @@ 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); @@ -184,7 +184,7 @@ void VectorNav::send_ins_ekf_packet(void) 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,9 +221,9 @@ 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); @@ -231,7 +231,7 @@ void VectorNav::send_ins_gnss_packet(void) 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); }