diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 5e7faf1a73..83c2c5c042 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -135,6 +135,24 @@ bool Serial::setFlowcontrol(FlowControl flowcontrol) return _impl.setFlowcontrol(flowcontrol); } +bool Serial::getSingleWireMode() const +{ + return _impl.getSingleWireMode(); +} +bool Serial::setSingleWireMode() +{ + return _impl.setSingleWireMode(); +} + +bool Serial::getSwapRxTxMode() const +{ + return _impl.getSwapRxTxMode(); +} +bool Serial::setSwapRxTxMode() +{ + return _impl.setSwapRxTxMode(); +} + const char *Serial::getPort() const { return _impl.getPort(); diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index ac5e014cbb..d8cd61b841 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -85,6 +85,12 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + const char *getPort() const; private: diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 61226f5919..655623782f 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -210,6 +210,14 @@ bool SerialImpl::open() _open = true; + if (_single_wire_mode) { + setSingleWireMode(); + } + + if (_swap_rx_tx_mode) { + setSwapRxTxMode(); + } + return _open; } @@ -398,4 +406,44 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) return flowcontrol == FlowControl::Disabled; } +bool SerialImpl::getSingleWireMode() const +{ + return _single_wire_mode; +} + +bool SerialImpl::setSingleWireMode() +{ +#if defined(TIOCSSINGLEWIRE) + + if (_open) { + ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + } + + _single_wire_mode = true; + return true; +#else + return false; +#endif // TIOCSSINGLEWIRE +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return _swap_rx_tx_mode; +} + +bool SerialImpl::setSwapRxTxMode() +{ +#if defined(TIOCSSWAP) + + if (_open) { + ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED); + } + + _swap_rx_tx_mode = true; + return true; +#else + return false; +#endif // TIOCSSWAP +} + } // namespace device diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 27415a84b7..c520972ba6 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -83,6 +83,12 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + private: int _serial_fd{-1}; @@ -101,6 +107,8 @@ private: bool validateBaudrate(uint32_t baudrate); bool configure(); + bool _single_wire_mode{false}; + bool _swap_rx_tx_mode{false}; }; } // namespace device diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index e3692fa336..c520972ba6 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -83,6 +83,12 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + private: int _serial_fd{-1}; @@ -100,6 +106,9 @@ private: bool validateBaudrate(uint32_t baudrate); bool configure(); + + bool _single_wire_mode{false}; + bool _swap_rx_tx_mode{false}; }; } // namespace device diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index b20278f77f..1e93349c06 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -208,6 +208,14 @@ bool SerialImpl::open() _open = true; + if (_single_wire_mode) { + setSingleWireMode(); + } + + if (_swap_rx_tx_mode) { + setSwapRxTxMode(); + } + return _open; } @@ -391,4 +399,44 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) return flowcontrol == FlowControl::Disabled; } +bool SerialImpl::getSingleWireMode() const +{ + return _single_wire_mode; +} + +bool SerialImpl::setSingleWireMode() +{ +#if defined(TIOCSSINGLEWIRE) + + if (_open) { + ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + } + + _single_wire_mode = true; + return true; +#else + return false; +#endif // TIOCSSINGLEWIRE +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return _swap_rx_tx_mode; +} + +bool SerialImpl::setSwapRxTxMode() +{ +#if defined(TIOCSSWAP) + + if (_open) { + ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED); + } + + _swap_rx_tx_mode = true; + return true; +#else + return false; +#endif // TIOCSSWAP +} + } // namespace device diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index d852f68a54..cdde5c62c9 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -83,6 +83,12 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + private: int _serial_fd{-1}; diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index f5c304a843..04a30bf30e 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -328,4 +328,26 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) return flowcontrol == FlowControl::Disabled; } +bool SerialImpl::getSingleWireMode() const +{ + return false; +} + +bool SerialImpl::setSingleWireMode() +{ + // Qurt platform does not support single wire mode + return false; +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return false; +} + +bool SerialImpl::setSwapRxTxMode() +{ + // Qurt platform does not support swap rx tx mode + return false; +} + } // namespace device diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 4ae4569e1e..42b61ea66f 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -155,16 +155,12 @@ void CrsfRc::Run() } if (board_rc_swap_rxtx(_device)) { -#if defined(TIOCSSWAP) - ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); -#endif // TIOCSSWAP + _uart->setSwapRxTxMode(); } if (board_rc_singlewire(_device)) { _is_singlewire = true; -#if defined(TIOCSSINGLEWIRE) - ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); -#endif // TIOCSSINGLEWIRE + _uart->setSingleWireMode(); } PX4_INFO("Crsf serial opened sucessfully");