fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: Serial driver

Browse Source
This commit is contained in:
Daniel Agar 2022-12-15 20:42:28 -05:00
parent 00bfd5436a
commit 68d756d319
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
7 changed files with 908 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -41,15 +41,11 @@
#include <nuttx/arch.h>
#endif
#ifndef __PX4_QURT
#include <poll.h>
#endif
#include <termios.h>
#include <cstring>
#include <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/drivers/device/nuttx/Serial.hpp>
#include <lib/parameters/param.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
@ -165,7 +161,9 @@ public:
void reset_if_scheduled();
private:
int _serial_fd{-1}; ///< serial interface to GPS
device::Serial _serial; ///< serial interface to GPS
unsigned _baudrate{0}; ///< current baudrate
const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect)
char _port[20] {}; ///< device / serial port path
@ -294,16 +292,12 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance,
unsigned configured_baudrate) :
Device(MODULE_NAME),
_serial(path),
_configured_baudrate(configured_baudrate),
_mode(mode),
_interface(interface),
_instance(instance)
{
/* store port name */
strncpy(_port, path, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = NAN;
@ -320,7 +314,9 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
if (_interface == GPSHelper::Interface::UART) {
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
const char *port = _serial.getPort();
char c = port[strlen(port) - 1]; // last digit of path (eg /dev/ttyS2)
set_device_bus(c - 48); // sub 48 to convert char to integer
} else if (_interface == GPSHelper::Interface::SPI) {
@ -398,7 +394,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
case GPSCallbackType::writeDeviceData:
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
return ::write(gps->_serial_fd, data1, (size_t)data2);
return gps->_serial.write(data1, data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
@ -443,70 +439,19 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
{
handleInjectDataTopic();
#if !defined(__PX4_QURT)
// read at least 32 bytes
int timeout_us = math::min(50'000, timeout * 1000);
int ret = _serial.readAtLeast(buf, buf_length, 1, timeout_us);
/* For non QURT, use the usual polling. */
//Poll only for the serial data. In the same thread we also need to handle orb messages,
//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
//two pollings use different underlying mechanisms (at least under posix), which makes this
//impossible. Instead we limit the maximum polling interval and regularly check for new orb
//messages.
//FIXME: add a unified poll() API
const int max_timeout = 50;
pollfd fds[1];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
if (ret <= 0) {
ret = _serial.read(buf, buf_length);
}
if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. But don't read immediately
* by 1-2 bytes, wait for some more data to save expensive read() calls.
* If we have all requested data available, read it without waiting.
* If more bytes are available, we'll go back to poll() again.
*/
const unsigned character_count = 32; // minimum bytes that we want to read
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
#ifdef __PX4_NUTTX
int err = 0;
int bytes_available = 0;
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
if (err != 0 || bytes_available < (int)character_count) {
px4_usleep(sleeptime);
}
#else
px4_usleep(sleeptime);
#endif
ret = ::read(_serial_fd, buf, buf_length);
if (ret > 0) {
_num_bytes_read += ret;
}
} else {
ret = -1;
}
_num_bytes_read += ret;
}
return ret;
#else
/* For QURT, just use read for now, since this doesn't block, we need to slow it down
* just a bit. */
px4_usleep(10000);
return ::read(_serial_fd, buf, buf_length);
#endif
}
void GPS::handleInjectDataTopic()
@ -571,105 +516,17 @@ bool GPS::injectData(uint8_t *data, size_t len)
{
dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
size_t written = ::write(_serial_fd, data, len);
::fsync(_serial_fd);
return written == len;
size_t written = _serial.write(data, len);
return (written == len);
}
int GPS::setBaudrate(unsigned baud)
{
/* process baud rate */
int speed;
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 -EINVAL;
if (_serial.setBaudrate(baud)) {
return 0;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_serial_fd, &uart_config);
/* 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) {
GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
return -1;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
GPS_ERR("ERR: %d (tcsetattr)", termios_state);
return -1;
}
return 0;
return -1;
}
void GPS::initializeCommunicationDump()
@ -821,36 +678,6 @@ GPS::run()
_helper = nullptr;
}
if (_serial_fd < 0) {
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
px4_sleep(1);
continue;
}
#ifdef __PX4_LINUX
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
}
#endif /* __PX4_LINUX */
}
switch (_mode) {
case gps_driver_mode_t::None:
_mode = gps_driver_mode_t::UBX;
@ -1032,10 +859,7 @@ GPS::run()
}
}
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
}
_serial.close();
if (_mode_auto) {
switch (_mode) {

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

@ -33,23 +33,49 @@
set(SRCS_PLATFORM)
if (${PX4_PLATFORM} STREQUAL "nuttx")
if ("${CONFIG_I2C}" STREQUAL "y")
list(APPEND SRCS_PLATFORM nuttx/I2C.cpp)
if("${CONFIG_I2C}" STREQUAL "y")
list(APPEND SRCS_PLATFORM
nuttx/I2C.cpp
nuttx/I2C.hpp
)
endif()
if ("${CONFIG_SPI}" STREQUAL "y")
list(APPEND SRCS_PLATFORM nuttx/SPI.cpp)
if("${CONFIG_SPI}" STREQUAL "y")
list(APPEND SRCS_PLATFORM
nuttx/SPI.cpp
nuttx/SPI.hpp
)
endif()
list(APPEND SRCS_PLATFORM
nuttx/Serial.cpp
nuttx/Serial.hpp
nuttx/SerialImpl.cpp
nuttx/SerialImpl.hpp
)
elseif((${PX4_PLATFORM} MATCHES "qurt"))
list(APPEND SRCS_PLATFORM qurt/I2C.cpp)
list(APPEND SRCS_PLATFORM qurt/SPI.cpp)
list(APPEND SRCS_PLATFORM qurt/uart.c)
list(APPEND SRCS_PLATFORM
qurt/I2C.cpp
)
list(APPEND SRCS_PLATFORM
qurt/SPI.cpp
)
list(APPEND SRCS_PLATFORM
qurt/uart.c
)
elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
# Linux I2Cdev and SPIdev
list(APPEND SRCS_PLATFORM
posix/I2C.cpp
posix/SPI.cpp
)
endif()
px4_add_library(drivers__device

114
src/lib/drivers/device/nuttx/Serial.cpp Normal file
View File

@ -0,0 +1,114 @@
#include "Serial.hpp"
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)
{
// TODO: Device
// set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
// char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
// set_device_bus(c - 48); // sub 48 to convert char to integer
}
Serial::~Serial()
{
}
bool Serial::open()
{
return _impl.open();
}
bool Serial::isOpen() const
{
return _impl.isOpen();
}
bool Serial::close()
{
return _impl.close();
}
ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
{
return _impl.read(buffer, buffer_size);
}
ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
}
ssize_t Serial::write(const void *buffer, size_t buffer_size)
{
return _impl.write(buffer, buffer_size);
}
const char *Serial::getPort() const
{
return _impl.getPort();
}
bool Serial::setPort(const char *port)
{
return _impl.setPort(port);
}
uint32_t Serial::getBaudrate() const
{
return _impl.getBaudrate();
}
bool Serial::setBaudrate(uint32_t baudrate)
{
return _impl.setBaudrate(baudrate);
}
ByteSize Serial::getBytesize() const
{
return _impl.getBytesize();
}
void Serial::setBytesize(ByteSize bytesize)
{
_impl.setBytesize(bytesize);
}
Parity Serial::getParity() const
{
return _impl.getParity();
}
void Serial::setParity(Parity parity)
{
_impl.setParity(parity);
}
StopBits Serial::getStopbits() const
{
return _impl.getStopbits();
}
void Serial::setStopbits(StopBits stopbits)
{
_impl.setStopbits(stopbits);
}
FlowControl Serial::getFlowcontrol() const
{
return _impl.getFlowcontrol();
}
void Serial::setFlowcontrol(FlowControl flowcontrol)
{
_impl.setFlowcontrol(flowcontrol);
}
} // namespace device

96
src/lib/drivers/device/nuttx/Serial.hpp Normal file
View File

@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (C) 2022 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 "SerialImpl.hpp"
#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);
virtual ~Serial();
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
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;
void setBytesize(ByteSize bytesize);
Parity getParity() const;
void setParity(Parity parity);
StopBits getStopbits() const;
void setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
void setFlowcontrol(FlowControl flowcontrol);
// printStatus()
// port, read bytes, write bytes
private:
// Disable copy constructors
Serial(const Serial &);
Serial &operator=(const Serial &);
// platform implementation
SerialImpl _impl;
};
} // namespace device

38
src/lib/drivers/device/nuttx/SerialCommon.hpp Normal file
View File

@ -0,0 +1,38 @@
#pragma once
namespace device
{
namespace SerialConfig
{
// ByteSize: number of data bits
enum class ByteSize {
FiveBits = 5,
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 Serial
} // namespace device

523
src/lib/drivers/device/nuttx/SerialImpl.cpp Normal file
View File

@ -0,0 +1,523 @@
#include "SerialImpl.hpp"
#include <errno.h>
#include <fcntl.h> // open
#include <poll.h>
#include <string.h> // strncpy
#include <termios.h>
#include <px4_log.h>
#include <lib/mathlib/mathlib.h>
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
}
setBaudrate(baudrate);
}
SerialImpl::~SerialImpl()
{
if (isOpen()) {
close();
}
}
bool SerialImpl::open()
{
// if already open first close
if (isOpen()) {
return true;
}
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (serial_fd < 0) {
PX4_ERR("failed to open %s, errno: %d, %s", _port, errno, strerror(errno));
return false;
}
_serial_fd = serial_fd;
_open = true;
if (configure()) {
_configured = true;
}
return _open;
}
bool SerialImpl::isOpen() const
{
return _open;
}
bool SerialImpl::close()
{
int ret = 0;
if (_serial_fd >= 0) {
ret = ::close(_serial_fd);
}
_serial_fd = -1;
_open = false;
_configured = false;
return (ret == 0);
}
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
{
if (!_open) {
open();
}
if (!_configured) {
configure();
}
int ret_read = ::read(_serial_fd, buffer, buffer_size);
if (ret_read == -1) {
switch (errno) {
case EBADF:
// invalid file descriptor
PX4_ERR("%s read error %d %s", _port, errno, strerror(errno));
close();
break;
}
PX4_DEBUG("%s read error %d %s", _port, errno, strerror(errno));
} else {
_bytes_read += ret_read;
}
return ret_read;
}
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
const size_t required_bytes = math::min(buffer_size, character_count);
const hrt_abstime start_time_us = hrt_absolute_time();
if (!_open) {
open();
}
if (!_configured) {
configure();
}
while (true) {
int bytes_available = 0;
int ioctl_ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
if ((ioctl_ret == 0) && (bytes_available >= (int)required_bytes)) {
return read(buffer, buffer_size);
}
if (bytes_available < (int)required_bytes) {
if (timeout_us > 0) {
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
if (elapsed_us > timeout_us) {
//PX4_WARN("readAtLeast timeout %d bytes available (%llu us elapsed)", bytes_available, elapsed_us);
return -1;
}
}
int desired_bytes = required_bytes - bytes_available;
uint32_t sleeptime_us = desired_bytes * 1'000'000 / (_baudrate / 10);
if (desired_bytes == 1 || sleeptime_us <= 1000) {
int poll_timeout_ms = 0;
if (timeout_us > 0) {
poll_timeout_ms = timeout_us / 1000;
}
pollfd fds[1];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
int poll_ret = ::poll(fds, 1, poll_timeout_ms);
if (poll_ret > 0) {
if (fds[0].revents & POLLIN) {
// There is data to read
}
}
} else {
if (timeout_us > 0) {
sleeptime_us = math::min(sleeptime_us, timeout_us);
}
//PX4_INFO("%s %d/%d bytes available, sleep time %" PRIu32 "us", _port, bytes_available, required_bytes, sleeptime_us);
px4_usleep(sleeptime_us);
}
}
}
return -1;
}
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
{
if (!_open) {
open();
}
if (!_configured) {
configure();
}
// POLLOUT: Writing is now possible
// FIONWRITE: Get the number of bytes that have been written to the TX buffer.
// FIONSPACE: Get the number of free bytes in the TX buffer
int ret_write = ::write(_serial_fd, buffer, buffer_size);
if (ret_write == -1) {
switch (errno) {
case EBADF:
// invalid file descriptor
close();
break;
}
PX4_ERR("%s write error %d %s", _port, errno, strerror(errno));
} else {
_bytes_written += ret_write;
}
return ret_write;
}
const char *SerialImpl::getPort() const
{
return _port;
}
bool SerialImpl::setPort(const char *port)
{
if (port) {
if (strcmp(port, _port) == 0) {
return true;
}
if (::access(port, R_OK) != 0) {
PX4_ERR("port %s not accessible", port);
return false;
}
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
_configured = false;
return true;
}
return false;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
// process baud rate change
int speed = 0;
switch (baudrate) {
case 4800: speed = B4800; break;
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;
#if defined(B460800)
case 460800: speed = B460800; break;
#endif // B460800
#if defined(B500000)
case 500000: speed = B500000; break;
#endif // B500000
#if defined(B576000)
case 576000: speed = B576000; break;
#endif // B576000
#if defined(B921600)
case 921600: speed = B921600; break;
#endif // B921600
#if defined(B1000000)
case 1000000: speed = B1000000; break;
#endif // B1000000
#if defined(B1500000)
case 1500000: speed = B1500000; break;
#endif // B1500000
#if defined(B2000000)
case 2000000: speed = B2000000; break;
#endif // B2000000
#if defined(B2500000)
case 2500000: speed = B2500000; break;
#endif // B2500000
#if defined(B3000000)
case 3000000: speed = B3000000; break;
#endif // B3000000
#if defined(B3500000)
case 3500000: speed = B3500000; break;
#endif // B3500000
#if defined(B4000000)
case 4000000: speed = B4000000; break;
#endif // B4000000
default:
PX4_ERR("unknown baudrate: %" PRIu32, baudrate);
return false;
}
// check if already configured
if (speed == (int)_baudrate) {
return true;
}
if (speed != 0) {
_baudrate = speed;
_configured = false;
return true;
}
return false;
}
ByteSize SerialImpl::getBytesize() const
{
return _bytesize;
}
void SerialImpl::setBytesize(ByteSize bytesize)
{
if (bytesize != _bytesize) {
_bytesize = bytesize;
_configured = false;
}
}
Parity SerialImpl::getParity() const
{
return _parity;
}
void SerialImpl::setParity(Parity parity)
{
if (parity != _parity) {
_parity = parity;
_configured = false;
}
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
void SerialImpl::setStopbits(StopBits stopbits)
{
if (stopbits != _stopbits) {
_stopbits = stopbits;
_configured = false;
}
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
void SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
if (flowcontrol != _flowcontrol) {
_flowcontrol = flowcontrol;
_configured = false;
}
}
bool SerialImpl::configure()
{
_configured = false;
if (!isOpen()) {
return false;
}
// fill the struct for the new configuration
struct termios uart_config;
if (tcgetattr(_serial_fd, &uart_config) != 0) {
PX4_ERR("tcgetattr failed");
return false;
}
// Input modes (c_iflag): Turn off input processing
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off software flow ctrl
// Output modes (c_oflag): Turn off output processing
uart_config.c_oflag = 0;
uart_config.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
uart_config.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
// Local modes (c_lflag): No line processing
uart_config.c_lflag &= ~(ECHO | ECHOE | ECHONL | ICANON | ISIG | IEXTEN);
// Control Modes (c_cflag)
uart_config.c_cflag = CREAD | CLOCAL;
// ByteSize: termios CSIZE
uart_config.c_cflag &= ~CSIZE; // Clear all the size bits, then use one of the statements below
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;
}
// StopBits: termios CSTOPB
switch (_stopbits) {
case StopBits::One:
uart_config.c_cflag &= ~CSTOPB;
break;
case StopBits::Two:
uart_config.c_cflag |= CSTOPB;
break;
}
// Parity: termios PARENB, PARODD
switch (_parity) {
case Parity::None:
uart_config.c_cflag &= ~(PARENB | PARODD);
break;
case Parity::Odd:
uart_config.c_cflag |= (PARENB | PARODD);
break;
case Parity::Even:
uart_config.c_cflag |= PARENB;
uart_config.c_cflag &= ~PARODD;
break;
}
// FlowControl: termios CRTSCTS
switch (_flowcontrol) {
case FlowControl::Disabled:
uart_config.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control
break;
case FlowControl::Enabled:
uart_config.c_cflag |= CRTSCTS; // Enable RTS/CTS hardware flow control
break;
}
// VMIN = 0, VTIME = 0: No blocking, return immediately with what is available
uart_config.c_cc[VMIN] = 0;
uart_config.c_cc[VTIME] = 0;
// baudrate: input speed (cfsetispeed)
int cfsetispeed_ret = cfsetispeed(&uart_config, _baudrate);
if (cfsetispeed_ret != 0) {
PX4_ERR("cfsetispeed failed: %d (errno=%d, %s)", cfsetispeed_ret, errno, strerror(errno));
return false;
}
// baudrate: output speed (cfsetospeed)
int cfsetospeed_ret = cfsetospeed(&uart_config, _baudrate);
if (cfsetospeed_ret != 0) {
PX4_ERR("cfsetospeed failed: %d (errno=%d, %s)", cfsetospeed_ret, errno, strerror(errno));
return false;
}
// tcsetattr: set attributes
int tcsetattr_ret = tcsetattr(_serial_fd, TCSANOW, &uart_config);
if (tcsetattr_ret != 0) {
PX4_ERR("tcsetattr failed: %d (errno=%d, %s)", tcsetattr_ret, errno, strerror(errno));
return false;
}
PX4_INFO("%s configured successfully", _port);
_configured = true;
return true;
}
} // namespace device

82
src/lib/drivers/device/nuttx/SerialImpl.hpp Normal file
View File

@ -0,0 +1,82 @@
#pragma once
#include <unistd.h>
#include "SerialCommon.hpp"
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device
{
class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
virtual ~SerialImpl();
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
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;
void setBytesize(ByteSize bytesize);
Parity getParity() const;
void setParity(Parity parity);
StopBits getStopbits() const;
void setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
void setFlowcontrol(FlowControl flowcontrol);
private:
bool configure();
int _serial_fd{-1};
size_t _bytes_read{0};
size_t _bytes_written{0};
bool _open{false};
bool _configured{false};
char _port[32] {};
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
// 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