From 9c74474196e222e60ac01da362dfdafff40d45af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 15 Nov 2020 13:15:44 +1100 Subject: [PATCH] GCS_MAVLink: setup baudrates for passthru serial ports --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 5 ++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 67631e00db..7110494bbd 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1017,6 +1017,8 @@ private: uint32_t start_ms; uint32_t last_ms; uint32_t last_port1_data_ms; + uint32_t baud1; + uint32_t baud2; uint8_t timeout_s; HAL_Semaphore sem; } _passthru; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 90a2fcf281..f4bc4e147c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4937,7 +4937,8 @@ void GCS::update_passthru(void) WITH_SEMAPHORE(_passthru.sem); uint32_t now = AP_HAL::millis(); - bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s); + bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, + _passthru.baud1, _passthru.baud2); if (enabled && !_passthru.enabled) { _passthru.start_ms = now; _passthru.last_ms = 0; @@ -4983,6 +4984,8 @@ void GCS::passthru_timer(void) return; } _passthru.start_ms = 0; + _passthru.port1->begin(_passthru.baud1); + _passthru.port2->begin(_passthru.baud2); } // while pass-thru is enabled lock both ports. They remain