From 3207d2f58a28caf3f41547bf8a67ca13831fb3b0 Mon Sep 17 00:00:00 2001 From: BLash Date: Fri, 5 Jul 2024 11:40:20 -0500 Subject: [PATCH 1/7] AP_ExternalAHRS: VectorNav: Add response validation to all sent commands Expands wait_register_response (and renames to send_command_blocking) to accept any arbitrary command, and uses the new method for all sent commands --- .../AP_ExternalAHRS_VectorNav.cpp | 70 ++++++++++--------- .../AP_ExternalAHRS_VectorNav.h | 11 +-- 2 files changed, 44 insertions(+), 37 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index f43ac8598d6e5..98279e154ae18 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -286,21 +286,18 @@ bool AP_ExternalAHRS_VectorNav::check_uart() return true; } -// Send command to read given register number and wait for response +// Send command and wait for response // Only run from thread! This blocks until a response is received #define READ_REQUEST_RETRY_MS 500 -void AP_ExternalAHRS_VectorNav::wait_register_responce(const uint8_t register_num) +void AP_ExternalAHRS_VectorNav::send_command_blocking() { - nmea.register_number = register_num; - uint32_t request_sent = 0; while (true) { hal.scheduler->delay(1); const uint32_t now = AP_HAL::millis(); if (now - request_sent > READ_REQUEST_RETRY_MS) { - // Send request to read - nmea_printf(uart, "$%s%u", "VNRRG,", nmea.register_number); + nmea_printf(uart, "$%s", message_to_send); request_sent = now; } @@ -371,57 +368,58 @@ bool AP_ExternalAHRS_VectorNav::decode(char c) } // decode the most recently consumed term -// returns true if new sentence has just passed checksum test and is validated +// returns true if new term is valid bool AP_ExternalAHRS_VectorNav::decode_latest_term() { + // Check the first two terms (In most cases header + reg number) that they match the sent + // message. If not, the response is invalid. switch (nmea.term_number) { case 0: - if (strcmp(nmea.term, "VNRRG") != 0) { + 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"); } break; - - case 1: - if (nmea.register_number != strtoul(nmea.term, nullptr, 10)) { + case 1: + if (strncmp(nmea.term, message_to_send + 6, + nmea.term_offset) != 0) { // Start after "VNXXX," return false; } break; - - case 2: - strncpy(model_name, nmea.term, sizeof(model_name)); + case 2: + if (strncmp(nmea.term, "VN-", 3) == 0) { + // This term is the model number + strncpy(model_name, nmea.term, sizeof(model_name)); + break; + } break; - default: - return false; + break; } return true; } -void AP_ExternalAHRS_VectorNav::update_thread() -{ +void AP_ExternalAHRS_VectorNav::initialize() { // Open port in the thread uart->begin(baudrate, 1024, 512); - // Reset and wait for module to reboot - // VN_100 takes 1.25 seconds - //nmea_printf(uart, "$VNRST"); - //hal.scheduler->delay(3000); - - // Stop NMEA Async Outputs (this UART only) - nmea_printf(uart, "$VNWRG,6,0"); + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,0"); + send_command_blocking(); - // Detect version // Read Model Number Register, ID 1 - wait_register_responce(1); + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNRRG,01"); + send_command_blocking(); // Setup for messages respective model types (on both UARTs) if (strncmp(model_name, "VN-1", 4) == 0) { - // VN-100 + // VN-1X0 type = TYPE::VN_AHRS; // This assumes unit is still configured at its default rate of 800hz - nmea_printf(uart, "$VNWRG,75,3,%u,14,073E,0004", unsigned(800/get_rate())); - + hal.util->snprintf(message_to_send, sizeof(message_to_send), + "VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); + send_command_blocking(); } else { // Default to setup for sensors other than VN-100 or VN-110 // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others @@ -432,11 +430,19 @@ void AP_ExternalAHRS_VectorNav::update_thread() if (strncmp(model_name, "VN-3", 4) == 0) { has_dual_gnss = true; } - nmea_printf(uart, "$VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate/get_rate())); - nmea_printf(uart, "$VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate/5)); + hal.util->snprintf(message_to_send, sizeof(message_to_send), + "VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); + send_command_blocking(); + hal.util->snprintf(message_to_send, sizeof(message_to_send), + "VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); + send_command_blocking(); } setup_complete = true; +} + +void AP_ExternalAHRS_VectorNav::update_thread() { + initialize(); while (true) { if (!check_uart()) { hal.scheduler->delay(1); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 05c4a01fae682..b6f4216b55a73 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -61,10 +61,12 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { void update_thread(); bool check_uart(); + void initialize(); + void process_ins_packet1(const uint8_t *b); void process_ins_packet2(const uint8_t *b); void process_ahrs_packet(const uint8_t *b); - void wait_register_responce(const uint8_t register_num); + void send_command_blocking(); uint8_t *pktbuf; uint16_t pktoffset; @@ -83,22 +85,21 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { bool has_dual_gnss = false; - char model_name[25]; + char model_name[20]; + char message_to_send[50]; // NMEA parsing for setup bool decode(char c); bool decode_latest_term(); struct NMEA_parser { - char term[25]; // buffer for the current term within the current sentence + char term[20]; // buffer for the current term within the current sentence uint8_t term_offset; // offset within the _term buffer where the next character should be placed uint8_t term_number; // term index within the current sentence uint8_t checksum; // checksum accumulator 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 - uint8_t register_number; // VectorNAV register number were reading } nmea; - }; #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED From d4722b1345565877be5cf7f24a398b77daedff47 Mon Sep 17 00:00:00 2001 From: BLash Date: Fri, 5 Jul 2024 11:40:51 -0500 Subject: [PATCH 2/7] AP_ExternalAHRS: VectorNav: Disable ASCII messages on both ports, rather than just active port This change prevents a commonly seen baudrate overflow error on the unused port. Also pause asynchronous communications during initial configuration. --- .../AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 98279e154ae18..c8fbeed119fd0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -404,7 +404,15 @@ void AP_ExternalAHRS_VectorNav::initialize() { // Open port in the thread uart->begin(baudrate, 1024, 512); - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,0"); + // Pause asynchronous communications to simplify packet finding + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,0"); + send_command_blocking(); + + // Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate + // overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,1"); + send_command_blocking(); + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,2"); send_command_blocking(); // Read Model Number Register, ID 1 @@ -438,6 +446,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { send_command_blocking(); } + // Resume asynchronous communications + hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,1"); + send_command_blocking(); setup_complete = true; } From 884a889b7a5c267e596c9151caa5b19a233d6e2d Mon Sep 17 00:00:00 2001 From: BLash Date: Mon, 8 Jul 2024 18:38:49 -0500 Subject: [PATCH 3/7] 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 c8fbeed119fd0..a815a53de7cca 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 b6f4216b55a73..d3b9ab26dd230 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -99,6 +99,7 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { 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; }; From 51db6ebd1aeeb6a80f858980201c76c8b41da6df Mon Sep 17 00:00:00 2001 From: BLash Date: Tue, 9 Jul 2024 10:53:16 -0500 Subject: [PATCH 4/7] AP_ExternalAHRS_VectorNav: Minor code clarity improvements Add clarity to send_command_blocking by improving comment and directly returning instead of breaking --- .../AP_ExternalAHRS_VectorNav.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index a815a53de7cca..d14a61f6e4cba 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -287,7 +287,7 @@ bool AP_ExternalAHRS_VectorNav::check_uart() } // Send command and wait for response -// Only run from thread! This blocks until a response is received +// Only run from thread! This blocks and retries until a non-error response is received #define READ_REQUEST_RETRY_MS 500 void AP_ExternalAHRS_VectorNav::send_command_blocking() { @@ -380,33 +380,30 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term() // message. If not, the response is invalid. switch (nmea.term_number) { case 0: - if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { // ignore sync byte in comparison + if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { return false; - } else if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) { + } + if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) { nmea.error_response = true; // Message will be printed on next term } - break; + return true; 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," + } else if (strncmp(nmea.term, message_to_send + 6, + nmea.term_offset) != 0) { // Start after "VNXXX," return false; } - break; + return true; case 2: if (strncmp(nmea.term, "VN-", 3) == 0) { // This term is the model number strncpy(model_name, nmea.term, sizeof(model_name)); - break; } - break; + return true; default: - break; + return true; } - return true; } void AP_ExternalAHRS_VectorNav::initialize() { From 96115031088927c5852e3d929da7665d095482ea Mon Sep 17 00:00:00 2001 From: BLash Date: Wed, 10 Jul 2024 12:19:17 -0500 Subject: [PATCH 5/7] AP_ExternalAHRS_VectorNav: Bugfixes and review comment address "VNERR" does not match beginning of message_to_send, so have to prevent it from returning due to the string compare. Also must prevent exiting the decode before the sentence has completed so that we can go on to print the error code term. Fix bug preventing disabling of ASCII measurements Instead of snprintf the command to message_to_send then executing run_command, allow run_command to accept a string and format spec Add check to ensure message_to_send is greater than length 6 before attempting to read past 6 --- .../AP_ExternalAHRS_VectorNav.cpp | 46 ++++++++----------- .../AP_ExternalAHRS_VectorNav.h | 2 +- 2 files changed, 21 insertions(+), 27 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index d14a61f6e4cba..aef7fe26528a9 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -289,8 +289,14 @@ bool AP_ExternalAHRS_VectorNav::check_uart() // Send command and wait for response // Only run from thread! This blocks and retries until a non-error response is received #define READ_REQUEST_RETRY_MS 500 -void AP_ExternalAHRS_VectorNav::send_command_blocking() +void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...) { + va_list ap; + + va_start(ap, fmt); + hal.util->vsnprintf(message_to_send, sizeof(message_to_send), fmt, ap); + va_end(ap); + uint32_t request_sent = 0; while (true) { hal.scheduler->delay(1); @@ -305,7 +311,7 @@ void AP_ExternalAHRS_VectorNav::send_command_blocking() while (nbytes-- > 0) { char c = uart->read(); if (decode(c)) { - if (nmea.error_response) { + if (nmea.error_response && nmea.sentence_done) { // Received a valid VNERR. Try to resend after the timeout length break; } @@ -380,18 +386,17 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term() // message. If not, the response is invalid. switch (nmea.term_number) { case 0: - if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { - return false; - } if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) { nmea.error_response = true; // Message will be printed on next term + } else if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) { + return false; } return true; case 1: if (nmea.error_response) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term); - } else if (strncmp(nmea.term, message_to_send + 6, - nmea.term_offset) != 0) { // Start after "VNXXX," + } else if (strlen(message_to_send) > 6 && + strncmp(nmea.term, &message_to_send[6], nmea.term_offset != 0)) { // Start after "VNXXX," return false; } return true; @@ -411,19 +416,15 @@ void AP_ExternalAHRS_VectorNav::initialize() { uart->begin(baudrate, 1024, 512); // Pause asynchronous communications to simplify packet finding - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,0"); - send_command_blocking(); + run_command("VNASY,0"); // Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate // overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,1"); - send_command_blocking(); - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,2"); - send_command_blocking(); + run_command("VNWRG,06,0,1"); + run_command("VNWRG,06,0,2"); // Read Model Number Register, ID 1 - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNRRG,01"); - send_command_blocking(); + run_command("VNRRG,01"); // Setup for messages respective model types (on both UARTs) if (strncmp(model_name, "VN-1", 4) == 0) { @@ -431,9 +432,7 @@ void AP_ExternalAHRS_VectorNav::initialize() { type = TYPE::VN_AHRS; // This assumes unit is still configured at its default rate of 800hz - hal.util->snprintf(message_to_send, sizeof(message_to_send), - "VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); - send_command_blocking(); + run_command("VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); } else { // Default to setup for sensors other than VN-100 or VN-110 // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others @@ -444,17 +443,12 @@ void AP_ExternalAHRS_VectorNav::initialize() { if (strncmp(model_name, "VN-3", 4) == 0) { has_dual_gnss = true; } - hal.util->snprintf(message_to_send, sizeof(message_to_send), - "VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); - send_command_blocking(); - hal.util->snprintf(message_to_send, sizeof(message_to_send), - "VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); - send_command_blocking(); + run_command("VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); + run_command("VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); } // Resume asynchronous communications - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNASY,1"); - send_command_blocking(); + run_command("VNASY,1"); setup_complete = true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index d3b9ab26dd230..db73138befe34 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -66,7 +66,7 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { void process_ins_packet1(const uint8_t *b); void process_ins_packet2(const uint8_t *b); void process_ahrs_packet(const uint8_t *b); - void send_command_blocking(); + void run_command(const char *fmt, ...); uint8_t *pktbuf; uint16_t pktoffset; From a8370f324b021f9030b56912d7ac6b76b7fa3dda Mon Sep 17 00:00:00 2001 From: BLash Date: Tue, 16 Jul 2024 00:48:01 -0500 Subject: [PATCH 6/7] AP_ExternalAHRS_VectorNav: Update VectorNav sim to respond to received commands Aside from the RRG,1 call, the simulation should reply with exactly what was received as a receipt confirmation to allow the driver to proceed past init --- libraries/SITL/SIM_VectorNav.cpp | 12 +++++++----- libraries/SITL/SIM_VectorNav.h | 2 ++ 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index 2c7d54953c02a..09bce92994624 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -213,11 +213,13 @@ void VectorNav::update(void) send_packet2(); } - // Strictly we should send this in responce to the request - // but sending it occasionally acheaves the same thing - if (now - last_type_us >= 1000000) { - last_type_us = now; - nmea_printf("$VNRRG,01,VN-300-SITL"); + 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) { + nmea_printf("$VNRRG,01,VN-300-SITL"); + } else { + nmea_printf("$%s", receive_buf); + } } } diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index 589539e5d9a8f..b9f7e3962d0dd 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -45,6 +45,8 @@ class VectorNav : public SerialDevice { uint32_t last_pkt2_us; uint32_t last_type_us; + char receive_buf[50]; + void send_packet1(); void send_packet2(); void nmea_printf(const char *fmt, ...); From 93673c7e47b0ae4647703c653cbea7ad181b90df Mon Sep 17 00:00:00 2001 From: BLash Date: Tue, 16 Jul 2024 16:18:22 -0500 Subject: [PATCH 7/7] AP_ExternalAHRS_VectorNav: Move allocation to local scope Move receive_buf to method scope allocation rather than class scope --- libraries/SITL/SIM_VectorNav.cpp | 1 + libraries/SITL/SIM_VectorNav.h | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index 09bce92994624..ed4c1bcaedd53 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -213,6 +213,7 @@ void VectorNav::update(void) send_packet2(); } + 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) { diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index b9f7e3962d0dd..589539e5d9a8f 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -45,8 +45,6 @@ class VectorNav : public SerialDevice { uint32_t last_pkt2_us; uint32_t last_type_us; - char receive_buf[50]; - void send_packet1(); void send_packet2(); void nmea_printf(const char *fmt, ...);