From 1f3625371d787cdc452b45ad9d9a01423ae51f96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 10:18:00 +0200 Subject: [PATCH] Obey radio status in opportunistic transmissions as well. Last missing bit are adaptive rates --- src/modules/mavlink/mavlink_main.cpp | 29 +++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6b558ca69..e7e96dc3a4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -359,7 +359,34 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - return buf_free; + if (_rstatus.timestamp > 0 && + (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { + + unsigned low_buf; + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + low_buf = 50; + break; + default: + low_buf = 50; + break; + } + + if (_rstatus.txbuf < low_buf) { + /* + * If the TX buf measure is initialized and up to date and low + * return 0 to slow event based transmission + */ + return 0; + } else { + /* there is no SW flow control option, just use what our buffer tells us */ + return buf_free; + } + + } else { + return buf_free; + } } else { return 0; }