diff --git a/src/drivers/linux_sbus/linux_sbus.cpp b/src/drivers/linux_sbus/linux_sbus.cpp index b075832480..5cbff31f49 100644 --- a/src/drivers/linux_sbus/linux_sbus.cpp +++ b/src/drivers/linux_sbus/linux_sbus.cpp @@ -119,8 +119,8 @@ int RcInput::start(char *device, int channels) } _isRunning = true; - result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, - this, 0); + + ScheduleNow(); if (result == -1) { _isRunning = false; @@ -134,20 +134,14 @@ void RcInput::stop() close(_device_fd); _shouldExit = true; } + //---------------------------------------------------------------------------------------------------------// -void RcInput::cycle_trampoline(void *arg) -{ - RcInput *dev = reinterpret_cast(arg); - dev->_cycle(); -} -//---------------------------------------------------------------------------------------------------------// -void RcInput::_cycle() +void RcInput::Run() { _measure(); if (!_shouldExit) { - work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this, - USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); + ScheduleDelayed(RCINPUT_MEASURE_INTERVAL_US); } } //---------------------------------------------------------------------------------------------------------// diff --git a/src/drivers/linux_sbus/linux_sbus.h b/src/drivers/linux_sbus/linux_sbus.h index e650a92914..be51d1db08 100644 --- a/src/drivers/linux_sbus/linux_sbus.h +++ b/src/drivers/linux_sbus/linux_sbus.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -66,13 +66,13 @@ namespace linux_sbus { -class RcInput +class RcInput : public px4::ScheduledWorkItem { public: RcInput() : + ScheduledWorkItem(px4::wq_configurations::hp_default), _shouldExit(false), _isRunning(false), - _work { }, _rcinput_pub(nullptr), _data { }, _sbusData { 0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, @@ -82,7 +82,7 @@ public: { } ~RcInput() { - work_cancel(HPWORK, &_work); + ScheduleClear(); _isRunning = false; close(_device_fd); } @@ -90,20 +90,16 @@ public: int start(char *device, int channels); void stop(); - /** Trampoline for the work queue. */ - static void cycle_trampoline(void *arg); - bool isRunning() { return _isRunning; } private: - void _cycle(); + void Run() override; void _measure(); bool _shouldExit; bool _isRunning; - struct work_s _work; orb_advert_t _rcinput_pub; struct input_rc_s _data; uint8_t _sbusData[25];