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

First working version. Tested on ModalAI FlightCore V2

This commit is contained in:
Eric Katzfey 2023-06-15 11:52:04 -07:00
parent 6be8cbe439
commit 5d80d23668
14 changed files with 1640 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
platforms/common/CMakeLists.txt
View File

@ -51,9 +51,10 @@ add_library(px4_platform STATIC
px4_cli.cpp
shutdown.cpp
spi.cpp
Serial.cpp
${SRCS}
)
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
target_link_libraries(px4_platform prebuild_targets px4_work_queue px4_layer)
if(NOT "${PX4_BOARD}" MATCHES "io-v2")
add_subdirectory(uORB)

114
platforms/common/Serial.cpp Normal file
View File

@ -0,0 +1,114 @@
#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)
{
// 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();
}
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);
}
} // namespace device

102
platforms/common/include/px4_platform_common/Serial.hpp Normal file
View File

@ -0,0 +1,102 @@
/****************************************************************************
*
* 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 <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);
// Perhaps this can be removed? To change the port you need to create a new object.
const char *getPort() const;
bool setPort(const char *port);
// 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);
// printStatus()
// port, read bytes, write bytes
private:
// Disable copy constructors
Serial(const Serial &);
Serial &operator=(const Serial &);
// platform implementation
SerialImpl _impl;
};
} // namespace device

38
platforms/common/include/px4_platform_common/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

367
platforms/nuttx/src/px4/common/SerialImpl.cpp Normal file
View File

@ -0,0 +1,367 @@
#include <SerialImpl.hpp>
#include <string.h> // strncpy
#include <termios.h> // strncpy
#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::configure() {
/* process baud rate */
int speed;
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 */
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) {
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;
}
// Configure the serial port if a baudrate has been configured
if (_baudrate) {
if ( ! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
return false;
}
}
_serial_fd = serial_fd;
_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);
} else {
// Track total bytes read
_bytes_read += 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) {
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);
} else {
// Track total bytes written
_bytes_written += written;
}
return written;
}
const char *SerialImpl::getPort() const
{
return _port;
}
bool SerialImpl::setPort(const char *port)
{
if (strcmp(port, _port) == 0) {
return true;
}
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
// If old port is already opened then close it and reopen it on new port
if (_open) {
close();
return open();
}
return true;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
// 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)
{
if (bytesize != ByteSize::EightBits) {
return false;
}
return true;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
if (parity != Parity::None) {
return false;
}
return true;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
if (stopbits != StopBits::One) {
return false;
}
return true;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
if (flowcontrol != FlowControl::Disabled) {
return false;
}
return true;
}
} // namespace device

81
platforms/nuttx/src/px4/common/include/SerialImpl.hpp Normal file
View File

@ -0,0 +1,81 @@
#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;
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};
size_t _bytes_read{0};
size_t _bytes_written{0};
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 configure();
// 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

1
platforms/nuttx/src/px4/common/px4_layer.cmake
View File

@ -3,6 +3,7 @@
add_library(px4_layer
${KERNEL_SRCS}
cdc_acm_check.cpp
SerialImpl.cpp
)
target_link_libraries(px4_layer

81
platforms/posix/include/SerialImpl.hpp Normal file
View File

@ -0,0 +1,81 @@
#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;
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};
size_t _bytes_read{0};
size_t _bytes_written{0};
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 configure();
// 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

1
platforms/posix/src/px4/common/CMakeLists.txt
View File

@ -46,6 +46,7 @@ add_library(px4_layer
drv_hrt.cpp
cpuload.cpp
print_load.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

365
platforms/posix/src/px4/common/SerialImpl.cpp Normal file
View File

@ -0,0 +1,365 @@
#include <SerialImpl.hpp>
#include <string.h> // strncpy
#include <termios.h> // strncpy
#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::configure() {
/* process baud rate */
int speed;
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 */
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) {
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;
}
// Configure the serial port if a baudrate has been configured
if (_baudrate) {
if ( ! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
return false;
}
}
_serial_fd = serial_fd;
_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);
} else {
// Track total bytes read
_bytes_read += 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) {
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);
} else {
// Track total bytes written
_bytes_written += written;
}
return written;
}
const char *SerialImpl::getPort() const
{
return _port;
}
bool SerialImpl::setPort(const char *port)
{
if (strcmp(port, _port) == 0) {
return true;
}
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
// If old port is already opened then close it and reopen it on new port
if (_open) {
close();
return open();
}
return true;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
// 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)
{
if (bytesize != ByteSize::EightBits) {
return false;
}
return true;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
if (parity != Parity::None) {
return false;
}
return true;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
if (stopbits != StopBits::One) {
return false;
}
return true;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
if (flowcontrol != FlowControl::Disabled) {
return false;
}
return true;
}
} // namespace device

78
platforms/qurt/include/SerialImpl.hpp Normal file
View File

@ -0,0 +1,78 @@
#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};
size_t _bytes_read{0};
size_t _bytes_written{0};
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};
// 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

1
platforms/qurt/src/px4/CMakeLists.txt
View File

@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
px4_qurt_impl.cpp
main.cpp
qurt_log.cpp
SerialImpl.cpp
)
add_library(px4_layer

294
platforms/qurt/src/px4/SerialImpl.cpp Normal file
View File

@ -0,0 +1,294 @@
#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;
}
// Start off with a valid bitrate to make sure open can succeed
if (_baudrate == 0) _baudrate = 9600;
}
SerialImpl::~SerialImpl()
{
if (isOpen()) {
close();
}
}
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 (_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);
} 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)
{
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);
} else {
_bytes_written += ret_write;
}
return ret_write;
}
const char *SerialImpl::getPort() const
{
return _port;
}
bool SerialImpl::setPort(const char *port)
{
if (strcmp(port, _port) == 0) {
return true;
}
return false;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
// 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)
{
if (bytesize != ByteSize::EightBits) {
return false;
}
return true;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
if (parity != Parity::None) {
return false;
}
return true;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
if (stopbits != StopBits::One) {
return false;
}
return true;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
if (flowcontrol != FlowControl::Disabled) {
return false;
}
return true;
}
} // namespace device

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

@ -45,7 +45,6 @@
#include <poll.h>
#endif
#include <termios.h>
#include <cstring>
#include <drivers/drv_sensor.h>
@ -57,6 +56,7 @@
#include <px4_platform_common/cli.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/Serial.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
@ -81,6 +81,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 +170,8 @@ public:
void reset_if_scheduled();
private:
int _serial_fd{-1}; ///< serial interface to GPS
int _spi_fd{-1}; ///< SPI interface to GPS
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
@ -403,10 +405,19 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
return num_read;
}
case GPSCallbackType::writeDeviceData:
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);
} else if (gps->_spi_fd >= 0) {
ret = ::write(gps->_spi_fd, data1, (size_t)data2);
}
return ret;
}
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
@ -449,25 +460,34 @@ 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 = 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);
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
// Qurt does not support poll for serial devices (nor external SPI devices for that matter)
#if !defined(__PX4_QURT)
/* For non QURT, use the usual polling. */
//Poll only for the serial data. In the same thread we also need to handle orb messages,
//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 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].fd = _spi_fd;
fds[0].events = POLLIN;
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
ret = poll(fds, sizeof(fds) / sizeof(fds[0]), _timeout);
if (ret > 0) {
/* if we have new data from GPS, go handle it */
@ -479,24 +499,22 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
* 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);
err = ::ioctl(_spi_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);
ret = ::read(_spi_fd, buf, buf_length);
if (ret > 0) {
_num_bytes_read += ret;
@ -506,15 +524,10 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
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,31 @@ 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);
} else if (_interface == GPSHelper::Interface::SPI) {
written = ::write(_spi_fd, data, len);
::fsync(_spi_fd);
}
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 (_interface == GPSHelper::Interface::UART) {
if (_uart) {
return _uart->setBaudrate(baud);
}
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;
}
} else if (_interface == GPSHelper::Interface::SPI) {
// Can't set the baudrate on a SPI port but just return a success
return 0;
}
return -1;
}
void GPS::initializeCommunicationDump()
@ -840,33 +779,39 @@ 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)) {
_uart = new Serial(_port, _configured_baudrate);
if (_serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _port);
px4_sleep(1);
continue;
}
_uart->open();
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)){
_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_spi_fd < 0) {
PX4_ERR("failed to open SPI port %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);
int status_value = ::ioctl(_spi_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);
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 +1001,13 @@ GPS::run()
}
}
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
if ((_interface == GPSHelper::Interface::UART) && (_uart)){
_uart->close();
delete _uart;
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)){
::close(_spi_fd);
_spi_fd = -1;
}
if (_mode_auto) {