diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 8a9600ca4d6c1..b9058f7d10113 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include @@ -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 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 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 - diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 11a272d7524ef..11005dd0e0327 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -5,6 +5,8 @@ #include #include +#include +#include #include "AP_HAL_SITL_Namespace.h" #include #include @@ -13,6 +15,12 @@ #include +// Timestamped packets to push into a double-ended queue to simulate latency +struct TimestampedData { + uint64_t timestamp_us; + std::vector 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 latencyQueueWrite; + std::deque latencyQueueRead; + struct { bool active; uint8_t term[20]; diff --git a/libraries/AP_Scripting/examples/link-simulator.lua b/libraries/AP_Scripting/examples/link-simulator.lua new file mode 100644 index 0000000000000..cb9c9f35647b7 --- /dev/null +++ b/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() diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b05851bdfe7f8..c7394e8a2a34b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/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 diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 7ca40de3d9d5c..8ad221705169f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -543,6 +543,7 @@ const AP_Param::GroupInfo SIM::var_info3[] = { AP_GROUPINFO("OSD_ROWS", 54, SIM, osd_rows, 16), #endif + AP_SUBGROUPEXTENSION("", 62, SIM, var_uart), #ifdef SFML_JOYSTICK AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick), #endif // SFML_JOYSTICK @@ -861,6 +862,137 @@ const AP_Param::GroupInfo SIM::var_mag[] = { AP_GROUPEND }; +const AP_Param::GroupInfo SIM::var_uart[] = +{ + // @Param: S0_PKT_LOSS + // @DisplayName: UART 0 Packet Loss + // @Description: Sets percentage of incoming/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[0], 0), + // @Param: S1_PKT_LOSS + // @DisplayName: UART 1 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 1. 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("S1_PKT_LOSS", 2, SIM, uart_pkt_loss_pct[1], 0), + // @Param: S2_PKT_LOSS + // @DisplayName: UART 2 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 2. 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("S2_PKT_LOSS", 3, SIM, uart_pkt_loss_pct[2], 0), + // @Param: S3_PKT_LOSS + // @DisplayName: UART 3 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 3. 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("S3_PKT_LOSS", 4, SIM, uart_pkt_loss_pct[3], 0), + // @Param: S4_PKT_LOSS + // @DisplayName: UART 4 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 4. 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("S4_PKT_LOSS", 5, SIM, uart_pkt_loss_pct[4], 0), + // @Param: S5_PKT_LOSS + // @DisplayName: UART 5 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 5. 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("S5_PKT_LOSS", 6, SIM, uart_pkt_loss_pct[5], 0), + // @Param: S6_PKT_LOSS + // @DisplayName: UART 6 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 6. 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("S6_PKT_LOSS", 7, SIM, uart_pkt_loss_pct[6], 0), + // @Param: S7_PKT_LOSS + // @DisplayName: UART 7 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 7. 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("S7_PKT_LOSS", 8, SIM, uart_pkt_loss_pct[7], 0), + // @Param: S8_PKT_LOSS + // @DisplayName: UART 8 Packet Loss + // @Description: Sets percentage of incoming/outgoing packet loss on UART 8. 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("S8_PKT_LOSS", 9, SIM, uart_pkt_loss_pct[8], 0), + // @Param: S0_DELAY + // @DisplayName: UART 0 Delay + // @Description: Sets in/out delay in seconds 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: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S0_DELAY", 10, SIM, uart_pkt_delay[0], 0), + // @Param: S1_DELAY + // @DisplayName: UART 1 Delay + // @Description: Sets in/out delay in seconds for UART 1 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S1_DELAY", 11, SIM, uart_pkt_delay[1], 0), + // @Param: S2_DELAY + // @DisplayName: UART 2 Delay + // @Description: Sets in/out delay in seconds for UART 2 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S2_DELAY", 12, SIM, uart_pkt_delay[2], 0), + // @Param: S3_DELAY + // @DisplayName: UART 3 Delay + // @Description: Sets in/out delay in seconds for UART 3 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S3_DELAY", 13, SIM, uart_pkt_delay[3], 0), + // @Param: S4_DELAY + // @DisplayName: UART 4 Delay + // @Description: Sets in/out delay in seconds for UART 4 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S4_DELAY", 14, SIM, uart_pkt_delay[4], 0), + // @Param: S5_DELAY + // @DisplayName: UART 5 Delay + // @Description: Sets in/out delay in seconds for UART 5 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S5_DELAY", 15, SIM, uart_pkt_delay[5], 0), + // @Param: S6_DELAY + // @DisplayName: UART 6 Delay + // @Description: Sets in/out delay in seconds for UART 6 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S6_DELAY", 16, SIM, uart_pkt_delay[6], 0), + // @Param: S7_DELAY + // @DisplayName: UART 7 Delay + // @Description: Sets in/out delay in seconds for UART 7 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S7_DELAY", 17, SIM, uart_pkt_delay[7], 0), + // @Param: S8_DELAY + // @DisplayName: UART 8 Delay + // @Description: Sets in/out delay in seconds for UART 8 to simulate ping time. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. + // @Units: s + // @Range: 0 30 + // @User: Advanced + AP_GROUPINFO("S8_DELAY", 18, SIM, uart_pkt_delay[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 index 4d8beb2eddd9b..6022b35bb28b7 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -117,6 +117,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 @@ -168,6 +169,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 @@ -256,6 +259,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[9]; #ifdef SFML_JOYSTICK AP_Int8 sfml_joystick_id;