AP_HAL_SITL: add packet loss and delay to all uart

ArduPilot · Dec 14, 2023 · 2f3a4c3 · 2f3a4c3
1 parent 56cc78b
commit 2f3a4c3
Show file tree

Hide file tree

Showing 5 changed files with 157 additions and 8 deletions.
diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -25,6 +25,8 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <stdarg.h>
+#include <deque>
+#include <vector>
 #include <AP_Math/AP_Math.h>
 
 #include <errno.h>
@@ -253,21 +255,62 @@ size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
         return 0;
     }
 
-        /*
-          simulate byte loss at the link layer
-         */
-        uint8_t lost_byte = 0;
+    uint8_t lost_byte = 0;
 #if !defined(HAL_BUILD_AP_PERIPH)
-        SITL::SIM *_sitl = AP::sitl();
+    SITL::SIM *_sitl = AP::sitl();
+
+    // simulate byte loss at the link layer
+    if (_sitl && _sitl->uart_byte_loss_pct > 0) {
+        if (fabsf(rand_float()) < _sitl->uart_byte_loss_pct.get() * 0.01 * size) {
+            lost_byte = 1;
+        }
+    }
 
-        if (_sitl && _sitl->uart_byte_loss_pct > 0) {
-            if (fabsf(rand_float()) < _sitl->uart_byte_loss_pct.get() * 0.01 * size) {
-                lost_byte = 1;
+    // simulate whole packet loss rate per uart driver
+    const float pkt_loss_pct = _sitl ? _sitl->uart_pkt_loss_pct[_portNumber].get() : 0;
+    if (_sitl && pkt_loss_pct > 0) {
+        if (fabsf(rand_float()) < pkt_loss_pct * 0.01) {
+            // Pretend we wrote the whole packet
+            return size;
+        }
+    }
+
+    // simulate communication delay
+    const float delay_us = _sitl ? _sitl->uart_pkt_delay_ms[_portNumber].get() * 1000 : 0;
+    if (delay_us > 0 || !latencyQueueWrite.empty()) {
+        // Copy data to a new vector
+        std::vector<uint8_t> dataVec(buffer, buffer + size);
+
+        // Push to deque
+        uint64_t now = AP_HAL::micros64();
+        latencyQueueWrite.push_back({now, dataVec});
+
+        // Check and send data if latency period is passed
+        while (!latencyQueueWrite.empty()) {
+            TimestampedData& front = latencyQueueWrite.front();
+            if (front.data.size() > _writebuffer.space()) {
+                break; // Not enough space in writebuffer
             }
+            if (now - front.timestamp_us >= delay_us) {
+                _writebuffer.write(front.data.data(), front.data.size());
+                latencyQueueWrite.pop_front();
+            } else {
+                break; // The rest of the queue has not reached the latency period
+            }
+        }
+
+        if (_unbuffered_writes) {
+            handle_writing_from_writebuffer_to_device();
         }
+
+        // Return the number of bytes that have landed on the queue
+        return size;
+    }
+
 #endif // HAL_BUILD_AP_PERIPH
 
 
+
     const size_t ret = _writebuffer.write(buffer, size - lost_byte) + lost_byte;
     if (_unbuffered_writes) {
         handle_writing_from_writebuffer_to_device();
@@ -930,6 +973,48 @@ void UARTDriver::handle_reading_from_device_to_readbuffer()
             return;
         }
     }
+
+#if !defined(HAL_BUILD_AP_PERIPH)
+    // Simulate packet loss
+    const float _packet_loss_pct = _sitl ? _sitl->uart_pkt_loss_pct[_portNumber].get() : 0;
+    if (_packet_loss_pct > 0) {
+        if (fabsf(rand_float()) < _packet_loss_pct * 0.01) {
+            // Pretend we read nothing
+            return;
+        }
+    }
+
+    // Simulate communication delay
+    const float delay_us = _sitl ? _sitl->uart_pkt_delay_ms[_portNumber].get() * 1000 : 0;
+    if (delay_us > 0 || !latencyQueueRead.empty()) {
+        uint64_t now = AP_HAL::micros64();
+        if (nread > 0) {
+            // Copy data to a new vector
+            std::vector<uint8_t> dataVec(buf, buf + nread);
+
+            // Push to deque
+            latencyQueueRead.push_back({now, dataVec});
+        }
+
+        // Pop data from the queue if latency period is passed
+        while (!latencyQueueRead.empty()) {
+            TimestampedData& front = latencyQueueRead.front();
+            if (front.data.size() > _readbuffer.space()) {
+                break; // Not enough space in the buffer
+            }
+            if (now - front.timestamp_us >= delay_us) {
+                _readbuffer.write(front.data.data(), front.data.size());
+                latencyQueueRead.pop_front();
+                _receive_timestamp = now;
+            } else {
+                break; // The rest of the queue has not reached the latency period
+            }
+        }
+        return;
+    }
+#endif // HAL_BUILD_AP_PERIPH
+
+    // Handle normal operation
     if (nread > 0) {
         _readbuffer.write((uint8_t *)buf, nread);
         _receive_timestamp = AP_HAL::micros64();

diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -5,13 +5,21 @@
 
 #include <stdint.h>
 #include <stdarg.h>
+#include <deque>
+#include <vector>
 #include "AP_HAL_SITL_Namespace.h"
 #include <AP_HAL/utility/Socket.h>
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_CSVReader/AP_CSVReader.h>
 
 #include <SITL/SIM_SerialDevice.h>
 
+// Timestamped packets to push into a double-ended queue to simulate latency
+struct TimestampedData {
+    uint64_t timestamp_us;
+    std::vector<uint8_t> data;
+};
+
 class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
 public:
     friend class HALSITL::SITL_State;
@@ -115,6 +123,10 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
 
     SITL::SerialDevice *_sim_serial_device;
 
+    // Double-ended queues to simulate latency
+    std::deque<TimestampedData> latencyQueueWrite;
+    std::deque<TimestampedData> latencyQueueRead;
+
     struct {
         bool active;
         uint8_t term[20];

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -286,6 +286,15 @@ void AP_Vehicle::setup()
     AP_Param::check_var_info();
     load_parameters();
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // set uart packet loss and delay to zero to prevent accidentally
+    // making the autopilot unreachable
+    for (uint8_t i=0; i<9; i++) {
+        AP::sitl()->uart_pkt_loss_pct[i].set_and_save(0);
+        AP::sitl()->uart_pkt_delay_ms[i].set_and_save(0);
+    }
+#endif
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
     if (AP_BoardConfig::get_sdcard_slowdown() != 0) {
         // user wants the SDcard slower, we need to remount

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
@@ -455,6 +455,7 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     // @Bitmask: 0:MAVLink,3:SageTechMXS
     AP_GROUPINFO("ADSB_TYPES",    52, SIM,  adsb_types, 1),
 
+    AP_SUBGROUPEXTENSION("",      62, SIM,  var_uart),
 #ifdef SFML_JOYSTICK
     AP_SUBGROUPEXTENSION("",      63, SIM,  var_sfml_joystick),
 #endif // SFML_JOYSTICK
@@ -768,6 +769,43 @@ const AP_Param::GroupInfo SIM::var_mag[] = {
     AP_GROUPEND
 };
 
+// this mapping captures the historical use of uartB as SERIAL3
+const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 };
+const AP_Param::GroupInfo SIM::var_uart[] =
+{
+    // @Param: S0_PKT_LOSS
+    // @DisplayName: UART 0 Packet Loss
+    // @Description: Sets percentage of incomming/outgoing packet loss on UART 0. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted.
+    // @Units: %
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("S0_PKT_LOSS",    1, SIM,  uart_pkt_loss_pct[mapping[0]], 0),
+    AP_GROUPINFO("S1_PKT_LOSS",    2, SIM,  uart_pkt_loss_pct[mapping[1]], 0),
+    AP_GROUPINFO("S2_PKT_LOSS",    3, SIM,  uart_pkt_loss_pct[mapping[2]], 0),
+    AP_GROUPINFO("S3_PKT_LOSS",    4, SIM,  uart_pkt_loss_pct[mapping[3]], 0),
+    AP_GROUPINFO("S4_PKT_LOSS",    5, SIM,  uart_pkt_loss_pct[mapping[4]], 0),
+    AP_GROUPINFO("S5_PKT_LOSS",    6, SIM,  uart_pkt_loss_pct[mapping[5]], 0),
+    AP_GROUPINFO("S6_PKT_LOSS",    7, SIM,  uart_pkt_loss_pct[mapping[6]], 0),
+    AP_GROUPINFO("S7_PKT_LOSS",    8, SIM,  uart_pkt_loss_pct[mapping[7]], 0),
+    AP_GROUPINFO("S8_PKT_LOSS",    9, SIM,  uart_pkt_loss_pct[mapping[8]], 0),
+    // @Param: S0_DELAY_MS
+    // @DisplayName: UART 0 Delay
+    // @Description: Sets in/out delay in milliseconds for UART 0 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted.
+    // @Units: %
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("S0_DELAY_MS",   10, SIM,  uart_pkt_delay_ms[mapping[0]], 0),
+    AP_GROUPINFO("S1_DELAY_MS",   11, SIM,  uart_pkt_delay_ms[mapping[1]], 0),
+    AP_GROUPINFO("S2_DELAY_MS",   12, SIM,  uart_pkt_delay_ms[mapping[2]], 0),
+    AP_GROUPINFO("S3_DELAY_MS",   13, SIM,  uart_pkt_delay_ms[mapping[3]], 0),
+    AP_GROUPINFO("S4_DELAY_MS",   14, SIM,  uart_pkt_delay_ms[mapping[4]], 0),
+    AP_GROUPINFO("S5_DELAY_MS",   15, SIM,  uart_pkt_delay_ms[mapping[5]], 0),
+    AP_GROUPINFO("S6_DELAY_MS",   16, SIM,  uart_pkt_delay_ms[mapping[6]], 0),
+    AP_GROUPINFO("S7_DELAY_MS",   17, SIM,  uart_pkt_delay_ms[mapping[7]], 0),
+    AP_GROUPINFO("S8_DELAY_MS",   18, SIM,  uart_pkt_delay_ms[mapping[8]], 0),
+    AP_GROUPEND
+};
+
 #ifdef SFML_JOYSTICK
 const AP_Param::GroupInfo SIM::var_sfml_joystick[] = {
     AP_GROUPINFO("SF_JS_STICK",    1, SIM,  sfml_joystick_id,   0),

diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
@@ -109,6 +109,7 @@ class SIM {
 #endif
         AP_Param::setup_object_defaults(this, var_mag);
         AP_Param::setup_object_defaults(this, var_ins);
+        AP_Param::setup_object_defaults(this, var_uart);
 #ifdef SFML_JOYSTICK
         AP_Param::setup_object_defaults(this, var_sfml_joystick);
 #endif // SFML_JOYSTICK
@@ -163,6 +164,8 @@ class SIM {
 #endif
     static const struct AP_Param::GroupInfo var_mag[];
     static const struct AP_Param::GroupInfo var_ins[];
+    static const struct AP_Param::GroupInfo var_uart[];
+
 #ifdef SFML_JOYSTICK
     static const struct AP_Param::GroupInfo var_sfml_joystick[];
 #endif //SFML_JOYSTICK
@@ -252,6 +255,8 @@ class SIM {
     AP_Int16 on_hardware_relay_enable_mask;   // mask of relays passed through to actual hardware
 
     AP_Float uart_byte_loss_pct;
+    AP_Float uart_pkt_loss_pct[9];
+    AP_Float uart_pkt_delay_ms[9];
 
 #ifdef SFML_JOYSTICK
     AP_Int8 sfml_joystick_id;