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) { if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _port); PX4_ERR("Error creating serial device %s", _port);
px4_sleep(1); px4_usleep(100000);
continue; continue;
} }
} }
@ -802,7 +802,7 @@ GPS::run()
if (_configured_baudrate) { if (_configured_baudrate) {
if (! _uart->setBaudrate(_configured_baudrate)) { if (! _uart->setBaudrate(_configured_baudrate)) {
PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
px4_sleep(1); px4_usleep(100000);
continue; continue;
} }
} }
@ -810,7 +810,7 @@ GPS::run()
// Open the UART. If this is successful then the UART is ready to use. // Open the UART. If this is successful then the UART is ready to use.
if (! _uart->open()) { if (! _uart->open()) {
PX4_ERR("Error opening serial device %s", _port); PX4_ERR("Error opening serial device %s", _port);
px4_sleep(1); px4_usleep(100000);
continue; 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() endif()
list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp) list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp)
list(APPEND SRCS_PLATFORM posix/SerialSBUSImpl.cpp)
list(APPEND SRCS_PLATFORM posix/SerialStandardImpl.cpp)
endif() endif()
px4_add_library(drivers__device px4_add_library(drivers__device

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

@ -36,14 +36,9 @@
namespace device namespace device
{ {
Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, Serial::Serial(const char *port, uint32_t baudrate) :
FlowControl flowcontrol) : _impl(port, baudrate)
_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
{ {
// If no baudrate was specified then set it to a reasonable default value
if (baudrate == 0) {
(void) _impl.setBaudrate(9600);
}
} }
Serial::~Serial() Serial::~Serial()
@ -90,44 +85,14 @@ bool Serial::setBaudrate(uint32_t baudrate)
return _impl.setBaudrate(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); return _impl.setSBUSMode(enable);
}
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);
} }
const char *Serial::getPort() const const char *Serial::getPort() const

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

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

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

@ -39,17 +39,12 @@
#include <errno.h> #include <errno.h>
#include <poll.h> #include <poll.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <board_config.h>
namespace device namespace device
{ {
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{ {
if (port) { if (port) {
strncpy(_port, port, sizeof(_port) - 1); strncpy(_port, port, sizeof(_port) - 1);
@ -58,6 +53,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else { } else {
_port[0] = 0; _port[0] = 0;
} }
if (baudrate) {
_baudrate = baudrate;
} else {
// If baudrate is zero then choose a reasonable default
_baudrate = 9600;
}
} }
SerialImpl::~SerialImpl() SerialImpl::~SerialImpl()
@ -67,28 +69,33 @@ 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() bool SerialImpl::configure()
{ {
struct termios uart_config;
if (_SBUSMode) {
if (_baudrate != DEFAULT_SBUS_BAUDRATE) {
PX4_WARN("Warning, SBUS baud rate being set to %lu", _baudrate);
}
/* 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);
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 */ /* process baud rate */
int speed; int speed;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
switch (_baudrate) { switch (_baudrate) {
case 9600: speed = B9600; break; case 9600: speed = B9600; break;
@ -119,8 +126,6 @@ bool SerialImpl::configure()
return false; return false;
} }
struct termios uart_config;
int termios_state; int termios_state;
/* fill the struct for the new configuration */ /* fill the struct for the new configuration */
@ -180,6 +185,7 @@ bool SerialImpl::configure()
PX4_ERR("ERR: %d (tcsetattr)", termios_state); PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false; return false;
} }
}
return true; return true;
} }
@ -203,6 +209,7 @@ bool SerialImpl::open()
// Configure the serial port // Configure the serial port
if (! configure()) { if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno); PX4_ERR("failed to configure %s err: %d", _port, errno);
close();
return false; return false;
} }
@ -218,7 +225,6 @@ bool SerialImpl::isOpen() const
bool SerialImpl::close() bool SerialImpl::close()
{ {
if (_serial_fd >= 0) { if (_serial_fd >= 0) {
::close(_serial_fd); ::close(_serial_fd);
} }
@ -319,64 +325,39 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate) bool SerialImpl::setBaudrate(uint32_t baudrate)
{ {
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
return false;
}
// check if already configured // check if already configured
if ((baudrate == _baudrate) && _open) { if (baudrate == _baudrate) {
return true; return true;
} }
_baudrate = baudrate; _baudrate = baudrate;
// process baud rate change now if port is already open // process baud rate change now if port is already open
if (_open) { if ((_open) && (configure() != 0)) {
return configure(); // Configure failed! Close the port
close();
return false;
} }
return true; 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 _SBUSMode = enable;
{ _baudrate = DEFAULT_SBUS_BAUDRATE;
return _parity;
}
bool SerialImpl::setParity(Parity parity) return true;
{
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 } // namespace device

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

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

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

@ -32,8 +32,10 @@
****************************************************************************/ ****************************************************************************/
#include "SerialImpl.hpp" #include "SerialImpl.hpp"
#include <unistd.h>
#include <string.h> // strncpy #include <string.h> // strncpy
#include <termios.h>
#include <px4_log.h> #include <px4_log.h>
#include <fcntl.h> #include <fcntl.h>
#include <errno.h> #include <errno.h>
@ -43,13 +45,7 @@
namespace device namespace device
{ {
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{ {
if (port) { if (port) {
strncpy(_port, port, sizeof(_port) - 1); strncpy(_port, port, sizeof(_port) - 1);
@ -58,6 +54,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else { } else {
_port[0] = 0; _port[0] = 0;
} }
if (baudrate) {
_baudrate = baudrate;
} else {
// If baudrate is zero then choose a reasonable default
_baudrate = 9600;
}
} }
SerialImpl::~SerialImpl() 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() bool SerialImpl::configure()
{ {
/* process baud rate */ if (_SBUSMode) {
int speed; return _sbus.configure(_serial_fd, _baudrate);
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
return false;
} }
switch (_baudrate) { return _standard.configure(_serial_fd, _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;
} }
bool SerialImpl::open() bool SerialImpl::open()
@ -203,6 +98,7 @@ bool SerialImpl::open()
// Configure the serial port // Configure the serial port
if (! configure()) { if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno); PX4_ERR("failed to configure %s err: %d", _port, errno);
close();
return false; return false;
} }
@ -218,7 +114,6 @@ bool SerialImpl::isOpen() const
bool SerialImpl::close() bool SerialImpl::close()
{ {
if (_serial_fd >= 0) { if (_serial_fd >= 0) {
::close(_serial_fd); ::close(_serial_fd);
} }
@ -240,7 +135,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
if (ret < 0) { if (ret < 0) {
PX4_DEBUG("%s read error %d", _port, ret); PX4_DEBUG("%s read error %d", _port, ret);
} }
return ret; return ret;
@ -321,64 +215,39 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate) bool SerialImpl::setBaudrate(uint32_t baudrate)
{ {
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %u", baudrate);
return false;
}
// check if already configured // check if already configured
if ((baudrate == _baudrate) && _open) { if (baudrate == _baudrate) {
return true; return true;
} }
_baudrate = baudrate; _baudrate = baudrate;
// process baud rate change now if port is already open // process baud rate change now if port is already open
if (_open) { if ((_open) && (configure() != 0)) {
return configure(); // Configure failed! Close the port
close();
return false;
} }
return true; 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 _SBUSMode = enable;
{ _baudrate = SerialSBUSImpl::DEFAULT_BAUDRATE;
return _parity;
}
bool SerialImpl::setParity(Parity parity) return true;
{
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 } // namespace device

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

@ -34,14 +34,9 @@
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>
#include <unistd.h>
#include <drivers/device/SerialCommon.hpp> #include "SerialSBUSImpl.hpp"
#include "SerialStandardImpl.hpp"
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device namespace device
{ {
@ -50,8 +45,7 @@ class SerialImpl
{ {
public: public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, SerialImpl(const char *port, uint32_t baudrate);
FlowControl flowcontrol);
virtual ~SerialImpl(); virtual ~SerialImpl();
bool open(); bool open();
@ -69,17 +63,8 @@ public:
uint32_t getBaudrate() const; uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate); bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const; bool getSBUSMode() const;
bool setBytesize(ByteSize bytesize); bool setSBUSMode(bool enable);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
private: private:
@ -91,12 +76,13 @@ private:
uint32_t _baudrate{0}; uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits}; bool _SBUSMode{false};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One}; // The configuration routines for SBUS versus other needed to be separated
FlowControl _flowcontrol{FlowControl::Disabled}; // out because they use different methods with different, conflicting header files
SerialSBUSImpl _sbus;
SerialStandardImpl _standard;
bool validateBaudrate(uint32_t baudrate);
bool configure(); 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 #pragma once
#include <stdint.h>
namespace device namespace device
{ {
namespace SerialConfig
class SerialSBUSImpl
{ {
public:
SerialSBUSImpl() {};
virtual ~SerialSBUSImpl() {};
// ByteSize: number of data bits bool configure(int fd, uint32_t baud);
enum class ByteSize {
FiveBits = 5, static const uint32_t DEFAULT_BAUDRATE{100000};
SixBits = 6,
SevenBits = 7,
EightBits = 8,
}; };
// 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 } // 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 namespace device
{ {
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{ {
if (port) { if (port) {
strncpy(_port, port, sizeof(_port) - 1); strncpy(_port, port, sizeof(_port) - 1);
@ -55,6 +49,14 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
} else { } else {
_port[0] = 0; _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() 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() bool SerialImpl::open()
{ {
// There's no harm in calling open multiple times on the same port. // There's no harm in calling open multiple times on the same port.
@ -92,36 +74,12 @@ bool SerialImpl::open()
_open = false; _open = false;
_serial_fd = -1; _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 // qurt_uart_open will check validity of port and baudrate
int serial_fd = qurt_uart_open(_port, _baudrate); int serial_fd = qurt_uart_open(_port, _baudrate);
if (serial_fd < 0) { 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; return false;
} else { } else {
@ -263,11 +221,6 @@ uint32_t SerialImpl::getBaudrate() const
bool SerialImpl::setBaudrate(uint32_t baudrate) bool SerialImpl::setBaudrate(uint32_t baudrate)
{ {
if (! validateBaudrate(baudrate)) {
PX4_ERR("Invalid baudrate: %u", baudrate);
return false;
}
// check if already configured // check if already configured
if (baudrate == _baudrate) { if (baudrate == _baudrate) {
return true; return true;
@ -283,44 +236,4 @@ bool SerialImpl::setBaudrate(uint32_t baudrate)
return true; 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 } // namespace device

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

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