AP_HAL_SITL: add packet loss and delay to each uart

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>
@@ -266,25 +268,67 @@ void UARTDriver::_flush(void)
 size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
 {
     const auto _txspace = txspace();
-    if (_txspace < size) {
+    if (_txspace < size && latencyQueueWrite.empty()) {
+        // If we're using a latecy queue, we don't actually care about txspace
+        // at this point, as we're not going to write to the device yet.
         size = _txspace;
     }
     if (size <= 0) {
         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[_portNumber].get() * 1000000 : 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
 
     // Include lost byte in tx count, we think we sent it even though it was never added to the write buffer
@@ -952,6 +996,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[_portNumber].get() * 1000000 : 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();
@@ -1039,4 +1125,3 @@ void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint
 #endif
 
 #endif // CONFIG_HAL_BOARD
-

libraries/AP_HAL_SITL/UARTDriver.h

@@ -5,6 +5,8 @@
 
 #include <stdint.h>
 #include <stdarg.h>
+#include <deque>
+#include <vector>
 #include "AP_HAL_SITL_Namespace.h"
 #include <AP_HAL/utility/Socket_native.h>
 #include <AP_HAL/utility/RingBuffer.h>
@@ -13,6 +15,12 @@
 
 #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;
@@ -123,6 +131,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];

libraries/AP_Scripting/examples/link-simulator.lua

@@ -0,0 +1,166 @@
+--[[
+    Radio link simulator example
+
+    This script provides some examples of how to use the Sn_PKT_LOSS and
+    Sn_DELAY parameters to simulate different types of radio links to the
+    aircaft.
+
+    1. Direct link
+      - This is a direct RF link between the aircraft and the ground station.
+        There is no delay, but packet loss increases as the aircraft moves
+        further away from the ground station.
+    2. Cellular link
+      - This simulates a cellular link with a 200ms delay. At random intervals,
+        there is 100% loss on the link for a period of time to simulate
+        something like tower switching. The packet loss is independent of the
+        aircraft location (i.e., the script assumes perfect coverage).
+    3. Satellite link
+      - This simulates a link with a constant delay, but is otherwise perfect.
+        The stream rates are set to 1Hz for all streams.
+
+    All three of these links are simulated at the same time. Set serial ports
+    1-3 up as MAVLink and connect to port 5760 for direct link, 5762 for
+    cellular, and 5763 for satellite.
+--]]
+
+-- bind a parameter to a variable
+local function bind_param(name)
+    local p = Parameter()
+    assert(p:init(name), string.format('could not find %s parameter', name))
+    return p
+end
+
+local DIRECT_PACKET_LOSS = bind_param("SIM_S0_PKT_LOSS")
+local DIRECT_DELAY = bind_param("SIM_S0_DELAY")
+local LTE_PACKET_LOSS = bind_param("SIM_S1_PKT_LOSS")
+local LTE_DELAY = bind_param("SIM_S1_DELAY")
+local SATCOM_DELAY = bind_param("SIM_S2_DELAY")
+
+--[[
+    Constants for direct radio link
+
+    Loss is modelled (too) simply as a piecewise linear function of discance
+    from the home position. The loss percentage grows to DIRECT_LINK_RANGE1_LOSS
+    at a distance of DIRECT_LINK_RANGE1, and then grows linearly to 100% at a
+    distance of DIRECT_LINK_MAX_RANGE.
+--]]
+DIRECT_DELAY:set(0)                -- No delay
+local DIRECT_LINK_RANGE1 = 5000    -- Range (m) at which the link rapidly degrades
+local DIRECT_LINK_RANGE1_LOSS = 2  -- Packet loss (%) at RANGE1
+local DIRECT_LINK_MAX_RANGE = 8000 -- Range (m) at which the link is completely lost
+
+-- Constants for cellular link
+LTE_DELAY:set(0.2)             -- 200ms delay
+local LTE_SHORT_INTERVAL = 120 -- Time (s), on average, between short loss events
+local LTE_SHORT_DURATION = 5   -- Time (s) that short loss events last
+local LTE_LONG_INTERVAL = 600  -- Time (s), on average, between long loss events
+local LTE_LONG_DURATION = 25   -- Time (s) that long loss events last
+
+-- Constants for satellite link
+SATCOM_DELAY:set(2) -- 2s delay
+
+-- Force satcom serial options to ignore GCS stream rate requests
+local SATCOM_OPTIONS = bind_param("SERIAL2_OPTIONS")
+SATCOM_OPTIONS:set_and_save(SATCOM_OPTIONS:get() | 0x1000)
+
+-- Set satcom stream rates
+-- (The 2 here is the number of the MAVLink stream, not the serial port number;
+-- in this case, that happens to be the same, because Serials 0-2 are MAVLink,
+-- but watch out)
+param:set_and_save("SR2_RAW_SENS", 1)
+param:set_and_save("SR2_EXT_STAT", 1)
+param:set_and_save("SR2_RC_CHAN", 1)
+param:set_and_save("SR2_RAW_CTRL", 1)
+param:set_and_save("SR2_POSITION", 1)
+param:set_and_save("SR2_EXTRA1", 1)
+param:set_and_save("SR2_EXTRA2", 1)
+param:set_and_save("SR2_EXTRA3", 1)
+param:set_and_save("SR2_PARAMS", 0) -- Don't stream params
+param:set_and_save("SR2_ADSB", 0)   -- Don't stream ADSB
+
+local MAV_SEVERITY_INFO = 6
+
+-- Function to update packet loss of the direct link based on distance
+local function update_direct_link_loss()
+    local distance = 0
+    local home_pos = ahrs:get_relative_position_NED_home()
+    if home_pos then
+        distance = home_pos:length()
+    end
+    local loss
+    if distance < DIRECT_LINK_RANGE1 then
+        loss = (distance / DIRECT_LINK_RANGE1) * DIRECT_LINK_RANGE1_LOSS
+    elseif distance < DIRECT_LINK_MAX_RANGE then
+        loss = DIRECT_LINK_RANGE1_LOSS +
+            ((distance - DIRECT_LINK_RANGE1) / (DIRECT_LINK_MAX_RANGE - DIRECT_LINK_RANGE1)) *
+            (100 - DIRECT_LINK_RANGE1_LOSS)
+    else
+        loss = 100
+    end
+    DIRECT_PACKET_LOSS:set(loss)
+end
+
+-- Timers for packet loss events
+local next_short_loss_time = -math.log(1.0 - math.random()) * LTE_SHORT_INTERVAL
+local next_long_loss_time = -math.log(1.0 - math.random()) * LTE_LONG_INTERVAL
+local short_loss_timer = 0
+local long_loss_timer = 0
+
+local function update_lte_loss_events()
+    -- Decrement timers
+    next_short_loss_time = next_short_loss_time - 1
+    next_long_loss_time = next_long_loss_time - 1
+
+    -- Handle short loss event
+    if next_short_loss_time <= 0 then
+        next_short_loss_time = -math.log(1.0 - math.random()) * LTE_SHORT_INTERVAL
+        -- Print the time of the next loss event
+        gcs:send_text(MAV_SEVERITY_INFO, string.format("LTE short packet loss. Next: %.0f", next_short_loss_time))
+        LTE_PACKET_LOSS:set(100)
+        short_loss_timer = LTE_SHORT_DURATION
+    end
+
+    -- Handle long loss event
+    if next_long_loss_time <= 0 then
+        next_long_loss_time = -math.log(1.0 - math.random()) * LTE_LONG_INTERVAL
+        gcs:send_text(MAV_SEVERITY_INFO, string.format("LTE long packet loss. Next: %.0f", next_long_loss_time))
+        LTE_PACKET_LOSS:set(100)
+        long_loss_timer = LTE_LONG_DURATION
+    end
+
+    -- Reset packet loss after duration
+    if short_loss_timer > 0 then
+        short_loss_timer = short_loss_timer - 1
+        if short_loss_timer <= 0 then
+            LTE_PACKET_LOSS:set(0)
+        end
+    end
+
+    if long_loss_timer > 0 then
+        long_loss_timer = long_loss_timer - 1
+        if long_loss_timer <= 0 then
+            LTE_PACKET_LOSS:set(0)
+        end
+    end
+end
+
+local function update()
+    update_direct_link_loss()
+    update_lte_loss_events()
+end
+
+local function protected_wrapper()
+    local success, err = pcall(update)
+    if not success then
+        gcs:send_text(0, "Internal Error: " .. err)
+        -- when we fault we run the update function again after 1s, slowing it
+        -- down a bit so we don't flood the console with errors
+        return protected_wrapper, 5000
+    end
+    return protected_wrapper, 1000
+end
+
+gcs:send_text(MAV_SEVERITY_INFO, "Loaded radio simulator")
+
+-- start running update loop
+return protected_wrapper()

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -321,6 +321,15 @@ void AP_Vehicle::setup()
     AP_Param::check_var_info();
     load_parameters();
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // at boot, set uart packet loss and delay to zero to prevent accidentally
+    // making the autopilot permanently 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[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