dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Cleaned up the API and added SBUS support

This commit is contained in:
Eric Katzfey 2023-12-28 17:33:18 -08:00
parent 4b53be9444
commit fbfb93ab26
14 changed files with 500 additions and 570 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/gps/gps.cpp
View File

@ -791,7 +791,7 @@ GPS::run()
if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _port);
px4_sleep(1);
px4_usleep(100000);
continue;
}
}
@ -802,7 +802,7 @@ GPS::run()
if (_configured_baudrate) {
if (! _uart->setBaudrate(_configured_baudrate)) {
PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
px4_sleep(1);
px4_usleep(100000);
continue;
}
}
@ -810,7 +810,7 @@ GPS::run()
// Open the UART. If this is successful then the UART is ready to use.
if (! _uart->open()) {
PX4_ERR("Error opening serial device %s", _port);
px4_sleep(1);
px4_usleep(100000);
continue;
}

2
src/lib/drivers/device/CMakeLists.txt
View File

@ -58,6 +58,8 @@ elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
endif()
list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp)
list(APPEND SRCS_PLATFORM posix/SerialSBUSImpl.cpp)
list(APPEND SRCS_PLATFORM posix/SerialStandardImpl.cpp)
endif()
px4_add_library(drivers__device

47
src/lib/drivers/device/Serial.cpp
View File

@ -36,14 +36,9 @@
namespace device
{
Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
Serial::Serial(const char *port, uint32_t baudrate) :
_impl(port, baudrate)
{
// If no baudrate was specified then set it to a reasonable default value
if (baudrate == 0) {
(void) _impl.setBaudrate(9600);
}
}
Serial::~Serial()
@ -90,44 +85,14 @@ bool Serial::setBaudrate(uint32_t baudrate)
return _impl.setBaudrate(baudrate);
}
ByteSize Serial::getBytesize() const
bool Serial::getSBUSMode() const
{
return _impl.getBytesize();
return _impl.getSBUSMode();
}
bool Serial::setBytesize(ByteSize bytesize)
bool Serial::setSBUSMode(bool enable)
{
return _impl.setBytesize(bytesize);
}
Parity Serial::getParity() const
{
return _impl.getParity();
}
bool Serial::setParity(Parity parity)
{
return _impl.setParity(parity);
}
StopBits Serial::getStopbits() const
{
return _impl.getStopbits();
}
bool Serial::setStopbits(StopBits stopbits)
{
return _impl.setStopbits(stopbits);
}
FlowControl Serial::getFlowcontrol() const
{
return _impl.getFlowcontrol();
}
bool Serial::setFlowcontrol(FlowControl flowcontrol)
{
return _impl.setFlowcontrol(flowcontrol);
return _impl.setSBUSMode(enable);
}
const char *Serial::getPort() const

38
src/lib/drivers/device/Serial.hpp
View File

@ -33,6 +33,7 @@
#pragma once
// Bring in the correct platform implementation
#ifdef __PX4_NUTTX
#include "nuttx/SerialImpl.hpp"
#elif defined(__PX4_QURT)
@ -41,25 +42,18 @@
#include "posix/SerialImpl.hpp"
#endif
#include "SerialCommon.hpp"
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device __EXPORT
{
class Serial
{
public:
Serial(const char *port, uint32_t baudrate = 57600,
ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
// Baud rate can be selected with constructor or by using setBaudrate
Serial(const char *port, uint32_t baudrate = 0);
virtual ~Serial();
// Open sets up the port and gets it configured based on desired configuration
// Open sets up the port and gets it configured. Unless an alternate mode
// is selected the port will be configured with parity disabled and 1 stop bit.
bool open();
bool isOpen() const;
@ -70,24 +64,16 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
// 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.
uint32_t getBaudrate() const;
// If the port has already been opened it will be reconfigured with a change
// of baudrate.
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
// SBUS has special configuration considerations and methods so it
// is given a special mode. It has parity enabled and 2 stop bits
bool getSBUSMode() const;
bool setSBUSMode(bool enable);
const char *getPort() const;

241
src/lib/drivers/device/nuttx/SerialImpl.cpp
View File

@ -39,17 +39,12 @@
#include <errno.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <board_config.h>
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
@ -58,6 +53,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else {
_port[0] = 0;
}
if (baudrate) {
_baudrate = baudrate;
} else {
// If baudrate is zero then choose a reasonable default
_baudrate = 9600;
}
}
SerialImpl::~SerialImpl()
@ -67,118 +69,122 @@ SerialImpl::~SerialImpl()
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
return ((baudrate == 9600) ||
(baudrate == 19200) ||
(baudrate == 38400) ||
(baudrate == 57600) ||
(baudrate == 115200) ||
(baudrate == 230400) ||
(baudrate == 460800) ||
(baudrate == 921600));
}
bool SerialImpl::configure()
{
/* process baud rate */
int speed;
struct termios uart_config;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
if (_SBUSMode) {
if (_baudrate != DEFAULT_SBUS_BAUDRATE) {
PX4_WARN("Warning, SBUS baud rate being set to %lu", _baudrate);
}
switch (_baudrate) {
case 9600: speed = B9600; break;
/* even parity, two stop bits */
tcgetattr(_serial_fd, &uart_config);
cfsetspeed(&uart_config, _baudrate);
uart_config.c_cflag |= (CSTOPB | PARENB);
tcsetattr(_serial_fd, TCSANOW, &uart_config);
case 19200: speed = B19200; break;
if (board_rc_singlewire(_port)) {
/* only defined in configs capable of IOCTL
* Note It is never turned off
*/
#ifdef TIOCSSINGLEWIRE
ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif
}
} else {
/* process baud rate */
int speed;
case 38400: speed = B38400; break;
switch (_baudrate) {
case 9600: speed = B9600; break;
case 57600: speed = B57600; break;
case 19200: speed = B19200; break;
case 115200: speed = B115200; break;
case 38400: speed = B38400; break;
case 230400: speed = B230400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
default:
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
struct termios uart_config;
int termios_state;
int termios_state;
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
}
}
return true;
@ -203,6 +209,7 @@ bool SerialImpl::open()
// Configure the serial port
if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
close();
return false;
}
@ -218,7 +225,6 @@ bool SerialImpl::isOpen() const
bool SerialImpl::close()
{
if (_serial_fd >= 0) {
::close(_serial_fd);
}
@ -319,64 +325,39 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
return false;
}
// check if already configured
if ((baudrate == _baudrate) && _open) {
if (baudrate == _baudrate) {
return true;
}
_baudrate = baudrate;
// process baud rate change now if port is already open
if (_open) {
return configure();
if ((_open) && (configure() != 0)) {
// Configure failed! Close the port
close();
return false;
}
return true;
}
ByteSize SerialImpl::getBytesize() const
bool SerialImpl::getSBUSMode() const
{
return _bytesize;
return _SBUSMode;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
bool SerialImpl::setSBUSMode(bool enable)
{
return bytesize == ByteSize::EightBits;
}
if (_open) {
PX4_ERR("Cannot configure SBUS mode after port has already been opened");
return false;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
_SBUSMode = enable;
_baudrate = DEFAULT_SBUS_BAUDRATE;
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
return true;
}
} // namespace device

31
src/lib/drivers/device/nuttx/SerialImpl.hpp
View File

@ -34,14 +34,6 @@
#pragma once
#include <stdint.h>
#include <unistd.h>
#include <drivers/device/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device
{
@ -50,8 +42,7 @@ class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
SerialImpl(const char *port, uint32_t baudrate);
virtual ~SerialImpl();
bool open();
@ -69,17 +60,8 @@ public:
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSBUSMode() const;
bool setSBUSMode(bool enable);
private:
@ -91,12 +73,9 @@ private:
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool _SBUSMode{false};
static const uint32_t DEFAULT_SBUS_BAUDRATE{100000};
bool validateBaudrate(uint32_t baudrate);
bool configure();
};

191
src/lib/drivers/device/posix/SerialImpl.cpp
View File

@ -32,8 +32,10 @@
****************************************************************************/
#include "SerialImpl.hpp"
#include <unistd.h>
#include <string.h> // strncpy
#include <termios.h>
#include <px4_log.h>
#include <fcntl.h>
#include <errno.h>
@ -43,13 +45,7 @@
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
@ -58,6 +54,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else {
_port[0] = 0;
}
if (baudrate) {
_baudrate = baudrate;
} else {
// If baudrate is zero then choose a reasonable default
_baudrate = 9600;
}
}
SerialImpl::~SerialImpl()
@ -67,121 +70,13 @@ SerialImpl::~SerialImpl()
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
return ((baudrate == 9600) ||
(baudrate == 19200) ||
(baudrate == 38400) ||
(baudrate == 57600) ||
(baudrate == 115200) ||
(baudrate == 230400) ||
(baudrate == 460800) ||
(baudrate == 921600));
}
bool SerialImpl::configure()
{
/* process baud rate */
int speed;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
return false;
if (_SBUSMode) {
return _sbus.configure(_serial_fd, _baudrate);
}
switch (_baudrate) {
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
return false;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
}
return true;
return _standard.configure(_serial_fd, _baudrate);
}
bool SerialImpl::open()
@ -203,6 +98,7 @@ bool SerialImpl::open()
// Configure the serial port
if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
close();
return false;
}
@ -218,7 +114,6 @@ bool SerialImpl::isOpen() const
bool SerialImpl::close()
{
if (_serial_fd >= 0) {
::close(_serial_fd);
}
@ -240,7 +135,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;
@ -321,64 +215,39 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %u", baudrate);
return false;
}
// check if already configured
if ((baudrate == _baudrate) && _open) {
if (baudrate == _baudrate) {
return true;
}
_baudrate = baudrate;
// process baud rate change now if port is already open
if (_open) {
return configure();
if ((_open) && (configure() != 0)) {
// Configure failed! Close the port
close();
return false;
}
return true;
}
ByteSize SerialImpl::getBytesize() const
bool SerialImpl::getSBUSMode() const
{
return _bytesize;
return _SBUSMode;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
bool SerialImpl::setSBUSMode(bool enable)
{
return bytesize == ByteSize::EightBits;
}
if (_open) {
PX4_ERR("Cannot configure SBUS mode after port has already been opened");
return false;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
_SBUSMode = enable;
_baudrate = SerialSBUSImpl::DEFAULT_BAUDRATE;
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
return true;
}
} // namespace device

36
src/lib/drivers/device/posix/SerialImpl.hpp
View File

@ -34,14 +34,9 @@
#pragma once
#include <stdint.h>
#include <unistd.h>
#include <drivers/device/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
#include "SerialSBUSImpl.hpp"
#include "SerialStandardImpl.hpp"
namespace device
{
@ -50,8 +45,7 @@ class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
SerialImpl(const char *port, uint32_t baudrate);
virtual ~SerialImpl();
bool open();
@ -69,17 +63,8 @@ public:
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSBUSMode() const;
bool setSBUSMode(bool enable);
private:
@ -91,12 +76,13 @@ private:
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool _SBUSMode{false};
// The configuration routines for SBUS versus other needed to be separated
// out because they use different methods with different, conflicting header files
SerialSBUSImpl _sbus;
SerialStandardImpl _standard;
bool validateBaudrate(uint32_t baudrate);
bool configure();
};

80
src/lib/drivers/device/posix/SerialSBUSImpl.cpp Normal file
View File

@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "SerialSBUSImpl.hpp"
#include <sys/ioctl.h>
#include <asm-generic/termbits.h>
#include <px4_log.h>
namespace device
{
bool SerialSBUSImpl::configure(int fd, uint32_t baud)
{
struct termios2 tio = {};
if (ioctl(fd, TCGETS2, &tio)) {
return false;
}
if (baud != DEFAULT_BAUDRATE) {
PX4_WARN("Warning, SBUS baud rate being set to %u", baud);
}
/**
* Setting serial port,8E2, non-blocking.100Kbps
*/
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL
| IXON);
tio.c_iflag |= (INPCK | IGNPAR);
tio.c_oflag &= ~OPOST;
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
/**
* use BOTHER to specify speed directly in c_[io]speed member
*/
tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
tio.c_ispeed = baud;
tio.c_ospeed = baud;
tio.c_cc[VMIN] = 25;
tio.c_cc[VTIME] = 0;
if (ioctl(fd, TCSETS2, &tio)) {
return false;
}
return true;
}
} // namespace device

37
src/lib/drivers/device/SerialCommon.hpp → src/lib/drivers/device/posix/SerialSBUSImpl.hpp
View File

@ -33,38 +33,21 @@
#pragma once
#include <stdint.h>
namespace device
{
namespace SerialConfig
class SerialSBUSImpl
{
public:
SerialSBUSImpl() {};
virtual ~SerialSBUSImpl() {};
// ByteSize: number of data bits
enum class ByteSize {
FiveBits = 5,
SixBits = 6,
SevenBits = 7,
EightBits = 8,
bool configure(int fd, uint32_t baud);
static const uint32_t DEFAULT_BAUDRATE{100000};
};
// Parity: enable parity checking
enum class Parity {
None = 0,
Odd = 1,
Even = 2,
};
// StopBits: number of stop bits
enum class StopBits {
One = 1,
Two = 2
};
// FlowControl: enable flow control
enum class FlowControl {
Disabled = 0,
Enabled = 1,
};
} // namespace SerialConfig
} // namespace device

159
src/lib/drivers/device/posix/SerialStandardImpl.cpp Normal file
View File

@ -0,0 +1,159 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "SerialStandardImpl.hpp"
#include <sys/termios.h>
#include <px4_log.h>
namespace device
{
bool SerialStandardImpl::validateBaudrate(uint32_t baudrate)
{
return ((baudrate == 9600) ||
(baudrate == 19200) ||
(baudrate == 38400) ||
(baudrate == 57600) ||
(baudrate == 115200) ||
(baudrate == 230400) ||
(baudrate == 460800) ||
(baudrate == 921600));
}
bool SerialStandardImpl::configure(int fd, uint32_t baud)
{
/* process baud rate */
int speed;
if (! validateBaudrate(baud)) {
PX4_ERR("ERR: unknown baudrate: %u", baud);
return false;
}
switch (baud) {
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return false;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
}
return true;
}
} // namespace device

55
src/lib/drivers/device/posix/SerialStandardImpl.hpp Normal file
View File

@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
namespace device
{
class SerialStandardImpl
{
public:
SerialStandardImpl() {};
virtual ~SerialStandardImpl() {};
bool configure(int fd, uint32_t baud);
private:
bool validateBaudrate(uint32_t baudrate);
};
} // namespace device

109
src/lib/drivers/device/qurt/SerialImpl.cpp
View File

@ -40,13 +40,7 @@
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
@ -55,6 +49,14 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else {
_port[0] = 0;
}
if (baudrate) {
_baudrate = baudrate;
} else {
// If baudrate is zero then choose a reasonable default.
// The default is the GPS UBX M10 default rate.
_baudrate = 115200;
}
}
SerialImpl::~SerialImpl()
@ -64,26 +66,6 @@ SerialImpl::~SerialImpl()
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
if ((baudrate != 9600) &&
(baudrate != 38400) &&
(baudrate != 57600) &&
(baudrate != 115200) &&
(baudrate != 230400) &&
(baudrate != 250000) &&
(baudrate != 420000) &&
(baudrate != 460800) &&
(baudrate != 921600) &&
(baudrate != 1000000) &&
(baudrate != 1843200) &&
(baudrate != 2000000)) {
return false;
}
return true;
}
bool SerialImpl::open()
{
// There's no harm in calling open multiple times on the same port.
@ -92,36 +74,12 @@ bool SerialImpl::open()
_open = false;
_serial_fd = -1;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("Invalid baudrate: %u", _baudrate);
return false;
}
if (_bytesize != ByteSize::EightBits) {
PX4_ERR("Qurt platform only supports ByteSize::EightBits");
return false;
}
if (_parity != Parity::None) {
PX4_ERR("Qurt platform only supports Parity::None");
return false;
}
if (_stopbits != StopBits::One) {
PX4_ERR("Qurt platform only supports StopBits::One");
return false;
}
if (_flowcontrol != FlowControl::Disabled) {
PX4_ERR("Qurt platform only supports FlowControl::Disabled");
return false;
}
// qurt_uart_open will check validity of port and baudrate
int serial_fd = qurt_uart_open(_port, _baudrate);
if (serial_fd < 0) {
PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
PX4_ERR("failed to open %s at baudrate %u, fd: %d", _port, _baudrate, serial_fd);
close();
return false;
} else {
@ -263,11 +221,6 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("Invalid baudrate: %u", baudrate);
return false;
}
// check if already configured
if (baudrate == _baudrate) {
return true;
@ -283,44 +236,4 @@ bool SerialImpl::setBaudrate(uint32_t baudrate)
return true;
}
ByteSize SerialImpl::getBytesize() const
{
return _bytesize;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
{
return bytesize == ByteSize::EightBits;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
}
} // namespace device

38
src/lib/drivers/device/qurt/SerialImpl.hpp
View File

@ -33,14 +33,7 @@
#pragma once
#include <unistd.h>
#include <drivers/device/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
#include <stdint.h>
namespace device
{
@ -49,8 +42,7 @@ class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
SerialImpl(const char *port, uint32_t baudrate);
virtual ~SerialImpl();
bool open();
@ -64,22 +56,13 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
const char *getPort() const;
bool setPort(const char *port);
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
// Cannot configure Qurt UARTs for SBUS!
bool getSBUSMode() const { return false; }
bool setSBUSMode(bool enable) { return false; }
private:
@ -91,18 +74,7 @@ private:
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool validateBaudrate(uint32_t baudrate);
// Mutex used to lock the read functions
//pthread_mutex_t read_mutex;
// Mutex used to lock the write functions
//pthread_mutex_t write_mutex;
};
} // namespace device