diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index f43ac8598d6e5f..aef7fe26528a97 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -286,12 +286,16 @@ bool AP_ExternalAHRS_VectorNav::check_uart() return true; } -// Send command to read given register number and wait for response -// Only run from thread! This blocks until a response is received +// 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::wait_register_responce(const uint8_t register_num) +void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...) { - nmea.register_number = register_num; + 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) { @@ -299,8 +303,7 @@ void AP_ExternalAHRS_VectorNav::wait_register_responce(const uint8_t register_nu 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; } @@ -308,6 +311,10 @@ void AP_ExternalAHRS_VectorNav::wait_register_responce(const uint8_t register_nu while (nbytes-- > 0) { char c = uart->read(); if (decode(c)) { + if (nmea.error_response && nmea.sentence_done) { + // Received a valid VNERR. Try to resend after the timeout length + break; + } return; } } @@ -356,6 +363,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; } @@ -371,57 +379,60 @@ 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, "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; } - break; - - case 1: - if (nmea.register_number != strtoul(nmea.term, nullptr, 10)) { + return true; + case 1: + if (nmea.error_response) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term); + } else if (strlen(message_to_send) > 6 && + 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)); - 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)); + } + return true; default: - return false; + return true; } - 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); + // Pause asynchronous communications to simplify packet finding + run_command("VNASY,0"); - // Stop NMEA Async Outputs (this UART only) - nmea_printf(uart, "$VNWRG,6,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 + run_command("VNWRG,06,0,1"); + run_command("VNWRG,06,0,2"); - // Detect version // Read Model Number Register, ID 1 - wait_register_responce(1); + run_command("VNRRG,01"); // 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())); - + 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 @@ -432,11 +443,17 @@ 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)); + 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 + run_command("VNASY,1"); 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 05c4a01fae6829..db73138befe348 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 run_command(const char *fmt, ...); uint8_t *pktbuf; uint16_t pktoffset; @@ -83,22 +85,22 @@ 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 + bool error_response; // true if received a VNERR response } nmea; - }; #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index 2c7d54953c02a9..ed4c1bcaedd53d 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -213,11 +213,14 @@ 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"); + 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) { + nmea_printf("$VNRRG,01,VN-300-SITL"); + } else { + nmea_printf("$%s", receive_buf); + } } }