From d176776357e3088fd4b72a30810320976423c969 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 25 Jan 2019 11:26:38 +1100
Subject: [PATCH] HAL_SITL: support limiting baudrate of simulated telemetry

---
 libraries/AP_HAL_SITL/UARTDriver.cpp | 20 ++++++++++++++++++--
 libraries/AP_HAL_SITL/UARTDriver.h   |  1 +
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index 4c61e300f5..ca51059538 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -57,8 +57,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
 
     const char *path = _sitlState->_uart_path[_portNumber];
 
-    // default to 1MBit
-    _uart_baudrate = 1000000U;
+    if (baud != 0) {
+        _uart_baudrate = baud;
+    }
     
     if (strcmp(path, "GPS1") == 0) {
         /* gps */
@@ -612,9 +613,22 @@ void UARTDriver::_timer_tick(void)
         return;
     }
     ssize_t nwritten;
+    uint32_t max_bytes = 10000;
+    SITL::SITL *_sitl = AP::sitl();
+    if (_sitl && _sitl->telem_baudlimit_enable) {
+        // limit byte rate to configured baudrate
+        uint32_t now = AP_HAL::micros();
+        float dt = 1.0e-6 * (now - last_tick_us);
+        max_bytes = _uart_baudrate * dt / 10;
+        if (max_bytes == 0) {
+            return;
+        }
+        last_tick_us = now;
+    }
 
     if (_packetise) {
         uint16_t n = _writebuffer.available();
+        n = MIN(n, max_bytes);
         if (n > 0) {
             n = mavlink_packetise(_writebuffer, n);
         }
@@ -631,6 +645,7 @@ void UARTDriver::_timer_tick(void)
         uint32_t navail;
         const uint8_t *readptr = _writebuffer.readptr(navail);
         if (readptr && navail > 0) {
+            navail = MIN(navail, max_bytes);
             if (!_use_send_recv) {
                 nwritten = ::write(_fd, readptr, navail);
                 if (nwritten == -1 && errno != EAGAIN && _uart_path) {
@@ -651,6 +666,7 @@ void UARTDriver::_timer_tick(void)
     if (space == 0) {
         return;
     }
+    space = MIN(space, max_bytes);
     
     char buf[space];
     ssize_t nread = 0;
diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index 808942c6fb..2083c675f8 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -123,6 +123,7 @@ private:
     bool _is_udp;
     bool _packetise;
     uint16_t _mc_myport;
+    uint32_t last_tick_us;
 };
 
 #endif