diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 7485c58c63..8af669e66e 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -770,5 +770,25 @@ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes) return last_receive_us; } +ssize_t UARTDriver::get_system_outqueue_length() const +{ + if (!_connected) { + return 0; + } + +#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) + return 0; +#elif defined(__APPLE__) && defined(__MACH__) + return 0; +#else + int size; + if (ioctl(_fd, TIOCOUTQ, &size) == -1) { + // ::fprintf(stderr, "ioctl TIOCOUTQ failed: %m\n"); + return 0; + } + return size; +#endif +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index cd5a4e4f72..6c70b4080a 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -33,6 +33,8 @@ public: return true; } + ssize_t get_system_outqueue_length() const; + void set_blocking_writes(bool blocking) override { _nonblocking_writes = !blocking;