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

@@ -0,0 +1,471 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ *
+ * RC protocol definition for IRC Ghost (Immersion RC Ghost).
+ *
+ * @author Igor Misic <igy1000mb@gmail.com>
+ * @author Juraj Ciberlin <jciberlin1@gmail.com>
+ */
+
+#if 0 // enable non-verbose debugging
+#define GHST_DEBUG PX4_WARN
+#else
+#define GHST_DEBUG(...)
+#endif
+
+#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
+#define GHST_VERBOSE PX4_WARN
+#else
+#define GHST_VERBOSE(...)
+#endif
+
+#include <drivers/drv_hrt.h>
+#include <limits.h>
+#include <termios.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "ghst_parser.hpp"
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+
+#define GHST_FRAME_PAYLOAD_SIZE_TELEMETRY	(10U)
+#define GHST_FRAME_CRC_SIZE			(1U)
+#define GHST_FRAME_TYPE_SIZE			(1U)
+#define GHST_TYPE_DATA_CRC_SIZE			(12U)
+#define GHST_MAX_NUM_CHANNELS			(16)
+
+enum class ghst_parser_state_t : uint8_t {
+	unsynced = 0U,
+	synced
+};
+
+// 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;
+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
+ */
+static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels);
+
+int ghst_config(int uart_fd)
+{
+	struct termios t;
+	int ret_val;
+
+	// no parity, one stop bit
+	tcgetattr(uart_fd, &t);
+	cfsetspeed(&t, GHST_BAUDRATE);
+	t.c_cflag &= ~(CSTOPB | PARENB);
+	memset(prev_rc_vals, static_cast<int>(UINT16_MAX), sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
+	ret_val = tcsetattr(uart_fd, TCSANOW, &t);
+	return ret_val;
+}
+
+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)
+{
+	bool success = false;
+	uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame;
+
+	memcpy(values, prev_rc_vals, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
+
+	while (len > 0U) {
+
+		// fill in the ghst_buffer, as much as we can
+		const uint32_t current_len = MIN(len, sizeof(ghst_frame_t) - current_frame_position);
+		memcpy(ghst_frame_ptr + current_frame_position, frame, current_len);
+		current_frame_position += current_len;
+
+		// protection to guarantee parsing progress
+		if (current_len == 0U) {
+			GHST_DEBUG("========== parser bug: no progress (%u) ===========", len);
+
+			for (uint32_t i = 0U; i < current_frame_position; ++i) {
+				GHST_DEBUG("ghst_frame_ptr[%u]: 0x%x", i, ghst_frame_ptr[i]);
+			}
+
+			// reset the parser
+			current_frame_position = 0U;
+			parser_state = ghst_parser_state_t::unsynced;
+			success = false;
+
+		} else {
+			len -= current_len;
+			frame += current_len;
+
+			if (ghst_parse_buffer(values, rssi, num_values, max_channels)) {
+				success = true;
+			}
+		}
+	}
+
+	return success;
+}
+
+uint8_t ghst_frame_CRC(const ghst_frame_t &frame)
+{
+	uint8_t crc = crc8_dvb_s2(0U, frame.type);
+
+	for (uint32_t i = 0U; i < frame.header.length - GHST_FRAME_CRC_SIZE - GHST_FRAME_TYPE_SIZE; ++i) {
+		crc = crc8_dvb_s2(crc, frame.payload[i]);
+	}
+
+	return crc;
+}
+
+/**
+ * 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
+	 * min  172 ->  988us
+	 * mid  992 -> 1500us
+	 * max 1811 -> 2012us
+	 */
+	static constexpr float scale = (2012.0F - 988.0F) / (1811.0F - 172.0F);
+	static constexpr float offset = 988.0F - (172.0F * scale);
+	float scaled_chan_value = scale * static_cast<float>(chan_value);
+	float scaled_chan_value_with_offset = scaled_chan_value + offset;
+	uint16_t converted_chan_value = static_cast<uint16_t>(scaled_chan_value_with_offset);
+
+	return converted_chan_value;
+}
+
+static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels)
+{
+	uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame;
+
+	if (parser_state == ghst_parser_state_t::unsynced) {
+		// there is no sync yet, try to find an RC packet by searching for a matching frame length and type
+		for (uint32_t i = 1U; i < current_frame_position - 1U; ++i) {
+			if ((ghst_frame_ptr[i + 1U] >= static_cast<uint8_t>(ghstFrameType::frameTypeFirst)) &&
+			    (ghst_frame_ptr[i + 1U] <= static_cast<uint8_t>(ghstFrameType::frameTypeLast))) {
+				if (ghst_frame_ptr[i] == GHST_TYPE_DATA_CRC_SIZE) {
+					parser_state = ghst_parser_state_t::synced;
+					uint32_t frame_offset = i - 1U;
+					GHST_VERBOSE("RC channels found at offset %u", frame_offset);
+
+					// move the rest of the buffer to the beginning
+					if (frame_offset != 0U) {
+						memmove(ghst_frame_ptr, ghst_frame_ptr + frame_offset, current_frame_position - frame_offset);
+						current_frame_position -= frame_offset;
+					}
+
+					break;
+				}
+			}
+		}
+	}
+
+	if (parser_state != ghst_parser_state_t::synced) {
+		if (current_frame_position >= sizeof(ghst_frame_t)) {
+			// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
+			current_frame_position = 3U;
+
+			memcpy(ghst_frame_ptr, ghst_frame_ptr + sizeof(ghst_frame_t) - current_frame_position, current_frame_position);
+
+			GHST_VERBOSE("Discarding buffer");
+		}
+
+		return false;
+	}
+
+
+	if (current_frame_position < 3U) {
+		// wait until we have the address, length and type
+		return false;
+	}
+
+	// now we have at least the header and the type
+
+	const unsigned current_frame_length = ghst_frame.header.length + sizeof(ghst_frame_header_t);
+
+	if ((current_frame_length > sizeof(ghst_frame_t)) || (current_frame_length < 4U)) {
+		// frame too long or bogus (frame length should be longer than 4, at least 1 address, 1 length, 1 type, 1 data, 1 crc)
+		// discard everything and go into unsynced state
+		current_frame_position = 0U;
+		parser_state = ghst_parser_state_t::unsynced;
+		GHST_DEBUG("Frame too long/bogus (%u, type=%u) -> unsync", current_frame_length, ghst_frame.type);
+		return false;
+	}
+
+	if (current_frame_position < current_frame_length) {
+		// we do not have the full frame yet -> wait for more data
+		GHST_VERBOSE("waiting for more data (%u < %u)", current_frame_position, current_frame_length);
+		return false;
+	}
+
+	bool ret = false;
+
+	// now we have the full frame
+
+	if ((ghst_frame.type >= static_cast<uint8_t>(ghstFrameType::frameTypeFirst)) &&
+	    (ghst_frame.type <= static_cast<uint8_t>(ghstFrameType::frameTypeLast)) &&
+	    (ghst_frame.header.length == GHST_TYPE_DATA_CRC_SIZE)) {
+		const uint8_t crc = ghst_frame.payload[ghst_frame.header.length - 2U];
+
+		if (crc == ghst_frame_CRC(ghst_frame)) {
+			const ghstPayloadData_t *const rcChannels = (ghstPayloadData_t *)&ghst_frame.payload;
+			*num_values = MIN(max_channels, GHST_MAX_NUM_CHANNELS);
+
+			// all frames contain data from chan1to4
+			if (max_channels > 0U) { values[0] = convert_channel_value(rcChannels->chan1to4.chan1 >> 1U); }
+
+			if (max_channels > 1U) { values[1] = convert_channel_value(rcChannels->chan1to4.chan2 >> 1U); }
+
+			if (max_channels > 2U) { values[2] = convert_channel_value(rcChannels->chan1to4.chan3 >> 1U); }
+
+			if (max_channels > 3U) { values[3] = convert_channel_value(rcChannels->chan1to4.chan4 >> 1U); }
+
+			if (ghst_frame.type == static_cast<uint8_t>(ghstFrameType::frameType5to8)) {
+				if (max_channels > 4U) { values[4] = convert_channel_value(rcChannels->chanA << 3U); }
+
+				if (max_channels > 5U) { values[5] = convert_channel_value(rcChannels->chanB << 3U); }
+
+				if (max_channels > 6U) { values[6] = convert_channel_value(rcChannels->chanC << 3U); }
+
+				if (max_channels > 7U) { values[7] = convert_channel_value(rcChannels->chanD << 3U); }
+
+			} else if (ghst_frame.type == static_cast<uint8_t>(ghstFrameType::frameType9to12)) {
+				if (max_channels > 8U) { values[8] = convert_channel_value(rcChannels->chanA << 3U); }
+
+				if (max_channels > 9U) { values[9] = convert_channel_value(rcChannels->chanB << 3U); }
+
+				if (max_channels > 10U) { values[10] = convert_channel_value(rcChannels->chanC << 3U); }
+
+				if (max_channels > 11U) { values[11] = convert_channel_value(rcChannels->chanD << 3U); }
+
+			} else if (ghst_frame.type == static_cast<uint8_t>(ghstFrameType::frameType13to16)) {
+				if (max_channels > 12U) { values[12] = convert_channel_value(rcChannels->chanA << 3U); }
+
+				if (max_channels > 13U) { values[13] = convert_channel_value(rcChannels->chanB << 3U); }
+
+				if (max_channels > 14U) { values[14] = convert_channel_value(rcChannels->chanC << 3U); }
+
+				if (max_channels > 15U) { values[15] = convert_channel_value(rcChannels->chanD << 3U); }
+
+			} 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 = dbm_to_percent(static_cast<int8_t>(rssiValues->rssidBm));
+
+			} else {
+				GHST_DEBUG("Frame type: %u", ghst_frame.type);
+			}
+
+			*rssi = ghst_rssi;
+
+			memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
+
+			GHST_VERBOSE("Got Channels");
+
+			ret = true;
+
+		} else {
+			GHST_DEBUG("CRC check failed");
+		}
+
+	} else {
+		GHST_DEBUG("Got Non-RC frame (len=%u, type=%u)", current_frame_length, ghst_frame.type);
+	}
+
+	// either reset or move the rest of the buffer
+	if (current_frame_position > current_frame_length) {
+		GHST_VERBOSE("Moving buffer (%u > %u)", current_frame_position, current_frame_length);
+		memmove(ghst_frame_ptr, ghst_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
+		current_frame_position -= current_frame_length;
+
+	} else {
+		current_frame_position = 0U;
+	}
+
+	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)
+{
+	buf[offset] = value & 0xFFU;
+	buf[offset + 1] = value >> 8U;
+	offset += 2;
+}
+
+/**
+ * write an uint32_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint32_t(uint8_t *buf, int &offset, uint32_t value)
+{
+	// Little Endian
+	buf[offset] = value & 0xFFU;
+	buf[offset + 1] = (value & 0xFF00) >> 8U;
+	buf[offset + 2] = (value & 0xFF0000) >> 16U;
+	buf[offset + 3] = (value & 0xFF000000) >> 24U;
+	offset += 4;
+}
+
+/**
+ * write frame header
+ */
+static inline void write_frame_header(uint8_t *buf, int &offset, ghstTelemetryType type, uint8_t payload_size)
+{
+	write_uint8_t(buf, offset, static_cast<uint8_t>(ghstAddress::rxAddress));
+	write_uint8_t(buf, offset, payload_size + GHST_FRAME_CRC_SIZE + GHST_FRAME_TYPE_SIZE);
+	write_uint8_t(buf, offset, static_cast<uint8_t>(type));
+}
+
+/**
+ * write frame CRC
+ */
+static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
+{
+	write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2U, buf_size - 3));
+}
+
+bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
+					uint16_t current_in_10mA, uint16_t fuel_in_10mAh)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::batteryPack, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint16_t(buf, offset, voltage_in_10mV);
+	write_uint16_t(buf, offset, current_in_10mA);
+	write_uint16_t(buf, offset, fuel_in_10mAh);
+	write_uint8_t(buf, offset, 0x00U); // empty
+	write_uint8_t(buf, offset, 0x00U); // empty
+	write_uint8_t(buf, offset, 0x00U); // empty
+	write_uint8_t(buf, offset, 0x00U); // empty
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+
+bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsPrimary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint32_t(buf, offset, latitude);
+	write_uint32_t(buf, offset, longitude);
+	write_uint16_t(buf, offset, altitude);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+
+bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course, uint8_t numSats,
+				     uint16_t home_dist, uint16_t home_dir, uint8_t flags)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsSecondary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint16_t(buf, offset, ground_speed);
+	write_uint16_t(buf, offset, ground_course);
+	write_uint8_t(buf, offset, numSats);
+	write_uint16_t(buf, offset, home_dist);
+	write_uint16_t(buf, offset, home_dir);
+	write_uint8_t(buf, offset, flags);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}

src/drivers/rc/ghst_rc/ghst_parser.hpp

@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *   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.h
+ *
+ * RC protocol definition for IRC Ghost (Immersion RC Ghost).
+ *
+ * @author Igor Misic <igy1000mb@gmail.com>
+ * @author Juraj Ciberlin <jciberlin1@gmail.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <px4_platform_common/defines.h>
+
+__BEGIN_DECLS
+
+#define GHST_BAUDRATE		(420000u)
+#define GHST_PAYLOAD_MAX_SIZE	(14u)
+
+enum class ghstAddress {
+	rxAddress = 0x89	// Rx address
+};
+
+enum class ghstFrameType {
+	frameTypeFirst	= 0x10,	// first frame type
+	frameType5to8	= 0x10,	// channel 5-8
+	frameType9to12	= 0x11,	// channel 9-12
+	frameType13to16 = 0x12,	// channel 13-16
+	frameTypeRssi	= 0x13,	// RSSI frame type, contains LQ, RSSI, RF mode, Tx power
+	frameTypeLast	= 0x1f	// last frame type
+};
+
+enum class ghstTelemetryType {
+	batteryPack = 0x23,	// battery pack status frame type
+	gpsPrimary = 0x25,	// GPS primary data (lat/long/alt)
+	gpsSecondary = 0x26	// GPS secondary data (course, dist, flags)
+};
+
+struct ghst_frame_header_t {
+	uint8_t device_address;			// device address
+	uint8_t length;				// length
+};
+
+struct ghst_frame_t {
+	ghst_frame_header_t header;			// header
+	uint8_t type;					// frame type
+	uint8_t payload[GHST_PAYLOAD_MAX_SIZE + 1U];	// payload data including 1 byte CRC at the end
+};
+
+// Channel data (1-4)
+typedef struct {
+	unsigned int chan1: 12;
+	unsigned int chan2: 12;
+	unsigned int chan3: 12;
+	unsigned int chan4: 12;
+} __attribute__((__packed__)) ghstChannelData_t;
+
+// Payload data
+typedef struct {
+	ghstChannelData_t chan1to4;
+	unsigned int chanA: 8;
+	unsigned int chanB: 8;
+	unsigned int chanC: 8;
+	unsigned int chanD: 8;
+} __attribute__((__packed__)) ghstPayloadData_t;
+
+// Payload data - RSSI frame type
+typedef struct {
+	ghstChannelData_t chan1to4;
+	unsigned int lq: 8;		// link quality
+	unsigned int rssidBm: 8;	// RSSI [dBm]
+	unsigned int rfMode: 8;		// RF mode
+	int txPowerdBm: 8;		// Tx power [dBm]
+} __attribute__((__packed__)) ghstPayloadRssi_t;
+
+/**
+ * Configure an UART port to be used for GHST
+ * @param uart_fd UART file descriptor
+ * @return 0 on success, -errno otherwise
+ */
+__EXPORT int ghst_config(int uart_fd);
+
+
+/**
+ * Parse the GHST 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
+ * @param rssi received signal strength indicator
+ * @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 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);
+
+
+/**
+ * Send telemetry battery information
+ * @param uart_fd UART file descriptor
+ * @param voltage_in_10mV Voltage [10 mV]
+ * @param current_in_10mA Current [10 mA]
+ * @param fuel_in_10mAh Fuel [10 mAh]
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
+		uint16_t current_in_10mA, uint16_t fuel_in_10mAh);
+
+/**
+ * Send primary GPS information
+ * @param uart_fd UART file descriptor
+ * @param latitude GPS latitude [1e-7 degrees]
+ * @param longitude GPS longitude [1e-7 degrees]
+ * @param altitude GPS altitude [1m]
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude);
+
+/**
+ * Send secondary GPS information
+ * @param uart_fd UART file descriptor
+ * @param ground_speed Ground Speed [1 km/h]
+ * @param ground_course Ground Course [1e-7 degrees]
+ * @param num_sats GPS Satellite count
+ * @param home_dist Distance to Home [10 m]
+ * @param home_dir Direction to Home [1e-7 degrees]
+ * @param flags GPS Flags
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course,
+		uint8_t num_sats, uint16_t home_dist, uint16_t home_dir, uint8_t flags);
+__END_DECLS

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