From 3ab3fe2ee7986b2136c9475975b68f46cefea7d9 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 7 Mar 2024 10:27:56 -0800 Subject: [PATCH] Implemented flush for Serial UART API --- platforms/common/Serial.cpp | 5 +++ .../include/px4_platform_common/Serial.hpp | 2 + platforms/nuttx/src/px4/common/SerialImpl.cpp | 7 ++++ .../src/px4/common/include/SerialImpl.hpp | 2 + platforms/posix/include/SerialImpl.hpp | 2 + platforms/posix/src/px4/common/SerialImpl.cpp | 7 ++++ platforms/qurt/include/SerialImpl.hpp | 2 + platforms/qurt/src/px4/SerialImpl.cpp | 9 ++++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 42 ++++++++++--------- 9 files changed, 58 insertions(+), 20 deletions(-) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 2f93a66a6e..5e7faf1a73 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -80,6 +80,11 @@ ssize_t Serial::write(const void *buffer, size_t buffer_size) return _impl.write(buffer, buffer_size); } +void Serial::flush() +{ + return _impl.flush(); +} + uint32_t Serial::getBaudrate() const { return _impl.getBaudrate(); diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index ce02b5ac63..ac5e014cbb 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -64,6 +64,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); + void flush(); + // If port is already open then the following configuration functions // will reconfigure the port. If the port is not yet open then they will // simply store the configuration in preparation for the port to be opened. diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 7490dd604c..61226f5919 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -319,6 +319,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return written; } +void SerialImpl::flush() +{ + if (_open) { + tcflush(_serial_fd, TCIOFLUSH); + } +} + const char *SerialImpl::getPort() const { return _port; diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 58d41bf759..27415a84b7 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -64,6 +64,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); + void flush(); + const char *getPort() const; uint32_t getBaudrate() const; diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index efc95d7d51..e3692fa336 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -64,6 +64,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); + void flush(); + const char *getPort() const; uint32_t getBaudrate() const; diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 79e3422aed..b20278f77f 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -312,6 +312,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return written; } +void SerialImpl::flush() +{ + if (_open) { + tcflush(_serial_fd, TCIOFLUSH); + } +} + const char *SerialImpl::getPort() const { return _port; diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index 1b98d3bb40..d852f68a54 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -63,6 +63,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); + void flush(); + const char *getPort() const; bool setPort(const char *port); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index ec0fb73fce..2eacc84990 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -251,6 +251,15 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return ret_write; } +void SerialImpl::flush() +{ + if (_open) { + uint8_t buffer[4]; + // A read clears out all current data + read(buffer, 4); + } +} + const char *SerialImpl::getPort() const { return _port; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 23389c5789..4ae4569e1e 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -116,11 +116,13 @@ void CrsfRc::Run() { if (should_exit()) { ScheduleClear(); + if (_uart) { (void) _uart->close(); delete _uart; _uart = nullptr; } + exit_and_cleanup(); return; } @@ -152,26 +154,26 @@ void CrsfRc::Run() return; } -// if (board_rc_swap_rxtx(_device)) { -// #if defined(TIOCSSWAP) -// ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); -// #endif // TIOCSSWAP -// } -// -// if (board_rc_singlewire(_device)) { -// _is_singlewire = true; -// #if defined(TIOCSSINGLEWIRE) -// ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); -// #endif // TIOCSSINGLEWIRE -// } -// -// PX4_INFO("Crsf serial opened sucessfully"); -// -// if (_is_singlewire) { -// PX4_INFO("Crsf serial is single wire. Telemetry disabled"); -// } -// -// tcflush(_rc_fd, TCIOFLUSH); + if (board_rc_swap_rxtx(_device)) { +#if defined(TIOCSSWAP) + ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); +#endif // TIOCSSWAP + } + + if (board_rc_singlewire(_device)) { + _is_singlewire = true; +#if defined(TIOCSSINGLEWIRE) + ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif // TIOCSSINGLEWIRE + } + + PX4_INFO("Crsf serial opened sucessfully"); + + if (_is_singlewire) { + PX4_INFO("Crsf serial is single wire. Telemetry disabled"); + } + + _uart->flush(); Crc8Init(0xd5);