From ccd90ede7a8448ac471148b0b82a2cd5c10d7ff5 Mon Sep 17 00:00:00 2001 From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com> Date: Wed, 19 Oct 2022 15:15:12 +0200 Subject: [PATCH] Added io_bypass_control daemon to control IO directly from uORB useful RTPS full offboard (#20422) --- .../io_bypass_control/CMakeLists.txt | 42 ++++++ src/systemcmds/io_bypass_control/Kconfig | 14 ++ .../io_bypass_control/io_bypass_control.cpp | 123 +++++++++++++++++ .../io_bypass_control_main.cpp | 54 ++++++++ .../io_bypass_control_test.cpp | 112 ++++++++++++++++ .../io_bypass_control/io_controller.cpp | 125 ++++++++++++++++++ .../io_bypass_control/io_controller.hpp | 78 +++++++++++ 7 files changed, 548 insertions(+) create mode 100644 src/systemcmds/io_bypass_control/CMakeLists.txt create mode 100644 src/systemcmds/io_bypass_control/Kconfig create mode 100644 src/systemcmds/io_bypass_control/io_bypass_control.cpp create mode 100644 src/systemcmds/io_bypass_control/io_bypass_control_main.cpp create mode 100644 src/systemcmds/io_bypass_control/io_bypass_control_test.cpp create mode 100644 src/systemcmds/io_bypass_control/io_controller.cpp create mode 100644 src/systemcmds/io_bypass_control/io_controller.hpp diff --git a/src/systemcmds/io_bypass_control/CMakeLists.txt b/src/systemcmds/io_bypass_control/CMakeLists.txt new file mode 100644 index 0000000000..65668265c2 --- /dev/null +++ b/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 + ) diff --git a/src/systemcmds/io_bypass_control/Kconfig b/src/systemcmds/io_bypass_control/Kconfig new file mode 100644 index 0000000000..4a50afd948 --- /dev/null +++ b/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 diff --git a/src/systemcmds/io_bypass_control/io_bypass_control.cpp b/src/systemcmds/io_bypass_control/io_bypass_control.cpp new file mode 100644 index 0000000000..4e3c3b4a4e --- /dev/null +++ b/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 +#include +#include +#include + +#include + +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 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; +} diff --git a/src/systemcmds/io_bypass_control/io_bypass_control_main.cpp b/src/systemcmds/io_bypass_control/io_bypass_control_main.cpp new file mode 100644 index 0000000000..3f757749c5 --- /dev/null +++ b/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 +#include +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "IOController"); + + IOController io(MODULE_NAME, px4::wq_configurations::hp_default); + + return 0; +} diff --git a/src/systemcmds/io_bypass_control/io_bypass_control_test.cpp b/src/systemcmds/io_bypass_control/io_bypass_control_test.cpp new file mode 100644 index 0000000000..40d12e6cce --- /dev/null +++ b/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 +#include +#include +#include + +#include + +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 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; +} diff --git a/src/systemcmds/io_bypass_control/io_controller.cpp b/src/systemcmds/io_bypass_control/io_controller.cpp new file mode 100644 index 0000000000..3732a2a6b5 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include + +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); + } + } + + } + + +} diff --git a/src/systemcmds/io_bypass_control/io_controller.hpp b/src/systemcmds/io_bypass_control/io_controller.hpp new file mode 100644 index 0000000000..2b88c6a7c7 --- /dev/null +++ b/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 +#include +#include +#include + +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 */ +};