Obey radio status in opportunistic transmissions as well. Last missing bit are adaptive rates

1 files changed, 28 insertions, 1 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e6b558ca6..e7e96dc3a 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;
 	}