Port CRSF RC driver to new Serial UART API (#22917)

* Added implementations of Rx Tx swap and single wire for new UART API needed by CRSF driver
* Added inverted mode to Serial interface API
This commit is contained in:
Eric Katzfey
2024-04-01 19:09:13 -07:00
committed by GitHub
parent a9ba0acb2a
commit 8e61026511
14 changed files with 367 additions and 79 deletions
@@ -11,6 +11,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
CONFIG_DRIVERS_RC_CRSF_RC=y
CONFIG_DRIVERS_QSHELL_QURT=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
+33
View File
@@ -84,6 +84,11 @@ ssize_t Serial::write(const void *buffer, size_t buffer_size)
return _impl.write(buffer, buffer_size);
}
void Serial::flush()
{
return _impl.flush();
}
uint32_t Serial::getBaudrate() const
{
return _impl.getBaudrate();
@@ -134,6 +139,34 @@ bool Serial::setFlowcontrol(FlowControl flowcontrol)
return _impl.setFlowcontrol(flowcontrol);
}
bool Serial::getSingleWireMode() const
{
return _impl.getSingleWireMode();
}
bool Serial::setSingleWireMode()
{
return _impl.setSingleWireMode();
}
bool Serial::getSwapRxTxMode() const
{
return _impl.getSwapRxTxMode();
}
bool Serial::setSwapRxTxMode()
{
return _impl.setSwapRxTxMode();
}
bool Serial::getInvertedMode() const
{
return _impl.getInvertedMode();
}
bool Serial::setInvertedMode(bool enable)
{
return _impl.setInvertedMode(enable);
}
const char *Serial::getPort() const
{
return _impl.getPort();
@@ -65,6 +65,8 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
void flush();
// 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.
@@ -84,6 +86,15 @@ public:
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSingleWireMode() const;
bool setSingleWireMode();
bool getSwapRxTxMode() const;
bool setSwapRxTxMode();
bool getInvertedMode() const;
bool setInvertedMode(bool enable);
static bool validatePort(const char *port);
bool setPort(const char *port);
const char *getPort() const;
@@ -214,6 +214,17 @@ bool SerialImpl::open()
_open = true;
// Do pin operations after port has been opened
if (_single_wire_mode) {
setSingleWireMode();
}
if (_swap_rx_tx_mode) {
setSwapRxTxMode();
}
setInvertedMode(_inverted_mode);
return _open;
}
@@ -323,6 +334,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
return written;
}
void SerialImpl::flush()
{
if (_open) {
tcflush(_serial_fd, TCIOFLUSH);
}
}
const char *SerialImpl::getPort() const
{
return _port;
@@ -416,4 +434,69 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
return flowcontrol == FlowControl::Disabled;
}
bool SerialImpl::getSingleWireMode() const
{
return _single_wire_mode;
}
bool SerialImpl::setSingleWireMode()
{
#if defined(TIOCSSINGLEWIRE)
if (_open) {
ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
}
_single_wire_mode = true;
return true;
#else
return false;
#endif // TIOCSSINGLEWIRE
}
bool SerialImpl::getSwapRxTxMode() const
{
return _swap_rx_tx_mode;
}
bool SerialImpl::setSwapRxTxMode()
{
#if defined(TIOCSSWAP)
if (_open) {
ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED);
}
_swap_rx_tx_mode = true;
return true;
#else
return false;
#endif // TIOCSSWAP
}
bool SerialImpl::getInvertedMode() const
{
return _inverted_mode;
}
bool SerialImpl::setInvertedMode(bool enable)
{
#if defined(TIOCSINVERT)
if (_open) {
if (enable) {
ioctl(_serial_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
} else {
ioctl(_serial_fd, TIOCSINVERT, 0);
}
}
_inverted_mode = enable;
return true;
#else
return _inverted_mode == enable;
#endif // TIOCSINVERT
}
} // namespace device
@@ -64,6 +64,8 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
void flush();
const char *getPort() const;
static bool validatePort(const char *port);
bool setPort(const char *port);
@@ -83,6 +85,15 @@ public:
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSingleWireMode() const;
bool setSingleWireMode();
bool getSwapRxTxMode() const;
bool setSwapRxTxMode();
bool getInvertedMode() const;
bool setInvertedMode(bool enable);
private:
int _serial_fd{-1};
@@ -101,6 +112,9 @@ private:
bool validateBaudrate(uint32_t baudrate);
bool configure();
bool _single_wire_mode{false};
bool _swap_rx_tx_mode{false};
bool _inverted_mode{false};
};
} // namespace device
@@ -46,6 +46,7 @@ target_link_libraries(px4_layer
add_library(px4_kernel_layer
${KERNEL_SRCS}
SerialImpl.cpp
)
target_link_libraries(px4_kernel_layer
+15
View File
@@ -64,6 +64,8 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
void flush();
const char *getPort() const;
static bool validatePort(const char *port);
bool setPort(const char *port);
@@ -83,6 +85,15 @@ public:
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSingleWireMode() const;
bool setSingleWireMode();
bool getSwapRxTxMode() const;
bool setSwapRxTxMode();
bool getInvertedMode() const;
bool setInvertedMode(bool enable);
private:
int _serial_fd{-1};
@@ -100,6 +111,10 @@ private:
bool validateBaudrate(uint32_t baudrate);
bool configure();
bool _single_wire_mode{false};
bool _swap_rx_tx_mode{false};
bool _inverted_mode{false};
};
} // namespace device
@@ -212,6 +212,16 @@ bool SerialImpl::open()
_open = true;
if (_single_wire_mode) {
setSingleWireMode();
}
if (_swap_rx_tx_mode) {
setSwapRxTxMode();
}
setInvertedMode(_inverted_mode);
return _open;
}
@@ -316,6 +326,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
return written;
}
void SerialImpl::flush()
{
if (_open) {
tcflush(_serial_fd, TCIOFLUSH);
}
}
const char *SerialImpl::getPort() const
{
return _port;
@@ -409,4 +426,69 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
return flowcontrol == FlowControl::Disabled;
}
bool SerialImpl::getSingleWireMode() const
{
return _single_wire_mode;
}
bool SerialImpl::setSingleWireMode()
{
#if defined(TIOCSSINGLEWIRE)
if (_open) {
ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
}
_single_wire_mode = true;
return true;
#else
return false;
#endif // TIOCSSINGLEWIRE
}
bool SerialImpl::getSwapRxTxMode() const
{
return _swap_rx_tx_mode;
}
bool SerialImpl::setSwapRxTxMode()
{
#if defined(TIOCSSWAP)
if (_open) {
ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED);
}
_swap_rx_tx_mode = true;
return true;
#else
return false;
#endif // TIOCSSWAP
}
bool SerialImpl::getInvertedMode() const
{
return _inverted_mode;
}
bool SerialImpl::setInvertedMode(bool enable)
{
#if defined(TIOCSINVERT)
if (_open) {
if (enable) {
ioctl(_serial_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
} else {
ioctl(_serial_fd, TIOCSINVERT, 0);
}
}
_inverted_mode = enable;
return true;
#else
return _inverted_mode == enable;
#endif // TIOCSINVERT
}
} // namespace device
+11
View File
@@ -63,6 +63,8 @@ public:
ssize_t write(const void *buffer, size_t buffer_size);
void flush();
const char *getPort() const;
bool setPort(const char *port);
static bool validatePort(const char *port);
@@ -82,6 +84,15 @@ public:
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
bool getSingleWireMode() const;
bool setSingleWireMode();
bool getSwapRxTxMode() const;
bool setSwapRxTxMode();
bool getInvertedMode() const;
bool setInvertedMode(bool enable);
private:
int _serial_fd{-1};
+37
View File
@@ -255,6 +255,11 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
return ret_write;
}
void SerialImpl::flush()
{
// TODO: Flush not implemented yet on Qurt
}
const char *SerialImpl::getPort() const
{
return _port;
@@ -348,4 +353,36 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
return flowcontrol == FlowControl::Disabled;
}
bool SerialImpl::getSingleWireMode() const
{
return false;
}
bool SerialImpl::setSingleWireMode()
{
// Qurt platform does not support single wire mode
return false;
}
bool SerialImpl::getSwapRxTxMode() const
{
return false;
}
bool SerialImpl::setSwapRxTxMode()
{
// Qurt platform does not support swap rx tx mode
return false;
}
bool SerialImpl::setInvertedMode(bool enable)
{
// Qurt platform does not support inverted mode
return false == enable;
}
bool SerialImpl::getInvertedMode() const
{
return false;
}
} // namespace device
-2
View File
@@ -46,6 +46,4 @@ px4_add_module(
MODULE_CONFIG
module.yaml
DEPENDS
rc
)
+18 -17
View File
@@ -161,6 +161,8 @@ static float MapF(const float x, const float in_min, const float in_max, const f
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
#define CONSTRAIN_CHAN(x) ConstrainF(x, CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX)
static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
{
uint32_t raw_channels[CRSF_CHANNEL_COUNT];
@@ -169,25 +171,24 @@ static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPac
new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS;
// Decode channel data
raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF;
raw_channels[1] = (data[1] >> 3 | data[2] << 5) & 0x07FF;
raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF;
raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF;
raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF;
raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF;
raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF;
raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF;
raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF;
raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF;
raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF;
raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF;
raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF;
raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF;
raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF;
raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF;
raw_channels[0] = CONSTRAIN_CHAN((data[0] | data[1] << 8) & 0x07FF);
raw_channels[1] = CONSTRAIN_CHAN((data[1] >> 3 | data[2] << 5) & 0x07FF);
raw_channels[2] = CONSTRAIN_CHAN((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
raw_channels[3] = CONSTRAIN_CHAN((data[4] >> 1 | data[5] << 7) & 0x07FF);
raw_channels[4] = CONSTRAIN_CHAN((data[5] >> 4 | data[6] << 4) & 0x07FF);
raw_channels[5] = CONSTRAIN_CHAN((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
raw_channels[6] = CONSTRAIN_CHAN((data[8] >> 2 | data[9] << 6) & 0x07FF);
raw_channels[7] = CONSTRAIN_CHAN((data[9] >> 5 | data[10] << 3) & 0x07FF);
raw_channels[8] = CONSTRAIN_CHAN((data[11] | data[12] << 8) & 0x07FF);
raw_channels[9] = CONSTRAIN_CHAN((data[12] >> 3 | data[13] << 5) & 0x07FF);
raw_channels[10] = CONSTRAIN_CHAN((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
raw_channels[11] = CONSTRAIN_CHAN((data[15] >> 1 | data[16] << 7) & 0x07FF);
raw_channels[12] = CONSTRAIN_CHAN((data[16] >> 4 | data[17] << 4) & 0x07FF);
raw_channels[13] = CONSTRAIN_CHAN((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
raw_channels[14] = CONSTRAIN_CHAN((data[19] >> 2 | data[20] << 6) & 0x07FF);
raw_channels[15] = CONSTRAIN_CHAN((data[20] >> 5 | data[21] << 3) & 0x07FF);
for (i = 0; i < CRSF_CHANNEL_COUNT; i++) {
raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX);
new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX,
1000.0f, 2000.0f);
}
+56 -59
View File
@@ -35,8 +35,6 @@
#include "CrsfParser.hpp"
#include "Crc8.hpp"
#include <poll.h>
#include <termios.h>
#include <fcntl.h>
#include <uORB/topics/battery_status.h>
@@ -118,76 +116,74 @@ void CrsfRc::Run()
{
if (should_exit()) {
ScheduleClear();
::close(_rc_fd);
_rc_fd = -1;
if (_uart) {
(void) _uart->close();
delete _uart;
_uart = nullptr;
}
exit_and_cleanup();
return;
}
if (_rc_fd < 0) {
_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
if (_uart == nullptr) {
// Create the UART port instance
_uart = new Serial(_device);
if (_rc_fd >= 0) {
struct termios t;
tcgetattr(_rc_fd, &t);
cfsetspeed(&t, CRSF_BAUDRATE);
t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
t.c_oflag = 0;
tcsetattr(_rc_fd, TCSANOW, &t);
if (board_rc_swap_rxtx(_device)) {
#if defined(TIOCSSWAP)
ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
#endif // TIOCSSWAP
}
if (board_rc_singlewire(_device)) {
_is_singlewire = true;
#if defined(TIOCSSINGLEWIRE)
ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif // TIOCSSINGLEWIRE
}
PX4_INFO("Crsf serial opened sucessfully");
if (_is_singlewire) {
PX4_INFO("Crsf serial is single wire. Telemetry disabled");
}
tcflush(_rc_fd, TCIOFLUSH);
Crc8Init(0xd5);
if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _device);
px4_sleep(1);
return;
}
}
if (! _uart->isOpen()) {
// Configure the desired baudrate if one was specified by the user.
// Otherwise the default baudrate will be used.
if (! _uart->setBaudrate(CRSF_BAUDRATE)) {
PX4_ERR("Error setting baudrate to %u on %s", CRSF_BAUDRATE, _device);
px4_sleep(1);
return;
}
// Open the UART. If this is successful then the UART is ready to use.
if (! _uart->open()) {
PX4_ERR("Error opening serial device %s", _device);
px4_sleep(1);
return;
}
if (board_rc_swap_rxtx(_device)) {
_uart->setSwapRxTxMode();
}
if (board_rc_singlewire(_device)) {
_is_singlewire = true;
_uart->setSingleWireMode();
}
PX4_INFO("Crsf serial opened sucessfully");
if (_is_singlewire) {
PX4_INFO("Crsf serial is single wire. Telemetry disabled");
}
_uart->flush();
Crc8Init(0xd5);
_input_rc.rssi_dbm = NAN;
_input_rc.link_quality = -1;
CrsfParser_Init();
}
// poll with 100mS timeout
pollfd fds[1];
fds[0].fd = _rc_fd;
fds[0].events = POLLIN;
int ret = ::poll(fds, 1, 100);
if (ret < 0) {
PX4_ERR("poll error");
// try again with delay
ScheduleDelayed(100_ms);
return;
}
const hrt_abstime time_now_us = hrt_absolute_time();
perf_count_interval(_cycle_interval_perf, time_now_us);
// Read all available data from the serial RC input UART
int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100);
if (new_bytes > 0) {
_bytes_rx += new_bytes;
@@ -433,7 +429,8 @@ bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current
write_uint24_t(buf, offset, fuel);
write_uint8_t(buf, offset, remaining);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
@@ -449,7 +446,7 @@ bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, c
write_uint16_t(buf, offset, altitude);
write_uint8_t(buf, offset, num_satellites);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw)
@@ -461,7 +458,7 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons
write_uint16_t(buf, offset, roll);
write_uint16_t(buf, offset, yaw);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
@@ -480,7 +477,7 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
offset += length;
buf[offset - 1] = 0; // ensure null-terminated string
WriteFrameCrc(buf, offset, length + 4);
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
int CrsfRc::print_status()
+5 -1
View File
@@ -40,6 +40,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/Serial.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
@@ -53,6 +54,8 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
using namespace device;
class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@@ -87,7 +90,8 @@ private:
bool SendTelemetryFlightMode(const char *flight_mode);
int _rc_fd{-1};
Serial *_uart = nullptr; ///< UART interface to RC
char _device[20] {}; ///< device / serial port path
bool _is_singlewire{false};