forked from Archive/PX4-Autopilot
New platform independent Serial interface (#21723)
This commit is contained in:
Eric Katzfey
GitHub
parent
bb9f4d42f3
commit
69028f37a9
|
|
@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
|||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@ add_library(px4_platform STATIC
|
|||
shutdown.cpp
|
||||
spi.cpp
|
||||
pab_manifest.c
|
||||
Serial.cpp
|
||||
${SRCS}
|
||||
)
|
||||
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,138 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/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)
|
||||
{
|
||||
// If no baudrate was specified then set it to a reasonable default value
|
||||
if (baudrate == 0) {
|
||||
(void) _impl.setBaudrate(9600);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
bool Serial::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
const char *Serial::getPort() const
|
||||
{
|
||||
return _impl.getPort();
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <SerialImpl.hpp>
|
||||
|
||||
#include <px4_platform_common/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();
|
||||
|
||||
// Open sets up the port and gets it configured based on desired configuration
|
||||
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);
|
||||
|
||||
// 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;
|
||||
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);
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
private:
|
||||
// Disable copy constructors
|
||||
Serial(const Serial &);
|
||||
Serial &operator=(const Serial &);
|
||||
|
||||
// platform implementation
|
||||
SerialImpl _impl;
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
|
||||
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 SerialConfig
|
||||
} // namespace device
|
||||
|
|
@ -13,11 +13,21 @@ __END_DECLS
|
|||
#define px4_clock_gettime system_clock_gettime
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
||||
__END_DECLS
|
||||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_usleep(useconds_t usec);
|
||||
__EXPORT unsigned int px4_sleep(unsigned int seconds);
|
||||
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
|
||||
|
|
@ -27,7 +37,6 @@ __END_DECLS
|
|||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
#define px4_usleep system_usleep
|
||||
#define px4_sleep system_sleep
|
||||
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait
|
||||
|
|
|
|||
|
|
@ -0,0 +1,394 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <termios.h>
|
||||
#include <px4_log.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define MODULE_NAME "SerialImpl"
|
||||
|
||||
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)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
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: %lu", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
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: %lu", _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()
|
||||
{
|
||||
if (isOpen()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Open the serial port
|
||||
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
|
||||
// Configure the serial port
|
||||
if (! configure()) {
|
||||
PX4_ERR("failed to configure %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
}
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = ::read(_serial_fd, buffer, buffer_size);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
|
||||
// Poll for incoming UART data.
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (remaining_time <= 0) { break; }
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
|
||||
|
||||
if (ret > 0) {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (ret > 0) {
|
||||
total_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Got a poll error");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int written = ::write(_serial_fd, buffer, buffer_size);
|
||||
::fsync(_serial_fd);
|
||||
|
||||
if (written < 0) {
|
||||
PX4_ERR("%s write error %d", _port, written);
|
||||
}
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
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) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return configure();
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/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;
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
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 configure();
|
||||
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -3,6 +3,8 @@
|
|||
add_library(px4_layer
|
||||
${KERNEL_SRCS}
|
||||
cdc_acm_check.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
|
|
|||
|
|
@ -15,6 +15,8 @@ add_library(px4_layer
|
|||
usr_board_ctrl.c
|
||||
usr_hrt.cpp
|
||||
usr_mcu_version.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
|
|
|||
|
|
@ -0,0 +1,103 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/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;
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
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 configure();
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -46,6 +46,8 @@ add_library(px4_layer
|
|||
drv_hrt.cpp
|
||||
cpuload.cpp
|
||||
print_load.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
|
||||
target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
|
||||
|
|
|
|||
|
|
@ -0,0 +1,387 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <termios.h>
|
||||
#include <px4_log.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_hrt.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)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool SerialImpl::open()
|
||||
{
|
||||
if (isOpen()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Open the serial port
|
||||
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
|
||||
// Configure the serial port
|
||||
if (! configure()) {
|
||||
PX4_ERR("failed to configure %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
}
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = ::read(_serial_fd, buffer, buffer_size);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret);
|
||||
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
|
||||
// Poll for incoming UART data.
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (remaining_time <= 0) { break; }
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
|
||||
|
||||
if (ret > 0) {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
|
||||
px4_usleep(sleeptime);
|
||||
|
||||
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (ret > 0) {
|
||||
total_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Got a poll error");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int written = ::write(_serial_fd, buffer, buffer_size);
|
||||
::fsync(_serial_fd);
|
||||
|
||||
if (written < 0) {
|
||||
PX4_ERR("%s write error %d", _port, written);
|
||||
|
||||
}
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
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) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return configure();
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -50,5 +50,4 @@ add_library(px4 SHARED
|
|||
target_link_libraries(px4
|
||||
modules__muorb__slpi
|
||||
${module_libraries}
|
||||
px4_layer
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <unistd.h>
|
||||
|
||||
#include <px4_platform_common/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;
|
||||
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);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
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
|
||||
|
|
@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
|
|||
px4_qurt_impl.cpp
|
||||
main.cpp
|
||||
qurt_log.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
add_library(px4_layer
|
||||
|
|
|
|||
|
|
@ -0,0 +1,326 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <px4_log.h>
|
||||
#include <drivers/device/qurt/uart.h>
|
||||
#include <drivers/drv_hrt.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)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
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.
|
||||
// In fact, that's the only way to change the baudrate
|
||||
|
||||
_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);
|
||||
return false;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
// No close defined for qurt uart yet
|
||||
// if (_serial_fd >= 0) {
|
||||
// qurt_uart_close(_serial_fd);
|
||||
// }
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
|
||||
|
||||
if (ret_read < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret_read);
|
||||
|
||||
}
|
||||
|
||||
return ret_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while (total_bytes_read < (int) character_count) {
|
||||
|
||||
if (timeout_us > 0) {
|
||||
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (elapsed_us >= timeout_us) {
|
||||
// If there was a partial read but not enough to satisfy the minimum then they will be lost
|
||||
// but this really should never happen when everything is working normally.
|
||||
// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
|
||||
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
|
||||
return total_bytes_read;
|
||||
}
|
||||
}
|
||||
|
||||
int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (current_bytes_read < 0) {
|
||||
// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
|
||||
// but this really should never happen when everything is working normally.
|
||||
PX4_ERR("%s failed to read uart", __FUNCTION__);
|
||||
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Current bytes read could be zero
|
||||
total_bytes_read += current_bytes_read;
|
||||
|
||||
// If we have at least reached our desired minimum number of characters
|
||||
// then we can return now
|
||||
if (total_bytes_read >= (int) character_count) {
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
// Wait a set amount of time before trying again or the remaining time
|
||||
// until the timeout if we are getting close
|
||||
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
|
||||
int64_t time_until_timeout = timeout_us - elapsed_us;
|
||||
uint64_t time_to_sleep = 5000;
|
||||
|
||||
if ((time_until_timeout >= 0) &&
|
||||
(time_until_timeout < (int64_t) time_to_sleep)) {
|
||||
time_to_sleep = time_until_timeout;
|
||||
}
|
||||
|
||||
px4_usleep(time_to_sleep);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
|
||||
|
||||
if (ret_write < 0) {
|
||||
PX4_ERR("%s write error %d", _port, ret_write);
|
||||
|
||||
}
|
||||
|
||||
return ret_write;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return open();
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -81,7 +81,7 @@ static void hrt_unlock()
|
|||
px4_sem_post(&_hrt_lock);
|
||||
}
|
||||
|
||||
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
|
||||
int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@
|
|||
#include <poll.h>
|
||||
#endif
|
||||
|
||||
#include <termios.h>
|
||||
#include <cstring>
|
||||
|
||||
#include <drivers/drv_sensor.h>
|
||||
|
|
@ -57,6 +56,8 @@
|
|||
#include <px4_platform_common/cli.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
|
@ -81,6 +82,7 @@
|
|||
#include <linux/spi/spidev.h>
|
||||
#endif /* __PX4_LINUX */
|
||||
|
||||
using namespace device;
|
||||
using namespace time_literals;
|
||||
|
||||
#define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
|
||||
|
|
@ -169,7 +171,10 @@ public:
|
|||
void reset_if_scheduled();
|
||||
|
||||
private:
|
||||
int _serial_fd{-1}; ///< serial interface to GPS
|
||||
#ifdef __PX4_LINUX
|
||||
int _spi_fd {-1}; ///< SPI interface to GPS
|
||||
#endif
|
||||
Serial *_uart = nullptr; ///< UART 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
|
||||
|
|
@ -329,8 +334,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
|
|||
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
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
|
||||
#endif
|
||||
}
|
||||
|
||||
if (_mode == gps_driver_mode_t::None) {
|
||||
|
|
@ -403,10 +411,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
|
|||
return num_read;
|
||||
}
|
||||
|
||||
case GPSCallbackType::writeDeviceData:
|
||||
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
|
||||
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);
|
||||
int ret = 0;
|
||||
|
||||
if (gps->_uart) {
|
||||
ret = gps->_uart->write((void *) data1, (size_t) data2);
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (gps->_spi_fd >= 0) {
|
||||
ret = ::write(gps->_spi_fd, data1, (size_t)data2);
|
||||
#endif
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
case GPSCallbackType::setBaudrate:
|
||||
return gps->setBaudrate(data2);
|
||||
|
|
@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
|
|||
|
||||
int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
|
||||
{
|
||||
int ret = 0;
|
||||
const unsigned character_count = 32; // minimum bytes that we want to read
|
||||
const int max_timeout = 50;
|
||||
int timeout_adjusted = math::min(max_timeout, timeout);
|
||||
|
||||
handleInjectDataTopic();
|
||||
|
||||
#if !defined(__PX4_QURT)
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
|
||||
|
||||
/* For non QURT, use the usual polling. */
|
||||
// SPI is only supported on LInux
|
||||
#if defined(__PX4_LINUX)
|
||||
|
||||
//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;
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
|
||||
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
//Poll only for the SPI data. In the same thread we also need to handle orb messages,
|
||||
//so ideally we would poll on both, the SPI 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
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _spi_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
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);
|
||||
ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
int err = 0;
|
||||
int bytes_available = 0;
|
||||
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
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.
|
||||
*/
|
||||
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
|
||||
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
|
||||
|
||||
if (err != 0 || bytes_available < (int)character_count) {
|
||||
px4_usleep(sleeptime);
|
||||
|
||||
ret = ::read(_spi_fd, buf, buf_length);
|
||||
|
||||
if (ret > 0) {
|
||||
_num_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
#else
|
||||
px4_usleep(sleeptime);
|
||||
#endif
|
||||
|
||||
ret = ::read(_serial_fd, buf, buf_length);
|
||||
|
||||
if (ret > 0) {
|
||||
_num_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
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()
|
||||
|
|
@ -583,105 +596,38 @@ 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);
|
||||
size_t written = 0;
|
||||
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
written = _uart->write((const void *) data, len);
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
written = ::write(_spi_fd, data, len);
|
||||
::fsync(_spi_fd);
|
||||
#endif
|
||||
}
|
||||
|
||||
return written == len;
|
||||
}
|
||||
|
||||
int GPS::setBaudrate(unsigned baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
if (_interface == GPSHelper::Interface::UART) {
|
||||
if ((_uart) && (_uart->setBaudrate(baud))) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
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
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
// Can't set the baudrate on a SPI port but just return a success
|
||||
return 0;
|
||||
#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;
|
||||
}
|
||||
|
||||
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()
|
||||
|
|
@ -840,31 +786,58 @@ GPS::run()
|
|||
_helper = nullptr;
|
||||
}
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
// Create the UART port instance
|
||||
_uart = new Serial(_port);
|
||||
|
||||
if (_uart == nullptr) {
|
||||
PX4_ERR("Error creating serial device %s", _port);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
|
||||
// Configure the desired baudrate if one was specified by the user.
|
||||
// Otherwise the default baudrate will be used.
|
||||
if (_configured_baudrate) {
|
||||
if (! _uart->setBaudrate(_configured_baudrate)) {
|
||||
PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
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);
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
|
||||
_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
if (_spi_fd < 0) {
|
||||
PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
|
||||
int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
|
||||
status_value = ::ioctl(_spi_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 */
|
||||
|
|
@ -1056,9 +1029,17 @@ GPS::run()
|
|||
}
|
||||
}
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
_serial_fd = -1;
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
(void) _uart->close();
|
||||
delete _uart;
|
||||
_uart = nullptr;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
|
||||
::close(_spi_fd);
|
||||
_spi_fd = -1;
|
||||
#endif
|
||||
}
|
||||
|
||||
if (_mode_auto) {
|
||||
|
|
@ -1477,12 +1458,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'i':
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface = GPSHelper::Interface::SPI;
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface = GPSHelper::Interface::UART;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
interface = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
} else {
|
||||
PX4_ERR("unknown interface: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
|
@ -1490,12 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'j':
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface_secondary = GPSHelper::Interface::SPI;
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface_secondary = GPSHelper::Interface::UART;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
interface_secondary = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
} else {
|
||||
PX4_ERR("unknown interface for secondary: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
|
|
|||
Loading…
Reference in New Issue