Added fix for fmu_v5_protected build

boards/modalai/voxl2-slpi/default.px4board

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

platforms/common/Serial.cpp

@@ -80,6 +80,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();
@@ -130,6 +135,24 @@ 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();
+}
+
 const char *Serial::getPort() const
 {
 	return _impl.getPort();

platforms/common/include/px4_platform_common/Serial.hpp

@@ -64,6 +64,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.
@@ -83,6 +85,12 @@ public:
 	FlowControl getFlowcontrol() const;
 	bool setFlowcontrol(FlowControl flowcontrol);
 
+	bool getSingleWireMode() const;
+	bool setSingleWireMode();
+
+	bool getSwapRxTxMode() const;
+	bool setSwapRxTxMode();
+
 	const char *getPort() const;
 
 private:

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -210,6 +210,14 @@ bool SerialImpl::open()
 
 	_open = true;
 
+	if (_single_wire_mode) {
+		setSingleWireMode();
+	}
+
+	if (_swap_rx_tx_mode) {
+		setSwapRxTxMode();
+	}
+
 	return _open;
 }
 
@@ -319,6 +327,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;
@@ -391,4 +406,44 @@ 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
+}
+
 } // namespace device

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -64,6 +64,8 @@ public:
 
 	ssize_t write(const void *buffer, size_t buffer_size);
 
+	void flush();
+
 	const char *getPort() const;
 
 	uint32_t getBaudrate() const;
@@ -81,6 +83,12 @@ public:
 	FlowControl getFlowcontrol() const;
 	bool setFlowcontrol(FlowControl flowcontrol);
 
+	bool getSingleWireMode() const;
+	bool setSingleWireMode();
+
+	bool getSwapRxTxMode() const;
+	bool setSwapRxTxMode();
+
 private:
 
 	int _serial_fd{-1};
@@ -99,6 +107,8 @@ private:
 	bool validateBaudrate(uint32_t baudrate);
 	bool configure();
 
+	bool _single_wire_mode{false};
+	bool _swap_rx_tx_mode{false};
 };
 
 } // namespace device

platforms/nuttx/src/px4/common/px4_protected_layers.cmake

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

platforms/posix/include/SerialImpl.hpp

@@ -64,6 +64,8 @@ public:
 
 	ssize_t write(const void *buffer, size_t buffer_size);
 
+	void flush();
+
 	const char *getPort() const;
 
 	uint32_t getBaudrate() const;
@@ -81,6 +83,12 @@ public:
 	FlowControl getFlowcontrol() const;
 	bool setFlowcontrol(FlowControl flowcontrol);
 
+	bool getSingleWireMode() const;
+	bool setSingleWireMode();
+
+	bool getSwapRxTxMode() const;
+	bool setSwapRxTxMode();
+
 private:
 
 	int _serial_fd{-1};
@@ -98,6 +106,9 @@ private:
 
 	bool validateBaudrate(uint32_t baudrate);
 	bool configure();
+
+	bool _single_wire_mode{false};
+	bool _swap_rx_tx_mode{false};
 };
 
 } // namespace device

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -208,6 +208,14 @@ bool SerialImpl::open()
 
 	_open = true;
 
+	if (_single_wire_mode) {
+		setSingleWireMode();
+	}
+
+	if (_swap_rx_tx_mode) {
+		setSwapRxTxMode();
+	}
+
 	return _open;
 }
 
@@ -312,6 +320,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;
@@ -384,4 +399,44 @@ 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
+}
+
 } // namespace device

platforms/qurt/include/SerialImpl.hpp

@@ -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);
 
@@ -81,6 +83,12 @@ public:
 	FlowControl getFlowcontrol() const;
 	bool setFlowcontrol(FlowControl flowcontrol);
 
+	bool getSingleWireMode() const;
+	bool setSingleWireMode();
+
+	bool getSwapRxTxMode() const;
+	bool setSwapRxTxMode();
+
 private:
 
 	int _serial_fd{-1};

platforms/qurt/src/px4/SerialImpl.cpp

@@ -251,6 +251,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;
@@ -323,4 +328,26 @@ 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;
+}
+
 } // namespace device

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