diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt index 7686770292..52d4350715 100644 --- a/src/modules/load_mon/CMakeLists.txt +++ b/src/modules/load_mon/CMakeLists.txt @@ -38,5 +38,6 @@ px4_add_module( SRCS load_mon.cpp DEPENDS + px4_work_queue ) diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp index 42a903261c..e561927f53 100644 --- a/src/modules/load_mon/load_mon.cpp +++ b/src/modules/load_mon/load_mon.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include @@ -68,7 +68,7 @@ extern "C" __EXPORT int load_mon_main(int argc, char *argv[]); // Run it at 1 Hz. const unsigned LOAD_MON_INTERVAL_US = 1000000; -class LoadMon : public ModuleBase, public ModuleParams +class LoadMon : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: LoadMon(); @@ -88,12 +88,11 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - /** Trampoline for the work queue. */ - static void cycle_trampoline(void *arg); + void start(); private: /** Do a compute and schedule the next cycle. */ - void _cycle(); + void Run() override; /** Do a calculation of the CPU load and publish it. */ void _cpuload(); @@ -113,8 +112,6 @@ private: (ParamBool) _param_sys_stck_en ) - work_s _work{}; - orb_advert_t _cpuload_pub{nullptr}; hrt_abstime _last_idle_time{0}; @@ -125,12 +122,15 @@ private: LoadMon::LoadMon() : ModuleParams(nullptr), + ScheduledWorkItem(px4::wq_configurations::lp_default), _stack_perf(perf_alloc(PC_ELAPSED, "stack_check")) { } LoadMon::~LoadMon() { + ScheduleClear(); + perf_free(_stack_perf); } @@ -143,28 +143,22 @@ int LoadMon::task_spawn(int argc, char *argv[]) return -1; } - /* Schedule a cycle to start things. */ - int ret = work_queue(LPWORK, &obj->_work, (worker_t)&LoadMon::cycle_trampoline, obj, 0); - - if (ret < 0) { - delete obj; - return ret; - } - _object.store(obj); _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + return 0; } void -LoadMon::cycle_trampoline(void *arg) +LoadMon::start() { - LoadMon *dev = reinterpret_cast(arg); - - dev->_cycle(); + ScheduleOnInterval(LOAD_MON_INTERVAL_US); } -void LoadMon::_cycle() +void LoadMon::Run() { _cpuload(); @@ -176,11 +170,8 @@ void LoadMon::_cycle() #endif - if (!should_exit()) { - work_queue(LPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, - USEC2TICK(LOAD_MON_INTERVAL_US)); - - } else { + if (should_exit()) { + ScheduleClear(); exit_and_cleanup(); } }