navio_sysfs_rc_in: move to PX4 WQ and cleanup

boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016-2020 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,11 +31,12 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__navio_sysfs_rc_in
+	MODULE navio_sysfs_rc_in
 	MAIN navio_sysfs_rc_in
-	COMPILE_FLAGS
 	SRCS
-		navio_sysfs_rc_in.cpp
+		NavioSysRCInput.cpp
+		NavioSysRCInput.hpp
+		navio_sysfs_rc_in_main.cpp
 	DEPENDS
-	)
+		px4_work_queue
-
+)

boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "NavioSysRCInput.hpp"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+namespace navio_sysfs_rc_in
+{
+
+#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin"
+
+#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds
+
+NavioSysRCInput::NavioSysRCInput() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	_isRunning = true;
+};
+
+NavioSysRCInput::~NavioSysRCInput()
+{
+	ScheduleClear();
+
+	_isRunning = false;
+
+	for (int i = 0; i < _channels; ++i) {
+		if (_ch_fd[i] != -1) {
+			::close(_ch_fd[i]);
+		}
+	}
+}
+
+int NavioSysRCInput::navio_rc_init()
+{
+	int i;
+	char buf[64];
+
+	for (i = 0; i < _channels; ++i) {
+		::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
+		int fd = ::open(buf, O_RDONLY);
+
+		if (fd < 0) {
+			PX4_WARN("error: open %d failed", i);
+			break;
+		}
+
+		_ch_fd[i] = fd;
+	}
+
+	for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
+		_data.values[i] = UINT16_MAX;
+	}
+
+	return 0;
+}
+
+int NavioSysRCInput::start()
+{
+	int result = navio_rc_init();
+
+	if (result != 0) {
+		PX4_WARN("error: RC initialization failed");
+		return -1;
+	}
+
+	_should_exit.store(false);
+	ScheduleOnInterval(RCINPUT_MEASURE_INTERVAL_US);
+
+	return result;
+}
+
+void NavioSysRCInput::stop()
+{
+	_should_exit.store(true);
+}
+
+void NavioSysRCInput::Run()
+{
+	if (_should_exit.load()) {
+		ScheduleClear();
+		return;
+	}
+
+	uint64_t ts = 0;
+	char buf[12] {};
+
+	for (int i = 0; i < _channels; ++i) {
+		int res;
+
+		if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
+			_data.values[i] = UINT16_MAX;
+			continue;
+		}
+
+		ts = hrt_absolute_time();
+
+		buf[sizeof(buf) - 1] = '\0';
+
+		_data.values[i] = atoi(buf);
+	}
+
+	if (ts > 0) {
+		_data.timestamp_last_signal = ts;
+		_data.channel_count = _channels;
+		_data.rssi = 100;
+		_data.rc_lost_frame_count = 0;
+		_data.rc_total_frame_count = 1;
+		_data.rc_ppm_frame_length = 100;
+		_data.rc_failsafe = false;
+		_data.rc_lost = false;
+		_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
+		_data.timestamp = hrt_absolute_time();
+
+		_input_rc_pub.publish(_data);
+	}
+}
+
+}; // namespace navio_sysfs_rc_in

boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <drivers/drv_hrt.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/input_rc.h>
+
+namespace navio_sysfs_rc_in
+{
+
+class NavioSysRCInput : public px4::ScheduledWorkItem
+{
+public:
+	NavioSysRCInput();
+	~NavioSysRCInput() override;
+
+	/* @return 0 on success, -errno on failure */
+	int start();
+
+	/* @return 0 on success, -errno on failure */
+	void stop();
+
+	bool isRunning() { return _isRunning; }
+
+private:
+	void Run() override;
+
+	int navio_rc_init();
+
+	px4::atomic_bool _should_exit{false};
+
+	bool _isRunning{false};
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	int _channels{8};	// D8R-II plus
+	int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+
+	input_rc_s _data{};
+
+};
+
+}; // namespace navio_sysfs_rc_in

boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp

@@ -32,179 +32,15 @@
  *
  ****************************************************************************/
 
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <string.h>
-#include <stdlib.h>
-
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/workqueue.h>
+#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/defines.h>
 
-#include <drivers/drv_hrt.h>
+#include "NavioSysRCInput.hpp"
-
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/input_rc.h>
 
 namespace navio_sysfs_rc_in
 {
 
-extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]);
+static void usage(const char *reason)
-
-#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin"
-
-#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds
-
-
-class RcInput
-{
-public:
-	RcInput() = default;
-
-	~RcInput()
-	{
-		work_cancel(HPWORK, &_work);
-		_isRunning = false;
-	}
-
-	/* @return 0 on success, -errno on failure */
-	int start();
-
-	/* @return 0 on success, -errno on failure */
-	void stop();
-
-	/* Trampoline for the work queue. */
-	static void cycle_trampoline(void *arg);
-
-	bool isRunning() { return _isRunning; }
-
-private:
-	void _cycle();
-	void _measure();
-
-	int navio_rc_init();
-
-	bool _shouldExit{false};
-	bool _isRunning{false};
-	work_s _work{};
-
-	uORB::PublicationMulti<input_rc_s>	_rcinput_pub{ORB_ID(input_rc)};
-
-	int _channels{8};	// D8R-II plus
-	int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
-
-	input_rc_s _data{};
-
-};
-
-int RcInput::navio_rc_init()
-{
-	int i;
-	char buf[64];
-
-	for (i = 0; i < _channels; ++i) {
-		::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
-		int fd = ::open(buf, O_RDONLY);
-
-		if (fd < 0) {
-			PX4_WARN("error: open %d failed", i);
-			break;
-		}
-
-		_ch_fd[i] = fd;
-	}
-
-	for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
-		_data.values[i] = UINT16_MAX;
-	}
-
-	return 0;
-}
-
-int RcInput::start()
-{
-	int result = navio_rc_init();
-
-	if (result != 0) {
-		PX4_WARN("error: RC initialization failed");
-		return -1;
-	}
-
-	_isRunning = true;
-	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);
-
-	if (result == -1) {
-		_isRunning = false;
-	}
-
-	return result;
-}
-
-void RcInput::stop()
-{
-	_shouldExit = true;
-}
-
-void RcInput::cycle_trampoline(void *arg)
-{
-	RcInput *dev = static_cast<RcInput *>(arg);
-	dev->_cycle();
-}
-
-void RcInput::_cycle()
-{
-	_measure();
-
-	if (!_shouldExit) {
-		work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this,
-			   USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
-	}
-}
-
-void RcInput::_measure(void)
-{
-	uint64_t ts;
-	char buf[12];
-
-	for (int i = 0; i < _channels; ++i) {
-		int res;
-
-		if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
-			_data.values[i] = UINT16_MAX;
-			continue;
-		}
-
-		buf[sizeof(buf) - 1] = '\0';
-
-		_data.values[i] = atoi(buf);
-	}
-
-	ts = hrt_absolute_time();
-	_data.timestamp = ts;
-	_data.timestamp_last_signal = ts;
-	_data.channel_count = _channels;
-	_data.rssi = 100;
-	_data.rc_lost_frame_count = 0;
-	_data.rc_total_frame_count = 1;
-	_data.rc_ppm_frame_length = 100;
-	_data.rc_failsafe = false;
-	_data.rc_lost = false;
-	_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
-
-	_rcinput_pub.publish(_data);
-}
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
 {
 	if (reason) {
 		PX4_ERR("%s", reason);
@@ -213,9 +49,9 @@ usage(const char *reason)
 	PX4_INFO("usage: navio_sysfs_rc_in {start|stop|status}");
 }
 
-static RcInput *rc_input = nullptr;
+static NavioSysRCInput *rc_input = nullptr;
 
-int navio_sysfs_rc_in_main(int argc, char *argv[])
+extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage("missing command");
@@ -230,7 +66,7 @@ int navio_sysfs_rc_in_main(int argc, char *argv[])
 			return 0;
 		}
 
-		rc_input = new RcInput();
+		rc_input = new NavioSysRCInput();
 
 		// Check if alloc worked.
 		if (rc_input == nullptr) {