From 79a61372c8600d19d336055c16e06d6838c5793b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 12:40:21 -0400 Subject: [PATCH 1/7] PX4 work queue: extend to UARTs --- .../px4_work_queue/WorkQueueManager.hpp | 21 +++++++++- .../px4_work_queue/WorkQueueManager.cpp | 39 +++++++++++++++++++ 2 files changed, 59 insertions(+), 1 deletion(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 98a21e77bcba..048a036f6c14 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -66,9 +66,20 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1400, -12}; static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors +static constexpr wq_config_t hp_default{"wq:hp_default", 1800, -12}; + static constexpr wq_config_t uavcan{"uavcan", 2400, -13}; -static constexpr wq_config_t hp_default{"wq:hp_default", 1800, -12}; +static constexpr wq_config_t UART0{"wq:UART0", 1400, -14}; +static constexpr wq_config_t UART1{"wq:UART1", 1400, -15}; +static constexpr wq_config_t UART2{"wq:UART2", 1400, -16}; +static constexpr wq_config_t UART3{"wq:UART3", 1400, -17}; +static constexpr wq_config_t UART4{"wq:UART4", 1400, -18}; +static constexpr wq_config_t UART5{"wq:UART5", 1400, -19}; +static constexpr wq_config_t UART6{"wq:UART6", 1400, -20}; +static constexpr wq_config_t UART7{"wq:UART7", 1400, -21}; +static constexpr wq_config_t UART8{"wq:UART8", 1400, -22}; + static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; static constexpr wq_config_t test1{"wq:test1", 800, 0}; @@ -107,5 +118,13 @@ WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq); */ const wq_config_t &device_bus_to_wq(uint32_t device_id); +/** + * Map a serial device path (eg /dev/ttyS1) to a work queue. + * + * @param device_id The device path. + * @return A work queue configuration. + */ +const wq_config_t &serial_port_to_wq(const char *serial); + } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 5f7c733aea98..f0fb3a093c8a 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -162,6 +162,45 @@ device_bus_to_wq(uint32_t device_id_int) return wq_configurations::hp_default; }; +const wq_config_t & +serial_port_to_wq(const char *serial) +{ + if (serial == nullptr) { + return wq_configurations::hp_default; + + } else if (strstr(serial, "ttyS0")) { + return wq_configurations::UART0; + + } else if (strstr(serial, "ttyS1")) { + return wq_configurations::UART1; + + } else if (strstr(serial, "ttyS2")) { + return wq_configurations::UART2; + + } else if (strstr(serial, "ttyS3")) { + return wq_configurations::UART3; + + } else if (strstr(serial, "ttyS4")) { + return wq_configurations::UART4; + + } else if (strstr(serial, "ttyS5")) { + return wq_configurations::UART5; + + } else if (strstr(serial, "ttyS6")) { + return wq_configurations::UART6; + + } else if (strstr(serial, "ttyS7")) { + return wq_configurations::UART7; + + } else if (strstr(serial, "ttyS8")) { + return wq_configurations::UART8; + } + + PX4_ERR("unknown serial port: %s", serial); + + return wq_configurations::hp_default; +} + static void * WorkQueueRunner(void *context) { From f14c036554d410ac61026fb97910e8f3dea68ad6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 12:41:04 -0400 Subject: [PATCH 2/7] rc_input: move to new PX4 WQ (per UART) --- boards/airmind/mindpx-v2/src/board_config.h | 1 - boards/px4/fmu-v4/src/board_config.h | 1 - boards/uvify/core/src/board_config.h | 1 - .../px4_platform_common/board_common.h | 50 ----- src/drivers/rc_input/RCInput.cpp | 212 ++++-------------- src/drivers/rc_input/RCInput.hpp | 47 ++-- 6 files changed, 64 insertions(+), 248 deletions(-) diff --git a/boards/airmind/mindpx-v2/src/board_config.h b/boards/airmind/mindpx-v2/src/board_config.h index ceaa70840f70..f7acf98ebc6f 100644 --- a/boards/airmind/mindpx-v2/src/board_config.h +++ b/boards/airmind/mindpx-v2/src/board_config.h @@ -271,7 +271,6 @@ // #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) -#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true); #define GPIO_FRSKY_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) #define INVERT_FRSKY(_invert_true) px4_arch_gpiowrite(GPIO_FRSKY_INV, _invert_true); diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index 0c808db94034..2d15641babb7 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -279,7 +279,6 @@ /* For R12, this signal is active high. */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h index e1c8ae1d1448..60cb22d13f6c 100644 --- a/boards/uvify/core/src/board_config.h +++ b/boards/uvify/core/src/board_config.h @@ -293,7 +293,6 @@ /* For,this signal is active high. */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 9d87fcf9bdd9..b28623cf6d2c 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -487,56 +487,6 @@ static inline bool board_rc_singlewire(const char *device) { return strcmp(devic static inline bool board_rc_singlewire(const char *device) { return false; } #endif -/************************************************************************************ - * Name: board_rc_swap_rxtx - * - * Description: - * A board may define RC_SERIAL_SWAP_RXTX, so that RC_SERIAL_PORT is configured - * as UART with RX/TX swapped. - * - * Input Parameters: - * device: serial device, e.g. "/dev/ttyS0" - * - * Returned Value: - * true RX/RX should be swapped. - * false if not. - * - ************************************************************************************/ - -#if defined(RC_SERIAL_SWAP_RXTX) -static inline bool board_rc_swap_rxtx(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; } -#else -static inline bool board_rc_swap_rxtx(const char *device) { return false; } -#endif - -/************************************************************************************ - * Name: board_rc_invert_input - * - * Description: - * All boards may optionally define RC_INVERT_INPUT(bool invert) that is - * used to invert the RC_SERIAL_PORT RC port (e.g. to toggle an external XOR via - * GPIO). - * - * Input Parameters: - * invert_on - A positive logic value, that when true (on) will set the HW in - * inverted NRZ mode where a MARK will be 0 and SPACE will be a 1. - * - * Returned Value: - * true the UART inversion got set. - * - ************************************************************************************/ - -#ifdef RC_INVERT_INPUT -static inline bool board_rc_invert_input(const char *device, bool invert) -{ - if (strcmp(device, RC_SERIAL_PORT) == 0) { RC_INVERT_INPUT(invert); return true; } - - return false; -} -#else -static inline bool board_rc_invert_input(const char *device, bool invert) { return false; } -#endif - /* Provide an interface for reading the connected state of VBUS */ /************************************************************************************ diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 5821c60cbdee..adb46dbf4f6c 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")) { @@ -59,25 +59,17 @@ RCInput::RCInput(bool run_as_task, char *device) : _raw_rc_values[i] = UINT16_MAX; } -#ifdef RC_SERIAL_PORT - if (device) { strncpy(_device, device, sizeof(_device)); _device[sizeof(_device) - 1] = '\0'; } - -#endif } RCInput::~RCInput() { -#ifdef RC_SERIAL_PORT dsm_deinit(); -#endif - if (_crsf_telemetry) { - delete (_crsf_telemetry); - } + delete _crsf_telemetry; perf_free(_cycle_perf); perf_free(_publish_interval_perf); @@ -86,13 +78,6 @@ RCInput::~RCInput() int RCInput::init() { -#ifdef RC_SERIAL_PORT - -# ifdef RF_RADIO_POWER_CONTROL - // power radio on - RF_RADIO_POWER_CONTROL(true); -# endif - // dsm_init sets some file static variables and returns a file descriptor _rcs_fd = dsm_init(_device); @@ -100,18 +85,16 @@ RCInput::init() return -errno; } - if (board_rc_swap_rxtx(_device)) { - ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED); - } + ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED); // assume SBUS input and immediately switch it to // so that if Single wire mode on TX there will be only // a short contention sbus_config(_rcs_fd, board_rc_singlewire(_device)); -# ifdef GPIO_PPM_IN + +#if defined(GPIO_PPM_IN) // disable CPPM input by mapping it away from the timer capture input px4_arch_unconfiggpio(GPIO_PPM_IN); -# endif #endif return 0; @@ -120,7 +103,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 +110,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,69 +131,20 @@ 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; - } - } - - return PX4_OK; -} - -void -RCInput::cycle_trampoline_init(void *arg) -{ - RCInput *dev = new RCInput(false, (char *)arg); - - if (!dev) { + if (instance == nullptr) { PX4_ERR("alloc failed"); - return; + delete instance; + return PX4_ERROR; } - int ret = dev->init(); + _object.store(instance); + _task_id = task_id_is_work_queue; - if (ret != 0) { - PX4_ERR("init failed (%i)", ret); - delete dev; - return; - } + instance->ScheduleOnInterval(4_ms); // 250 Hz - _object.store(dev); - - dev->cycle(); -} -void -RCInput::cycle_trampoline(void *arg) -{ - RCInput *dev = static_cast(arg); - dev->cycle(); + return PX4_OK; } void @@ -283,10 +212,9 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, _rc_in.rc_total_frame_count = 0; } -#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; } @@ -295,30 +223,36 @@ void RCInput::rc_io_invert(bool invert) { // First check if the board provides a board-specific inversion method (e.g. via GPIO), // and if not use an IOCTL - if (!board_rc_invert_input(_device, invert)) { - ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0); + +#if defined(RC_SERIAL_PORT) && defined(GPIO_SBUS_INV) + if (strcmp(_device, RC_SERIAL_PORT) == 0) { + px4_arch_gpiowrite(GPIO_SBUS_INV, invert); + return; } + +#endif // GPIO_SBUS_INV + + ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0); } -#endif void -RCInput::run() +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); @@ -387,7 +321,6 @@ RCInput::cycle() bool rc_updated = false; -#ifdef RC_SERIAL_PORT // This block scans for a supported serial RC input and locks onto the first one found // Scan for 300 msec, then switch protocol constexpr hrt_abstime rc_scan_max = 300_ms; @@ -403,17 +336,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: @@ -652,21 +581,6 @@ RCInput::cycle() break; } -#else // RC_SERIAL_PORT not defined -#ifdef HRT_PPM_CHANNEL - - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - rc_updated = true; - fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0); - _rc_in.rc_ppm_frame_length = ppm_frame_length; - _rc_in.timestamp_last_signal = ppm_last_valid_decode; - } - -#endif // HRT_PPM_CHANNEL -#endif // RC_SERIAL_PORT - perf_end(_cycle_perf); if (rc_updated) { @@ -677,23 +591,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,13 +635,8 @@ 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[]) +int +RCInput::custom_command(int argc, char *argv[]) { #if defined(SPEKTRUM_POWER) const char *verb = argv[0]; @@ -768,13 +660,12 @@ int RCInput::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int RCInput::print_status() +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 +707,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 00b0de6efc09..88e2a9902b2e 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -35,7 +35,6 @@ #include -#include #include #include #include @@ -48,9 +47,9 @@ #include #include #include -#include -#include +#include #include +#include #include #include #include @@ -61,19 +60,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,17 +77,12 @@ class RCInput : public ModuleBase 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; - int init(); + int init(); private: enum RC_SCAN { @@ -114,42 +105,34 @@ class RCInput : public ModuleBase hrt_abstime _rc_scan_begin{0}; - bool _rc_scan_locked{false}; - bool _report_lock{true}; + 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; - - uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _adc_sub{ORB_ID(adc_report)}; + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; + uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; input_rc_s _rc_in{}; float _analog_rc_rssi_volt{-1.0f}; bool _analog_rc_rssi_stable{false}; - uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; - int _rcs_fd{-1}; char _device[20] {}; ///< device / serial port path - uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; + uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; - uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; - uint16_t _raw_rc_count{}; + uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t _raw_rc_count{}; - CRSFTelemetry *_crsf_telemetry{nullptr}; + CRSFTelemetry *_crsf_telemetry{nullptr}; 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, From ed265b4c15cf3f9e099f245508faa95651e05fb2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 14:05:29 -0400 Subject: [PATCH 3/7] cm8jl65: move to UART WQ --- src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 7ec9da90c438..4b591e3f4525 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -84,7 +84,7 @@ static constexpr unsigned char crc_lsb_vector[] = { }; CM8JL65::CM8JL65(const char *port, uint8_t rotation) : - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation) { // Store the port name. From 8a9faec0c961ae0be00dba381b0b2b2497854899 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 14:05:41 -0400 Subject: [PATCH 4/7] leddar_one: move to UART WQ --- src/drivers/distance_sensor/leddar_one/leddar_one.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp index 41041b825ce1..f8639f19da58 100644 --- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -174,7 +174,7 @@ class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation): CDev(device_path), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)), _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation) { _serial_port = strdup(serial_port); From ba22b82fca2cbc86329a68cbea8ed2bc9a6dd7d4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 14:05:57 -0400 Subject: [PATCH 5/7] sf0x: move to UART WQ --- src/drivers/distance_sensor/sf0x/sf0x.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index ad88bce251fe..0f9c94db26d0 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -158,7 +158,7 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); SF0X::SF0X(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _rotation(rotation), _min_distance(0.30f), _max_distance(40.0f), From 9e095ee4d27309fa232b421d1fc2bd018c27acf6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 14:06:06 -0400 Subject: [PATCH 6/7] tfmini: move to UART WQ --- src/drivers/distance_sensor/tfmini/TFMINI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index a361a11a8ff3..cefb74f2ab53 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -34,7 +34,7 @@ #include "TFMINI.hpp" TFMINI::TFMINI(const char *port, uint8_t rotation) : - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) { // store port name From 73d02025d7a5dffbbd358a7be078814a5ca1301a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Oct 2019 14:06:18 -0400 Subject: [PATCH 7/7] ulanding: move to UART WQ --- src/drivers/distance_sensor/ulanding/ulanding.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/ulanding/ulanding.cpp b/src/drivers/distance_sensor/ulanding/ulanding.cpp index 22807d4c2ef6..a8a7cd2ade3e 100644 --- a/src/drivers/distance_sensor/ulanding/ulanding.cpp +++ b/src/drivers/distance_sensor/ulanding/ulanding.cpp @@ -172,7 +172,7 @@ class Radar : public cdev::CDev, public px4::ScheduledWorkItem Radar::Radar(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _rotation(rotation) { /* store port name */