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, ...);