From a2b0bbffdbb1bb384356d8623b43f41abf82ea57 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 15 Jan 2023 15:55:54 +1100
Subject: [PATCH] HAL_SITL: implement SIM_UART_LOSS

---
 libraries/AP_HAL_SITL/UARTDriver.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index cb896d1810..3e31021087 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -282,7 +282,23 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
         // these have no effect
         tcdrain(_fd);
     } else {
-        _writebuffer.write(buffer, size);
+        /*
+          simulate byte loss at the link layer
+         */
+        size_t nwrite = size;
+#if !defined(HAL_BUILD_AP_PERIPH)
+        SITL::SIM *_sitl = AP::sitl();
+
+        if (_sitl && _sitl->uart_byte_loss_pct > 0) {
+            if (fabsf(rand_float()) < _sitl->uart_byte_loss_pct.get() * 0.01 * size) {
+                nwrite--;
+            }
+            if (nwrite == 0) {
+                return size;
+            }
+        }
+#endif // HAL_BUILD_AP_PERIPH
+        _writebuffer.write(buffer, nwrite);
     }
     return size;
 }