From 2cb3265131b589742e0786e04715097676dc2131 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 18 Jul 2023 12:04:09 +1000
Subject: [PATCH] AP_HAL_SITL: factor _timer_tick into read/write methods

---
 libraries/AP_HAL_SITL/UARTDriver.cpp | 39 +++++++++++++++++++++++++---
 libraries/AP_HAL_SITL/UARTDriver.h   |  9 ++++++-
 2 files changed, 43 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index 64ffbf9377..2c75d77e15 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -796,7 +796,7 @@ uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space)
     return i;
 }
 
-void UARTDriver::_timer_tick(void)
+void UARTDriver::handle_writing_from_writebuffer_to_device()
 {
     if (!_connected) {
         _check_reconnect();
@@ -809,12 +809,12 @@ void UARTDriver::_timer_tick(void)
     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);
+        float dt = 1.0e-6 * (now - last_write_tick_us);
         max_bytes = _uart_baudrate * dt / 10;
         if (max_bytes == 0) {
             return;
         }
-        last_tick_us = now;
+        last_write_tick_us = now;
     }
 #endif
     if (_packetise) {
@@ -856,13 +856,37 @@ void UARTDriver::_timer_tick(void)
             }
         }
     }
+}
+
+void UARTDriver::handle_reading_from_device_to_readbuffer()
+{
+    if (!_connected) {
+        _check_reconnect();
+        return;
+    }
 
     uint32_t space = _readbuffer.space();
     if (space == 0) {
         return;
     }
+
+    uint32_t max_bytes = 10000;
+#if !defined(HAL_BUILD_AP_PERIPH)
+    SITL::SIM *_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_read_tick_us);
+        max_bytes = _uart_baudrate * dt / 10;
+        if (max_bytes == 0) {
+            return;
+        }
+        last_read_tick_us = now;
+    }
+#endif
+
     space = MIN(space, max_bytes);
-    
+
     char buf[space];
     ssize_t nread = 0;
     if (_mc_fd >= 0) {
@@ -929,6 +953,13 @@ void UARTDriver::_timer_tick(void)
     }
 }
 
+void UARTDriver::_timer_tick(void)
+{
+    handle_writing_from_writebuffer_to_device();
+    handle_reading_from_device_to_readbuffer();
+}
+
+
 /*
   return timestamp estimate in microseconds for when the start of
   a nbytes packet arrived on the uart. This should be treated as a
diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index 02878d2fd2..1538908864 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -106,7 +106,10 @@ private:
     bool _is_udp;
     bool _packetise;
     uint16_t _mc_myport;
-    uint32_t last_tick_us;
+
+    // for baud-rate limiting:
+    uint32_t last_read_tick_us;
+    uint32_t last_write_tick_us;
 
     SITL::SerialDevice *_sim_serial_device;
 
@@ -134,6 +137,10 @@ protected:
     void _end() override;
     void _flush() override;
     bool _discard_input() override;
+
+private:
+    void handle_writing_from_writebuffer_to_output();
+    void handle_reading_from_device_to_readbuffer();
 };
 
 #endif