From 5f1121c216c254a6bb7e752999ab1b9d2a0833d9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 8 Jan 2020 13:00:59 -0500 Subject: [PATCH] rc_input: move to PX4 WQ --- src/drivers/rc_input/RCInput.cpp | 160 +++++++------------------------ src/drivers/rc_input/RCInput.hpp | 31 ++---- 2 files changed, 42 insertions(+), 149 deletions(-) diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 5821c60cbd..e851280fbf 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -41,10 +41,10 @@ using namespace time_literals; static bool bind_spektrum(int arg); #endif /* SPEKTRUM_POWER */ -work_s RCInput::_work = {}; constexpr char const *RCInput::RC_SCAN_STRING[]; -RCInput::RCInput(bool run_as_task, char *device) : +RCInput::RCInput(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")) { @@ -75,9 +75,7 @@ RCInput::~RCInput() dsm_deinit(); #endif - if (_crsf_telemetry) { - delete (_crsf_telemetry); - } + delete _crsf_telemetry; perf_free(_cycle_perf); perf_free(_publish_interval_perf); @@ -120,7 +118,6 @@ RCInput::init() int RCInput::task_spawn(int argc, char *argv[]) { - bool run_as_task = false; bool error_flag = false; int myoptind = 1; @@ -128,12 +125,8 @@ RCInput::task_spawn(int argc, char *argv[]) const char *myoptarg = nullptr; const char *device = RC_SERIAL_PORT; - while ((ch = px4_getopt(argc, argv, "td:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 't': - run_as_task = true; - break; - case 'd': device = myoptarg; break; @@ -153,71 +146,21 @@ RCInput::task_spawn(int argc, char *argv[]) return -1; } + RCInput *instance = new RCInput(device); - if (!run_as_task) { - - /* schedule a cycle to start things */ - int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline_init, (void *)device, 0); - - if (ret < 0) { - return ret; - } - - // we need to wait, otherwise 'device' could go out of scope while still being accessed - wait_until_running(); - - _task_id = task_id_is_work_queue; - - } else { - - /* start the IO interface task */ - - const char *const args[] = { device, nullptr }; - _task_id = px4_task_spawn_cmd("rc_input", - SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, - 1000, - (px4_main_t)&run_trampoline, - (char *const *)args); - - if (_task_id < 0) { - _task_id = -1; - return -errno; - } + 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; } -void -RCInput::cycle_trampoline_init(void *arg) -{ - RCInput *dev = new RCInput(false, (char *)arg); - - if (!dev) { - PX4_ERR("alloc failed"); - return; - } - - int ret = dev->init(); - - if (ret != 0) { - PX4_ERR("init failed (%i)", ret); - delete dev; - return; - } - - _object.store(dev); - - dev->cycle(); -} -void -RCInput::cycle_trampoline(void *arg) -{ - RCInput *dev = static_cast(arg); - dev->cycle(); -} - void RCInput::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -286,7 +229,7 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, #ifdef RC_SERIAL_PORT void RCInput::set_rc_scan_state(RC_SCAN newState) { -// PX4_WARN("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]); + PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]); _rc_scan_begin = 0; _rc_scan_state = newState; } @@ -301,24 +244,23 @@ void RCInput::rc_io_invert(bool invert) } #endif -void -RCInput::run() +void RCInput::Run() { - int ret = init(); - - if (ret != 0) { - PX4_ERR("init failed (%i)", ret); + if (should_exit()) { exit_and_cleanup(); return; } - cycle(); -} + if (!_initialized) { + if (init() == PX4_OK) { + _initialized = true; -void -RCInput::cycle() -{ - while (true) { + } else { + PX4_ERR("init failed"); + exit_and_cleanup(); + } + + } else { perf_begin(_cycle_perf); @@ -403,17 +345,13 @@ RCInput::cycle() int newBytes = 0; - if (_run_as_task) { - // TODO: needs work (poll _rcs_fd) - // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100); - // then update priority to SCHED_PRIORITY_FAST_DRIVER - // read all available data from the serial RC input UART - newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + // TODO: needs work (poll _rcs_fd) + // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100); + // then update priority to SCHED_PRIORITY_FAST_DRIVER + // read all available data from the serial RC input UART - } else { - // read all available data from the serial RC input UART - newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); - } + // read all available data from the serial RC input UART + newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); switch (_rc_scan_state) { case RC_SCAN_SBUS: @@ -677,23 +615,6 @@ RCInput::cycle() } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1_s)) { _rc_scan_locked = false; } - - if (_run_as_task) { - if (should_exit()) { - break; - } - - } else { - if (should_exit()) { - exit_and_cleanup(); - - } else { - /* schedule next cycle */ - work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, this, USEC2TICK(_current_update_interval)); - } - - break; - } } } @@ -738,12 +659,6 @@ bool bind_spektrum(int arg) } #endif /* SPEKTRUM_POWER */ -RCInput *RCInput::instantiate(int argc, char *argv[]) -{ - // No arguments to parse. We also know that we should run as task - return new RCInput(true, argv[0]); -} - int RCInput::custom_command(int argc, char *argv[]) { #if defined(SPEKTRUM_POWER) @@ -770,11 +685,9 @@ int RCInput::custom_command(int argc, char *argv[]) int RCInput::print_status() { - PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue")); + PX4_INFO("Running"); - if (!_run_as_task) { - PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval); - } + PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval); if (_device[0] != '\0') { PX4_INFO("Serial device: %s", _device); @@ -816,15 +729,10 @@ This module does the RC input parsing and auto-selecting the method. Supported m - ST24 - TBS Crossfire (CRSF) -### Implementation -By default the module runs on the work queue, to reduce RAM usage. It can also be run in its own thread, -specified via start flag -t, to reduce latency. -When running on the work queue, it schedules at a fixed frequency. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rc_input", "driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); - PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true); + PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); #if defined(SPEKTRUM_POWER) diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 00b0de6efc..1b42dc36e5 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2019 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 @@ -48,9 +48,9 @@ #include #include #include -#include -#include +#include #include +#include #include #include #include @@ -61,19 +61,16 @@ # include #endif -class RCInput : public ModuleBase +class RCInput : public ModuleBase, public px4::ScheduledWorkItem { public: - RCInput(bool run_as_task, char *device); + RCInput(const char *device); virtual ~RCInput(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static RCInput *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); @@ -81,12 +78,7 @@ public: static int print_usage(const char *reason = nullptr); /** @see ModuleBase::run() */ - void run() override; - - /** - * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle - */ - void cycle(); + void Run() override; /** @see ModuleBase::print_status() */ int print_status() override; @@ -114,14 +106,11 @@ private: hrt_abstime _rc_scan_begin{0}; + bool _initialized{false}; bool _rc_scan_locked{false}; bool _report_lock{true}; - unsigned _current_update_interval{4000}; - - bool _run_as_task{false}; - - static struct work_s _work; + static constexpr unsigned _current_update_interval{4000}; // 250 Hz uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _adc_sub{ORB_ID(adc_report)}; @@ -146,10 +135,6 @@ private: perf_counter_t _cycle_perf; perf_counter_t _publish_interval_perf; - static void cycle_trampoline(void *arg); - static void cycle_trampoline_init(void *arg); - int start(); - void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe,