split drivers/rc_input per protocol

src/drivers/px4io/px4io.cpp

@@ -58,7 +58,7 @@
 #include <lib/mixer_module/mixer_module.hpp>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/rc/dsm.h>
+#include <lib/rc/dsm/dsm.h>
 #include <lib/systemlib/mavlink_log.h>
 #include <lib/button/ButtonPublisher.hpp>
 

src/drivers/rc/Kconfig

@@ -1,9 +1,15 @@
 menu "RC"
     menuconfig COMMON_RC
-        bool "Common RC"
+        bool "Common RC drivers"
         default n
-        select DRIVERS_RC_CRSF_RC
+        select DRIVERS_RC_CRSF
+        select DRIVERS_RC_SPEKTRUM_DSM
+        select DRIVERS_RC_GHST_RC
+        #select DRIVERS_RC_PPM_IN
+        select DRIVERS_RC_SBUS
+        #select DRIVERS_RC_ST24_RC
+        #select DRIVERS_RC_SUMD_RC
         ---help---
-            Enable default set of magnetometer drivers
+            Enable default set of RC drivers
     rsource "*/Kconfig"
 endmenu

src/drivers/rc/crsf/CMakeLists.txt

@@ -31,8 +31,8 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__rc__crsf_rc
-	MAIN crsf_rc
+	MODULE drivers__rc__crsf
+	MAIN crsf
 	COMPILE_FLAGS
 	SRCS
 		CrsfRc.cpp
@@ -47,5 +47,5 @@ px4_add_module(
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
-		rc
+		px4_work_queue
 	)

src/drivers/rc/crsf/CrsfRc.cpp

@@ -500,12 +500,12 @@ int CrsfRc::print_status()
 	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);
 
-	PX4_INFO("Disposed bytes: %li",  _packet_parser_statistics.disposed_bytes);
-	PX4_INFO("Valid known packet CRCs: %li",  _packet_parser_statistics.crcs_valid_known_packets);
-	PX4_INFO("Valid unknown packet CRCs: %li",  _packet_parser_statistics.crcs_valid_unknown_packets);
-	PX4_INFO("Invalid CRCs: %li",  _packet_parser_statistics.crcs_invalid);
-	PX4_INFO("Invalid known packet sizes: %li",  _packet_parser_statistics.invalid_known_packet_sizes);
-	PX4_INFO("Invalid unknown packet sizes: %li",  _packet_parser_statistics.invalid_unknown_packet_sizes);
+	PX4_INFO("Disposed bytes: %" PRIu32, _packet_parser_statistics.disposed_bytes);
+	PX4_INFO("Valid known packet CRCs: %" PRIu32, _packet_parser_statistics.crcs_valid_known_packets);
+	PX4_INFO("Valid unknown packet CRCs: %" PRIu32, _packet_parser_statistics.crcs_valid_unknown_packets);
+	PX4_INFO("Invalid CRCs: %" PRIu32, _packet_parser_statistics.crcs_invalid);
+	PX4_INFO("Invalid known packet sizes: %" PRIu32, _packet_parser_statistics.invalid_known_packet_sizes);
+	PX4_INFO("Invalid unknown packet sizes: %" PRIu32, _packet_parser_statistics.invalid_unknown_packet_sizes);
 
 	return 0;
 }
@@ -528,7 +528,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("crsf_rc", "driver");
+	PRINT_MODULE_USAGE_NAME("crsf", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
 
@@ -537,7 +537,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem
 	return 0;
 }
 
-extern "C" __EXPORT int crsf_rc_main(int argc, char *argv[])
+extern "C" __EXPORT int crsf_main(int argc, char *argv[])
 {
 	return CrsfRc::main(argc, argv);
 }

src/drivers/rc/crsf/Kconfig

@@ -1,5 +1,5 @@
-menuconfig DRIVERS_RC_CRSF_RC
-	bool "crsf_rc"
+menuconfig DRIVERS_RC_CRSF
+	bool "crsf"
 	default n
 	---help---
 		Enable support for crsf rc

src/drivers/rc/crsf/module.yaml

@@ -1,6 +1,6 @@
 module_name: CRSF RC Input Driver
 serial_config:
-    - command: "crsf_rc start -d ${SERIAL_DEV}"
+    - command: "crsf start -d ${SERIAL_DEV}"
       port_config_param:
         name: RC_CRSF_PRT_CFG
         group: Serial

src/drivers/rc/ghst_rc/CMakeLists.txt

@@ -0,0 +1,51 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+if(PX4_TESTING AND (${PX4_PLATFORM} MATCHES "posix"))
+	add_subdirectory(test)
+endif()
+
+px4_add_module(
+	MODULE drivers__rc__ghst
+	MAIN rc_ghst
+	COMPILE_FLAGS
+	SRCS
+		RCInput.cpp
+		RCInput.hpp
+		ghst_parser.cpp
+		ghst_parser.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		px4_work_queue
+	)

src/drivers/rc/ghst_rc/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_GHST_RC
+	bool "rc_ghst"
+	default n
+	---help---
+		Enable support for rc_ghst

src/drivers/rc/ghst_rc/RCInput.cpp

@@ -0,0 +1,427 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+#include <termios.h>
+
+using namespace time_literals;
+
+RcGhst::RcGhst(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+RcGhst::~RcGhst()
+{
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcGhst::init()
+{
+	_rcs_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+
+	if (_rcs_fd >= 0) {
+		ghst_config(_rcs_fd);
+	}
+
+	if (_rcs_fd < 0) {
+		return -errno;
+	}
+
+	if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+	}
+
+	rc_io_invert(false);
+
+	return 0;
+}
+
+int RcGhst::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+#if defined(RC_SERIAL_PORT)
+	device_name = RC_SERIAL_PORT;
+#endif // RC_SERIAL_PORT
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RcGhst *instance = new RcGhst(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void RcGhst::fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void RcGhst::rc_io_invert(bool invert)
+{
+	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
+	// and if not use an IOCTL
+	if (!board_rc_invert_input(_device, invert)) {
+#if defined(TIOCSINVERT)
+
+		if (invert) {
+			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
+
+		} else {
+			ioctl(_rcs_fd, TIOCSINVERT, 0);
+		}
+
+#endif // TIOCSINVERT
+	}
+}
+
+void RcGhst::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+		if (init() == PX4_OK) {
+			_initialized = true;
+
+		} else {
+			PX4_ERR("init failed");
+			exit_and_cleanup();
+		}
+
+	} else {
+
+		perf_begin(_cycle_perf);
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+		bool rc_updated = false;
+
+		// This block scans for a supported serial RC input and locks onto the first one found
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
+
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
+
+		// read all available data from the serial RC input UART
+		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+		if (newBytes > 0) {
+			_bytes_rx += newBytes;
+		}
+
+		const bool rc_scan_locked = _rc_scan_locked;
+
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = cycle_timestamp;
+			// Configure serial port for GHST
+			ghst_config(_rcs_fd);
+
+			// flush serial buffer and any existing buffered data
+			tcflush(_rcs_fd, TCIOFLUSH);
+			memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+		} else if (_rc_scan_locked
+			   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+			// parse new data
+			if (newBytes > 0) {
+				int8_t ghst_rssi = -1;
+				rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
+							&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+				if (rc_updated) {
+					// we have a new GHST frame. Publish it.
+					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+					fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+
+					_rc_scan_locked = true;
+
+					// telemetry
+					if ((cycle_timestamp - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) {
+
+						switch (_next_type) {
+						case 0U:
+							send_battery_status();
+							break;
+
+						case 1U:
+							send_gps1_status();
+							break;
+
+						case 2U:
+							send_gps2_status();
+							break;
+
+						default:
+							break;
+						}
+
+						_last_update = cycle_timestamp;
+						_next_type = (_next_type + 1U) % NUM_DATA_TYPES;
+					}
+				}
+			}
+		}
+
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+			_rc_scan_locked = false;
+		}
+
+		if (!rc_scan_locked && _rc_scan_locked) {
+			PX4_INFO("RC input locked");
+		}
+	}
+}
+
+bool RcGhst::send_battery_status()
+{
+	battery_status_s battery_status;
+
+	if (_battery_status_sub.update(&battery_status)) {
+
+		float voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
+		float current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+		float fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
+		return ghst_send_telemetry_battery_status(_rcs_fd,
+				static_cast<uint16_t>(voltage_in_10mV),
+				static_cast<uint16_t>(current_in_10mA),
+				static_cast<uint16_t>(fuel_in_10mAh));
+	}
+
+	return false;
+}
+
+bool RcGhst::send_gps1_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		int32_t latitude = vehicle_gps_position.lat;         // 1e-7 degrees
+		int32_t longitude = vehicle_gps_position.lon;        // 1e-7 degrees
+		uint16_t altitude = vehicle_gps_position.alt / 1000; // mm -> m
+
+		return ghst_send_telemetry_gps1_status(_rcs_fd, latitude, longitude, altitude);
+	}
+
+	return false;
+}
+
+bool RcGhst::send_gps2_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+
+		uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
+		uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
+		uint8_t num_sats = vehicle_gps_position.satellites_used;
+
+		// TBD: Can these be computed in a RC telemetry driver?
+		uint16_t home_dist = 0;
+		uint16_t home_dir = 0;
+		uint8_t flags = 0;
+
+		return ghst_send_telemetry_gps2_status(_rcs_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
+	}
+
+	return false;
+}
+
+int RcGhst::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		int ret = RcGhst::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcGhst::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+	// PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
+	return 0;
+}
+
+int RcGhst::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- GHST
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rc_ghst", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", false);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int rc_ghst_main(int argc, char *argv[])
+{
+	return RcGhst::main(argc, argv);
+}

src/drivers/rc/ghst_rc/RCInput.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -31,44 +31,83 @@
  *
  ****************************************************************************/
 
-/**
- * @file ghst_telemetry.cpp
- *
- * IRC Ghost (Immersion RC Ghost) telemetry.
- *
- * @author Igor Misic <igy1000mb@gmail.com>
- * @author Juraj Ciberlin <jciberlin1@gmail.com>
- */
-
 #pragma once
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/sensor_gps.h>
-#include <drivers/drv_hrt.h>
+#include <float.h>
 
-/**
- * High-level class that handles sending of GHST telemetry data
- */
-class GHSTTelemetry
+#include "ghst_parser.hpp"
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/sensor_gps.h>
+
+class RcGhst : public ModuleBase<RcGhst>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	/**
-	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
-	 * already.
-	 */
-	explicit GHSTTelemetry(int uart_fd);
+	RcGhst(const char *device);
+	virtual ~RcGhst();
 
-	~GHSTTelemetry() = default;
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
 
-	/**
-	 * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
-	 * limit the sending rate.
-	 * @return true if new data sent
-	 */
-	bool update(const hrt_abstime &now);
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int init();
 
 private:
+	void Run() override;
+
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+	void rc_io_invert(bool invert);
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _initialized{false};
+	bool _rc_scan_locked{false};
+
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+
+	input_rc_s	_rc_in{};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+
+	int		_rcs_fd{-1};
+	char		_device[20] {};					///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{sizeof(ghst_frame_t)};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+
+	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+	uint32_t	_bytes_rx{0};
+
+	// telemetry
 	bool send_battery_status();
 	bool send_gps1_status();
 	bool send_gps2_status();
@@ -76,9 +115,8 @@ private:
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
-	int _uart_fd;
-	hrt_abstime _last_update {0U};
-	uint32_t _next_type {0U};
+	hrt_abstime _last_update{};
+	uint32_t _next_type{};
 
 	static constexpr uint32_t NUM_DATA_TYPES {3U};	// number of different telemetry data types
 	static constexpr uint32_t UPDATE_RATE_HZ {10U};	// update rate [Hz]
@@ -87,5 +125,4 @@ private:
 	static constexpr float FACTOR_VOLTS_TO_10MV {100.0F};
 	static constexpr float FACTOR_AMPS_TO_10MA {100.0F};
 	static constexpr float FACTOR_MAH_TO_10MAH {0.1F};
-
 };

src/drivers/rc/ghst_rc/ghst_parser.cpp

@@ -53,15 +53,12 @@
 #endif
 
 #include <drivers/drv_hrt.h>
+#include <limits.h>
 #include <termios.h>
 #include <string.h>
 #include <unistd.h>
 
-// TODO: include RSSI dBm to percentage conversion for ghost receiver
-#include "spektrum_rssi.h"
-
-#include "ghst.hpp"
-#include "common_rc.h"
+#include "ghst_parser.hpp"
 
 #define MIN(a,b) (((a)<(b))?(a):(b))
 
@@ -79,12 +76,59 @@ enum class ghst_parser_state_t : uint8_t {
 // only RSSI frame contains value of RSSI, if it is not received, send last received RSSI
 static int8_t ghst_rssi = -1;
 
-static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame;
+static ghst_frame_t ghst_frame;
 static uint32_t current_frame_position = 0U;
 static ghst_parser_state_t parser_state = ghst_parser_state_t::unsynced;
 
 static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS];
 
+static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
+{
+	crc ^= a;
+
+	for (int i = 0; i < 8; ++i) {
+		if (crc & 0x80) {
+			crc = (crc << 1) ^ 0xD5;
+
+		} else {
+			crc = crc << 1;
+		}
+	}
+
+	return crc;
+}
+
+static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
+{
+	uint8_t crc = 0;
+
+	for (int i = 0; i < len; ++i) {
+		crc = crc8_dvb_s2(crc, buf[i]);
+	}
+
+	return crc;
+}
+
+static constexpr int8_t lu_dbm_percent[] {
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 99,  98,  98,  97,  96,
+	95,  94,  93,  92,  91,  90,  89,  88,  87,  86,  85,  84,  83,  82,  81,  79,
+	78,  77,  75,  74,  73,  71,  70,  68,  66,  65,  63,  61,  59,  57,  55,  52,
+	50,  47,  45,  42,  39,  35,  32,  28,  24,  19,  13,  7,   0,   0,   0,   0,
+	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+	0,
+};
+
+#define MASK(n) (n >> (sizeof(n) * CHAR_BIT - 1))
+
+/* constexpr-compatible version of abs() */
+static constexpr unsigned c_abs(int n) { return (n + MASK(n)) ^ MASK(n); }
+
+/* convert signed dbm (-92 to -42) to percentage */
+static constexpr int8_t dbm_to_percent(int8_t dbm) { return lu_dbm_percent[c_abs(dbm)]; }
+
 /**
  * parse the current ghst_frame buffer
  */
@@ -104,14 +148,6 @@ int ghst_config(int uart_fd)
 	return ret_val;
 }
 
-/**
- * Convert from RC to PWM value
- * @param chan_value channel value in [172, 1811]
- * @return PWM channel value in [1000, 2000]
- */
-static uint16_t convert_channel_value(unsigned chan_value);
-
-
 bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
 		int8_t *rssi, uint16_t *num_values, uint16_t max_channels)
 {
@@ -164,7 +200,12 @@ uint8_t ghst_frame_CRC(const ghst_frame_t &frame)
 	return crc;
 }
 
-static uint16_t convert_channel_value(unsigned int chan_value)
+/**
+ * Convert from RC to PWM value
+ * @param chan_value channel value in [172, 1811]
+ * @return PWM channel value in [1000, 2000]
+ */
+static uint16_t convert_channel_value(unsigned chan_value)
 {
 	/*
 	 *       RC     PWM
@@ -297,7 +338,7 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu
 			} else if (ghst_frame.type == static_cast<uint8_t>(ghstFrameType::frameTypeRssi)) {
 				const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload;
 				// TODO: call function for RSSI dBm to percentage conversion for ghost receiver
-				ghst_rssi = spek_dbm_to_percent(static_cast<int8_t>(rssiValues->rssidBm));
+				ghst_rssi = dbm_to_percent(static_cast<int8_t>(rssiValues->rssidBm));
 
 			} else {
 				GHST_DEBUG("Frame type: %u", ghst_frame.type);
@@ -428,4 +469,3 @@ bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_
 
 	return write(uart_fd, buf, offset) == offset;
 }
-

src/drivers/rc/ghst_rc/module.yaml

@@ -0,0 +1,7 @@
+module_name: RC GHST input driver
+serial_config:
+    - command: "rc_ghst start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_GHST_PRT_CFG
+        group: Serial
+        #default: RC

src/drivers/rc/ghst_rc/test/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016-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
@@ -32,10 +32,11 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__snapdragon_spektrum_rc
-	MAIN spektrum_rc
+	MODULE drivers__rc__ghst__test
+	MAIN rc_ghst_test
+	COMPILE_FLAGS
+		-Wno-unused-result
 	SRCS
-		spektrum_rc.cpp
-	DEPENDS
-		rc
+		RcGhstTest.cpp
+		../ghst_parser.cpp
 	)

src/drivers/rc/ghst_rc/test/RcGhstTest.cpp

@@ -0,0 +1,123 @@
+#include <unit_test.h>
+
+#include <systemlib/err.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "../ghst_parser.hpp"
+
+#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
+#define TEST_DATA_PATH "./test_data/"
+#else
+#define TEST_DATA_PATH "/fs/microsd"
+#endif
+
+class RcGhstTest : public UnitTest
+{
+public:
+	bool run_tests() override;
+
+private:
+	bool ghstTest();
+};
+
+bool RcGhstTest::run_tests()
+{
+	ut_run_test(ghstTest);
+
+	return (_tests_failed == 0);
+}
+
+bool RcGhstTest::ghstTest()
+{
+	const char *filepath = TEST_DATA_PATH "ghst_rc_channels.txt";
+
+	FILE *fp = fopen(filepath, "rt");
+
+	ut_test(fp);
+
+	int uart_fd = -1;
+	const int line_size = 500;
+	char line[line_size];
+	bool has_decoded_values = false;
+	const int max_channels = 16;
+	uint16_t rc_values[max_channels];
+	uint16_t num_values = 0;
+	int line_counter = 1;
+	int8_t ghst_rssi = -1;
+	ghst_config(uart_fd);
+
+	while (fgets(line, line_size, fp) != nullptr)  {
+
+		if (strncmp(line, "INPUT ", 6) == 0) {
+
+			if (has_decoded_values) {
+				PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter);
+				return false;
+			}
+
+			// read the values
+			const char *file_buffer = line + 6;
+			int frame_len = 0;
+			uint8_t frame[300];
+			int offset;
+			int number;
+
+			while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) {
+				frame[frame_len++] = number;
+				file_buffer += offset;
+			}
+
+			// Pipe the data into the parser
+			hrt_abstime now = hrt_absolute_time();
+
+			bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels);
+
+			if (result) {
+				has_decoded_values = true;
+			}
+
+		} else if (strncmp(line, "DECODED ", 8) == 0) {
+
+			if (!has_decoded_values) {
+				PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter);
+				return false;
+			}
+
+			// read the values
+			const char *file_buffer = line + 8;
+			int offset;
+			int expected_rc_value;
+			int expected_num_channels = 0;
+
+			while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) {
+
+				// allow a small difference
+				if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) {
+					PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels);
+					ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]);
+				}
+
+				file_buffer += offset;
+				++expected_num_channels;
+			}
+
+			if (expected_num_channels != num_values) {
+				PX4_ERR("File line: %d", line_counter);
+				ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values);
+			}
+
+			has_decoded_values = false;
+		}
+
+		++line_counter;
+	}
+
+	return true;
+}
+
+ut_declare_test_c(rc_ghst_test_main, RcGhstTest)

src/drivers/rc/ppm_in/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -31,15 +31,14 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__rc_input
-	MAIN rc_input
+	MODULE drivers__rc__ppm_in
+	MAIN ppm_in
 	COMPILE_FLAGS
 	SRCS
 		RCInput.cpp
-		crsf_telemetry.cpp
-		ghst_telemetry.cpp
+		RCInput.hpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
-		rc
+		px4_work_queue
 	)

src/drivers/rc/ppm_in/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_PPM_IN
+	bool "ppm_in"
+	default n
+	---help---
+		Enable support for ppm_in

src/drivers/rc/ppm_in/RCInput.cpp

@@ -0,0 +1,316 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+using namespace time_literals;
+
+RcPpm::RcPpm() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+}
+
+RcPpm::~RcPpm()
+{
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcPpm::init()
+{
+#ifdef GPIO_PPM_IN
+	// disable CPPM input by mapping it away from the timer capture input
+	px4_arch_unconfiggpio(GPIO_PPM_IN);
+#endif // GPIO_PPM_IN
+
+	return 0;
+}
+
+int RcPpm::task_spawn(int argc, char *argv[])
+{
+	RcPpm *instance = new RcPpm();
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+
+	instance->ScheduleOnInterval(_current_update_interval);
+
+	return PX4_OK;
+}
+
+void RcPpm::fill_rc_in(uint16_t raw_rc_count_local,
+		       uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+		       hrt_abstime now, bool frame_drop, bool failsafe,
+		       unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	/* fake rssi if no value was provided */
+	if (rssi == -1) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
+			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
+			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
+
+			// get RSSI from input channel
+			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+
+		} else if (_analog_rc_rssi_stable) {
+			// set RSSI if analog RSSI input is present
+			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
+
+			if (rssi_analog > 100.0f) {
+				rssi_analog = 100.0f;
+			}
+
+			if (rssi_analog < 0.0f) {
+				rssi_analog = 0.0f;
+			}
+
+			_rc_in.rssi = rssi_analog;
+
+		} else {
+			_rc_in.rssi = 255;
+		}
+
+	} else {
+		_rc_in.rssi = rssi;
+	}
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void RcPpm::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+		if (init() == PX4_OK) {
+			_initialized = true;
+
+		} else {
+			PX4_ERR("init failed");
+			exit_and_cleanup();
+		}
+
+	} else {
+
+		perf_begin(_cycle_perf);
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+#if defined(ADC_RC_RSSI_CHANNEL)
+
+		// update ADC sampling
+		if (_adc_report_sub.updated()) {
+			adc_report_s adc;
+
+			if (_adc_report_sub.copy(&adc)) {
+				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
+					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
+						float adc_volt = adc.raw_data[i] *
+								 adc.v_ref /
+								 adc.resolution;
+
+						if (_analog_rc_rssi_volt < 0.0f) {
+							_analog_rc_rssi_volt = adc_volt;
+						}
+
+						_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
+
+						/* only allow this to be used if we see a high RSSI once */
+						if (_analog_rc_rssi_volt > 2.5f) {
+							_analog_rc_rssi_stable = true;
+						}
+					}
+				}
+			}
+		}
+
+#endif // ADC_RC_RSSI_CHANNEL
+
+		bool rc_updated = false;
+
+		// This block scans for a supported serial RC input and locks onto the first one found
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
+
+		const bool rc_scan_locked = _rc_scan_locked;
+
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = cycle_timestamp;
+			// Configure timer input pin for CPPM
+			px4_arch_configgpio(GPIO_PPM_IN);
+
+		} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+			// see if we have new PPM input data
+			if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
+				// we have a new PPM frame. Publish it.
+				rc_updated = true;
+				_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+				fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
+				_rc_scan_locked = true;
+				_rc_in.rc_ppm_frame_length = ppm_frame_length;
+				_rc_in.timestamp_last_signal = ppm_last_valid_decode;
+			}
+
+		} else {
+			// disable CPPM input by mapping it away from the timer capture input
+			px4_arch_unconfiggpio(GPIO_PPM_IN);
+		}
+
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+			_rc_scan_locked = false;
+		}
+
+		if (!rc_scan_locked && _rc_scan_locked) {
+			PX4_INFO("RC input locked");
+		}
+	}
+}
+
+int RcPpm::custom_command(int argc, char *argv[])
+{
+	/* start the FMU if not running */
+	if (!is_running()) {
+		int ret = RcPpm::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcPpm::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+
+#if ADC_RC_RSSI_CHANNEL
+
+	if (_analog_rc_rssi_stable) {
+		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
+	}
+
+#endif
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
+	return 0;
+}
+
+int
+RcPpm::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- PPM
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ppm_in", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int ppm_in_main(int argc, char *argv[])
+{
+	return RcPpm::main(argc, argv);
+}

src/drivers/rc/ppm_in/RCInput.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 <float.h>
+
+#include <board_config.h>
+#include <drivers/drv_adc.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/adc_report.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <lib/systemlib/ppm_decode.h>
+
+using namespace time_literals;
+
+class RcPpm : public ModuleBase<RcPpm>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	RcPpm();
+	virtual ~RcPpm();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int init();
+
+private:
+	void Run() override;
+
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+	void rc_io_invert(bool invert);
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _initialized{false};
+	bool _rc_scan_locked{false};
+
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _adc_report_sub{ORB_ID(adc_report)};
+
+	input_rc_s	_rc_in{};
+
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+
+	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
+		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
+		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max
+	)
+};

src/drivers/rc/ppm_in/module.yaml

@@ -0,0 +1,12 @@
+module_name: RC PPM input driver
+
+parameters:
+    - group: RC
+      definitions:
+        PPM_IN_EN:
+            description:
+                short: RC PPM in enable
+                long: |
+                    RC PPM in enable.
+            type: boolean
+            default: false

src/drivers/rc/sbus/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__rc__sbus
+	MAIN sbus
+	COMPILE_FLAGS
+	SRCS
+		RCInput.cpp
+		RCInput.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc_sbus
+		px4_work_queue
+	)

src/drivers/rc/sbus/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_SBUS
+	bool "sbus"
+	default n
+	---help---
+		Enable support for sbus

src/drivers/rc/sbus/RCInput.cpp

@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+#include <termios.h>
+
+using namespace time_literals;
+
+RcSbus::RcSbus(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+RcSbus::~RcSbus()
+{
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcSbus::init()
+{
+	_rcs_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+
+	// assume SBUS input and immediately switch it to
+	// so that if Single wire mode on TX there will be only
+	// a short contention
+	sbus_config(_rcs_fd, board_rc_singlewire(_device));
+
+	if (_rcs_fd < 0) {
+		return -errno;
+	}
+
+	if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+	}
+
+#ifdef GPIO_PPM_IN
+	// disable CPPM input by mapping it away from the timer capture input
+	px4_arch_unconfiggpio(GPIO_PPM_IN);
+#endif // GPIO_PPM_IN
+
+	rc_io_invert(false);
+
+	return 0;
+}
+
+int RcSbus::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+#if defined(RC_SERIAL_PORT)
+	device_name = RC_SERIAL_PORT;
+#endif // RC_SERIAL_PORT
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RcSbus *instance = new RcSbus(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void RcSbus::fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	/* fake rssi if no value was provided */
+	if (rssi == -1) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
+			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
+			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
+
+			// get RSSI from input channel
+			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+
+		} else if (_analog_rc_rssi_stable) {
+			// set RSSI if analog RSSI input is present
+			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
+
+			if (rssi_analog > 100.0f) {
+				rssi_analog = 100.0f;
+			}
+
+			if (rssi_analog < 0.0f) {
+				rssi_analog = 0.0f;
+			}
+
+			_rc_in.rssi = rssi_analog;
+
+		} else {
+			_rc_in.rssi = 255;
+		}
+
+	} else {
+		_rc_in.rssi = rssi;
+	}
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void RcSbus::rc_io_invert(bool invert)
+{
+	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
+	// and if not use an IOCTL
+	if (!board_rc_invert_input(_device, invert)) {
+#if defined(TIOCSINVERT)
+
+		if (invert) {
+			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
+
+		} else {
+			ioctl(_rcs_fd, TIOCSINVERT, 0);
+		}
+
+#endif // TIOCSINVERT
+	}
+}
+
+void RcSbus::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+		if (init() == PX4_OK) {
+			_initialized = true;
+
+		} else {
+			PX4_ERR("init failed");
+			exit_and_cleanup();
+		}
+
+	} else {
+
+		perf_begin(_cycle_perf);
+
+		// Check if parameters have changed
+		if (_parameter_update_sub.updated()) {
+			// clear update
+			parameter_update_s param_update;
+			_parameter_update_sub.copy(&param_update);
+
+			updateParams();
+		}
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+		bool rc_updated = false;
+
+		// This block scans for a supported serial RC input and locks onto the first one found
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
+
+		unsigned frame_drops = 0;
+
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
+
+		// read all available data from the serial RC input UART
+		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+		if (newBytes > 0) {
+			_bytes_rx += newBytes;
+		}
+
+		const bool rc_scan_locked = _rc_scan_locked;
+
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = cycle_timestamp;
+			// Configure serial port for SBUS
+			sbus_config(_rcs_fd, board_rc_singlewire(_device));
+			rc_io_invert(true);
+
+			// flush serial buffer and any existing buffered data
+			tcflush(_rcs_fd, TCIOFLUSH);
+			memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+		} else if (_rc_scan_locked
+			   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+			// parse new data
+			if (newBytes > 0) {
+				bool sbus_failsafe = false;
+				bool sbus_frame_drop = false;
+
+				rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
+							&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+				if (rc_updated) {
+					// we have a new SBUS frame. Publish it.
+					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+					fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+						   sbus_frame_drop, sbus_failsafe, frame_drops);
+					_rc_scan_locked = true;
+				}
+			}
+
+		} else {
+			// Scan the next protocol
+			rc_io_invert(false);
+		}
+
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+			_rc_scan_locked = false;
+		}
+
+		if (!rc_scan_locked && _rc_scan_locked) {
+			PX4_INFO("RC input locked");
+		}
+
+	}
+}
+
+int RcSbus::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		int ret = RcSbus::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcSbus::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+	PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
+
+#if ADC_RC_RSSI_CHANNEL
+
+	if (_analog_rc_rssi_stable) {
+		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
+	}
+
+#endif
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
+	return 0;
+}
+
+int RcSbus::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- SBUS
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rc_sbus", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", false);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int sbus_main(int argc, char *argv[])
+{
+	return RcSbus::main(argc, argv);
+}

src/drivers/rc/sbus/RCInput.hpp

@@ -39,12 +39,7 @@
 #include <drivers/drv_adc.h>
 #include <drivers/drv_hrt.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/rc/crsf.h>
-#include <lib/rc/ghst.hpp>
-#include <lib/rc/dsm.h>
-#include <lib/rc/sbus.h>
-#include <lib/rc/st24.h>
-#include <lib/rc/sumd.h>
+#include <lib/rc/sbus/sbus.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
@@ -57,22 +52,15 @@
 #include <uORB/topics/adc_report.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
 
-#include "crsf_telemetry.h"
-#include "ghst_telemetry.hpp"
+using namespace time_literals;
 
-#ifdef HRT_PPM_CHANNEL
-# include <systemlib/ppm_decode.h>
-#endif
-
-class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::ScheduledWorkItem
+class RcSbus : public ModuleBase<RcSbus>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-
-	RCInput(const char *device);
-	virtual ~RCInput();
+	RcSbus(const char *device);
+	virtual ~RcSbus();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -86,45 +74,16 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
-	int	init();
+	int init();
 
 private:
-
-	enum RC_SCAN {
-		RC_SCAN_NONE = 0,
-		RC_SCAN_PPM  = 1,
-		RC_SCAN_SBUS = 2,
-		RC_SCAN_DSM  = 3,
-		RC_SCAN_ST24 = 5,
-		RC_SCAN_SUMD = 4,
-		RC_SCAN_CRSF = 6,
-		RC_SCAN_GHST = 7,
-	} _rc_scan_state{RC_SCAN_SBUS};
-
-	static constexpr char const *RC_SCAN_STRING[] {
-		"None",
-		"PPM",
-		"SBUS",
-		"DSM",
-		"ST24",
-		"SUMD",
-		"CRSF",
-		"GHST"
-	};
-
 	void Run() override;
 
-#if defined(SPEKTRUM_POWER)
-	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
-#endif // SPEKTRUM_POWER
-
 	void fill_rc_in(uint16_t raw_rc_count_local,
 			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
 			hrt_abstime now, bool frame_drop, bool failsafe,
 			unsigned frame_drops, int rssi);
 
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
-
 	void rc_io_invert(bool invert);
 
 	hrt_abstime _rc_scan_begin{0};
@@ -136,40 +95,29 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
-	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
-
 	input_rc_s	_rc_in{};
 
-	float		_analog_rc_rssi_volt{-1.0f};
-	bool		_analog_rc_rssi_stable{false};
-
-	bool _armed{false};
-
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
 
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
 
-	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
+	static constexpr size_t RC_MAX_BUFFER_SIZE{sizeof(sbus_frame_t)};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
 
 	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
 	uint16_t _raw_rc_count{};
 
-	CRSFTelemetry *_crsf_telemetry{nullptr};
-	GHSTTelemetry *_ghst_telemetry{nullptr};
-
-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
 	uint32_t	_bytes_rx{0};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
-		(ParamInt<px4::params::RC_INPUT_PROTO>) _param_rc_input_proto
+		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max
 	)
 };

src/drivers/rc/sbus/module.yaml

@@ -0,0 +1,7 @@
+module_name: RC SBUS input driver
+serial_config:
+    - command: "sbus start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_SBUS_PRT_CFG
+        group: Serial
+        #default: RC

src/drivers/rc/spektrum_dsm/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__rc__spektrum_dsm
+	MAIN spektrum_dsm
+	COMPILE_FLAGS
+	SRCS
+		RCInput.cpp
+		RCInput.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc_dsm
+		px4_work_queue
+	)

src/drivers/rc/spektrum_dsm/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_SPEKTRUM_DSM
+	bool "spektrum_dsm"
+	default n
+	---help---
+		Enable support for spektrum_dsm

src/drivers/rc/spektrum_dsm/RCInput.cpp

@@ -0,0 +1,456 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+#include <uORB/topics/vehicle_command_ack.h>
+
+#include <termios.h>
+
+using namespace time_literals;
+
+RcDsm::RcDsm(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+RcDsm::~RcDsm()
+{
+#if defined(RC_SERIAL_PORT) && defined(SPEKTRUM_POWER_PASSIVE)
+
+	if ((strcmp(RC_SERIAL_PORT, _device) == 0)) {
+		// Disable power controls for Spektrum receiver
+		SPEKTRUM_POWER_PASSIVE();
+	}
+
+#endif // RC_SERIAL_PORT && SPEKTRUM_POWER_PASSIVE
+
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcDsm::init()
+{
+
+
+	return 0;
+}
+
+int RcDsm::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RcDsm *instance = new RcDsm(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void RcDsm::Run()
+{
+	if (should_exit()) {
+
+		if (_rcs_fd >= 0) {
+			close(_rcs_fd);
+			_rcs_fd = -1;
+		}
+
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
+
+	const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+
+	/* vehicle command */
+	vehicle_command_s vcmd;
+
+	if (_vehicle_cmd_sub.update(&vcmd)) {
+		// Check for a pairing command
+		if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
+
+			uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+#if defined(RC_SERIAL_PORT)
+
+			if ((strcmp(RC_SERIAL_PORT, _device) == 0)) {
+#if defined(SPEKTRUM_POWER)
+
+				if (!_rc_scan_locked && !_armed) {
+					if ((int)vcmd.param1 == 0) {
+						// DSM binding command
+						int dsm_bind_mode = (int)vcmd.param2;
+
+						int dsm_bind_pulses = 0;
+
+						if (dsm_bind_mode == 0) {
+							dsm_bind_pulses = DSM2_BIND_PULSES;
+
+						} else if (dsm_bind_mode == 1) {
+							dsm_bind_pulses = DSMX_BIND_PULSES;
+
+						} else {
+							dsm_bind_pulses = DSMX8_BIND_PULSES;
+						}
+
+						// specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary
+						if (dsm_bind_pulses == DSM2_BIND_PULSES ||
+						    dsm_bind_pulses == DSMX_BIND_PULSES ||
+						    dsm_bind_pulses == DSMX8_BIND_PULSES) {
+
+							/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
+							PX4_INFO("DSM_BIND_START: DSM%s RX", (dsm_bind_pulses == 0) ? "2" : ((dsm_bind_pulses == 1) ? "-X" : "-X8"));
+
+							dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
+
+							dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
+							usleep(500000);
+
+							dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
+							usleep(72000);
+
+							irqstate_t flags = px4_enter_critical_section();
+							dsm_bind(DSM_CMD_BIND_SEND_PULSES, dsm_bind_pulses);
+							px4_leave_critical_section(flags);
+
+							usleep(50000);
+
+							dsm_bind(DSM_CMD_BIND_REINIT_UART, 0);
+
+							cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+						} else {
+							cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+							PX4_ERR("DSM bind failed");
+						}
+					}
+
+				} else {
+					cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+#endif // SPEKTRUM_POWER
+			}
+
+#endif // RC_SERIAL_PORT
+
+			// publish acknowledgement
+			vehicle_command_ack_s command_ack{};
+			command_ack.command = vcmd.command;
+			command_ack.result = cmd_ret;
+			command_ack.target_system = vcmd.source_system;
+			command_ack.target_component = vcmd.source_component;
+			command_ack.timestamp = hrt_absolute_time();
+			uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+			vehicle_command_ack_pub.publish(command_ack);
+		}
+	}
+
+	if (!_rc_scan_locked && (_rc_scan_begin == 0)) {
+		_rc_scan_begin = cycle_timestamp;
+
+		// Configure serial port for DSM
+#if defined(RC_SERIAL_PORT)
+
+		if ((strcmp(RC_SERIAL_PORT, _device) == 0)) {
+# if defined(SPEKTRUM_POWER_CONFIG)
+			// Enable power controls for Spektrum receiver
+			SPEKTRUM_POWER_CONFIG();
+# endif // SPEKTRUM_POWER_CONFIG
+# if defined(SPEKTRUM_POWER)
+			// enable power on DSM connector
+			SPEKTRUM_POWER(true);
+# endif // SPEKTRUM_POWER
+		}
+
+#endif // RC_SERIAL_PORT
+
+		if (_rcs_fd >= 0) {
+			close(_rcs_fd);
+			_rcs_fd = -1;
+		}
+
+		if (_rcs_fd < 0) {
+			// dsm_init sets some file static variables and returns a file descriptor
+			// it also powers on the radio if needed
+			_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+
+			if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+				ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+			}
+
+			// First check if the board provides a board-specific inversion method (e.g. via GPIO),
+			// and if not use an IOCTL
+			if (!board_rc_invert_input(_device, false)) {
+#if defined(TIOCSINVERT)
+				ioctl(_rcs_fd, TIOCSINVERT, 0);
+#endif // TIOCSINVERT
+			}
+		}
+
+		if (_rcs_fd >= 0) {
+			// 115200bps, no parity, one stop bit
+			struct termios t;
+			tcgetattr(_rcs_fd, &t);
+			cfsetspeed(&t, 115200);
+			t.c_cflag &= ~(CSTOPB | PARENB);
+			tcsetattr(_rcs_fd, TCSANOW, &t);
+
+			// initialise the decoder
+			dsm_proto_init();
+		}
+	}
+
+	if (_rcs_fd >= 0) {
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
+		const hrt_abstime timestamp_last_signal = hrt_absolute_time();
+
+		static constexpr size_t RC_MAX_BUFFER_SIZE{sizeof(dsm_decode_t)};
+		uint8_t rcs_buf[RC_MAX_BUFFER_SIZE] {};
+		int new_bytes = ::read(_rcs_fd, &rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+		if (new_bytes > 0) {
+			_bytes_rx += new_bytes;
+		}
+
+		if (new_bytes > 0) {
+			uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+
+			for (auto &rc_channel : raw_rc_values) {
+				rc_channel = UINT16_MAX;
+			}
+
+			uint16_t raw_rc_count = 0;
+			bool dsm_11_bit = false;
+			unsigned frame_drops = 0;
+			int8_t dsm_rssi = -1;
+
+			// parse new data
+			const bool rc_updated = dsm_parse(cycle_timestamp, &rcs_buf[0], new_bytes, &raw_rc_values[0], &raw_rc_count,
+							  &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+			if (rc_updated) {
+
+				if (!_rc_scan_locked) {
+					_rc_scan_locked = true;
+					PX4_INFO("RC input locked");
+				}
+
+				input_rc_s input_rc{};
+				input_rc.timestamp_last_signal = timestamp_last_signal;
+				input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+				unsigned valid_chans = 0;
+
+				for (unsigned i = 0; i < math::min(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); i++) {
+					input_rc.values[i] = raw_rc_values[i];
+
+					if (raw_rc_values[i] != UINT16_MAX) {
+						valid_chans++;
+					}
+				}
+
+				input_rc.channel_count = valid_chans;
+				input_rc.rssi = -1;
+				input_rc.link_quality = -1;
+				input_rc.rssi_dbm = NAN;
+
+				if ((valid_chans == 0) && (dsm_rssi >= 0) && (dsm_rssi <= 100)) {
+					input_rc.rssi = dsm_rssi;
+				}
+
+				input_rc.rc_lost = (valid_chans == 0);
+				input_rc.rc_lost_frame_count = frame_drops;
+				input_rc.rc_total_frame_count = 0; // TODO:
+				input_rc.timestamp = hrt_absolute_time();
+				_input_rc_pub.publish(input_rc);
+
+				perf_count_interval(_publish_interval_perf, timestamp_last_signal);
+				_last_valid_rc = timestamp_last_signal;
+			}
+		}
+	}
+
+	perf_end(_cycle_perf);
+
+	if (!_armed && (hrt_elapsed_time(&_last_valid_rc) > 5_s)) {
+		_rc_scan_locked = false;
+		_rc_scan_begin = 0;
+	}
+}
+
+int RcDsm::custom_command(int argc, char *argv[])
+{
+	const char *verb = argv[0];
+
+	if (!strcmp(verb, "bind")) {
+		if (is_running()) {
+			uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+			vehicle_command_s vcmd{};
+			vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
+			vcmd.timestamp = hrt_absolute_time();
+			vehicle_command_pub.publish(vcmd);
+			return 0;
+
+		} else {
+			return -1;
+		}
+	}
+
+	/* start the FMU if not running */
+	if (!is_running()) {
+		int ret = RcDsm::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcDsm::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	return 0;
+}
+
+int RcDsm::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- DSM
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rc_dsm", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", false);
+
+#if defined(SPEKTRUM_POWER)
+	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
+#endif /* SPEKTRUM_POWER */
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int spektrum_dsm_main(int argc, char *argv[])
+{
+	return RcDsm::main(argc, argv);
+}

src/drivers/rc/spektrum_dsm/RCInput.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/dsm/dsm.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
+
+class RcDsm : public ModuleBase<RcDsm>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	RcDsm(const char *device);
+	virtual ~RcDsm();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int init();
+
+private:
+	void Run() override;
+
+	static constexpr unsigned _current_update_interval{4000}; // 250 Hz
+
+	uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	int _rcs_fd{-1};
+	char _device[20] {};    ///< device / serial port path
+	uint32_t _bytes_rx{0};
+
+	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _last_valid_rc{0};
+	bool _armed{false};
+	bool _rc_scan_locked{false};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+
+};

src/drivers/rc/spektrum_dsm/module.yaml

@@ -0,0 +1,7 @@
+module_name: RC Spektrum DSM input driver
+serial_config:
+    - command: "spektrum_dsm start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_DSM_PRT_CFG
+        group: Serial
+        #default: RC

src/drivers/rc/st24_rc/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__rc__rc_st24
+	MAIN rc_st24
+	COMPILE_FLAGS
+	SRCS
+		RCInput.cpp
+		RCInput.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc_st24
+		px4_work_queue
+	)

src/drivers/rc/st24_rc/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_ST24_RC
+	bool "rc_st24"
+	default n
+	---help---
+		Enable support for rc_st24

src/drivers/rc/st24_rc/RCInput.cpp

@@ -0,0 +1,378 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+#include <termios.h>
+
+using namespace time_literals;
+
+RcSt24::RcSt24(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+RcSt24::~RcSt24()
+{
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcSt24::init()
+{
+	_rcs_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+
+	if (_rcs_fd >= 0) {
+		// 115200bps, no parity, one stop bit
+		struct termios t {};
+		tcgetattr(_rcs_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(_rcs_fd, TCSANOW, &t);
+	}
+
+	if (_rcs_fd < 0) {
+		return -errno;
+	}
+
+	if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+	}
+
+	rc_io_invert(false);
+
+	return 0;
+}
+
+int RcSt24::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+#if defined(RC_SERIAL_PORT)
+	device_name = RC_SERIAL_PORT;
+#endif // RC_SERIAL_PORT
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RcSt24 *instance = new RcSt24(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void RcSt24::fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void RcSt24::rc_io_invert(bool invert)
+{
+	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
+	// and if not use an IOCTL
+	if (!board_rc_invert_input(_device, invert)) {
+#if defined(TIOCSINVERT)
+
+		if (invert) {
+			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
+
+		} else {
+			ioctl(_rcs_fd, TIOCSINVERT, 0);
+		}
+
+#endif // TIOCSINVERT
+	}
+}
+
+void RcSt24::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+		if (init() == PX4_OK) {
+			_initialized = true;
+
+		} else {
+			PX4_ERR("init failed");
+			exit_and_cleanup();
+		}
+
+	} else {
+
+		perf_begin(_cycle_perf);
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+		bool rc_updated = false;
+
+		// This block scans for a supported serial RC input and locks onto the first one found
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
+
+		unsigned frame_drops = 0;
+
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
+
+		// read all available data from the serial RC input UART
+		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+		if (newBytes > 0) {
+			_bytes_rx += newBytes;
+		}
+
+		const bool rc_scan_locked = _rc_scan_locked;
+
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = cycle_timestamp;
+
+			// Configure serial port for DSM
+			if (_rcs_fd >= 0) {
+				// 115200bps, no parity, one stop bit
+				struct termios t {};
+				tcgetattr(_rcs_fd, &t);
+				cfsetspeed(&t, 115200);
+				t.c_cflag &= ~(CSTOPB | PARENB);
+				tcsetattr(_rcs_fd, TCSANOW, &t);
+			}
+
+			// flush serial buffer and any existing buffered data
+			tcflush(_rcs_fd, TCIOFLUSH);
+			memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+		} else if (_rc_scan_locked
+			   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+			if (newBytes > 0) {
+				// parse new data
+				uint8_t st24_rssi, lost_count;
+
+				rc_updated = false;
+
+				for (unsigned i = 0; i < (unsigned)newBytes; i++) {
+					/* set updated flag if one complete packet was parsed */
+					st24_rssi = input_rc_s::RSSI_MAX;
+					rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
+									&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+				}
+
+				// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
+				// The only way to detect RC loss is therefore to look at the lost_count.
+				if (rc_updated) {
+					if (lost_count == 0) {
+						// we have a new ST24 frame. Publish it.
+						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
+						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+							   false, false, frame_drops, st24_rssi);
+						_rc_scan_locked = true;
+
+					} else {
+						// if the lost count > 0 means that there is an RC loss
+						_rc_in.rc_lost = true;
+					}
+				}
+			}
+		}
+
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+			_rc_scan_locked = false;
+		}
+
+		if (!rc_scan_locked && _rc_scan_locked) {
+			PX4_INFO("RC input locked");
+		}
+	}
+}
+
+int RcSt24::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		int ret = RcSt24::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcSt24::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
+	return 0;
+}
+
+int RcSt24::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- ST24
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rc_st24", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", false);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int rc_st24_main(int argc, char *argv[])
+{
+	return RcSt24::main(argc, argv);
+}

src/drivers/rc/st24_rc/RCInput.hpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 <float.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/st24.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/input_rc.h>
+
+class RcSt24 : public ModuleBase<RcSt24>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	RcSt24(const char *device);
+	virtual ~RcSt24();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int init();
+
+private:
+	void Run() override;
+
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+	void rc_io_invert(bool invert);
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _initialized{false};
+	bool _rc_scan_locked{false};
+
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+
+	input_rc_s	_rc_in{};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+
+	int		_rcs_fd{-1};
+	char		_device[20] {};					///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{sizeof(ReceiverFcPacket)};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+
+	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+	uint32_t	_bytes_rx{0};
+};

src/drivers/rc/st24_rc/module.yaml

@@ -0,0 +1,7 @@
+module_name: RC ST24 input driver
+serial_config:
+    - command: "rc_st24 start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_ST24_PRT_CFG
+        group: Serial
+        #default: RC

src/drivers/rc/sumd_rc/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__rc__sumd
+	MAIN rc_sumd
+	COMPILE_FLAGS
+	SRCS
+		RCInput.cpp
+		RCInput.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc_sumd
+		px4_work_queue
+	)

src/drivers/rc/sumd_rc/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_SUMD_RC
+	bool "rc_sumd"
+	default n
+	---help---
+		Enable support for rc_sumd

src/drivers/rc/sumd_rc/RCInput.cpp

@@ -0,0 +1,371 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+#include "RCInput.hpp"
+
+#include <termios.h>
+
+using namespace time_literals;
+
+RcSumd::RcSumd(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+RcSumd::~RcSumd()
+{
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int RcSumd::init()
+{
+	_rcs_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+
+	if (_rcs_fd >= 0) {
+		// 115200bps, no parity, one stop bit
+		struct termios t {};
+		tcgetattr(_rcs_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(_rcs_fd, TCSANOW, &t);
+	}
+
+	if (_rcs_fd < 0) {
+		return -errno;
+	}
+
+	if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+	}
+
+	rc_io_invert(false);
+
+	return 0;
+}
+
+int RcSumd::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+#if defined(RC_SERIAL_PORT)
+	device_name = RC_SERIAL_PORT;
+#endif // RC_SERIAL_PORT
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RcSumd *instance = new RcSumd(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void RcSumd::fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void RcSumd::rc_io_invert(bool invert)
+{
+	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
+	// and if not use an IOCTL
+	if (!board_rc_invert_input(_device, invert)) {
+#if defined(TIOCSINVERT)
+
+		if (invert) {
+			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
+
+		} else {
+			ioctl(_rcs_fd, TIOCSINVERT, 0);
+		}
+
+#endif // TIOCSINVERT
+	}
+}
+
+void RcSumd::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+		if (init() == PX4_OK) {
+			_initialized = true;
+
+		} else {
+			PX4_ERR("init failed");
+			exit_and_cleanup();
+		}
+
+	} else {
+
+		perf_begin(_cycle_perf);
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+		bool rc_updated = false;
+
+		// This block scans for a supported serial RC input and locks onto the first one found
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
+
+		unsigned frame_drops = 0;
+
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
+
+		// read all available data from the serial RC input UART
+		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+		if (newBytes > 0) {
+			_bytes_rx += newBytes;
+		}
+
+		const bool rc_scan_locked = _rc_scan_locked;
+
+		if (_rc_scan_begin == 0) {
+			_rc_scan_begin = cycle_timestamp;
+
+			// Configure serial port for DSM
+			if (_rcs_fd >= 0) {
+				// 115200bps, no parity, one stop bit
+				struct termios t {};
+				tcgetattr(_rcs_fd, &t);
+				cfsetspeed(&t, 115200);
+				t.c_cflag &= ~(CSTOPB | PARENB);
+				tcsetattr(_rcs_fd, TCSANOW, &t);
+			}
+
+			// flush serial buffer and any existing buffered data
+			tcflush(_rcs_fd, TCIOFLUSH);
+			memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+		} else if (_rc_scan_locked
+			   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+			if (newBytes > 0) {
+				// parse new data
+				uint8_t sumd_rssi, rx_count;
+				bool sumd_failsafe;
+
+				rc_updated = false;
+
+				for (unsigned i = 0; i < (unsigned)newBytes; i++) {
+					/* set updated flag if one complete packet was parsed */
+					sumd_rssi = input_rc_s::RSSI_MAX;
+					rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
+									&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
+				}
+
+				if (rc_updated) {
+					// we have a new SUMD frame. Publish it.
+					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+					fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+						   false, sumd_failsafe, frame_drops, sumd_rssi);
+					_rc_scan_locked = true;
+				}
+			}
+		}
+
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+			_rc_scan_locked = false;
+		}
+
+		if (!rc_scan_locked && _rc_scan_locked) {
+			PX4_INFO("RC input locked");
+		}
+	}
+}
+
+int RcSumd::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		int ret = RcSumd::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int RcSumd::print_status()
+{
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal");
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
+	return 0;
+}
+
+int RcSumd::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the RC input parsing and auto-selecting the method. Supported methods are:
+- SUMD
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rc_sumd", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", false);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int rc_sumd_main(int argc, char *argv[])
+{
+	return RcSumd::main(argc, argv);
+}

src/drivers/rc/sumd_rc/RCInput.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 <float.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/sumd.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/adc_report.h>
+#include <uORB/topics/input_rc.h>
+
+class RcSumd : public ModuleBase<RcSumd>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	RcSumd(const char *device);
+	virtual ~RcSumd();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int init();
+
+private:
+	void Run() override;
+
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+	void rc_io_invert(bool invert);
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _initialized{false};
+	bool _rc_scan_locked{false};
+
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+
+	input_rc_s	_rc_in{};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+
+	int		_rcs_fd{-1};
+	char		_device[20] {};					///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{sizeof(ReceiverFcPacketHoTT)};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+
+	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+	uint32_t	_bytes_rx{0};
+};

src/drivers/rc/sumd_rc/module.yaml

@@ -0,0 +1,7 @@
+module_name: RC SUMD input driver
+serial_config:
+    - command: "rc_sumd start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_SUMD_PRT_CFG
+        group: Serial
+        #default: RC

src/drivers/rc_input/Kconfig

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_RC_INPUT
-	bool "rc_input"
-	default n
-	---help---
-		Enable support for rc_input

src/drivers/rc_input/RCInput.cpp

@@ -1,953 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-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.
- *
- ****************************************************************************/
-
-#include "RCInput.hpp"
-
-#include "crsf_telemetry.h"
-#include <uORB/topics/vehicle_command_ack.h>
-
-#include <termios.h>
-
-using namespace time_literals;
-
-constexpr char const *RCInput::RC_SCAN_STRING[];
-
-RCInput::RCInput(const char *device) :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
-{
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	if (device) {
-		strncpy(_device, device, sizeof(_device) - 1);
-		_device[sizeof(_device) - 1] = '\0';
-	}
-
-	if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
-		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
-	}
-}
-
-RCInput::~RCInput()
-{
-#if defined(SPEKTRUM_POWER_PASSIVE)
-	// Disable power controls for Spektrum receiver
-	SPEKTRUM_POWER_PASSIVE();
-#endif
-	dsm_deinit();
-
-	delete _crsf_telemetry;
-	delete _ghst_telemetry;
-
-	perf_free(_cycle_perf);
-	perf_free(_publish_interval_perf);
-}
-
-int
-RCInput::init()
-{
-#ifdef RF_RADIO_POWER_CONTROL
-	// power radio on
-	RF_RADIO_POWER_CONTROL(true);
-#endif // RF_RADIO_POWER_CONTROL
-
-	// dsm_init sets some file static variables and returns a file descriptor
-	// it also powers on the radio if needed
-	_rcs_fd = dsm_init(_device);
-
-	if (_rcs_fd < 0) {
-		return -errno;
-	}
-
-	if (board_rc_swap_rxtx(_device)) {
-#if defined(TIOCSSWAP)
-		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
-#endif // TIOCSSWAP
-	}
-
-	// assume SBUS input and immediately switch it to
-	// so that if Single wire mode on TX there will be only
-	// a short contention
-	sbus_config(_rcs_fd, board_rc_singlewire(_device));
-
-#ifdef GPIO_PPM_IN
-	// disable CPPM input by mapping it away from the timer capture input
-	px4_arch_unconfiggpio(GPIO_PPM_IN);
-#endif // GPIO_PPM_IN
-
-	rc_io_invert(false);
-
-	return 0;
-}
-
-int
-RCInput::task_spawn(int argc, char *argv[])
-{
-	bool error_flag = false;
-
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-	const char *device_name = nullptr;
-#if defined(RC_SERIAL_PORT)
-	device_name = RC_SERIAL_PORT;
-#endif // RC_SERIAL_PORT
-
-#if defined(RC_SERIAL_PORT) && defined(PX4IO_SERIAL_DEVICE)
-
-	// if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running
-	if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) {
-		device_name = nullptr;
-	}
-
-#endif // RC_SERIAL_PORT && PX4IO_SERIAL_DEVICE
-
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'd':
-			device_name = myoptarg;
-			break;
-
-		case '?':
-			error_flag = true;
-			break;
-
-		default:
-			PX4_WARN("unrecognized flag");
-			error_flag = true;
-			break;
-		}
-	}
-
-	if (error_flag) {
-		return -1;
-	}
-
-	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
-		RCInput *instance = new RCInput(device_name);
-
-		if (instance == nullptr) {
-			PX4_ERR("alloc failed");
-			return PX4_ERROR;
-		}
-
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		instance->ScheduleOnInterval(_current_update_interval);
-
-		return PX4_OK;
-
-	} else {
-		if (device_name) {
-			PX4_ERR("invalid device (-d) %s", device_name);
-
-		} else {
-			PX4_INFO("valid device required");
-		}
-	}
-
-	return PX4_ERROR;
-}
-
-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
-{
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
-
-		if (raw_rc_values_local[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-
-		// once filled, reset values back to default
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
-			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
-			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
-			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
-
-			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
-
-		} else if (_analog_rc_rssi_stable) {
-			// set RSSI if analog RSSI input is present
-			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
-
-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
-		}
-
-	} else {
-		_rc_in.rssi = rssi;
-	}
-
-	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
-	}
-
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
-}
-
-void RCInput::set_rc_scan_state(RC_SCAN newState)
-{
-	if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE)
-	    && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
-
-		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
-
-	} else if (_param_rc_input_proto.get() < 0) {
-		// only auto change if RC_INPUT_PROTO set to auto (-1)
-		PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
-		_rc_scan_state = newState;
-
-	} else {
-		_rc_scan_state = RC_SCAN::RC_SCAN_NONE;
-	}
-
-	_rc_scan_begin = 0;
-	_rc_scan_locked = false;
-}
-
-void RCInput::rc_io_invert(bool invert)
-{
-	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
-	// and if not use an IOCTL
-	if (!board_rc_invert_input(_device, invert)) {
-#if defined(TIOCSINVERT)
-
-		if (invert) {
-			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
-
-		} else {
-			ioctl(_rcs_fd, TIOCSINVERT, 0);
-		}
-
-#endif // TIOCSINVERT
-	}
-}
-
-void RCInput::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
-
-	if (!_initialized) {
-		if (init() == PX4_OK) {
-			_initialized = true;
-
-		} else {
-			PX4_ERR("init failed");
-			exit_and_cleanup();
-		}
-
-	} else {
-
-		perf_begin(_cycle_perf);
-
-		// Check if parameters have changed
-		if (_parameter_update_sub.updated()) {
-			// clear update
-			parameter_update_s param_update;
-			_parameter_update_sub.copy(&param_update);
-
-			updateParams();
-		}
-
-		if (_vehicle_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
-
-			if (_vehicle_status_sub.copy(&vehicle_status)) {
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			}
-		}
-
-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
-
-
-		/* vehicle command */
-		vehicle_command_s vcmd;
-
-		if (_vehicle_cmd_sub.update(&vcmd)) {
-			// Check for a pairing command
-			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
-
-				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-#if defined(SPEKTRUM_POWER)
-
-				if (!_rc_scan_locked && !_armed) {
-					if ((int)vcmd.param1 == 0) {
-						// DSM binding command
-						int dsm_bind_mode = (int)vcmd.param2;
-
-						int dsm_bind_pulses = 0;
-
-						if (dsm_bind_mode == 0) {
-							dsm_bind_pulses = DSM2_BIND_PULSES;
-
-						} else if (dsm_bind_mode == 1) {
-							dsm_bind_pulses = DSMX_BIND_PULSES;
-
-						} else {
-							dsm_bind_pulses = DSMX8_BIND_PULSES;
-						}
-
-						bind_spektrum(dsm_bind_pulses);
-
-						cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-					}
-
-				} else {
-					cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
-
-#endif // SPEKTRUM_POWER
-
-				// publish acknowledgement
-				vehicle_command_ack_s command_ack{};
-				command_ack.command = vcmd.command;
-				command_ack.result = cmd_ret;
-				command_ack.target_system = vcmd.source_system;
-				command_ack.target_component = vcmd.source_component;
-				command_ack.timestamp = hrt_absolute_time();
-				uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
-				vehicle_command_ack_pub.publish(command_ack);
-			}
-		}
-
-
-#if defined(ADC_RC_RSSI_CHANNEL)
-
-		// update ADC sampling
-		if (_adc_report_sub.updated()) {
-			adc_report_s adc;
-
-			if (_adc_report_sub.copy(&adc)) {
-				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
-					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
-
-						if (_analog_rc_rssi_volt < 0.0f) {
-							_analog_rc_rssi_volt = adc_volt;
-						}
-
-						_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
-
-						/* only allow this to be used if we see a high RSSI once */
-						if (_analog_rc_rssi_volt > 2.5f) {
-							_analog_rc_rssi_stable = true;
-						}
-					}
-				}
-			}
-		}
-
-#endif // ADC_RC_RSSI_CHANNEL
-
-		bool rc_updated = false;
-
-		// This block scans for a supported serial RC input and locks onto the first one found
-		// Scan for 500 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 500_ms;
-
-		unsigned frame_drops = 0;
-
-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
-
-		// read all available data from the serial RC input UART
-		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
-
-		if (newBytes > 0) {
-			_bytes_rx += newBytes;
-		}
-
-		const bool rc_scan_locked = _rc_scan_locked;
-
-		switch (_rc_scan_state) {
-		case RC_SCAN_NONE:
-			// do nothing
-			break;
-
-		case RC_SCAN_SBUS:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for SBUS
-				sbus_config(_rcs_fd, board_rc_singlewire(_device));
-				rc_io_invert(true);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					bool sbus_failsafe = false;
-					bool sbus_frame_drop = false;
-
-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
-								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				rc_io_invert(false);
-				set_rc_scan_state(RC_SCAN_DSM);
-			}
-
-			break;
-
-		case RC_SCAN_DSM:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					bool dsm_11_bit = false;
-
-					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
-							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_ST24);
-			}
-
-			break;
-
-		case RC_SCAN_ST24:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
-
-					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
-						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
-					}
-
-					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
-					// The only way to detect RC loss is therefore to look at the lost_count.
-
-					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
-
-						} else {
-							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
-						}
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SUMD);
-			}
-
-			break;
-
-		case RC_SCAN_SUMD:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
-
-					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
-						sumd_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
-					}
-
-					if (rc_updated) {
-						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_PPM);
-			}
-
-			break;
-
-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
-		case RC_SCAN_CRSF:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for CRSF
-				crsf_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
-								input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
-
-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
-						}
-
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
-
-						_rc_scan_locked = true;
-
-						if (_crsf_telemetry) {
-							_crsf_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
-			}
-
-			break;
-
-		case RC_SCAN_GHST:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for GHST
-				ghst_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
-
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-						}
-
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
-
-						_rc_scan_locked = true;
-
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SBUS);
-			}
-
-			break;
-		}
-
-		perf_end(_cycle_perf);
-
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
-
-			_rc_in.link_quality = -1;
-			_rc_in.rssi_dbm = NAN;
-
-			_to_input_rc.publish(_rc_in);
-
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
-			_rc_scan_locked = false;
-		}
-
-		if (!rc_scan_locked && _rc_scan_locked) {
-			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
-		}
-
-		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
-		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
-		    && (_param_rc_input_proto.get() < 0)
-		   ) {
-			// RC_INPUT_PROTO auto => locked selection
-			_param_rc_input_proto.set(_rc_scan_state);
-			_param_rc_input_proto.commit();
-		}
-	}
-}
-
-#if defined(SPEKTRUM_POWER)
-bool RCInput::bind_spektrum(int arg) const
-{
-	int ret = PX4_ERROR;
-
-	/* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */
-
-	/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
-	PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8"));
-
-	if (arg == DSM2_BIND_PULSES ||
-	    arg == DSMX_BIND_PULSES ||
-	    arg == DSMX8_BIND_PULSES) {
-
-		dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
-
-		dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
-		usleep(500000);
-
-		dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
-		usleep(72000);
-
-		irqstate_t flags = px4_enter_critical_section();
-		dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg);
-		px4_leave_critical_section(flags);
-
-		usleep(50000);
-
-		dsm_bind(DSM_CMD_BIND_REINIT_UART, 0);
-
-		ret = OK;
-
-	} else {
-		PX4_ERR("DSM bind failed");
-		ret = -EINVAL;
-	}
-
-	return (ret == PX4_OK);
-}
-#endif /* SPEKTRUM_POWER */
-
-int RCInput::custom_command(int argc, char *argv[])
-{
-#if defined(SPEKTRUM_POWER)
-	const char *verb = argv[0];
-
-	if (!strcmp(verb, "bind")) {
-		uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
-		vehicle_command_s vcmd{};
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
-		vcmd.timestamp = hrt_absolute_time();
-		vehicle_command_pub.publish(vcmd);
-		return 0;
-	}
-
-#endif /* SPEKTRUM_POWER */
-
-	/* start the FMU if not running */
-	if (!is_running()) {
-		int ret = RCInput::task_spawn(argc, argv);
-
-		if (ret) {
-			return ret;
-		}
-	}
-
-	return print_usage("unknown command");
-}
-
-int RCInput::print_status()
-{
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-
-	if (_device[0] != '\0') {
-		PX4_INFO("UART device: %s", _device);
-		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
-	}
-
-	PX4_INFO("RC state: %s: %s", _rc_scan_locked ? "found" : "searching for signal", RC_SCAN_STRING[_rc_scan_state]);
-
-	if (_rc_scan_locked) {
-		switch (_rc_scan_state) {
-		case RC_SCAN_NONE:
-			break;
-
-		case RC_SCAN_CRSF:
-			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
-			break;
-
-		case RC_SCAN_GHST:
-			PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
-			break;
-
-		case RC_SCAN_SBUS:
-			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
-			break;
-
-
-		case RC_SCAN_DSM:
-			// DSM status output
-#if defined(SPEKTRUM_POWER)
-#endif
-			break;
-
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
-
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
-
-		case RC_SCAN_ST24:
-			// SUMD status output
-			break;
-		}
-	}
-
-#if ADC_RC_RSSI_CHANNEL
-
-	if (_analog_rc_rssi_stable) {
-		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
-	}
-
-#endif
-
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_publish_interval_perf);
-
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
-
-	return 0;
-}
-
-int
-RCInput::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-This module does the RC input parsing and auto-selecting the method. Supported methods are:
-- PPM
-- SBUS
-- DSM
-- SUMD
-- ST24
-- TBS Crossfire (CRSF)
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("rc_input", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
-
-#if defined(SPEKTRUM_POWER)
-	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
-#endif /* SPEKTRUM_POWER */
-
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int rc_input_main(int argc, char *argv[])
-{
-	return RCInput::main(argc, argv);
-}

src/drivers/rc_input/crsf_telemetry.cpp

@@ -1,183 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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 "crsf_telemetry.h"
-#include <lib/rc/crsf.h>
-
-CRSFTelemetry::CRSFTelemetry(int uart_fd) :
-	_uart_fd(uart_fd)
-{
-}
-
-bool CRSFTelemetry::update(const hrt_abstime &now)
-{
-	const int update_rate_hz = 10;
-
-	if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
-		return false;
-	}
-
-	bool sent = false;
-
-	switch (_next_type) {
-	case 0:
-		sent = send_battery();
-		break;
-
-	case 1:
-		sent = send_gps();
-		break;
-
-	case 2:
-		sent = send_attitude();
-		break;
-
-	case 3:
-		sent = send_flight_mode();
-		break;
-	}
-
-	_last_update = now;
-	_next_type = (_next_type + 1) % num_data_types;
-
-	return sent;
-}
-
-bool CRSFTelemetry::send_battery()
-{
-	battery_status_s battery_status;
-
-	if (!_battery_status_sub.update(&battery_status)) {
-		return false;
-	}
-
-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
-	int fuel = battery_status.discharged_mah;
-	uint8_t remaining = battery_status.remaining * 100;
-	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
-}
-
-bool CRSFTelemetry::send_gps()
-{
-	sensor_gps_s vehicle_gps_position;
-
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
-
-	int32_t latitude = vehicle_gps_position.lat;
-	int32_t longitude = vehicle_gps_position.lon;
-	uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
-	uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
-	uint16_t altitude = vehicle_gps_position.alt + 1000;
-	uint8_t num_satellites = vehicle_gps_position.satellites_used;
-
-	return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
-				       gps_heading, altitude, num_satellites);
-}
-
-bool CRSFTelemetry::send_attitude()
-{
-	vehicle_attitude_s vehicle_attitude;
-
-	if (!_vehicle_attitude_sub.update(&vehicle_attitude)) {
-		return false;
-	}
-
-	matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
-	int16_t pitch = attitude(1) * 1e4f;
-	int16_t roll = attitude(0) * 1e4f;
-	int16_t yaw = attitude(2) * 1e4f;
-	return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw);
-}
-
-bool CRSFTelemetry::send_flight_mode()
-{
-	vehicle_status_s vehicle_status;
-
-	if (!_vehicle_status_sub.update(&vehicle_status)) {
-		return false;
-	}
-
-	const char *flight_mode = "(unknown)";
-
-	switch (vehicle_status.nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-		flight_mode = "Manual";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
-		flight_mode = "Altitude";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-		flight_mode = "Position";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-		flight_mode = "Return";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-		flight_mode = "Mission";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
-	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
-		flight_mode = "Auto";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ACRO:
-		flight_mode = "Acro";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
-		flight_mode = "Terminate";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-		flight_mode = "Offboard";
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_STAB:
-		flight_mode = "Stabilized";
-		break;
-	}
-
-	return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode);
-}

src/drivers/rc_input/crsf_telemetry.h

@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *	Copyright (c) 2018 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.
- *
- ****************************************************************************/
-
-/**
- * @file crsf_telemetry.h
- *
- * @author Beat Küng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/vehicle_status.h>
-#include <drivers/drv_hrt.h>
-
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-
-using namespace time_literals;
-
-/**
- * High-level class that handles sending of CRSF telemetry data
- */
-class CRSFTelemetry
-{
-public:
-	/**
-	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
-	 * already.
-	 */
-	CRSFTelemetry(int uart_fd);
-
-	~CRSFTelemetry() = default;
-
-	/**
-	 * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
-	 * limit the sending rate.
-	 * @return true if new data sent
-	 */
-	bool update(const hrt_abstime &now);
-
-private:
-	bool send_battery();
-	bool send_gps();
-	bool send_attitude();
-	bool send_flight_mode();
-
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	hrt_abstime _last_update{0};
-
-	static constexpr int num_data_types{4}; ///< number of different telemetry data types
-	int _next_type{0};
-
-	int _uart_fd;
-};

src/drivers/rc_input/ghst_telemetry.cpp

@@ -1,139 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-/**
- * @file ghst_telemetry.cpp
- *
- * IRC Ghost (Immersion RC Ghost) telemetry.
- *
- * @author Igor Misic <igy1000mb@gmail.com>
- * @author Juraj Ciberlin <jciberlin1@gmail.com>
- */
-
-#include "ghst_telemetry.hpp"
-#include <lib/rc/ghst.hpp>
-
-using time_literals::operator ""_s;
-
-GHSTTelemetry::GHSTTelemetry(int uart_fd) :
-	_uart_fd(uart_fd)
-{
-}
-
-bool GHSTTelemetry::update(const hrt_abstime &now)
-{
-	bool success = false;
-
-	if ((now - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) {
-
-		switch (_next_type) {
-		case 0U:
-			success = send_battery_status();
-			break;
-
-		case 1U:
-			success = send_gps1_status();
-			break;
-
-		case 2U:
-			success = send_gps2_status();
-			break;
-
-		default:
-			success = false;
-			break;
-		}
-
-		_last_update = now;
-		_next_type = (_next_type + 1U) % NUM_DATA_TYPES;
-	}
-
-	return success;
-}
-
-bool GHSTTelemetry::send_battery_status()
-{
-	bool success = false;
-	float voltage_in_10mV;
-	float current_in_10mA;
-	float fuel_in_10mAh;
-	battery_status_s battery_status;
-
-	if (_battery_status_sub.update(&battery_status)) {
-		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
-		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
-		success = ghst_send_telemetry_battery_status(_uart_fd,
-				static_cast<uint16_t>(voltage_in_10mV),
-				static_cast<uint16_t>(current_in_10mA),
-				static_cast<uint16_t>(fuel_in_10mAh));
-	}
-
-	return success;
-}
-
-bool GHSTTelemetry::send_gps1_status()
-{
-	sensor_gps_s vehicle_gps_position;
-
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
-
-	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
-	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
-	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
-
-	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
-}
-
-bool GHSTTelemetry::send_gps2_status()
-{
-	sensor_gps_s vehicle_gps_position;
-
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
-
-	uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
-	uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
-	uint8_t num_sats = vehicle_gps_position.satellites_used;
-
-	// TBD: Can these be computed in a RC telemetry driver?
-	uint16_t home_dist = 0;
-	uint16_t home_dir = 0;
-	uint8_t flags = 0;
-
-	return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
-}
-

src/drivers/rc_input/module.yaml

@@ -1,35 +0,0 @@
-module_name: RC Input Driver
-parameters:
-    - group: RC Input
-      definitions:
-        RC_INPUT_PROTO:
-            description:
-                short: RC input protocol
-                long: |
-                    Select your RC input protocol or auto to scan.
-            category: System
-            type: enum
-            values:
-                -1: Auto
-                0: None
-                1: PPM
-                2: SBUS
-                3: DSM
-                4: ST24
-                5: SUMD
-                6: CRSF
-                7: GHST
-            min: -1
-            max: 7
-            default: -1
-
-serial_config:
-    - command: set RC_INPUT_ARGS "-d ${SERIAL_DEV}"
-      port_config_param:
-        name: RC_PORT_CONFIG
-        group: Serial
-        default: RC
-        description_extended: |
-            Setting this to 'Disabled' will use a board-specific default port
-            for RC input.
-

src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp

@@ -1,298 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016 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.
- *
- ****************************************************************************/
-
-
-/**
- * @file spektrum_rc.cpp
- *
- * This is a driver for a Spektrum satellite receiver connected to a Snapdragon
- * on the serial port. By default port J12 (next to J13, power module side) is used.
- */
-
-#include <string.h>
-
-#include <px4_platform_common/tasks.h>
-#include <px4_platform_common/posix.h>
-#include <px4_platform_common/getopt.h>
-
-#include <lib/rc/dsm.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/input_rc.h>
-
-// Snapdraogon: use J12 (next to J13, power module side)
-#define SPEKTRUM_UART_DEVICE_PATH "/dev/tty-3"
-
-#define UNUSED(x) (void)(x)
-
-extern "C" { __EXPORT int spektrum_rc_main(int argc, char *argv[]); }
-
-
-namespace spektrum_rc
-{
-
-volatile bool _task_should_exit = false;
-static bool _is_running = false;
-static px4_task_t _task_handle = -1;
-
-int start();
-int stop();
-int info();
-void usage();
-void task_main(int argc, char *argv[]);
-
-void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		   hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi,
-		   input_rc_s &input_rc);
-
-void task_main(int argc, char *argv[])
-{
-	const char *device_path = SPEKTRUM_UART_DEVICE_PATH;
-	int ch;
-	int myoptind = 1;
-	const char *myoptarg = NULL;
-
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'd':
-			device_path = myoptarg;
-			break;
-
-		default:
-			break;
-		}
-	}
-
-	int uart_fd = dsm_init(device_path);
-
-	if (uart_fd < 1) {
-		PX4_ERR("dsm init failed");
-		return;
-	}
-
-	orb_advert_t rc_pub = nullptr;
-
-	// Use a buffer size of the double of the minimum, just to be safe.
-	uint8_t rx_buf[2 * DSM_BUFFER_SIZE];
-
-	_is_running = true;
-	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-	uint16_t raw_rc_count = 0;
-
-	// Main loop
-	while (!_task_should_exit) {
-
-		int newbytes = ::read(uart_fd, &rx_buf[0], sizeof(rx_buf));
-
-		if (newbytes < 0) {
-			PX4_WARN("read failed");
-			continue;
-		}
-
-		if (newbytes == 0) {
-			continue;
-		}
-
-		const hrt_abstime now = hrt_absolute_time();
-
-		bool dsm_11_bit;
-		unsigned frame_drops;
-		int8_t dsm_rssi;
-
-		// parse new data
-		bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count,
-					    &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
-		UNUSED(dsm_11_bit);
-
-		if (rc_updated) {
-
-			input_rc_s input_rc = {};
-
-			fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi,
-				      input_rc);
-
-			if (rc_pub == nullptr) {
-				rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc);
-
-			} else {
-				orb_publish(ORB_ID(input_rc), rc_pub, &input_rc);
-			}
-		}
-
-		// sleep since no poll for qurt
-		usleep(10000);
-
-	}
-
-	orb_unadvertise(rc_pub);
-	dsm_deinit();
-
-	_is_running = false;
-}
-
-void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		   hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi,
-		   input_rc_s &input_rc)
-{
-	input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
-
-	input_rc.channel_count = raw_rc_count;
-
-	if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < input_rc.channel_count; ++i) {
-		input_rc.values[i] = raw_rc_values[i];
-
-		if (raw_rc_values[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-	}
-
-	input_rc.timestamp = now;
-	input_rc.timestamp_last_signal = input_rc.timestamp;
-	input_rc.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-
-		input_rc.rssi = 255;
-
-	} else {
-		input_rc.rssi = rssi;
-	}
-
-	if (valid_chans == 0) {
-		input_rc.rssi = 0;
-	}
-
-	input_rc.rc_failsafe = failsafe;
-	input_rc.rc_lost = (valid_chans == 0);
-	input_rc.rc_lost_frame_count = frame_drops;
-	input_rc.rc_total_frame_count = 0;
-
-	input_rc.link_quality = -1;
-	input_rc.rssi_dbm = NAN;
-}
-
-int start(int argc, char *argv[])
-{
-	if (_is_running) {
-		PX4_WARN("already running");
-		return -1;
-	}
-
-	_task_should_exit = false;
-
-	_task_handle = px4_task_spawn_cmd("spektrum_rc_main",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2000,
-					  (px4_main_t)&task_main,
-					  (char *const *)argv);
-
-	if (_task_handle < 0) {
-		PX4_ERR("task start failed");
-		return -1;
-	}
-
-	return 0;
-}
-
-int stop()
-{
-	if (!_is_running) {
-		PX4_WARN("not running");
-		return -1;
-	}
-
-	_task_should_exit = true;
-
-	while (_is_running) {
-		usleep(200000);
-		PX4_INFO(".");
-	}
-
-	_task_handle = -1;
-	return 0;
-}
-
-int info()
-{
-	PX4_INFO("running: %s", _is_running ? "yes" : "no");
-
-	return 0;
-}
-
-void
-usage()
-{
-	PX4_INFO("Usage: spektrum_rc {start|info|stop}");
-}
-
-} // namespace spektrum_rc
-
-
-int spektrum_rc_main(int argc, char *argv[])
-{
-	int myoptind = 1;
-
-	if (argc <= 1) {
-		spektrum_rc::usage();
-		return 1;
-	}
-
-	const char *verb = argv[myoptind];
-
-
-	if (!strcmp(verb, "start")) {
-		return spektrum_rc::start(argc - 1, argv + 1);
-	}
-
-	else if (!strcmp(verb, "stop")) {
-		return spektrum_rc::stop();
-	}
-
-	else if (!strcmp(verb, "info")) {
-		return spektrum_rc::info();
-	}
-
-	else {
-		spektrum_rc::usage();
-		return 1;
-	}
-}

src/drivers/spektrum_rc/Kconfig

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_SPEKTRUM_RC
-	bool "spektrum_rc"
-	default n
-	---help---
-		Enable support for spektrum_rc

src/lib/rc/CMakeLists.txt

@@ -31,26 +31,11 @@
 #
 ############################################################################
 
-add_library(rc
-	crsf.cpp
-	ghst.cpp
-	st24.cpp
-	sumd.cpp
-	sbus.cpp
-	dsm.cpp
-	common_rc.cpp
-)
-target_compile_options(rc
-	PRIVATE
-		#-DDEBUG_BUILD
-		-Wno-unused-result
-)
-target_link_libraries(rc PRIVATE prebuild_targets)
+add_subdirectory(dsm)
+add_subdirectory(sbus)
+add_subdirectory(st24)
+add_subdirectory(sumd)
 
 if(PX4_TESTING AND (${PX4_PLATFORM} MATCHES "posix"))
 	add_subdirectory(rc_tests)
 endif()
-
-if(${PX4_PLATFORM} MATCHES "nuttx")
-	target_link_libraries(rc PRIVATE nuttx_fs)
-endif()

src/lib/rc/common_rc.cpp

@@ -1,31 +0,0 @@
-
-#include "common_rc.h"
-
-__EXPORT rc_decode_buf_t rc_decode_buf;
-
-uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
-{
-	crc ^= a;
-
-	for (int i = 0; i < 8; ++i) {
-		if (crc & 0x80) {
-			crc = (crc << 1) ^ 0xD5;
-
-		} else {
-			crc = crc << 1;
-		}
-	}
-
-	return crc;
-}
-
-uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
-{
-	uint8_t crc = 0;
-
-	for (int i = 0; i < len; ++i) {
-		crc = crc8_dvb_s2(crc, buf[i]);
-	}
-
-	return crc;
-}

src/lib/rc/common_rc.h

@@ -1,29 +0,0 @@
-
-#pragma once
-
-#include <stdint.h>
-
-#include "crsf.h"
-#include "dsm.h"
-#include "ghst.hpp"
-#include "sbus.h"
-#include "st24.h"
-#include "sumd.h"
-
-#pragma pack(push, 1)
-typedef  struct rc_decode_buf_ {
-	union {
-		crsf_frame_t crsf_frame;
-		ghst_frame_t ghst_frame;
-		dsm_decode_t dsm;
-		sbus_frame_t sbus_frame;
-		ReceiverFcPacket _strxpacket;
-		ReceiverFcPacketHoTT _hottrxpacket;
-	};
-} rc_decode_buf_t;
-#pragma pack(pop)
-
-extern rc_decode_buf_t rc_decode_buf;
-
-uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
-uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);

src/lib/rc/crsf.cpp

@@ -1,478 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
-
-#if 0 // enable non-verbose debugging
-#define CRSF_DEBUG PX4_WARN
-#else
-#define CRSF_DEBUG(...)
-#endif
-
-#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
-#define CRSF_VERBOSE PX4_WARN
-#else
-#define CRSF_VERBOSE(...)
-#endif
-
-#include <drivers/drv_hrt.h>
-#include <termios.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "crsf.h"
-#include "common_rc.h"
-
-#define MIN(a,b) (((a)<(b))?(a):(b))
-#define MAX(a,b) (((a)>(b))?(a):(b))
-
-
-#define CRSF_BAUDRATE 420000
-
-#define CRSF_SYNC_BYTE 0xC8
-
-enum class crsf_frame_type_t : uint8_t {
-	gps = 0x02,
-	battery_sensor = 0x08,
-	link_statistics = 0x14,
-	rc_channels_packed = 0x16,
-	attitude = 0x1E,
-	flight_mode = 0x21,
-
-	// Extended Header Frames, range: 0x28 to 0x96
-	device_ping = 0x28,
-	device_info = 0x29,
-	parameter_settings_entry = 0x2B,
-	parameter_read = 0x2C,
-	parameter_write = 0x2D,
-	command = 0x32
-};
-
-enum class crsf_payload_size_t : uint8_t {
-	gps = 15,
-	battery_sensor = 8,
-	link_statistics = 10,
-	rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
-	attitude = 6,
-};
-
-
-enum class crsf_address_t : uint8_t {
-	broadcast = 0x00,
-	usb = 0x10,
-	tbs_core_pnp_pro = 0x80,
-	reserved1 = 0x8A,
-	current_sensor = 0xC0,
-	gps = 0xC2,
-	tbs_blackbox = 0xC4,
-	flight_controller = 0xC8,
-	reserved2 = 0xCA,
-	race_tag = 0xCC,
-	radio_transmitter = 0xEA,
-	crsf_receiver = 0xEC,
-	crsf_transmitter = 0xEE
-};
-
-#pragma pack(push, 1)
-struct crsf_payload_RC_channels_packed_t {
-	// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
-	unsigned chan0 : 11;
-	unsigned chan1 : 11;
-	unsigned chan2 : 11;
-	unsigned chan3 : 11;
-	unsigned chan4 : 11;
-	unsigned chan5 : 11;
-	unsigned chan6 : 11;
-	unsigned chan7 : 11;
-	unsigned chan8 : 11;
-	unsigned chan9 : 11;
-	unsigned chan10 : 11;
-	unsigned chan11 : 11;
-	unsigned chan12 : 11;
-	unsigned chan13 : 11;
-	unsigned chan14 : 11;
-	unsigned chan15 : 11;
-};
-
-#pragma pack(pop)
-
-enum class crsf_parser_state_t : uint8_t {
-	unsynced = 0,
-	synced
-};
-
-static crsf_frame_t	 &crsf_frame = rc_decode_buf.crsf_frame;
-static unsigned current_frame_position = 0;
-static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
-
-/**
- * parse the current crsf_frame buffer
- */
-static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
-
-uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
-
-
-int
-crsf_config(int uart_fd)
-{
-	struct termios t;
-
-	/* no parity, one stop bit */
-	tcgetattr(uart_fd, &t);
-	cfsetspeed(&t, CRSF_BAUDRATE);
-	t.c_cflag &= ~(CSTOPB | PARENB);
-	return tcsetattr(uart_fd, TCSANOW, &t);
-}
-
-/**
- * Convert from RC to PWM value
- * @param chan_value channel value in [172, 1811]
- * @return PWM channel value in [1000, 2000]
- */
-static uint16_t convert_channel_value(unsigned chan_value);
-
-
-bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
-		uint16_t *num_values, uint16_t max_channels)
-{
-	bool ret = false;
-	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
-
-	while (len > 0) {
-
-		// fill in the crsf_buffer, as much as we can
-		const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position);
-		memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
-		current_frame_position += current_len;
-
-		// protection to guarantee parsing progress
-		if (current_len == 0) {
-			CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
-
-			for (unsigned i = 0; i < current_frame_position; ++i) {
-				CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
-			}
-
-			// reset the parser
-			current_frame_position = 0;
-			parser_state = crsf_parser_state_t::unsynced;
-			return false;
-		}
-
-		len -= current_len;
-		frame += current_len;
-
-		if (crsf_parse_buffer(values, num_values, max_channels)) {
-			ret = true;
-		}
-	}
-
-
-	return ret;
-}
-
-uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
-{
-	// CRC includes type and payload
-	uint8_t crc = crc8_dvb_s2(0, frame.type);
-
-	for (int i = 0; i < frame.header.length - 2; ++i) {
-		crc = crc8_dvb_s2(crc, frame.payload[i]);
-	}
-
-	return crc;
-}
-
-static uint16_t convert_channel_value(unsigned chan_value)
-{
-	/*
-	 *       RC     PWM
-	 * min  172 ->  988us
-	 * mid  992 -> 1500us
-	 * max 1811 -> 2012us
-	 */
-	static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
-	static constexpr float offset = 988.f - 172.f * scale;
-	return (scale * chan_value) + offset;
-}
-
-static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
-{
-	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
-
-	if (parser_state == crsf_parser_state_t::unsynced) {
-		// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
-		for (unsigned i = 1; i < current_frame_position - 1; ++i) {
-			if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
-			    crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
-				parser_state = crsf_parser_state_t::synced;
-				unsigned frame_offset = i - 1;
-				CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
-
-				// move the rest of the buffer to the beginning
-				if (frame_offset != 0) {
-					memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
-					current_frame_position -= frame_offset;
-				}
-
-				break;
-			}
-		}
-	}
-
-	if (parser_state != crsf_parser_state_t::synced) {
-		if (current_frame_position >= sizeof(crsf_frame_t)) {
-			// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
-			current_frame_position = 3;
-
-			for (unsigned i = 0; i < current_frame_position; ++i) {
-				crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
-			}
-
-			CRSF_VERBOSE("Discarding buffer");
-		}
-
-		return false;
-	}
-
-
-	if (current_frame_position < 3) {
-		// wait until we have the header & type
-		return false;
-	}
-
-	// Now we have at least the header and the type
-
-	const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
-
-	if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
-		// frame too long or bogus -> discard everything and go into unsynced state
-		current_frame_position = 0;
-		parser_state = crsf_parser_state_t::unsynced;
-		CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
-		return false;
-	}
-
-	if (current_frame_position < current_frame_length) {
-		// we don't have the full frame yet -> wait for more data
-		CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
-		return false;
-	}
-
-	bool ret = false;
-
-	// Now we have the full frame
-
-	if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
-	    crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
-		const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
-
-		if (crc == crsf_frame_CRC(crsf_frame)) {
-			const crsf_payload_RC_channels_packed_t *const rc_channels =
-				(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
-			*num_values = MIN(max_channels, 16);
-
-			if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); }
-
-			if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); }
-
-			if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); }
-
-			if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); }
-
-			if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); }
-
-			if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); }
-
-			if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); }
-
-			if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); }
-
-			if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); }
-
-			if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); }
-
-			if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); }
-
-			if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); }
-
-			if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); }
-
-			if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); }
-
-			if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); }
-
-			if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); }
-
-			CRSF_VERBOSE("Got Channels");
-
-			ret = true;
-
-		} else {
-			CRSF_DEBUG("CRC check failed");
-		}
-
-	} else {
-		CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
-		// We could check the CRC here and reset the parser into unsynced state if it fails.
-		// But in practise it's robust even without that.
-	}
-
-	// Either reset or move the rest of the buffer
-	if (current_frame_position > current_frame_length) {
-		CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
-		memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
-		current_frame_position -= current_frame_length;
-
-	} else {
-		current_frame_position = 0;
-	}
-
-	return ret;
-}
-
-/**
- * write an uint8_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
-{
-	buf[offset++] = value;
-}
-/**
- * write an uint16_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
-{
-	// Big endian
-	buf[offset] = value >> 8;
-	buf[offset + 1] = value & 0xff;
-	offset += 2;
-}
-/**
- * write an uint24_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
-{
-	// Big endian
-	buf[offset] = value >> 16;
-	buf[offset + 1] = (value >> 8) & 0xff;
-	buf[offset + 2] = value & 0xff;
-	offset += 3;
-}
-
-/**
- * write an int32_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
-{
-	// Big endian
-	buf[offset] = value >> 24;
-	buf[offset + 1] = (value >> 16) & 0xff;
-	buf[offset + 2] = (value >> 8) & 0xff;
-	buf[offset + 3] = value & 0xff;
-	offset += 4;
-}
-
-static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size)
-{
-	write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte
-	write_uint8_t(buf, offset, payload_size + 2);
-	write_uint8_t(buf, offset, (uint8_t)type);
-}
-static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
-{
-	// CRC does not include the address and length
-	write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));
-
-	// check correctness of buffer size (only needed during development)
-	//if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); }
-}
-
-bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
-	write_uint16_t(buf, offset, voltage);
-	write_uint16_t(buf, offset, current);
-	write_uint24_t(buf, offset, fuel);
-	write_uint8_t(buf, offset, remaining);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
-			     uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
-	write_int32_t(buf, offset, latitude);
-	write_int32_t(buf, offset, longitude);
-	write_uint16_t(buf, offset, groundspeed);
-	write_uint16_t(buf, offset, gps_heading);
-	write_uint16_t(buf, offset, altitude);
-	write_uint8_t(buf, offset, num_satellites);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
-	write_uint16_t(buf, offset, pitch);
-	write_uint16_t(buf, offset, roll);
-	write_uint16_t(buf, offset, yaw);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode)
-{
-	const int max_length = 16;
-	int length = strlen(flight_mode) + 1;
-
-	if (length > max_length) {
-		length = max_length;
-	}
-
-	uint8_t buf[max_length + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length);
-	memcpy(buf + offset, flight_mode, length);
-	offset += length;
-	buf[offset - 1] = 0; // ensure null-terminated string
-	write_frame_crc(buf, offset, length + 4);
-	return write(uart_fd, buf, offset) == offset;
-}

src/lib/rc/crsf.h

@@ -1,134 +0,0 @@
-/****************************************************************************
- *
- *	Copyright (c) 2018 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.
- *
- ****************************************************************************/
-
-/**
- * @file crsf.h
- *
- * RC protocol definition for CSRF (TBS Crossfire).
- * It is an uninverted protocol at 420000 baudrate.
- *
- * RC channels come in at 150Hz.
- *
- * @author Beat Küng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <stdint.h>
-#include <px4_platform_common/defines.h>
-
-__BEGIN_DECLS
-
-#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size
-#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4)
-
-
-struct crsf_frame_header_t {
-	uint8_t device_address;             ///< @see crsf_address_t
-	uint8_t length;                     ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
-};
-
-struct crsf_frame_t {
-	crsf_frame_header_t header;
-	uint8_t type;                       ///< @see crsf_frame_type_t
-	uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
-};
-
-/**
- * Configure an UART port to be used for CRSF
- * @param uart_fd UART file descriptor
- * @return 0 on success, -errno otherwise
- */
-__EXPORT int	crsf_config(int uart_fd);
-
-
-/**
- * Parse the CRSF protocol and extract RC channel data.
- *
- * @param now current time
- * @param frame data to parse
- * @param len length of frame
- * @param values output channel values, each in range [1000, 2000]
- * @param num_values set to the number of parsed channels in values
- * @param max_channels maximum length of values
- * @return true if channels successfully decoded
- */
-__EXPORT bool	crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
-			   uint16_t *num_values, uint16_t max_channels);
-
-
-/**
- * Send telemetry battery information
- * @param uart_fd UART file descriptor
- * @param voltage Voltage [0.1V]
- * @param current Current [0.1A]
- * @param fuel drawn mAh
- * @param remaining battery remaining [%]
- * @return true on success
- */
-__EXPORT bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining);
-
-/**
- * Send telemetry GPS information
- * @param uart_fd UART file descriptor
- * @param latitude latitude [degree * 1e7]
- * @param longitude longitude [degree * 1e7]
- * @param groundspeed Ground speed [km/h * 10]
- * @param gps_heading GPS heading [degree * 100]
- * @param altitude Altitude [meters + 1000m offset]
- * @param num_satellites number of satellites used
- * @return true on success
- */
-__EXPORT bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
-				      uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites);
-
-
-/**
- * Send telemetry Attitude information
- * @param uart_fd UART file descriptor
- * @param pitch Pitch angle [rad * 1e4]
- * @param roll Roll angle [rad * 1e4]
- * @param yaw Yaw angle [rad * 1e4]
- * @return true on success
- */
-__EXPORT bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw);
-
-/**
- * Send telemetry Flight Mode information
- * @param uart_fd UART file descriptor
- * @param flight_mode Flight Mode string (max length = 15)
- * @return true on success
- */
-__EXPORT bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode);
-
-__END_DECLS

src/lib/rc/dsm/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(rc_dsm
+	dsm.cpp
+	dsm.h
+)
+
+target_compile_options(rc_dsm
+	PRIVATE
+		#-DDEBUG_BUILD
+		#-Wno-unused-result
+)
+target_link_libraries(rc_dsm PRIVATE prebuild_targets)

src/lib/rc/dsm/dsm.cpp

@@ -51,8 +51,7 @@
 #include <string.h>
 
 #include "dsm.h"
-#include "spektrum_rssi.h"
-#include "common_rc.h"
+
 #include <drivers/drv_hrt.h>
 
 #include <include/containers/Bitset.hpp>
@@ -64,6 +63,47 @@
 #define dsm_udelay(arg) px4_usleep(arg)
 #endif
 
+#include <climits>
+
+/*
+min_rssi_dbm = -92.0
+max_rssi_dbm = -42.0
+
+def dbm_to_percent(abs_dbm):
+	if -abs_dbm < min_rssi_dbm:
+		return 0.0
+	elif -abs_dbm > max_rssi_dbm:
+		return 100.0
+	else:
+		return 100.0 * math.log10(1.0 + (float)(-abs_dbm - min_rssi_dbm) * (9.0 / (max_rssi_dbm - min_rssi_dbm)))
+*/
+constexpr int8_t lu_dbm_percent[] = {
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
+	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 99,  98,  98,  97,  96,
+	95,  94,  93,  92,  91,  90,  89,  88,  87,  86,  85,  84,  83,  82,  81,  79,
+	78,  77,  75,  74,  73,  71,  70,  68,  66,  65,  63,  61,  59,  57,  55,  52,
+	50,  47,  45,  42,  39,  35,  32,  28,  24,  19,  13,  7,   0,   0,   0,   0,
+	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+	0,
+};
+
+#define MASK(n) (n >> (sizeof(n) * CHAR_BIT - 1))
+
+/* constexpr-compatible version of abs() */
+static constexpr unsigned c_abs(int n) { return (n + MASK(n)) ^ MASK(n); }
+
+/* convert signed spektrum dbm (-92 to -42) to percentage */
+static constexpr int8_t spek_dbm_to_percent(int8_t dbm) { return lu_dbm_percent[c_abs(dbm)]; }
+
+static_assert(spek_dbm_to_percent(0) == 100, "0 dbm should be 100%");
+static_assert(spek_dbm_to_percent(-42) == 100, "-42 dbm should be 100%");
+static_assert(spek_dbm_to_percent(-80) == 50, "-80 dbm should be 50%");
+static_assert(spek_dbm_to_percent(-92) == 0, "-92 dbm should be 0%");
+static_assert(spek_dbm_to_percent(-128) == 0, "-128 dbm should be 0%");
+
+
 // #define DSM_DEBUG
 
 static enum DSM_DECODE_STATE {
@@ -81,8 +121,8 @@ static dsm_channel_t channel_buffer[DSM_MAX_CHANNEL_COUNT];
 static int dsm_fd = -1;						/**< File handle to the DSM UART */
 static hrt_abstime dsm_last_rx_time;            /**< Timestamp when we last received data */
 static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last valid dsm frame */
-static dsm_frame_t &dsm_frame = rc_decode_buf.dsm.frame;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
-static dsm_buf_t &dsm_buf = rc_decode_buf.dsm.buf;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
+static dsm_frame_t dsm_frame;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
+static dsm_buf_t dsm_buf;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
 
 static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
 static unsigned dsm_channel_shift = 0;			/**< Channel resolution, 0=unknown, 10=10 bit (1024), 11=11 bit (2048) */

src/lib/rc/dsm/dsm.h

@@ -53,8 +53,6 @@ __BEGIN_DECLS
 #define DSM_MAX_CHANNEL_COUNT   20  /**< Max channel count of any DSM RC */
 #define DSM_BUFFER_SIZE		(DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2)
 
-
-#pragma pack(push, 1)
 typedef   uint8_t dsm_frame_t[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */
 typedef   uint8_t dsm_buf_t[DSM_FRAME_SIZE * 2]; // Define working buffer
 
@@ -63,8 +61,6 @@ typedef  struct dsm_decode_t {
 	dsm_buf_t buf;
 } dsm_decode_t;
 
-#pragma pack(pop)
-
 __EXPORT int	dsm_init(const char *device);
 __EXPORT void	dsm_deinit(void);
 __EXPORT void	dsm_proto_init(void);

src/lib/rc/rc_tests/CMakeLists.txt

@@ -39,5 +39,8 @@ px4_add_module(
 	SRCS
 		RCTest.cpp
 	DEPENDS
-		rc
+		rc_dsm
+		rc_sbus
+		rc_st24
+		rc_sumd
 	)

src/lib/rc/rc_tests/RCTest.cpp

@@ -9,12 +9,10 @@
 #include <drivers/drv_hrt.h>
 
 #define DSM_DEBUG
-#include <lib/rc/sbus.h>
-#include <lib/rc/dsm.h>
-#include <lib/rc/st24.h>
-#include <lib/rc/sumd.h>
-#include <lib/rc/crsf.h>
-#include <lib/rc/ghst.hpp>
+#include <lib/rc/sbus/sbus.h>
+#include <lib/rc/dsm/dsm.h>
+#include <lib/rc/st24/st24.h>
+#include <lib/rc/sumd/sumd.h>
 
 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
 #define TEST_DATA_PATH "./test_data/"
@@ -30,8 +28,6 @@ public:
 	bool run_tests() override;
 
 private:
-	bool crsfTest();
-	bool ghstTest();
 	bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
 	bool dsmTest10Ch();
 	bool dsmTest16Ch();
@@ -44,8 +40,6 @@ private:
 
 bool RCTest::run_tests()
 {
-	ut_run_test(crsfTest);
-	ut_run_test(ghstTest);
 	ut_run_test(dsmTest10Ch);
 	ut_run_test(dsmTest16Ch);
 	ut_run_test(dsmTest22msDSMX16Ch);
@@ -57,180 +51,6 @@ bool RCTest::run_tests()
 	return (_tests_failed == 0);
 }
 
-bool RCTest::crsfTest()
-{
-	const char *filepath = TEST_DATA_PATH "crsf_rc_channels.txt";
-
-	FILE *fp = fopen(filepath, "rt");
-
-	ut_test(fp);
-	//PX4_INFO("loading data from: %s", filepath);
-
-	const int line_size = 500;
-	char line[line_size];
-	bool has_decoded_values = false;
-	const int max_channels = 16;
-	uint16_t rc_values[max_channels];
-	uint16_t num_values = 0;
-	int line_counter = 1;
-
-	while (fgets(line, line_size, fp) != nullptr)  {
-
-		if (strncmp(line, "INPUT ", 6) == 0) {
-
-			if (has_decoded_values) {
-				PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter);
-				return false;
-			}
-
-			// read the values
-			const char *file_buffer = line + 6;
-			int frame_len = 0;
-			uint8_t frame[300];
-			int offset;
-			int number;
-
-			while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) {
-				frame[frame_len++] = number;
-				file_buffer += offset;
-			}
-
-			// Pipe the data into the parser
-			hrt_abstime now = hrt_absolute_time();
-
-			bool result = crsf_parse(now, frame, frame_len, rc_values, &num_values, max_channels);
-
-			if (result) {
-				has_decoded_values = true;
-			}
-
-		} else if (strncmp(line, "DECODED ", 8) == 0) {
-
-			if (!has_decoded_values) {
-				PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter);
-				return false;
-			}
-
-			// read the values
-			const char *file_buffer = line + 8;
-			int offset;
-			int expected_rc_value;
-			int expected_num_channels = 0;
-
-			while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) {
-
-				// allow a small difference
-				if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) {
-					PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels);
-					ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]);
-				}
-
-				file_buffer += offset;
-				++expected_num_channels;
-			}
-
-			if (expected_num_channels != num_values) {
-				PX4_ERR("File line: %d", line_counter);
-				ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values);
-			}
-
-			has_decoded_values = false;
-		}
-
-		++line_counter;
-	}
-
-	return true;
-}
-
-bool RCTest::ghstTest()
-{
-	const char *filepath = TEST_DATA_PATH "ghst_rc_channels.txt";
-
-	FILE *fp = fopen(filepath, "rt");
-
-	ut_test(fp);
-
-	int uart_fd = -1;
-	const int line_size = 500;
-	char line[line_size];
-	bool has_decoded_values = false;
-	const int max_channels = 16;
-	uint16_t rc_values[max_channels];
-	uint16_t num_values = 0;
-	int line_counter = 1;
-	int8_t ghst_rssi = -1;
-	ghst_config(uart_fd);
-
-	while (fgets(line, line_size, fp) != nullptr)  {
-
-		if (strncmp(line, "INPUT ", 6) == 0) {
-
-			if (has_decoded_values) {
-				PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter);
-				return false;
-			}
-
-			// read the values
-			const char *file_buffer = line + 6;
-			int frame_len = 0;
-			uint8_t frame[300];
-			int offset;
-			int number;
-
-			while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) {
-				frame[frame_len++] = number;
-				file_buffer += offset;
-			}
-
-			// Pipe the data into the parser
-			hrt_abstime now = hrt_absolute_time();
-
-			bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels);
-
-			if (result) {
-				has_decoded_values = true;
-			}
-
-		} else if (strncmp(line, "DECODED ", 8) == 0) {
-
-			if (!has_decoded_values) {
-				PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter);
-				return false;
-			}
-
-			// read the values
-			const char *file_buffer = line + 8;
-			int offset;
-			int expected_rc_value;
-			int expected_num_channels = 0;
-
-			while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) {
-
-				// allow a small difference
-				if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) {
-					PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels);
-					ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]);
-				}
-
-				file_buffer += offset;
-				++expected_num_channels;
-			}
-
-			if (expected_num_channels != num_values) {
-				PX4_ERR("File line: %d", line_counter);
-				ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values);
-			}
-
-			has_decoded_values = false;
-		}
-
-		++line_counter;
-	}
-
-	return true;
-}
-
 bool RCTest::dsmTest10Ch()
 {
 	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 2, 1500);

src/lib/rc/sbus/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(rc_sbus
+	sbus.cpp
+	sbus.h
+)
+
+target_compile_options(rc_sbus
+	PRIVATE
+		#-DDEBUG_BUILD
+		-Wno-unused-result
+)
+target_link_libraries(rc_sbus PRIVATE prebuild_targets)
+
+if(${PX4_PLATFORM} MATCHES "nuttx")
+	target_link_libraries(rc_sbus PRIVATE nuttx_fs)
+endif()

src/lib/rc/sbus/sbus.cpp

@@ -48,7 +48,7 @@
 #endif
 
 #include "sbus.h"
-#include "common_rc.h"
+
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 
@@ -119,7 +119,7 @@ static enum SBUS2_DECODE_STATE {
 	SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34
 } sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
 
-static sbus_frame_t	 &sbus_frame = rc_decode_buf.sbus_frame;
+static sbus_frame_t sbus_frame;
 
 static unsigned partial_frame_count;
 static unsigned sbus1_frame_delay = (1000U * 1000U) / SBUS1_DEFAULT_RATE_HZ;

src/lib/rc/sbus/sbus.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *	Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *	Copyright (c) 2012-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

src/lib/rc/spektrum_rssi.h

@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- *	Copyright (c) 2012-2015 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.
- *
- ****************************************************************************/
-
-/**
- * @file spektrum_rssi.h
- *
- * RSSI dBm to percentage conversion for Spektrum telemetry receivers
- *
- * @author Kurt Kiefer <kekiefer@gmail.com>
- */
-
-#pragma once
-
-#include <climits>
-
-/*
-min_rssi_dbm = -92.0
-max_rssi_dbm = -42.0
-
-def dbm_to_percent(abs_dbm):
-	if -abs_dbm < min_rssi_dbm:
-		return 0.0
-	elif -abs_dbm > max_rssi_dbm:
-		return 100.0
-	else:
-		return 100.0 * math.log10(1.0 + (float)(-abs_dbm - min_rssi_dbm) * (9.0 / (max_rssi_dbm - min_rssi_dbm)))
-*/
-constexpr int8_t lu_dbm_percent[] = {
-	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
-	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
-	100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 99,  98,  98,  97,  96,
-	95,  94,  93,  92,  91,  90,  89,  88,  87,  86,  85,  84,  83,  82,  81,  79,
-	78,  77,  75,  74,  73,  71,  70,  68,  66,  65,  63,  61,  59,  57,  55,  52,
-	50,  47,  45,  42,  39,  35,  32,  28,  24,  19,  13,  7,   0,   0,   0,   0,
-	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-	0,
-};
-
-#define MASK(n) (n >> (sizeof(n) * CHAR_BIT - 1))
-
-/* constexpr-compatible version of abs() */
-constexpr unsigned c_abs(int n)
-{
-	return (n + MASK(n)) ^ MASK(n);
-}
-
-/* convert signed spektrum dbm (-92 to -42) to percentage */
-constexpr int8_t spek_dbm_to_percent(int8_t dbm)
-{
-	return lu_dbm_percent[c_abs(dbm)];
-}
-
-static_assert(spek_dbm_to_percent(0) == 100, "0 dbm should be 100%");
-static_assert(spek_dbm_to_percent(-42) == 100, "-42 dbm should be 100%");
-static_assert(spek_dbm_to_percent(-80) == 50, "-80 dbm should be 50%");
-static_assert(spek_dbm_to_percent(-92) == 0, "-92 dbm should be 0%");
-static_assert(spek_dbm_to_percent(-128) == 0, "-128 dbm should be 0%");

src/lib/rc/st24/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(rc_st24
+	st24.cpp
+	st24.h
+)
+
+target_compile_options(rc_st24
+	PRIVATE
+		#-DDEBUG_BUILD
+		#-Wno-unused-result
+)
+target_link_libraries(rc_st24 PRIVATE prebuild_targets)

src/lib/rc/st24/st24.cpp

@@ -42,7 +42,6 @@
 #include <stdbool.h>
 #include <stdio.h>
 #include "st24.h"
-#include "common_rc.h"
 
 const char *decode_states[] = {"UNSYNCED",
 			       "GOT_STX1",
@@ -66,8 +65,15 @@ const char *decode_states[] = {"UNSYNCED",
 static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
 static uint8_t _rxlen;
 
-static ReceiverFcPacket &_rxpacket = rc_decode_buf._strxpacket;
+static ReceiverFcPacket _rxpacket;
 
+/**
+ * CRC8 implementation for ST24 protocol
+ *
+ * @param prt Pointer to the data to CRC
+ * @param len number of bytes to accumulate in the checksum
+ * @return the checksum of these bytes over len
+ */
 uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 {
 	uint8_t i, crc ;

src/lib/rc/st24/st24.h

@@ -147,15 +147,6 @@ typedef struct {
 
 #pragma pack(pop)
 
-/**
- * CRC8 implementation for ST24 protocol
- *
- * @param prt Pointer to the data to CRC
- * @param len number of bytes to accumulate in the checksum
- * @return the checksum of these bytes over len
- */
-uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
-
 /**
  * Decoder for ST24 protocol
  *

src/lib/rc/sumd/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(rc_sumd
+	sumd.cpp
+	sumd.h
+)
+
+target_compile_options(rc_sumd
+	PRIVATE
+		#-DDEBUG_BUILD
+		#-Wno-unused-result
+)
+target_link_libraries(rc_sumd PRIVATE prebuild_targets)

src/lib/rc/sumd/sumd.cpp

@@ -42,7 +42,6 @@
 #include <stdbool.h>
 #include <stdio.h>
 #include "sumd.h"
-#include "common_rc.h"
 
 enum SUMD_DECODE_STATE {
 	SUMD_DECODE_STATE_UNSYNCED = 0,
@@ -67,12 +66,11 @@ const char *decode_states[] = {"UNSYNCED",
 			      };
 */
 
-uint8_t 	_crc8 	= 0x00;
+uint8_t 	_crc8 = 0x00;
 uint16_t 	_crc16 = 0x0000;
-bool 		_sumd 		= true;
-bool		_crcOK		= false;
-bool		_debug		= false;
-
+bool 		_sumd = true;
+bool		_crcOK = false;
+bool		_debug = false;
 
 /* define range mapping here, -+100% -> 1000..2000 */
 #define SUMD_RANGE_MIN 0.0f
@@ -88,8 +86,15 @@ bool		_debug		= false;
 static enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED;
 static uint8_t _rxlen;
 
-static ReceiverFcPacketHoTT &_rxpacket = rc_decode_buf._hottrxpacket;
+static ReceiverFcPacketHoTT _rxpacket;
 
+/**
+ * CRC16 implementation for SUMD protocol
+ *
+ * @param crc Initial CRC Value
+ * @param value to accumulate in the checksum
+ * @return the checksum
+ */
 uint16_t sumd_crc16(uint16_t crc, uint8_t value)
 {
 	int i;
@@ -102,7 +107,14 @@ uint16_t sumd_crc16(uint16_t crc, uint8_t value)
 	return crc;
 }
 
-uint8_t sumd_crc8(uint8_t crc, uint8_t value)
+/**
+ * CRC8 implementation for SUMH protocol
+ *
+ * @param crc Initial CRC Value
+ * @param value to accumulate in the checksum
+ * @return the checksum
+ */
+static uint8_t sumd_crc8(uint8_t crc, uint8_t value)
 {
 	crc += value;
 	return crc;

src/lib/rc/sumd/sumd.h

@@ -52,9 +52,6 @@ __BEGIN_DECLS
 #define SUMD_ID_SUMD		0x01
 #define SUMD_ID_FAILSAFE	0x81
 
-
-
-
 #pragma pack(push, 1)
 typedef struct {
 	uint8_t	header;							///< 0xA8 for a valid packet
@@ -68,25 +65,6 @@ typedef struct {
 } ReceiverFcPacketHoTT;
 #pragma pack(pop)
 
-
-/**
- * CRC16 implementation for SUMD protocol
- *
- * @param crc Initial CRC Value
- * @param value to accumulate in the checksum
- * @return the checksum
- */
-uint16_t sumd_crc16(uint16_t crc, uint8_t value);
-
-/**
- * CRC8 implementation for SUMH protocol
- *
- * @param crc Initial CRC Value
- * @param value to accumulate in the checksum
- * @return the checksum
- */
-uint8_t sumd_crc8(uint8_t crc, uint8_t value);
-
 /**
  * Decoder for SUMD/SUMH protocol
  *
@@ -98,10 +76,6 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value);
  * @param failsafe pointer to a boolean where the decoded failsafe flag is written back to
  * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
  */
-/*
-__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
-				 uint16_t *channels, uint16_t max_chan_count, bool *failsafe);
-*/
 __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
 			 uint16_t *channels, uint16_t max_chan_count, bool *failsafe);
 

src/modules/px4iofirmware/CMakeLists.txt

@@ -52,7 +52,10 @@ target_link_libraries(px4iofirmware
 		nuttx_arch
 		nuttx_c
 		nuttx_mm
-		rc
+		rc_dsm
+		rc_sbus
+		rc_st24
+		rc_sumd
 )
 
 if(PX4IO_PERF)

src/modules/px4iofirmware/controls.cpp

@@ -44,10 +44,10 @@
 
 #include <drivers/drv_hrt.h>
 #include <systemlib/ppm_decode.h>
-#include <rc/st24.h>
-#include <rc/sumd.h>
-#include <rc/sbus.h>
-#include <rc/dsm.h>
+#include <lib/rc/st24/st24.h>
+#include <lib/rc/sumd/sumd.h>
+#include <lib/rc/sbus/sbus.h>
+#include <lib/rc/dsm/dsm.h>
 #include <uORB/topics/input_rc.h>
 
 #if defined(PX4IO_PERF)

src/modules/px4iofirmware/mixer.cpp

@@ -51,7 +51,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
-#include <rc/sbus.h>
+#include <lib/rc/sbus/sbus.h>
 
 extern "C" {
 	/* #define DEBUG */

src/modules/px4iofirmware/registers.c

@@ -48,8 +48,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 #include <stm32_pwr.h>
-#include <rc/dsm.h>
-#include <rc/sbus.h>
+#include <lib/rc/dsm/dsm.h>
+#include <lib/rc/sbus/sbus.h>
 
 #include "px4io.h"
 #include "protocol.h"