diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 9bb84dd902..f8968e4881 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -180,8 +180,34 @@ bool SerialImpl::configure() // uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + // Control modes + uart_config.c_cflag = 0; + + switch (_bytesize) { + case ByteSize::FiveBits: uart_config.c_cflag |= CS5; break; + + case ByteSize::SixBits: uart_config.c_cflag |= CS6; break; + + case ByteSize::SevenBits: uart_config.c_cflag |= CS7; break; + + case ByteSize::EightBits: uart_config.c_cflag |= CS8; break; + } + + if (_flowcontrol == FlowControl::Enabled) { + uart_config.c_cflag |= CRTSCTS; + } + + if (_parity != Parity::None) { + uart_config.c_cflag |= PARENB; + } + + if (_parity == Parity::Odd) { + uart_config.c_cflag |= PARODD; + } + + if (_stopbits == StopBits::Two) { + uart_config.c_cflag |= CSTOPB; + } /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { @@ -486,7 +512,19 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - return bytesize == ByteSize::EightBits; + // check if already configured + if ((bytesize == _bytesize) && _open) { + return true; + } + + _bytesize = bytesize; + + // process bytesize change now if port is already open + if (_open) { + return configure(); + } + + return true; } Parity SerialImpl::getParity() const @@ -496,7 +534,19 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - return parity == Parity::None; + // check if already configured + if ((parity == _parity) && _open) { + return true; + } + + _parity = parity; + + // process parity change now if port is already open + if (_open) { + return configure(); + } + + return true; } StopBits SerialImpl::getStopbits() const @@ -506,7 +556,19 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - return stopbits == StopBits::One; + // check if already configured + if ((stopbits == _stopbits) && _open) { + return true; + } + + _stopbits = stopbits; + + // process stopbits change now if port is already open + if (_open) { + return configure(); + } + + return true; } FlowControl SerialImpl::getFlowcontrol() const @@ -516,7 +578,19 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - return flowcontrol == FlowControl::Disabled; + // check if already configured + if ((flowcontrol == _flowcontrol) && _open) { + return true; + } + + _flowcontrol = flowcontrol; + + // process flowcontrol change now if port is already open + if (_open) { + return configure(); + } + + return true; } bool SerialImpl::getSingleWireMode() const diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 0c23b050e0..cb12b6919d 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -169,8 +169,34 @@ bool SerialImpl::configure() // uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + // Control modes + uart_config.c_cflag = 0; + + switch (_bytesize) { + case ByteSize::FiveBits: uart_config.c_cflag |= CS5; break; + + case ByteSize::SixBits: uart_config.c_cflag |= CS6; break; + + case ByteSize::SevenBits: uart_config.c_cflag |= CS7; break; + + case ByteSize::EightBits: uart_config.c_cflag |= CS8; break; + } + + if (_flowcontrol == FlowControl::Enabled) { + uart_config.c_cflag |= CRTSCTS; + } + + if (_parity != Parity::None) { + uart_config.c_cflag |= PARENB; + } + + if (_parity == Parity::Odd) { + uart_config.c_cflag |= PARODD; + } + + if (_stopbits == StopBits::Two) { + uart_config.c_cflag |= CSTOPB; + } /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { @@ -445,7 +471,6 @@ uint32_t SerialImpl::getBaudrate() const bool SerialImpl::setBaudrate(uint32_t baudrate) { - // check if already configured if ((baudrate == _baudrate) && _open) { return true; } @@ -467,7 +492,17 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - return bytesize == ByteSize::EightBits; + if ((bytesize == _bytesize) && _open) { + return true; + } + + _bytesize = bytesize; + + if (_open) { + return configure(); + } + + return true; } Parity SerialImpl::getParity() const @@ -477,7 +512,17 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - return parity == Parity::None; + if ((parity == _parity) && _open) { + return true; + } + + _parity = parity; + + if (_open) { + return configure(); + } + + return true; } StopBits SerialImpl::getStopbits() const @@ -487,7 +532,17 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - return stopbits == StopBits::One; + if ((stopbits == _stopbits) && _open) { + return true; + } + + _stopbits = stopbits; + + if (_open) { + return configure(); + } + + return true; } FlowControl SerialImpl::getFlowcontrol() const @@ -497,7 +552,17 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - return flowcontrol == FlowControl::Disabled; + if ((flowcontrol == _flowcontrol) && _open) { + return true; + } + + _flowcontrol = flowcontrol; + + if (_open) { + return configure(); + } + + return true; } bool SerialImpl::getSingleWireMode() const diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index 312c830041..5c31be9a5c 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -103,25 +103,14 @@ bool SerialImpl::open() return false; } - if (_bytesize != ByteSize::EightBits) { - PX4_ERR("Qurt platform only supports ByteSize::EightBits"); - return false; - } + // Check all non-supported configurations without duplicating the error strings + if (!setBytesize(_bytesize)) { return false; } - if (_parity != Parity::None) { - PX4_ERR("Qurt platform only supports Parity::None"); - return false; - } + if (!setParity(_parity)) { return false; } - if (_stopbits != StopBits::One) { - PX4_ERR("Qurt platform only supports StopBits::One"); - return false; - } + if (!setStopbits(_stopbits)) { return false; } - if (_flowcontrol != FlowControl::Disabled) { - PX4_ERR("Qurt platform only supports FlowControl::Disabled"); - return false; - } + if (!setFlowcontrol(_flowcontrol)) { return false; } if (!validatePort(_port)) { PX4_ERR("Invalid port %s", _port); @@ -348,7 +337,12 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - return bytesize == ByteSize::EightBits; + if (bytesize != ByteSize::EightBits) { + PX4_ERR("Qurt platform only supports ByteSize::EightBits"); + return false; + } + + return true; } Parity SerialImpl::getParity() const @@ -358,7 +352,12 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - return parity == Parity::None; + if (parity != Parity::None) { + PX4_ERR("Qurt platform only supports Parity::None"); + return false; + } + + return true; } StopBits SerialImpl::getStopbits() const @@ -368,7 +367,12 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - return stopbits == StopBits::One; + if (stopbits != StopBits::One) { + PX4_ERR("Qurt platform only supports StopBits::One"); + return false; + } + + return true; } FlowControl SerialImpl::getFlowcontrol() const @@ -378,7 +382,12 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - return flowcontrol == FlowControl::Disabled; + if (flowcontrol != FlowControl::Disabled) { + PX4_ERR("Qurt platform only supports FlowControl::Disabled"); + return false; + } + + return true; } bool SerialImpl::getSingleWireMode() const @@ -388,8 +397,12 @@ bool SerialImpl::getSingleWireMode() const bool SerialImpl::setSingleWireMode() { - // Qurt platform does not support single wire mode - return false; + if (enable) { + PX4_ERR("Qurt platform does not support single wire mode"); + return false; + } + + return true; } bool SerialImpl::getSwapRxTxMode() const @@ -399,14 +412,22 @@ bool SerialImpl::getSwapRxTxMode() const bool SerialImpl::setSwapRxTxMode() { - // Qurt platform does not support swap rx tx mode - return false; + if (enable) { + PX4_ERR("Qurt platform does not support swap rx tx mode"); + return false; + } + + return true; } bool SerialImpl::setInvertedMode(bool enable) { - // Qurt platform does not support inverted mode - return false == enable; + if (enable) { + PX4_ERR("Qurt platform does not support inverted mode"); + return false; + } + + return true; } bool SerialImpl::getInvertedMode() const {