diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index 380ebc04d0..7c9c350c01 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -79,7 +79,12 @@ #define ASYNC_UART_READ_WAIT_US 2000 -extern "C" { __EXPORT int dsp_hitl_main(int argc, char *argv[]); } + +extern "C" { + __EXPORT int dsp_hitl_main(int argc, char *argv[]); + __EXPORT int fc_uart_rx_available(int fd, uint32_t *data); + __EXPORT int fc_uart_flush_rx(int fd); +} namespace dsp_hitl { @@ -952,7 +957,17 @@ int readResponse(void *buf, size_t len) return -1; } - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); + uint32_t rx_bytes_available = 0; + (void) fc_uart_rx_available(_uart_fd, &rx_bytes_available); + int bytes_read = 0; + + if (rx_bytes_available) { + bytes_read = qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); + } + + // (void) fc_uart_flush_rx(_uart_fd); + + return bytes_read; } int writeResponse(void *buf, size_t len) diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index 4977b3bd7d..e176f528c9 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -39,6 +39,11 @@ #define MODULE_NAME "SerialImpl" +extern "C" { + __EXPORT int fc_uart_rx_available(int fd, uint32_t *data); + __EXPORT int fc_uart_flush_rx(int fd); +} + namespace device { @@ -160,9 +165,14 @@ bool SerialImpl::close() ssize_t SerialImpl::bytesAvailable() { - // TODO: - PX4_WARN("bytesAvailable not implemented!"); - return 0; + if (!_open) { + PX4_ERR("Device not open!"); + return -1; + } + + uint32_t rx_bytes = 0; + (void) fc_uart_rx_available(_serial_fd, &rx_bytes); + return (ssize_t) rx_bytes; } ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) @@ -267,7 +277,9 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) void SerialImpl::flush() { - // TODO: Flush not implemented yet on Qurt + if (_open) { + (void) fc_uart_flush_rx(_serial_fd); + } } const char *SerialImpl::getPort() const