diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 88a29667c8..748dcba9b2 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -89,6 +89,11 @@ ssize_t Serial::write(const void *buffer, size_t buffer_size) return _impl.write(buffer, buffer_size); } +ssize_t Serial::writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_ms) +{ + return _impl.writeBlocking(buffer, buffer_size, timeout_ms); +} + void Serial::flush() { return _impl.flush(); diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index cad568d134..983834ccf6 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -66,6 +66,7 @@ public: ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_ms = 0); ssize_t write(const void *buffer, size_t buffer_size); + ssize_t writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_ms = 0); void flush(); diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index f3a9f2389b..9bb84dd902 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -362,11 +362,68 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) } } - ::fsync(_serial_fd); - return written; } +ssize_t SerialImpl::writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot writeBlocking to serial device until it has been opened"); + return -1; + } + + const uint8_t *data = static_cast(buffer); + size_t total_written = 0; + const hrt_abstime start_time_us = hrt_absolute_time(); + const hrt_abstime timeout_us = timeout_ms * 1000; + + while (total_written < buffer_size) { + if (hrt_elapsed_time(&start_time_us) > timeout_us) { + PX4_WARN("Write timeout, sent %zu", total_written); + break; + } + + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLOUT; + + hrt_abstime elapsed_us = hrt_elapsed_time(&start_time_us); + int remaining_timeout_ms = (timeout_us - elapsed_us) / 1000; + + if (remaining_timeout_ms <= 0) { + break; + } + + int result = ::poll(fds, 1, remaining_timeout_ms); + + if (result < 0) { + PX4_ERR("poll error %d", errno); + return -1; + } + + if (fds[0].revents & POLLOUT) { + // Write as much as we can + ssize_t written = ::write(_serial_fd, data + total_written, buffer_size - total_written); + + if (written < 0) { + if (errno == EAGAIN) { + // Buffer full, wait a bit and try again + px4_usleep(1000); + continue; + } + + PX4_ERR("Write error: %d", errno); + return -1; + + } else if (written > 0) { + total_written += written; + } + } + } + + return total_written; +} + void SerialImpl::flush() { if (_open) { diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index c7fb1ede81..82e4ddd29d 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -64,6 +64,7 @@ public: ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); ssize_t write(const void *buffer, size_t buffer_size); + ssize_t writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_us = 0); void flush(); diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index c7fb1ede81..82e4ddd29d 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -64,6 +64,7 @@ public: ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); ssize_t write(const void *buffer, size_t buffer_size); + ssize_t writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_us = 0); void flush(); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 9ad8d62567..0c23b050e0 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -274,7 +274,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) if (ret < 0) { PX4_DEBUG("%s read error %d", _port, ret); - } return ret; @@ -344,11 +343,68 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) } } - ::fsync(_serial_fd); - return written; } +ssize_t SerialImpl::writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot writeBlocking to serial device until it has been opened"); + return -1; + } + + const uint8_t *data = static_cast(buffer); + size_t total_written = 0; + const hrt_abstime start_time_us = hrt_absolute_time(); + const hrt_abstime timeout_us = timeout_ms * 1000; + + while (total_written < buffer_size) { + if (hrt_elapsed_time(&start_time_us) > timeout_us) { + PX4_WARN("Write timeout, sent %zu", total_written); + break; + } + + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLOUT; + + hrt_abstime elapsed_us = hrt_elapsed_time(&start_time_us); + int remaining_timeout_ms = (timeout_us - elapsed_us) / 1000; + + if (remaining_timeout_ms <= 0) { + break; + } + + int result = ::poll(fds, 1, remaining_timeout_ms); + + if (result < 0) { + PX4_ERR("poll error %d", errno); + return -1; + } + + if (fds[0].revents & POLLOUT) { + // Write as much as we can + ssize_t written = ::write(_serial_fd, data + total_written, buffer_size - total_written); + + if (written < 0) { + if (errno == EAGAIN) { + // Buffer full, wait a bit and try again + px4_usleep(1000); + continue; + } + + PX4_ERR("Write error: %d", errno); + return -1; + + } else if (written > 0) { + total_written += written; + } + } + } + + return total_written; +} + void SerialImpl::flush() { if (_open) { diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index 469bd6db97..c7739429ef 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -63,6 +63,7 @@ public: ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); ssize_t write(const void *buffer, size_t buffer_size); + ssize_t writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_us = 0); void flush(); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index c2c88e43b9..a019244cee 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -276,6 +276,23 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return ret_write; } +ssize_t SerialImpl::writeBlocking(const void *buffer, size_t buffer_size, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); + + if (ret_write < 0) { + PX4_ERR("%s write error %d", _port, ret_write); + + } + + return ret_write; +} + void SerialImpl::flush() { if (_open) {