drivers/rc/ghst_rc: create new standalone ghst_rc driver

boards/px4/fmu-v5x/default.px4board

@@ -38,6 +38,7 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
@@ -60,6 +61,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -81,7 +83,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y

src/drivers/rc/Kconfig

@@ -0,0 +1,9 @@
+menu "RC"
+    menuconfig COMMON_RC
+        bool "Common RC"
+        default n
+        select DRIVERS_RC_GHST_RC
+        ---help---
+            Enable default set of magnetometer drivers
+    rsource "*/Kconfig"
+endmenu

src/drivers/rc/ghst_rc/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 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__ghst_rc
+	MAIN ghst_rc
+	COMPILE_FLAGS
+	SRCS
+		ghst_telemetry.cpp
+		ghst_telemetry.hpp
+		GhstRc.cpp
+		GhstRc.hpp
+
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc
+	)

src/drivers/rc/ghst_rc/GhstRc.cpp

@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 "GhstRc.hpp"
+
+#include <poll.h>
+#include <termios.h>
+
+using namespace time_literals;
+
+GhstRc::GhstRc(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';
+	}
+}
+
+GhstRc::~GhstRc()
+{
+	delete _ghst_telemetry;
+
+	perf_free(_cycle_interval_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int GhstRc::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device == nullptr) {
+		PX4_ERR("valid device required");
+		return PX4_ERROR;
+	}
+
+	GhstRc *instance = new GhstRc(device);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+
+	instance->ScheduleNow();
+
+	return PX4_OK;
+}
+
+void GhstRc::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		::close(_rc_fd);
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_rc_fd < 0) {
+		_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+	}
+
+	// poll with 3 second timeout
+	pollfd fds[1];
+	fds[0].fd = _rc_fd;
+	fds[0].events = POLLIN;
+	::poll(fds, 1, 3000);
+
+	perf_count(_cycle_interval_perf);
+
+	const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+	// read all available data from the serial RC input UART
+	int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+	if (new_bytes > 0) {
+		_bytes_rx += new_bytes;
+	}
+
+	if (_rc_scan_begin == 0) {
+		_rc_scan_begin = cycle_timestamp;
+		// Configure serial port for GHST
+		ghst_config(_rc_fd);
+	}
+
+	if (_rc_locked || (cycle_timestamp - _rc_scan_begin < 300_ms)) {
+
+	}
+
+	// parse new data
+	if (new_bytes > 0) {
+		uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+		uint16_t raw_rc_count = 0;
+		int8_t ghst_rssi = -1;
+		bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+					     input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+		if (rc_updated) {
+			if (!_rc_locked) {
+				_rc_locked = true;
+				PX4_INFO("RC input locked");
+			}
+
+			// we have a new GHST frame. Publish it.
+			input_rc_s input_rc{};
+			input_rc.timestamp_last_signal = cycle_timestamp;
+			input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+			input_rc.rssi = ghst_rssi;
+			input_rc.rc_lost = (raw_rc_count == 0);
+			input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+
+			for (int i = 0; i < input_rc.channel_count; i++) {
+				input_rc.values[i] = raw_rc_values[i];
+			}
+
+			input_rc.timestamp = hrt_absolute_time();
+			_input_rc_pub.publish(input_rc);
+			perf_count(_publish_interval_perf);
+
+			if (!_ghst_telemetry) {
+				_ghst_telemetry = new GHSTTelemetry(_rc_fd);
+			}
+
+			if (_ghst_telemetry) {
+				_ghst_telemetry->update(cycle_timestamp);
+			}
+		}
+	}
+
+	ScheduleDelayed(4_ms);
+}
+
+int GhstRc::print_status()
+{
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal");
+
+	if (_rc_locked) {
+		PX4_INFO("Telemetry: %s", _ghst_telemetry ? "yes" : "no");
+	}
+
+	perf_print_counter(_cycle_interval_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	return 0;
+}
+
+int GhstRc::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int GhstRc::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the GHST RC input parsing.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ghst_rc", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[])
+{
+	return GhstRc::main(argc, argv);
+}

src/drivers/rc/ghst_rc/GhstRc.hpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/ghst.hpp>
+#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 "ghst_telemetry.hpp"
+
+class GhstRc : public ModuleBase<GhstRc>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	GhstRc(const char *device);
+	~GhstRc() override;
+
+	/** @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;
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _rc_locked{false};
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	int _rc_fd{-1};
+	char _device[20] {}; ///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{64};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+	uint32_t _bytes_rx{0};
+
+	GHSTTelemetry *_ghst_telemetry{nullptr};
+
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+};

src/drivers/rc/ghst_rc/Kconfig

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

src/drivers/rc/ghst_rc/ghst_telemetry.cpp

@@ -41,6 +41,7 @@
  */
 
 #include "ghst_telemetry.hpp"
+
 #include <lib/rc/ghst.hpp>
 
 using time_literals::operator ""_s;

src/drivers/rc/ghst_rc/module.yaml

@@ -0,0 +1,11 @@
+module_name: GHST RC Input Driver
+serial_config:
+    - command: "ghst_rc start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: GHST_RC_PRT_CFG
+        group: Serial
+        #default: RC
+        #depends_on_port: RC
+        description_extended: |
+            Ghost RC input driver.
+

src/drivers/rc_input/CMakeLists.txt

@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		RCInput.cpp
 		crsf_telemetry.cpp
-		ghst_telemetry.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS

src/drivers/rc_input/RCInput.cpp

@@ -74,7 +74,6 @@ RCInput::~RCInput()
 	dsm_deinit();
 
 	delete _crsf_telemetry;
-	delete _ghst_telemetry;
 
 	perf_free(_cycle_perf);
 	perf_free(_publish_interval_perf);
@@ -665,56 +664,6 @@ void RCInput::Run()
 					}
 				}
 
-			} 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);
@@ -827,10 +776,6 @@ int RCInput::print_status()
 			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;

src/drivers/rc_input/RCInput.hpp

@@ -40,7 +40,6 @@
 #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>
@@ -61,7 +60,6 @@
 #include <uORB/topics/vehicle_status.h>
 
 #include "crsf_telemetry.h"
-#include "ghst_telemetry.hpp"
 
 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
@@ -97,7 +95,6 @@ private:
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
 	} _rc_scan_state{RC_SCAN_SBUS};
 
 	static constexpr char const *RC_SCAN_STRING[7] {
@@ -107,7 +104,6 @@ private:
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
 	};
 
 	void Run() override;
@@ -159,7 +155,6 @@ private:
 	uint16_t _raw_rc_count{};
 
 	CRSFTelemetry *_crsf_telemetry{nullptr};
-	GHSTTelemetry *_ghst_telemetry{nullptr};
 
 	perf_counter_t	_cycle_perf;
 	perf_counter_t	_publish_interval_perf;