From 0f39760597dd630ad8f4f5998c1e247912a2ee9d Mon Sep 17 00:00:00 2001 From: BLash Date: Fri, 5 Jul 2024 11:40:51 -0500 Subject: [PATCH] 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 3b9a5d3b6b3009..3abec240670cff 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -405,7 +405,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 @@ -439,6 +447,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { send_command_blocking(); } + // Resume asynchronous communications + hal.util->snprintf(message_to_send, sizeof(message_to_send), "$VNASY,1\0"); + send_command_blocking(); setup_complete = true; }