diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 9c90928630dea3..a4933a364638f1 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 @@ -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 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 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 index d47378a8054dae..9972fef37bce51 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 @@ -12,6 +14,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; @@ -115,6 +123,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_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 9908cf767d60fe..42cd9ea16d6ede 100644 --- 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 index 2ff2220aa7ed7f..0113d530f51728 100644 --- 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 index 24dbdac91a6aec..746d4b73f63fd0 100644 --- 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;