Added io_bypass_control daemon to control IO directly from uORB useful RTPS full offboard (#20422)

src/systemcmds/io_bypass_control/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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 systemcmds__io_bypass_control
+	MAIN io_bypass_control
+	COMPILE_FLAGS
+	SRCS
+        io_bypass_control_main.cpp
+		io_bypass_control.cpp
+		io_controller.cpp
+	DEPENDS
+	)

src/systemcmds/io_bypass_control/Kconfig

@@ -0,0 +1,14 @@
+menuconfig SYSTEMCMDS_IO_BYPASS_CONTROL
+	bool "IO Bypass control deamon"
+	default n
+	---help---
+		Simple daemon that listens uORB actuator_outputs to control PWM output
+		Useful for full offboard control using RTPS.
+		WARNING: No mixer, hence no safety use at your own risk
+
+menuconfig USER_IO_BYPASS_CONTROL
+	bool "io_bypass_control running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && SYSTEMCMDS_SS_IO_TIMER_TEST
+	---help---
+		Put io_bypass_control in userspace memory

src/systemcmds/io_bypass_control/io_bypass_control.cpp

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file io_bypass_control.cpp
+ *
+ * Simple daemon that listens uORB actuator_outputs to control PWM output
+ * WARNING: No mixer, hence no safety use at your own risk
+ */
+
+#include "io_controller.hpp"
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/getopt.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <poll.h>
+
+extern "C" __EXPORT int io_bypass_control_main(int argc, char *argv[]);
+
+
+void print_help()
+{
+	PX4_WARN("usage: io_bypass_control {start|test|stop|rate|status}");
+	printf("Simple daemon that listens uORB actuator_outputs to control PWM output\n"
+	       "WARNING: No mixer, hence no safety use at your own risk\n"
+	       "Useful for full offboard control using RTPS.\n");
+}
+
+int io_bypass_control_main(int argc, char *argv[])
+{
+
+	if (argc < 2 || !strcmp(argv[1], "help")) {
+		print_help();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (IOController::instance()) {
+			PX4_ERR("already started");
+			return 1;
+		}
+
+		return IOController::start();
+	}
+
+	/* commands below require the app to be started */
+	IOController *const inst = IOController::instance();
+
+	if (!inst) {
+		PX4_ERR("application not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete inst;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		inst->print_info();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "test")) {
+		uORB::Publication<actuator_outputs_s> publisher{ORB_ID(actuator_outputs)};
+		publisher.advertise();
+
+		for (float i = -1; i < 1; i = i + 0.01f) {
+			actuator_outputs_s actuator_outputs{};
+			actuator_outputs.timestamp = hrt_absolute_time();
+			actuator_outputs.noutputs = DIRECT_PWM_OUTPUT_CHANNELS;
+
+			for (int j = 0; j < DIRECT_PWM_OUTPUT_CHANNELS; ++j) {
+				actuator_outputs.output[j] = i;
+			}
+
+			publisher.publish(actuator_outputs);
+			px4_usleep(10000);
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "rate")) {
+		inst->setRate(atoi(argv[2]));
+		return 0;
+	}
+
+	print_help();
+	return 1;
+}

src/systemcmds/io_bypass_control/io_bypass_control_main.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ss_io_timer_test.c
+ *
+ * Simple daemon that listens uORB actuator_outputs to control PWM output
+ * WARNING: No mixer, hence no safety use at your own risk
+ */
+
+#include "io_controller.hpp"
+
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/init.h>
+#include <stdio.h>
+
+int PX4_MAIN(int argc, char **argv)
+{
+	px4::init(argc, argv, "IOController");
+
+	IOController io(MODULE_NAME, px4::wq_configurations::hp_default);
+
+	return 0;
+}

src/systemcmds/io_bypass_control/io_bypass_control_test.cpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ss_io_timer_test.c
+ *
+ * Simple daemon that listens uORB actuator_outputs to control PWM output
+ * WARNING: No mixer, hence no safety use at your own risk
+ */
+
+#include "io_tester.h"
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/getopt.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_test.h>
+
+#include <poll.h>
+
+extern "C" __EXPORT int ss_io_timer_test_main(int argc, char *argv[]);
+
+int ss_io_timer_test_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		PX4_WARN("usage: ss_io_timer {start|test|stop|status}\n");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (IOTester::instance()) {
+			PX4_ERR("already started");
+			return 1;
+		}
+
+		return IOTester::start();
+	}
+
+	/* commands below require the app to be started */
+	IOTester *const inst = IOTester::instance();
+
+	if (!inst) {
+		PX4_ERR("application not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete inst;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		inst->print_info();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "test")) {
+		uORB::Publication<actuator_test_s> publisher{ORB_ID(actuator_test)};
+		publisher.advertise();
+
+		for (int i = 0; i < 1500; i++) {
+			actuator_test_s actuator_test{};
+			actuator_test.timestamp = hrt_absolute_time();
+			actuator_test.value = i;
+			actuator_test.action = actuator_test_s::ACTION_DO_CONTROL;
+			actuator_test.timeout_ms = 0;
+
+			for (int j = 0; j < actuator_test_s::MAX_NUM_MOTORS; ++j) {
+				actuator_test.function = actuator_test_s::FUNCTION_MOTOR1 + j;
+				publisher.publish(actuator_test);
+				px4_usleep(100);
+			}
+
+			px4_usleep(10000);
+		}
+
+		return 0;
+	}
+
+	PX4_WARN("usage: ss_io_timer {start|test|stop|status}\n");
+	return 1;
+}

src/systemcmds/io_bypass_control/io_controller.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file io_controller.cpp
+ * Simple daemon that listens uORB actuator_outputs to control PWM output
+ *
+ */
+
+#include "io_controller.hpp"
+#include <px4_platform_common/time.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <px4_arch/io_timer.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <px4_platform_common/log.h>
+#include <lib/perf/perf_counter.h>
+
+using namespace time_literals;
+
+IOController *IOController::_instance;
+
+
+int IOController::start()
+{
+	if (_instance != nullptr) {
+		PX4_WARN("Already started");
+		return -1;
+	}
+
+	_instance = new IOController(MODULE_NAME, px4::wq_configurations::hp_default);
+
+	if (_instance == nullptr) {
+		PX4_ERR("Out of memory");
+		return -1;
+	}
+
+	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
+
+	return PX4_OK;
+}
+
+void IOController::print_info()
+{
+	PX4_INFO("Rate %li", _rate);
+
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; i++) {
+		PX4_INFO("Channel %i %i", i, up_pwm_servo_get(i));
+	}
+}
+
+void IOController::setRate(uint32_t rate)
+{
+	_rate = rate;
+
+	for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+		up_pwm_servo_set_rate_group_update(timer, _rate);
+	}
+}
+
+IOController::IOController(const char *name, const px4::wq_config_t &config) :
+	px4::ScheduledWorkItem(name, config)
+{
+	up_pwm_servo_init(_pwm_mask);
+	up_pwm_servo_arm(1, _pwm_mask);
+}
+
+void IOController::Run()
+{
+
+	actuator_outputs_s actuator_outputs;
+
+	while (_actuator_outputs_sub.update(&actuator_outputs)) {
+
+		if (actuator_outputs.timestamp == 0 ||
+		    hrt_elapsed_time(&actuator_outputs.timestamp) > 100_ms) {
+			continue;
+		}
+
+		if (actuator_outputs.noutputs <= DIRECT_PWM_OUTPUT_CHANNELS) {
+			/* Convert value to duty */
+			for (uint32_t i = 0; i < actuator_outputs.noutputs; i++) {
+				uint32_t setpoint = ((actuator_outputs.output[i] + 1) *
+						     (1.0f / (float)(_rate * 2)) * 1000000);
+
+				up_pwm_servo_set(i, setpoint);
+				up_pwm_update(i);
+			}
+		}
+
+	}
+
+
+}

src/systemcmds/io_bypass_control/io_controller.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file io_controller.h
+ * Simple IOController class
+ *
+ */
+#pragma once
+
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/Subscription.hpp>
+
+class IOController : public px4::ScheduledWorkItem
+{
+public:
+	IOController(const char *name, const px4::wq_config_t &config);
+
+	~IOController() {_instance = nullptr;}
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+	static IOController *instance() { return _instance; }
+
+	void print_info();
+
+	static int start();
+
+	void setRate(uint32_t rate);
+
+	uint32_t getRate(uint32_t rate)
+	{
+		return _rate;
+	}
+private:
+	void Run() override;
+	uORB::Subscription _actuator_outputs_sub{ORB_ID(actuator_outputs)};
+
+	const uint32_t _pwm_mask = ((1u << DIRECT_PWM_OUTPUT_CHANNELS) - 1);
+
+	static constexpr unsigned ScheduleIntervalMs = 1;
+
+	static IOController *_instance;
+
+	uint32_t _rate = 400; /* in Hz */
+};