drivers/rc: new standalone dsm_rc driver

- extracted from monolithic drivers/rc_input which will eventually be
   dropped once all drivers migrated and boards updated

src/drivers/rc/dsm_rc/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-2024 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__dsm_rc
+	MAIN dsm_rc
+	COMPILE_FLAGS
+	SRCS
+		DsmRc.cpp
+		DsmRc.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc
+	)

src/drivers/rc/dsm_rc/DsmRc.cpp

@@ -0,0 +1,417 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2024 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 "DsmRc.hpp"
+
+#include <uORB/topics/vehicle_command_ack.h>
+
+#include <termios.h>
+
+using namespace time_literals;
+
+DsmRc::DsmRc(const char *device) :
+	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"))
+{
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+DsmRc::~DsmRc()
+{
+#if defined(SPEKTRUM_POWER_PASSIVE)
+	// Disable power controls for Spektrum receiver
+	SPEKTRUM_POWER_PASSIVE();
+#endif // SPEKTRUM_POWER_PASSIVE
+
+	perf_free(_cycle_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int DsmRc::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device_name = nullptr;
+	bool silent = false;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			silent = false;
+			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)) {
+		DsmRc *instance = new DsmRc(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 (silent) {
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_ERR("valid device required");
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void DsmRc::Run()
+{
+	if (should_exit()) {
+
+		close(_rcs_fd);
+
+		dsm_deinit();
+
+		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(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);
+		}
+	}
+
+	bool rc_updated = false;
+
+	constexpr hrt_abstime rc_scan_max = 3_s;
+
+	// read all available data from the serial RC input UART
+	uint8_t rcs_buf[DSM_BUFFER_SIZE] {};
+	int newBytes = ::read(_rcs_fd, &rcs_buf[0], DSM_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
+		if (_rcs_fd < 0) {
+			_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+		}
+
+		dsm_config(_rcs_fd);
+
+		// flush serial buffer and any existing buffered data
+		tcflush(_rcs_fd, TCIOFLUSH);
+
+	} else if (_rc_scan_locked
+		   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+		if (newBytes > 0) {
+			uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+			uint16_t raw_rc_count = 0;
+
+			int8_t dsm_rssi = 0;
+			bool dsm_11_bit = false;
+			unsigned frame_drops = 0;
+
+			if (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)
+			   ) {
+				// we have a new DSM frame. Publish it.
+				input_rc_s input_rc{};
+				input_rc.timestamp_last_signal = cycle_timestamp;
+				input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+				input_rc.rssi = dsm_rssi;
+				input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+				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.channel_count = valid_chans;
+
+				if (valid_chans == 0) {
+					input_rc.rssi = 0;
+				}
+
+				input_rc.rc_lost = (valid_chans == 0);
+				input_rc.rc_lost_frame_count = frame_drops;
+
+				input_rc.link_quality = -1;
+				input_rc.rssi_dbm = NAN;
+
+				input_rc.timestamp = hrt_absolute_time();
+				_input_rc_pub.publish(input_rc);
+				perf_count(_publish_interval_perf);
+
+				_timestamp_last_signal = cycle_timestamp;
+				rc_updated = true;
+
+				if (valid_chans > 0) {
+					_rc_scan_locked = true;
+				}
+			}
+		}
+
+	} else {
+		_rc_scan_begin = 0;
+		_rc_scan_locked = false;
+
+		close(_rcs_fd);
+		_rcs_fd = -1;
+	}
+
+	if (!rc_updated && !_armed && (hrt_elapsed_time(&_timestamp_last_signal) > 1_s)) {
+		_rc_scan_locked = false;
+	}
+
+	if (!rc_scan_locked && _rc_scan_locked) {
+		PX4_INFO("RC input locked");
+	}
+
+	perf_end(_cycle_perf);
+}
+
+#if defined(SPEKTRUM_POWER)
+bool DsmRc::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 DsmRc::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
+
+	if (!is_running()) {
+		int ret = DsmRc::task_spawn(argc, argv);
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	return print_usage("unknown command");
+}
+
+int DsmRc::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 DsmRc::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does Spektrum DSM RC input parsing.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("dsm_rc", "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 dsm_rc_main(int argc, char *argv[])
+{
+	return DsmRc::main(argc, argv);
+}

src/drivers/rc/dsm_rc/DsmRc.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2024 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/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 DsmRc : public ModuleBase<DsmRc>, public px4::ScheduledWorkItem
+{
+public:
+
+	DsmRc(const char *device);
+	virtual ~DsmRc();
+
+	/** @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;
+
+private:
+
+	void Run() override;
+
+#if defined(SPEKTRUM_POWER)
+	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
+#endif // SPEKTRUM_POWER
+
+	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _timestamp_last_signal{0};
+
+	bool _rc_scan_locked{false};
+
+	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)};
+
+	bool _armed{false};
+
+	int		_rcs_fd{-1};
+	char		_device[20] {};					///< device / serial port path
+
+	perf_counter_t	_cycle_perf;
+	perf_counter_t	_publish_interval_perf;
+	uint32_t	_bytes_rx{0};
+};

src/drivers/rc/dsm_rc/Kconfig

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

src/drivers/rc/dsm_rc/module.yaml

@@ -0,0 +1,11 @@
+module_name: DSM RC Input Driver
+serial_config:
+    - command: "dsm_rc start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_DSM_PRT_CFG
+        group: Serial
+        #default: RC
+        #depends_on_port: RC
+        description_extended: |
+            DSM RC (Spektrum) driver.
+