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

src/drivers/rc/crsf_rc/CMakeLists.txt

@@ -46,6 +46,4 @@ px4_add_module(
 
 	MODULE_CONFIG
 		module.yaml
-	DEPENDS
-		rc
 	)

src/drivers/rc/crsf_rc/CrsfParser.cpp

@@ -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);
 	}

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -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()

src/drivers/rc/crsf_rc/CrsfRc.hpp

@@ -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};