diff --git a/boards/cuav/x25-evo/core_heater/core_heater.cpp b/boards/cuav/x25-evo/core_heater/core_heater.cpp index 0656504cba..776bd1bd85 100644 --- a/boards/cuav/x25-evo/core_heater/core_heater.cpp +++ b/boards/cuav/x25-evo/core_heater/core_heater.cpp @@ -43,6 +43,8 @@ #include #include +ModuleBase::Descriptor Core_Heater::desc{task_spawn, custom_command, print_usage}; + # ifndef GPIO_CORE_HEATER_OUTPUT # error "To use the heater driver, the board_config.h must define and initialize GPIO_CORE_HEATER_OUTPUT" # endif @@ -62,7 +64,7 @@ Core_Heater::~Core_Heater() int Core_Heater::custom_command(int argc, char *argv[]) { // Check if the driver is running. - if (!is_running()) { + if (!is_running(desc)) { PX4_INFO("not running"); return PX4_ERROR; } @@ -117,7 +119,7 @@ bool Core_Heater::initialize_topics() void Core_Heater::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -216,8 +218,8 @@ int Core_Heater::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(core_heater); - _task_id = task_id_is_work_queue; + desc.object.store(core_heater); + desc.task_id = task_id_is_work_queue; core_heater->start(); return 0; @@ -257,5 +259,5 @@ Background process running periodically on the LP work queue to regulate IMU tem extern "C" __EXPORT int core_heater_main(int argc, char *argv[]) { - return Core_Heater::main(argc, argv); + return ModuleBase::main(Core_Heater::desc, argc, argv); } diff --git a/boards/cuav/x25-evo/core_heater/core_heater.h b/boards/cuav/x25-evo/core_heater/core_heater.h index 6ac34ad53a..d90ca6b410 100644 --- a/boards/cuav/x25-evo/core_heater/core_heater.h +++ b/boards/cuav/x25-evo/core_heater/core_heater.h @@ -56,9 +56,11 @@ using namespace time_literals; #define CONTROLLER_PERIOD_DEFAULT 10000 #define TEMPERATURE_TARGET_THRESHOLD 2.5f -class Core_Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class Core_Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + Core_Heater(); virtual ~Core_Heater(); diff --git a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp index 4cc8213537..755e588961 100644 --- a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp +++ b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp @@ -33,6 +33,8 @@ #include "NavioRGBLed.hpp" +ModuleBase::Descriptor NavioRGBLed::desc{task_spawn, custom_command, print_usage}; + NavioRGBLed::NavioRGBLed() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { @@ -130,8 +132,8 @@ int NavioRGBLed::task_spawn(int argc, char *argv[]) NavioRGBLed *instance = new NavioRGBLed(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -142,8 +144,8 @@ int NavioRGBLed::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -170,5 +172,5 @@ Emlid Navio2 RGB LED driver. extern "C" __EXPORT int navio_rgbled_main(int argc, char *argv[]) { - return NavioRGBLed::main(argc, argv); + return ModuleBase::main(NavioRGBLed::desc, argc, argv); } diff --git a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp index 01bcc26580..c62da16af0 100644 --- a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp +++ b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp @@ -40,9 +40,11 @@ #include -class NavioRGBLed : public ModuleBase, public px4::ScheduledWorkItem +class NavioRGBLed : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + NavioRGBLed(); ~NavioRGBLed() override; diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp index 9ad45c1389..eb35830e2e 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp @@ -49,6 +49,8 @@ using namespace time_literals; +ModuleBase::Descriptor GhstRc::desc{task_spawn, custom_command, print_usage}; + uint32_t GhstRc::baudrate = GHST_BAUDRATE; GhstRc::GhstRc(const char *device) : @@ -114,8 +116,8 @@ int GhstRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); @@ -174,7 +176,7 @@ void GhstRc::Run() if (should_exit()) { ScheduleClear(); _rc_fd = -1; - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -308,5 +310,5 @@ This module parses the GHST RC uplink protocol and can generate GHST downlink te extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) { - return GhstRc::main(argc, argv); + return ModuleBase::main(GhstRc::desc, argc, argv); } diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp index c9fe697e62..5f3de5cf3a 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp @@ -54,9 +54,11 @@ #define GHST_MAX_NUM_CHANNELS (16) -class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + GhstRc(const char *device); ~GhstRc() override; diff --git a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp index c075a07f46..ae02407462 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp @@ -59,6 +59,8 @@ #include "rc_controller.hpp" +ModuleBase::Descriptor RC_ControllerModule::desc{task_spawn, custom_command, print_usage}; + int RC_ControllerModule::print_status() { PX4_INFO("Running"); @@ -69,35 +71,35 @@ int RC_ControllerModule::print_status() int RC_ControllerModule::custom_command(int argc, char *argv[]) { - if (!is_running()) { + if (!is_running(desc)) { print_usage("not running"); return 1; } if (!strcmp(argv[0], "throttle")) { uint16_t val = atoi(argv[1]); - get_instance()->set_throttle(val); + get_instance(desc)->set_throttle(val); PX4_INFO("Setting throttle to %u", val); return 0; } if (!strcmp(argv[0], "yaw")) { uint16_t val = atoi(argv[1]); - get_instance()->set_yaw(val); + get_instance(desc)->set_yaw(val); PX4_INFO("Setting yaw to %u", val); return 0; } if (!strcmp(argv[0], "pitch")) { uint16_t val = atoi(argv[1]); - get_instance()->set_pitch(val); + get_instance(desc)->set_pitch(val); PX4_INFO("Setting pitch to %u", val); return 0; } if (!strcmp(argv[0], "roll")) { uint16_t val = atoi(argv[1]); - get_instance()->set_roll(val); + get_instance(desc)->set_roll(val); PX4_INFO("Setting roll to %u", val); return 0; } @@ -106,17 +108,24 @@ int RC_ControllerModule::custom_command(int argc, char *argv[]) } +int RC_ControllerModule::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + return RC_ControllerModule::instantiate(ac, av); + }, argc, argv); +} + int RC_ControllerModule::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("RC_ControllerModule", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1024, - (px4_main_t)&run_trampoline, - (char *const *)argv); + desc.task_id = px4_task_spawn_cmd("RC_ControllerModule", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (desc.task_id < 0) { + desc.task_id = -1; return -errno; } @@ -252,5 +261,5 @@ int RC_ControllerModule::print_usage(const char *reason) int rc_controller_main(int argc, char *argv[]) { - return RC_ControllerModule::main(argc, argv); + return ModuleBase::main(RC_ControllerModule::desc, argc, argv); } diff --git a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp index bfcaf40028..9891f5d8c7 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp @@ -41,9 +41,11 @@ extern "C" __EXPORT int rc_controller_main(int argc, char *argv[]); -class RC_ControllerModule : public ModuleBase, public ModuleParams +class RC_ControllerModule : public ModuleBase, public ModuleParams { public: + static Descriptor desc; + RC_ControllerModule(); virtual ~RC_ControllerModule() = default; @@ -51,6 +53,9 @@ public: /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); + /** @see ModuleBase */ + static int run_trampoline(int argc, char *argv[]); + /** @see ModuleBase */ static RC_ControllerModule *instantiate(int argc, char *argv[]); diff --git a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp index 5c8717db26..790901bc0e 100644 --- a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp +++ b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp @@ -41,6 +41,8 @@ using namespace std; +ModuleBase::Descriptor VoxlSaveCalParams::desc{task_spawn, custom_command, print_usage}; + static bool debug = false; VoxlSaveCalParams::VoxlSaveCalParams() : @@ -145,7 +147,7 @@ VoxlSaveCalParams::Run() { if (should_exit()) { _parameter_primary_set_value_request_sub.unregisterCallback(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -186,8 +188,8 @@ int VoxlSaveCalParams::task_spawn(int argc, char *argv[]) VoxlSaveCalParams *instance = new VoxlSaveCalParams(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -198,8 +200,8 @@ int VoxlSaveCalParams::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -230,5 +232,5 @@ This implements autosaving of calibration parameters on VOXL2 platform. extern "C" __EXPORT int voxl_save_cal_params_main(int argc, char *argv[]) { - return VoxlSaveCalParams::main(argc, argv); + return ModuleBase::main(VoxlSaveCalParams::desc, argc, argv); } diff --git a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp index 791bbf5073..cb98ba2ce1 100644 --- a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp +++ b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp @@ -46,10 +46,12 @@ using namespace time_literals; -class VoxlSaveCalParams : public ModuleBase, public ModuleParams, +class VoxlSaveCalParams : public ModuleBase, public ModuleParams, public px4::WorkItem { public: + static Descriptor desc; + VoxlSaveCalParams(); ~VoxlSaveCalParams() = default; diff --git a/platforms/common/include/px4_platform_common/module.h b/platforms/common/include/px4_platform_common/module.h index e64ae7e979..56f8b21d78 100644 --- a/platforms/common/include/px4_platform_common/module.h +++ b/platforms/common/include/px4_platform_common/module.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2025 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 @@ -33,23 +33,27 @@ /** * @file module.h + * + * Non-template base class for modules. Replaces the CRTP ModuleBase + * pattern with a descriptor-based approach that shares a single copy of the + * common static methods (main, start, stop, status, etc.) across all modules. */ #pragma once -#include -#include -#include - -#include -#include #include +#include #include #include +#include +#include +#include +#include + #ifdef __cplusplus -#include +#include /** * @brief This mutex protects against race conditions during startup & shutdown of modules. @@ -61,527 +65,153 @@ extern pthread_mutex_t px4_modules_mutex; /** * @class ModuleBase - * Base class for modules, implementing common functionality, + * Non-template base class for modules, implementing common functionality * such as 'start', 'stop' and 'status' commands. - * Currently does not support modules which allow multiple instances, - * such as mavlink. * - * The class is implemented as curiously recurring template pattern (CRTP). - * It allows to have a static object in the base class that is different for - * each module type, and call static methods from the base class. - * - * @note Required methods for a derived class: - * When running in its own thread: - * static int task_spawn(int argc, char *argv[]) - * { - * // call px4_task_spawn_cmd() with &run_trampoline as startup method - * // optional: wait until _object is not null, which means the task got initialized (use a timeout) - * // set _task_id and return 0 - * // on error return != 0 (and _task_id must be -1) - * } - * - * static T *instantiate(int argc, char *argv[]) - * { - * // this is called from within the new thread, from run_trampoline() - * // parse the arguments - * // create a new object T & return it - * // or return nullptr on error - * } - * - * static int custom_command(int argc, char *argv[]) - * { - * // support for custom commands - * // if none are supported, just do: - * return print_usage("unrecognized command"); - * } - * - * static int print_usage(const char *reason = nullptr) - * { - * // use the PRINT_MODULE_* methods... - * } - * - * When running on the work queue: - * - custom_command & print_usage as above - * static int task_spawn(int argc, char *argv[]) { - * // parse the arguments - * // set _object (here or from the work_queue() callback) - * // call work_queue() (with a custom cycle trampoline) - * // optional: wait until _object is not null, which means the task got initialized (use a timeout) - * // set _task_id to task_id_is_work_queue and return 0 - * // on error return != 0 (and _task_id must be -1) - * } + * Each module provides a static Descriptor instance that holds + * module-specific function pointers and per-module storage (object + * pointer and task ID). The shared static methods operate on the + * descriptor instead of CRTP template statics. */ -template class ModuleBase { public: - ModuleBase() : _task_should_exit{false} {} - virtual ~ModuleBase() {} + /** + * Per-module descriptor providing module-specific function pointers + * and storage (object pointer, task ID). + */ + struct Descriptor { + Descriptor(int (*ts)(int, char **), int (*cc)(int, char **), int (*pu)(const char *)) + : task_spawn(ts), custom_command(cc), print_usage(pu) {} + + int (*task_spawn)(int argc, char *argv[]); + int (*custom_command)(int argc, char *argv[]); + int (*print_usage)(const char *reason); + px4::atomic object{nullptr}; + int task_id{-1}; + }; + + ModuleBase() = default; + virtual ~ModuleBase() = default; /** - * @brief main Main entry point to the module that should be - * called directly from the module's main method. - * @param argc The task argument count. - * @param argc Pointer to the task argument variable array. - * @return Returns 0 iff successful, -1 otherwise. + * @brief Main entry point to the module that should be called directly + * from the module's main method. */ - static int main(int argc, char *argv[]) - { - if (argc <= 1 || - strcmp(argv[1], "-h") == 0 || - strcmp(argv[1], "help") == 0 || - strcmp(argv[1], "info") == 0 || - strcmp(argv[1], "usage") == 0) { - return T::print_usage(); - } - - if (strcmp(argv[1], "start") == 0) { - // Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument. - return start_command_base(argc - 1, argv + 1); - } - - if (strcmp(argv[1], "status") == 0) { - return status_command(); - } - - if (strcmp(argv[1], "stop") == 0) { - return stop_command(); - } - - lock_module(); // Lock here, as the method could access _object. - int ret = T::custom_command(argc - 1, argv + 1); - unlock_module(); - - return ret; - } + static int main(Descriptor &desc, int argc, char *argv[]); /** - * @brief Entry point for px4_task_spawn_cmd() if the module runs in its own thread. - * It does: - * - instantiate the object - * - call run() on it to execute the main loop - * - cleanup: delete the object - * @param argc The task argument count. - * @param argc Pointer to the task argument variable array. - * @return Returns 0 iff successful, -1 otherwise. + * @brief Start command: checks if running, calls desc.task_spawn(). */ - static int run_trampoline(int argc, char *argv[]) - { - int ret = 0; - - // We don't need the task name at this point. - argc -= 1; - argv += 1; - - T *object = T::instantiate(argc, argv); - _object.store(object); - - if (object) { - object->run(); - - } else { - PX4_ERR("failed to instantiate object"); - ret = -1; - } - - exit_and_cleanup(); - - return ret; - } + static int start_command(Descriptor &desc, int argc, char *argv[]); /** - * @brief Stars the command, ('command start'), checks if if is already - * running and calls T::task_spawn() if it's not. - * @param argc The task argument count. - * @param argc Pointer to the task argument variable array. - * @return Returns 0 iff successful, -1 otherwise. + * @brief Stop command: request stop and wait for exit. */ - static int start_command_base(int argc, char *argv[]) - { - int ret = 0; - lock_module(); - - if (is_running()) { - ret = -1; - PX4_ERR("Task already running"); - - } else { - ret = T::task_spawn(argc, argv); - - if (ret < 0) { - PX4_ERR("Task start failed (%i)", ret); - } - } - - unlock_module(); - return ret; - } + static int stop_command(Descriptor &desc); /** - * @brief Stops the command, ('command stop'), checks if it is running and if it is, request the module to stop - * and waits for the task to complete. - * @return Returns 0 iff successful, -1 otherwise. + * @brief Status command: call print_status() on running instance. */ - static int stop_command() - { - int ret = 0; - lock_module(); - - if (is_running()) { - T *object = _object.load(); - - if (object) { - object->request_stop(); - - unsigned int i = 0; - - do { - unlock_module(); - px4_usleep(10000); // 10 ms - lock_module(); - - if (++i > 500 && _task_id != -1) { // wait at most 5 sec - PX4_ERR("timeout, forcing stop"); - - if (_task_id != task_id_is_work_queue) { - px4_task_delete(_task_id); - } - - _task_id = -1; - - delete _object.load(); - _object.store(nullptr); - - ret = -1; - break; - } - } while (_task_id != -1); - - } else { - // In the very unlikely event that can only happen on work queues, - // if the starting thread does not wait for the work queue to initialize, - // and inside the work queue, the allocation of _object fails - // and exit_and_cleanup() is not called, set the _task_id as invalid. - _task_id = -1; - } - } - - unlock_module(); - return ret; - } + static int status_command(Descriptor &desc); /** - * @brief Handle 'command status': check if running and call print_status() if it is - * @return Returns 0 iff successful, -1 otherwise. + * @brief Cleanup: delete object, reset task_id. Called from module thread. */ - static int status_command() - { - int ret = -1; - lock_module(); - - if (is_running() && _object.load()) { - T *object = _object.load(); - ret = object->print_status(); - - } else { - PX4_INFO("not running"); - } - - unlock_module(); - return ret; - } + static void exit_and_cleanup(Descriptor &desc); /** - * @brief Print the status if the module is running. This can be overridden by the module to provide - * more specific information. - * @return Returns 0 iff successful, -1 otherwise. + * @brief Check if the module is running. */ - virtual int print_status() - { - PX4_INFO("running"); - return 0; - } + static bool is_running(const Descriptor &desc) { return desc.task_id != -1; } /** - * @brief Main loop method for modules running in their own thread. Called from run_trampoline(). - * This method must return when should_exit() returns true. + * @brief Wait until the object is initialized (called from task_spawn). */ - virtual void run() - { - } + static int wait_until_running(Descriptor &desc, int timeout_ms = 1000); /** - * @brief Returns the status of the module. - * @return Returns true if the module is running, false otherwise. + * @brief Shared run-trampoline for thread-based modules. + * + * Each thread-based module provides a thin per-module trampoline + * that calls this with its descriptor + instantiate function. */ - static bool is_running() + using instantiate_fn = ModuleBase * (*)(int argc, char *argv[]); + static int run_trampoline_impl(Descriptor &desc, instantiate_fn instantiate, + int argc, char *argv[]); + + /** + * @brief Main loop method for modules running in their own thread. + */ + virtual void run() {} + + /** + * @brief Print the status of the module. + */ + virtual int print_status(); + + /** + * @brief Tells the module to stop. + */ + virtual void request_stop() { _task_should_exit.store(true); } + + /** + * @brief Checks if the module should stop. + */ + bool should_exit() const { return _task_should_exit.load(); } + + /** + * @brief Typed accessor for the running module instance. + * Centralizes the ModuleBase* -> T* downcast in one place. + */ + template + static T *get_instance(Descriptor &desc) { - return _task_id != -1; + return static_cast(desc.object.load()); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast) } + static constexpr int task_id_is_work_queue = -2; + protected: - - /** - * @brief Tells the module to stop (used from outside or inside the module thread). - */ - virtual void request_stop() - { - _task_should_exit.store(true); - } - - /** - * @brief Checks if the module should stop (used within the module thread). - * @return Returns True iff successful, false otherwise. - */ - bool should_exit() const - { - return _task_should_exit.load(); - } - - /** - * @brief Exits the module and delete the object. Called from within the module's thread. - * For work queue modules, this needs to be called from the derived class in the - * cycle method, when should_exit() returns true. - */ - static void exit_and_cleanup() - { - // Take the lock here: - // - if startup fails and we're faster than the parent thread, it will set - // _task_id and subsequently it will look like the task is running. - // - deleting the object must take place inside the lock. - lock_module(); - - delete _object.load(); - _object.store(nullptr); - - _task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue. - unlock_module(); - } - - /** - * @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn(). - * @return Returns 0 iff successful, -1 on timeout or otherwise. - */ - static int wait_until_running(int timeout_ms = 1000) - { - int i = 0; - - do { - px4_usleep(2000); - - } while (!_object.load() && ++i < timeout_ms / 2); - - if (i >= timeout_ms / 2) { - PX4_ERR("Timed out while waiting for thread to start"); - return -1; - } - - return 0; - } - - /** - * @brief Get the module's object instance, (this is null if it's not running). - */ - static T *get_instance() - { - return (T *)_object.load(); - } - - /** - * @var _object Instance if the module is running. - * @note There will be one instance for each template type. - */ - static px4::atomic _object; - - /** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */ - static int _task_id; - - /** @var task_id_is_work_queue Value to indicate if the task runs on the work queue. */ - static constexpr const int task_id_is_work_queue = -2; - -private: - /** - * @brief lock_module Mutex to lock the module thread. - */ - static void lock_module() - { - pthread_mutex_lock(&px4_modules_mutex); - } - - /** - * @brief unlock_module Mutex to unlock the module thread. - */ - static void unlock_module() - { - pthread_mutex_unlock(&px4_modules_mutex); - } - - /** @var _task_should_exit Boolean flag to indicate if the task should exit. */ px4::atomic_bool _task_should_exit{false}; }; -template -px4::atomic ModuleBase::_object{nullptr}; - -template -int ModuleBase::_task_id = -1; - - #endif /* __cplusplus */ +/* -------- C-linkage declarations for PRINT_MODULE_* helpers -------- */ + __BEGIN_DECLS -/** - * @brief Module documentation and command usage help methods. - * These are extracted with the Tools/px_process_module_doc.py - * script and must be kept in sync. - */ - #ifdef __PX4_NUTTX -/** - * @note Disable module description on NuttX to reduce Flash usage. - * There's a GCC bug (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=55971), preventing us to use - * a macro, but GCC will remove the string as well with this empty inline method. - * @param description The provided functionality of the module and potentially the most important parameters. - */ static inline void PRINT_MODULE_DESCRIPTION(const char *description) {} #else - -/** - * @brief Prints module documentation (will also be used for online documentation). It uses Markdown syntax - * and should include these sections: - * - ### Description - * Provided functionality of the module and potentially the most important parameters. - * - ### Implementation - * High-level implementation overview - * - ### Examples - * Examples how to use the CLI interface (if it's non-trivial) - * - * In addition to the Markdown syntax, a line beginning with '$ ' can be used to mark a command: - * $ module start -p param - */ __EXPORT void PRINT_MODULE_DESCRIPTION(const char *description); #endif -/** - * @brief Prints the command name. - * @param executable_name: command name used in scripts & CLI - * @param category one of: driver, estimator, controller, system, communication, command, template - */ __EXPORT void PRINT_MODULE_USAGE_NAME(const char *executable_name, const char *category); - -/** - * @brief Specify a subcategory (optional). - * @param subcategory e.g. if the category is 'driver', subcategory can be 'distance_sensor' - */ __EXPORT void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory); - -/** - * @brief Prints the name for a command without any sub-commands (@see PRINT_MODULE_USAGE_NAME()). - */ __EXPORT void PRINT_MODULE_USAGE_NAME_SIMPLE(const char *executable_name, const char *category); - - -/** - * @brief Prints a command with a short description what it does. - */ __EXPORT void PRINT_MODULE_USAGE_COMMAND_DESCR(const char *name, const char *description); #define PRINT_MODULE_USAGE_COMMAND(name) \ PRINT_MODULE_USAGE_COMMAND_DESCR(name, NULL); -/** - * @brief Prints the default commands: stop & status. - */ #define PRINT_MODULE_USAGE_DEFAULT_COMMANDS() \ PRINT_MODULE_USAGE_COMMAND("stop"); \ PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info"); -/** - * Print default params for I2C or SPI drivers - * @param i2c_support true if the driver supports I2C - * @param spi_support true if the driver supports SPI - */ __EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(bool i2c_support, bool spi_support); - -/** - * Configurable I2C address (via -a
) - */ __EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(uint8_t default_address); - -/** - * -k flag - */ __EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(void); - -/** @note Each of the PRINT_MODULE_USAGE_PARAM_* methods apply to the previous PRINT_MODULE_USAGE_COMMAND_DESCR(). */ - -/** - * @brief Prints an integer parameter. - * @param option_char The option character. - * @param default_val The parameter default value (set to -1 if not applicable). - * @param min_val The parameter minimum value. - * @param max_val The parameter value. - * @param description Pointer to the usage description. - * @param is_optional true if this parameter is optional - */ __EXPORT void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val, int max_val, const char *description, bool is_optional); - -/** - * @brief Prints a float parameter. - * @note See PRINT_MODULE_USAGE_PARAM_INT(). - * @param default_val The parameter default value (set to NaN if not applicable). - * @param min_val The parameter minimum value. - * @param max_val The parameter maximum value. - * @param description Pointer to the usage description. Pointer to the usage description. - * @param is_optional true if this parameter is optional - */ __EXPORT void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float min_val, float max_val, const char *description, bool is_optional); - -/** - * @brief Prints a flag parameter, without any value. - * @note See PRINT_MODULE_USAGE_PARAM_INT(). - * @param option_char The option character. - * @param description Pointer to the usage description. - * @param is_optional true if this parameter is optional - */ __EXPORT void PRINT_MODULE_USAGE_PARAM_FLAG(char option_char, const char *description, bool is_optional); - -/** - * @brief Prints a string parameter. - * @param option_char The option character. - * @param default_val The default value, can be nullptr. - * @param values The valid values, it has one of the following forms: - * - nullptr: leave unspecified, or any value is valid - * - "" or "": a file or more specifically a device file (eg. serial device) - * - "": uORB topic name - * - " []": a list of values - * - "on|off": a concrete set of valid strings separated by "|". - * @param description Pointer to the usage description. - * @param is_optional True iff this parameter is optional. - */ __EXPORT void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, const char *values, const char *description, bool is_optional); - -/** - * @brief Prints a comment, that applies to the next arguments or parameters. For example to indicate that - * a parameter applies to several or all commands. - * @param comment - */ __EXPORT void PRINT_MODULE_USAGE_PARAM_COMMENT(const char *comment); - - -/** - * @brief Prints the definition for an argument, which does not have the typical -p form, - * but for example 'param set ' - * @param values eg. "", " " or " []" - * @param description Pointer to the usage description. - * @param is_optional true if this parameter is optional - */ __EXPORT void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is_optional); - __END_DECLS diff --git a/platforms/common/module_base.cpp b/platforms/common/module_base.cpp new file mode 100644 index 0000000000..58e5262267 --- /dev/null +++ b/platforms/common/module_base.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2025 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 module_base.cpp + * + * Shared implementations of ModuleBase static methods. One copy linked + * for all modules, replacing per-module CRTP template instantiations. + */ + +#ifndef MODULE_NAME +#define MODULE_NAME "module" +#endif + +#include + +#include +#include + +extern pthread_mutex_t px4_modules_mutex; + +int ModuleBase::print_status() +{ + printf("running\n"); + return 0; +} + +int ModuleBase::main(Descriptor &desc, int argc, char *argv[]) +{ + if (argc <= 1 || + strcmp(argv[1], "-h") == 0 || + strcmp(argv[1], "help") == 0 || + strcmp(argv[1], "info") == 0 || + strcmp(argv[1], "usage") == 0) { + return desc.print_usage(nullptr); + } + + if (strcmp(argv[1], "start") == 0) { + // Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument. + return start_command(desc, argc - 1, argv + 1); + } + + if (strcmp(argv[1], "status") == 0) { + return status_command(desc); + } + + if (strcmp(argv[1], "stop") == 0) { + return stop_command(desc); + } + + pthread_mutex_lock(&px4_modules_mutex); + int ret = desc.custom_command(argc - 1, argv + 1); + pthread_mutex_unlock(&px4_modules_mutex); + + return ret; +} + +int ModuleBase::start_command(Descriptor &desc, int argc, char *argv[]) +{ + int ret = 0; + pthread_mutex_lock(&px4_modules_mutex); + + if (is_running(desc)) { + ret = -1; + PX4_ERR("Task already running"); + + } else { + ret = desc.task_spawn(argc, argv); + + if (ret < 0) { + PX4_ERR("Task start failed (%i)", ret); + } + } + + pthread_mutex_unlock(&px4_modules_mutex); + return ret; +} + +int ModuleBase::stop_command(Descriptor &desc) +{ + int ret = 0; + pthread_mutex_lock(&px4_modules_mutex); + + if (is_running(desc)) { + ModuleBase *object = desc.object.load(); + + if (object) { + object->request_stop(); + + unsigned int i = 0; + + do { + pthread_mutex_unlock(&px4_modules_mutex); + px4_usleep(10000); // 10 ms + pthread_mutex_lock(&px4_modules_mutex); + + if (++i > 500 && desc.task_id != -1) { // wait at most 5 sec + PX4_ERR("timeout, forcing stop"); + + if (desc.task_id != task_id_is_work_queue) { + px4_task_delete(desc.task_id); + } + + desc.task_id = -1; + + delete desc.object.load(); + desc.object.store(nullptr); + + ret = -1; + break; + } + } while (desc.task_id != -1); + + } else { + // In the very unlikely event that can only happen on work queues, + // if the starting thread does not wait for the work queue to initialize, + // and inside the work queue, the allocation of _object fails + // and exit_and_cleanup() is not called, set the task_id as invalid. + desc.task_id = -1; + } + } + + pthread_mutex_unlock(&px4_modules_mutex); + return ret; +} + +int ModuleBase::status_command(Descriptor &desc) +{ + int ret = -1; + pthread_mutex_lock(&px4_modules_mutex); + + if (is_running(desc) && desc.object.load()) { + ModuleBase *object = desc.object.load(); + ret = object->print_status(); + + } else { + PX4_INFO("not running"); + } + + pthread_mutex_unlock(&px4_modules_mutex); + return ret; +} + +void ModuleBase::exit_and_cleanup(Descriptor &desc) +{ + // Take the lock here: + // - if startup fails and we're faster than the parent thread, it will set + // task_id and subsequently it will look like the task is running. + // - deleting the object must take place inside the lock. + pthread_mutex_lock(&px4_modules_mutex); + + delete desc.object.load(); + desc.object.store(nullptr); + + desc.task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue. + pthread_mutex_unlock(&px4_modules_mutex); +} + +int ModuleBase::wait_until_running(Descriptor &desc, int timeout_ms) +{ + int i = 0; + + do { + px4_usleep(2000); + + } while (!desc.object.load() && ++i < timeout_ms / 2); + + if (i >= timeout_ms / 2) { + PX4_ERR("Timed out while waiting for thread to start"); + return -1; + } + + return 0; +} + +int ModuleBase::run_trampoline_impl(Descriptor &desc, instantiate_fn instantiate, + int argc, char *argv[]) +{ + int ret = 0; + + // We don't need the task name at this point. + argc -= 1; + argv += 1; + + ModuleBase *object = instantiate(argc, argv); + desc.object.store(object); + + if (object) { + object->run(); + + } else { + PX4_ERR("failed to instantiate object"); + ret = -1; + } + + exit_and_cleanup(desc); + + return ret; +} diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake index de8fe5a0c6..144cba542c 100644 --- a/platforms/nuttx/src/px4/common/px4_layer.cmake +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -3,6 +3,7 @@ add_library(px4_layer ${KERNEL_SRCS} ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp SerialImpl.cpp ) diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 5fbbef1644..cf71f81b08 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -5,6 +5,7 @@ add_library(px4_layer board_dma_alloc.c board_fat_dma_alloc.c tasks.cpp + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp console_buffer_usr.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp @@ -45,6 +46,7 @@ target_link_libraries(px4_layer add_library(px4_kernel_layer ${KERNEL_SRCS} + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp SerialImpl.cpp ) diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 90d4a77992..e99ac9ca90 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -40,6 +40,7 @@ set(EXTRA_DEPENDS) add_library(px4_layer px4_posix_impl.cpp tasks.cpp + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp px4_sem.cpp px4_init.cpp lib_crc32.c diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index 8a25322755..097ec098f2 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -35,6 +35,7 @@ set(QURT_LAYER_SRCS drv_hrt.cpp tasks.cpp + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp px4_qurt_impl.cpp main.cpp qurt_log.cpp diff --git a/platforms/ros2/include/px4_platform_common/module.h b/platforms/ros2/include/px4_platform_common/module.h index d577a9d9be..0bca0b3faf 100644 --- a/platforms/ros2/include/px4_platform_common/module.h +++ b/platforms/ros2/include/px4_platform_common/module.h @@ -43,220 +43,6 @@ #include "rclcpp/rclcpp.hpp" -/** - * @class ModuleBase - * Base class for modules, implementing common functionality, - * such as 'start', 'stop' and 'status' commands. - * Currently does not support modules which allow multiple instances, - * such as mavlink. - * - * The class is implemented as curiously recurring template pattern (CRTP). - * It allows to have a static object in the base class that is different for - * each module type, and call static methods from the base class. - * - * @note Required methods for a derived class: - * When running in its own thread: - * static int task_spawn(int argc, char *argv[]) - * { - * // call px4_task_spawn_cmd() with &run_trampoline as startup method - * // optional: wait until _object is not null, which means the task got initialized (use a timeout) - * // set _task_id and return 0 - * // on error return != 0 (and _task_id must be -1) - * } - * - * static T *instantiate(int argc, char *argv[]) - * { - * // this is called from within the new thread, from run_trampoline() - * // parse the arguments - * // create a new object T & return it - * // or return nullptr on error - * } - * - * static int custom_command(int argc, char *argv[]) - * { - * // support for custom commands - * // if none are supported, just do: - * return print_usage("unrecognized command"); - * } - * - * static int print_usage(const char *reason = nullptr) - * { - * // use the PRINT_MODULE_* methods... - * } - * - * When running on the work queue: - * - custom_command & print_usage as above - * static int task_spawn(int argc, char *argv[]) { - * // parse the arguments - * // set _object (here or from the work_queue() callback) - * // call work_queue() (with a custom cycle trampoline) - * // optional: wait until _object is not null, which means the task got initialized (use a timeout) - * // set _task_id to task_id_is_work_queue and return 0 - * // on error return != 0 (and _task_id must be -1) - * } - */ -template -class ModuleBase : public rclcpp::Node -{ -public: - ModuleBase() : Node(MODULE_NAME) - { - - RCLCPP_INFO(get_logger(), "constructing %lu", hrt_absolute_time()); - } - - virtual ~ModuleBase() {} - - /** - * @brief main Main entry point to the module that should be - * called directly from the module's main method. - * @param argc The task argument count. - * @param argc Pointer to the task argument variable array. - * @return Returns 0 iff successful, -1 otherwise. - */ - static int main(int argc, char *argv[]) - { - - - return ret; - } - - /** - * @brief Print the status if the module is running. This can be overridden by the module to provide - * more specific information. - * @return Returns 0 iff successful, -1 otherwise. - */ - virtual int print_status() - { - PX4_INFO("running"); - return 0; - } - - /** - * @brief Main loop method for modules running in their own thread. Called from run_trampoline(). - * This method must return when should_exit() returns true. - */ - virtual void run() - { - } - - /** - * @brief Returns the status of the module. - * @return Returns true if the module is running, false otherwise. - */ - static bool is_running() - { - return _task_id != -1; - } - -protected: - - /** - * @brief Tells the module to stop (used from outside or inside the module thread). - */ - virtual void request_stop() - { - _task_should_exit.store(true); - } - - /** - * @brief Checks if the module should stop (used within the module thread). - * @return Returns True iff successful, false otherwise. - */ - bool should_exit() const - { - return _task_should_exit.load(); - } - - /** - * @brief Exits the module and delete the object. Called from within the module's thread. - * For work queue modules, this needs to be called from the derived class in the - * cycle method, when should_exit() returns true. - */ - static void exit_and_cleanup() - { - // Take the lock here: - // - if startup fails and we're faster than the parent thread, it will set - // _task_id and subsequently it will look like the task is running. - // - deleting the object must take place inside the lock. - lock_module(); - - delete _object.load(); - _object.store(nullptr); - - _task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue. - unlock_module(); - } - - /** - * @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn(). - * @return Returns 0 iff successful, -1 on timeout or otherwise. - */ - static int wait_until_running(int timeout_ms = 1000) - { - int i = 0; - - do { - px4_usleep(2000); - - } while (!_object.load() && ++i < timeout_ms / 2); - - if (i >= timeout_ms / 2) { - PX4_ERR("Timed out while waiting for thread to start"); - return -1; - } - - return 0; - } - - /** - * @brief Get the module's object instance, (this is null if it's not running). - */ - static T *get_instance() - { - return (T *)_object.load(); - } - - /** - * @var _object Instance if the module is running. - * @note There will be one instance for each template type. - */ - static px4::atomic _object; - - /** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */ - static int _task_id; - - /** @var task_id_is_work_queue Value to indicate if the task runs on the work queue. */ - static constexpr const int task_id_is_work_queue = -2; - -private: - /** - * @brief lock_module Mutex to lock the module thread. - */ - static void lock_module() - { - pthread_mutex_lock(&px4_modules_mutex); - } - - /** - * @brief unlock_module Mutex to unlock the module thread. - */ - static void unlock_module() - { - pthread_mutex_unlock(&px4_modules_mutex); - } - - /** @var _task_should_exit Boolean flag to indicate if the task should exit. */ - px4::atomic_bool _task_should_exit{false}; -}; - -template -px4::atomic ModuleBase::_object{nullptr}; - -template -int ModuleBase::_task_id = -1; - - #endif /* __cplusplus */ diff --git a/platforms/ros2/src/px4/common/CMakeLists.txt b/platforms/ros2/src/px4/common/CMakeLists.txt index dc850c246e..ed06198c26 100644 --- a/platforms/ros2/src/px4/common/CMakeLists.txt +++ b/platforms/ros2/src/px4/common/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(px4_layer STATIC drv_hrt.cpp px4_sem.cpp + ${PX4_SOURCE_DIR}/platforms/common/module_base.cpp ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 3414851b4b..c41bc69642 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -37,6 +37,8 @@ #include "voxl_esc.hpp" +ModuleBase::Descriptor VoxlEsc::desc{task_spawn, custom_command, print_usage}; + // future use: #define MODALAI_PUBLISH_ESC_STATUS 0 @@ -425,8 +427,8 @@ int VoxlEsc::task_spawn(int argc, char *argv[]) } if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -439,8 +441,8 @@ int VoxlEsc::task_spawn(int argc, char *argv[]) // This will cause a crash on SLPI DSP // delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -693,12 +695,12 @@ int VoxlEsc::custom_command(int argc, char *argv[]) /* start the driver if not running */ if (!strcmp(verb, "start")) { - if (!is_running()) { + if (!is_running(desc)) { return VoxlEsc::task_spawn(argc, argv); } } - if (!is_running()) { + if (!is_running(desc)) { PX4_INFO("Not running"); return -1; @@ -761,7 +763,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) PX4_ERR("Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -774,7 +776,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 2000; - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -787,7 +789,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 5000; - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -799,7 +801,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) PX4_ERR("Request tone for ESC mask: %i", esc_id); cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -808,14 +810,14 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "led")) { if (led_mask <= 0x0FFF) { - get_instance()->_led_rsc.test = true; - get_instance()->_led_rsc.breath_en = false; + get_instance(desc)->_led_rsc.test = true; + get_instance(desc)->_led_rsc.breath_en = false; PX4_ERR("Request LED control for ESCs with mask: %i", led_mask); - get_instance()->_esc_chans[0].led = (led_mask & 0x0007); - get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; - get_instance()->_esc_chans[2].led = (led_mask & 0x01C0) >> 6; - get_instance()->_esc_chans[3].led = (led_mask & 0x0E00) >> 9; + get_instance(desc)->_esc_chans[0].led = (led_mask & 0x0007); + get_instance(desc)->_esc_chans[1].led = (led_mask & 0x0038) >> 3; + get_instance(desc)->_esc_chans[2].led = (led_mask & 0x01C0) >> 6; + get_instance(desc)->_esc_chans[3].led = (led_mask & 0x0E00) >> 9; return 0; } else { @@ -851,7 +853,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) id_fb, cmd.buf, sizeof(cmd.buf), - get_instance()->_extended_rpm); + get_instance(desc)->_extended_rpm); cmd.response = true; cmd.repeats = repeat_count; @@ -862,7 +864,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) PX4_ERR("Feedback id debug: %i", id_fb); PX4_ERR("Sending UART ESC RPM command %i", rate); - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -907,7 +909,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) PX4_ERR("Feedback id debug: %i", id_fb); PX4_ERR("Sending UART ESC power command %i", rate); - return get_instance()->send_cmd_thread_safe(&cmd); + return get_instance(desc)->send_cmd_thread_safe(&cmd); } else { print_usage("Invalid ESC ID, use 0-3"); @@ -1422,7 +1424,7 @@ void VoxlEsc::Run() ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -1446,7 +1448,7 @@ void VoxlEsc::Run() PX4_ERR("Failed to initialize device, exiting the module"); ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -1767,5 +1769,5 @@ extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]); int voxl_esc_main(int argc, char *argv[]) { - return VoxlEsc::main(argc, argv); + return ModuleBase::main(VoxlEsc::desc, argc, argv); } diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index d103f902d0..a73f26ae10 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -58,9 +58,11 @@ using namespace device; -class VoxlEsc : public ModuleBase, public OutputModuleInterface +class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + VoxlEsc(); virtual ~VoxlEsc(); diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 936f0971ac..0fdd23c517 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -39,6 +39,8 @@ #include #endif +ModuleBase::Descriptor ADC::desc{task_spawn, custom_command, print_usage}; + ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _publish_adc_report(publish_adc_report), @@ -362,8 +364,8 @@ int ADC::custom_command(int argc, char *argv[]) const char *verb = argv[0]; if (!strcmp(verb, "test")) { - if (is_running()) { - return _object.load()->test(); + if (is_running(desc)) { + return get_instance(desc)->test(); } return PX4_ERROR; @@ -378,8 +380,8 @@ int ADC::task_spawn(int argc, char *argv[]) ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -390,8 +392,8 @@ int ADC::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -421,5 +423,5 @@ ADC driver. extern "C" __EXPORT int board_adc_main(int argc, char *argv[]) { - return ADC::main(argc, argv); + return ModuleBase::main(ADC::desc, argc, argv); } diff --git a/src/drivers/adc/board_adc/ADC.hpp b/src/drivers/adc/board_adc/ADC.hpp index c765d596b4..0e966c6d5b 100644 --- a/src/drivers/adc/board_adc/ADC.hpp +++ b/src/drivers/adc/board_adc/ADC.hpp @@ -62,9 +62,11 @@ using namespace time_literals; #define ADC_TOTAL_CHANNELS 32 -class ADC : public ModuleBase, public px4::ScheduledWorkItem +class ADC : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS, bool publish_adc_report = true); ~ADC() override; diff --git a/src/drivers/auterion_autostarter/AuterionAutostarter.cpp b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp index 28bd1b9a84..7b22949891 100644 --- a/src/drivers/auterion_autostarter/AuterionAutostarter.cpp +++ b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp @@ -41,6 +41,8 @@ #include #include +ModuleBase::Descriptor AuterionAutostarter::desc{task_spawn, custom_command, print_usage}; + AuterionAutostarter::AuterionAutostarter() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -57,7 +59,7 @@ bool AuterionAutostarter::init() void AuterionAutostarter::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -65,7 +67,7 @@ void AuterionAutostarter::Run() _actuator_armed_sub.copy(&actuator_armed); if (actuator_armed.armed) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -510,8 +512,8 @@ int AuterionAutostarter::task_spawn(int argc, char *argv[]) AuterionAutostarter *instance = new AuterionAutostarter(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -522,8 +524,8 @@ int AuterionAutostarter::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -555,5 +557,5 @@ Driver for starting and auto-detecting different power monitors. extern "C" __EXPORT int auterion_autostarter_main(int argc, char *argv[]) { - return AuterionAutostarter::main(argc, argv); + return ModuleBase::main(AuterionAutostarter::desc, argc, argv); } diff --git a/src/drivers/auterion_autostarter/AuterionAutostarter.h b/src/drivers/auterion_autostarter/AuterionAutostarter.h index f83107db3b..8784bf1bd6 100644 --- a/src/drivers/auterion_autostarter/AuterionAutostarter.h +++ b/src/drivers/auterion_autostarter/AuterionAutostarter.h @@ -71,10 +71,12 @@ private: struct i2c_master_s *_i2c {nullptr}; }; -class AuterionAutostarter : public ModuleBase, public px4::ScheduledWorkItem +class AuterionAutostarter : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + AuterionAutostarter(); virtual ~AuterionAutostarter(); diff --git a/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp b/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp index d86788ff7f..2dc77aae42 100644 --- a/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp +++ b/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp @@ -45,6 +45,8 @@ __END_DECLS #include +ModuleBase::Descriptor CdcAcmAutostart::desc{task_spawn, custom_command, print_usage}; + #define USB_DEVICE_PATH "/dev/ttyACM0" #if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) @@ -95,7 +97,7 @@ int CdcAcmAutostart::Start() void CdcAcmAutostart::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -566,8 +568,8 @@ int CdcAcmAutostart::task_spawn(int argc, char *argv[]) return ret; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; return ret; } @@ -660,7 +662,7 @@ and continue to check for VBUS and start mavlink once it is detected. extern "C" __EXPORT int cdcacm_autostart_main(int argc, char *argv[]) { #if defined(CONFIG_SYSTEM_CDCACM) - return CdcAcmAutostart::main(argc, argv); + return ModuleBase::main(CdcAcmAutostart::desc, argc, argv); #endif return 1; } diff --git a/src/drivers/cdcacm_autostart/cdcacm_autostart.h b/src/drivers/cdcacm_autostart/cdcacm_autostart.h index a0e1271515..d530542d7d 100644 --- a/src/drivers/cdcacm_autostart/cdcacm_autostart.h +++ b/src/drivers/cdcacm_autostart/cdcacm_autostart.h @@ -44,9 +44,11 @@ using namespace time_literals; -class CdcAcmAutostart : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class CdcAcmAutostart : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + CdcAcmAutostart(); ~CdcAcmAutostart() override; diff --git a/src/drivers/distance_sensor/pga460/pga460.cpp b/src/drivers/distance_sensor/pga460/pga460.cpp index a94f7c5b57..d8e26ba3a2 100644 --- a/src/drivers/distance_sensor/pga460/pga460.cpp +++ b/src/drivers/distance_sensor/pga460/pga460.cpp @@ -40,6 +40,8 @@ #include "pga460.h" +ModuleBase::Descriptor PGA460::desc{task_spawn, custom_command, print_usage}; + PGA460::PGA460(const char *port) { @@ -751,6 +753,13 @@ int PGA460::take_measurement(const uint8_t mode) return PX4_OK; } +int PGA460::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + return PGA460::instantiate(ac, av); + }, argc, argv); +} + int PGA460::task_spawn(int argc, char *argv[]) { px4_main_t entry_point = (px4_main_t)&run_trampoline; @@ -765,7 +774,7 @@ int PGA460::task_spawn(int argc, char *argv[]) return -errno; } - _task_id = task_id; + desc.task_id = task_id; return PX4_OK; } @@ -905,5 +914,5 @@ int PGA460::write_register(const uint8_t reg, const uint8_t val) extern "C" __EXPORT int pga460_main(int argc, char *argv[]) { - return PGA460::main(argc, argv); + return ModuleBase::main(PGA460::desc, argc, argv); } diff --git a/src/drivers/distance_sensor/pga460/pga460.h b/src/drivers/distance_sensor/pga460/pga460.h index 8783dcc947..781096f62f 100644 --- a/src/drivers/distance_sensor/pga460/pga460.h +++ b/src/drivers/distance_sensor/pga460/pga460.h @@ -207,10 +207,12 @@ #define P2_THR_15 0x0 //reg addr 0x7E #define THR_CRC 0x1D //reg addr 0x7F -class PGA460 : public ModuleBase +class PGA460 : public ModuleBase { public: + static Descriptor desc; + PGA460(const char *port = PGA460_DEFAULT_PORT); virtual ~PGA460(); @@ -245,6 +247,11 @@ public: */ static int task_spawn(int argc, char *argv[]); + /** + * @see ModuleBase + */ + static int run_trampoline(int argc, char *argv[]); + /** * @brief Closes the serial port. * @return Returns 0 if success or ERRNO. diff --git a/src/drivers/distance_sensor/srf05/SR05.hpp b/src/drivers/distance_sensor/srf05/SR05.hpp index 71a7bb982f..8b675d152f 100644 --- a/src/drivers/distance_sensor/srf05/SR05.hpp +++ b/src/drivers/distance_sensor/srf05/SR05.hpp @@ -67,9 +67,11 @@ static constexpr uint32_t HXSRX0X_CONVERSION_INTERVAL{50_ms}; // Maximum time to wait for a conversion to complete. static constexpr uint32_t HXSRX0X_CONVERSION_TIMEOUT{30_ms}; -class SRF05 : public ModuleBase, public px4::ScheduledWorkItem +class SRF05 : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + SRF05(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~SRF05() override; diff --git a/src/drivers/distance_sensor/srf05/SRF05.cpp b/src/drivers/distance_sensor/srf05/SRF05.cpp index 495fa57c7e..88fc6a42af 100644 --- a/src/drivers/distance_sensor/srf05/SRF05.cpp +++ b/src/drivers/distance_sensor/srf05/SRF05.cpp @@ -46,6 +46,8 @@ #include +ModuleBase::Descriptor SRF05::desc{task_spawn, custom_command, print_usage}; + SRF05::SRF05(const uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0 /* no device type for GPIO input */, rotation) @@ -114,7 +116,7 @@ SRF05::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -195,8 +197,8 @@ int SRF05::task_spawn(int argc, char *argv[]) SRF05 *instance = new SRF05(rotation); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -207,8 +209,8 @@ int SRF05::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -251,7 +253,7 @@ SRF05::print_status() extern "C" __EXPORT int srf05_main(int argc, char *argv[]) { - return SRF05::main(argc, argv); + return ModuleBase::main(SRF05::desc, argc, argv); } #else # error ("GPIO_ULTRASOUND_xxx not defined. Driver not supported."); diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 92fec8e76e..5150a81e5a 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -37,6 +37,8 @@ #include +ModuleBase::Descriptor DShot::desc{task_spawn, custom_command, print_usage}; + char DShot::_telemetry_device[] {}; bool DShot::_telemetry_swap_rxtx{false}; px4::atomic_bool DShot::_request_telemetry_init{false}; @@ -81,8 +83,8 @@ int DShot::task_spawn(int argc, char *argv[]) DShot *instance = new DShot(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -93,8 +95,8 @@ int DShot::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -466,7 +468,7 @@ void DShot::Run() ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -698,16 +700,16 @@ int DShot::custom_command(int argc, char *argv[]) for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) { if (!strcmp(verb, commands[i].name)) { - if (!is_running()) { + if (!is_running(desc)) { PX4_ERR("module not running"); return -1; } - return get_instance()->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index); + return get_instance(desc)->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index); } } - if (!is_running()) { + if (!is_running(desc)) { int ret = DShot::task_spawn(argc, argv); if (ret) { @@ -805,5 +807,5 @@ After saving, the reversed direction will be regarded as the normal one. So to r extern "C" __EXPORT int dshot_main(int argc, char *argv[]) { - return DShot::main(argc, argv); + return ModuleBase::main(DShot::desc, argc, argv); } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 8137e461e2..bb67996acd 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -57,9 +57,11 @@ static constexpr int DSHOT_DISARM_VALUE = 0; static constexpr int DSHOT_MIN_THROTTLE = 1; static constexpr int DSHOT_MAX_THROTTLE = 1999; -class DShot final : public ModuleBase, public OutputModuleInterface +class DShot final : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + DShot(); ~DShot() override; diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index a20b0b9365..8cd71875e1 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -67,6 +67,8 @@ using namespace device; namespace septentrio { +ModuleBase::Descriptor SeptentrioDriver::desc{task_spawn, custom_command, print_usage}; + /** * RTC drift time when time synchronization is needed (in seconds). */ @@ -347,6 +349,13 @@ void SeptentrioDriver::run() } +int SeptentrioDriver::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + return SeptentrioDriver::instantiate(ac, av); + }, argc, argv); +} + int SeptentrioDriver::task_spawn(int argc, char *argv[]) { return task_spawn(argc, argv, Instance::Main); @@ -372,14 +381,14 @@ int SeptentrioDriver::task_spawn(int argc, char *argv[], Instance instance) (char *const *)argv); if (task_id < 0) { - // `_task_id` of module that hasn't been started before or has been stopped should already be -1. + // `desc.task_id` of module that hasn't been started before or has been stopped should already be -1. // This is just to make sure. - _task_id = -1; + desc.task_id = -1; return -errno; } if (instance == Instance::Main) { - _task_id = task_id; + desc.task_id = task_id; } return 0; @@ -489,12 +498,12 @@ int SeptentrioDriver::custom_command(int argc, char *argv[]) const char *failure_reason {"unknown command"}; SeptentrioDriver *driver_instance; - if (!is_running()) { + if (!is_running(desc)) { PX4_INFO("not running"); return -1; } - driver_instance = get_instance(); + driver_instance = get_instance(desc); if (argc >= 1 && strcmp(argv[0], "reset") == 0) { if (argc == 2) { @@ -1861,5 +1870,5 @@ uint32_t SeptentrioDriver::get_parameter(const char *name, float *value) extern "C" __EXPORT int septentrio_main(int argc, char *argv[]) { - return septentrio::SeptentrioDriver::main(argc, argv); + return ModuleBase::main(septentrio::SeptentrioDriver::desc, argc, argv); } diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index cdc21fd092..78761432a9 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -227,9 +227,11 @@ enum class ReceiverOutputTracker { PositioningMessages = DOP + PVTGeodetic + VelCovGeodetic + AttEuler + AttCovEuler, }; -class SeptentrioDriver : public ModuleBase, public device::Device +class SeptentrioDriver : public ModuleBase, public device::Device { public: + static Descriptor desc; + SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate); ~SeptentrioDriver() override; @@ -244,6 +246,9 @@ public: static int task_spawn(int argc, char *argv[], Instance instance); + /** @see ModuleBase */ + static int run_trampoline(int argc, char *argv[]); + /** * @brief Secondary run trampoline to support two driver instances. */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 464eb892e9..5f8138180c 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -117,7 +117,7 @@ struct GPS_Sat_Info { static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2040); -class GPS : public ModuleBase, public device::Device +class GPS : public ModuleBase, public device::Device { public: @@ -133,6 +133,8 @@ public: unsigned configured_baudrate); ~GPS() override; + static Descriptor desc; + /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -150,6 +152,11 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + /** + * task spawn trampoline for the primary GPS + */ + static int run_trampoline(int argc, char *argv[]); + /** * task spawn trampoline for the secondary GPS */ @@ -302,6 +309,7 @@ private: px4::atomic_bool GPS::_is_gps_main_advertised{false}; px4::atomic GPS::_secondary_instance{nullptr}; +ModuleBase::Descriptor GPS::desc{task_spawn, custom_command, print_usage}; /* * Driver 'main' command. @@ -1418,12 +1426,12 @@ int GPS::custom_command(int argc, char *argv[]) { // Check if the driver is running. - if (!is_running()) { + if (!is_running(desc)) { PX4_INFO("not running"); return PX4_ERROR; } - GPS *_instance = get_instance(); + GPS *_instance = get_instance(desc); bool res = false; @@ -1517,17 +1525,24 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) entry_point, (char *const *)argv); if (task_id < 0) { - _task_id = -1; + desc.task_id = -1; return -errno; } if (instance == Instance::Main) { - _task_id = task_id; + desc.task_id = task_id; } return 0; } +int GPS::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + return GPS::instantiate(ac, av); + }, argc, argv); +} + int GPS::run_trampoline_secondary(int argc, char *argv[]) { // the task name is the first argument @@ -1693,5 +1708,5 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) int gps_main(int argc, char *argv[]) { - return GPS::main(argc, argv); + return ModuleBase::main(GPS::desc, argc, argv); } diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 956419c661..f11bc46782 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -48,6 +48,8 @@ #include #include +ModuleBase::Descriptor Heater::desc{task_spawn, custom_command, print_usage}; + #if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED) // Heater on some boards is on IO MCU // Use ioctl calls to IO driver to turn heater on/off @@ -75,7 +77,7 @@ Heater::~Heater() int Heater::custom_command(int argc, char *argv[]) { // Check if the driver is running. - if (!is_running()) { + if (!is_running(desc)) { PX4_INFO("not running"); return PX4_ERROR; } @@ -180,7 +182,7 @@ void Heater::Run() } #endif - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -292,8 +294,8 @@ int Heater::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(heater); - _task_id = task_id_is_work_queue; + desc.object.store(heater); + desc.task_id = task_id_is_work_queue; heater->start(); return 0; @@ -334,5 +336,5 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE extern "C" __EXPORT int heater_main(int argc, char *argv[]) { - return Heater::main(argc, argv); + return ModuleBase::main(Heater::desc, argc, argv); } diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 43cc130be0..1a38800a51 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -60,9 +60,11 @@ using namespace time_literals; #define CONTROLLER_PERIOD_DEFAULT 10000 #define TEMPERATURE_TARGET_THRESHOLD 2.5f -class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + Heater(); virtual ~Heater(); diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp index 674db6dbf1..62e8a97e10 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -43,6 +43,6 @@ extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) { - EulerNavDriver::main(argc, argv); + ModuleBase::main(EulerNavDriver::desc, argc, argv); return OK; } diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 1592308d76..536ee6afa6 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -52,20 +52,27 @@ EulerNavDriver::~EulerNavDriver() deinitialize(); } +int EulerNavDriver::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + return EulerNavDriver::instantiate(ac, av); + }, argc, argv); +} + int EulerNavDriver::task_spawn(int argc, char *argv[]) { int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER, Config::TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv); if (task_id < 0) { - _task_id = -1; + desc.task_id = -1; PX4_ERR("Failed to spawn task."); } else { - _task_id = task_id; + desc.task_id = task_id; } - return (_task_id < 0) ? 1 : 0; + return (desc.task_id < 0) ? 1 : 0; } EulerNavDriver *EulerNavDriver::instantiate(int argc, char *argv[]) @@ -232,6 +239,8 @@ void EulerNavDriver::deinitialize() _is_initialized = false; } +ModuleBase::Descriptor EulerNavDriver::desc{task_spawn, custom_command, print_usage}; + void EulerNavDriver::processDataBuffer() { static_assert(Config::MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 851b8d63b1..f9aeae3901 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -45,9 +45,11 @@ #include #include "CSerialProtocol.h" -class EulerNavDriver : public ModuleBase, public ModuleParams +class EulerNavDriver : public ModuleBase, public ModuleParams { public: + static Descriptor desc; + /// @brief Class constructor /// @param device_name Serial port to open EulerNavDriver(const char *device_name); @@ -57,6 +59,9 @@ public: /// @brief Required by ModuleBase static int task_spawn(int argc, char *argv[]); + /// @brief Required by ModuleBase + static int run_trampoline(int argc, char *argv[]); + /// @brief Required by ModuleBase static EulerNavDriver *instantiate(int argc, char *argv[]); diff --git a/src/drivers/ins/ilabs/ILabs.cpp b/src/drivers/ins/ilabs/ILabs.cpp index 1c81a818ab..e221ee50e6 100644 --- a/src/drivers/ins/ilabs/ILabs.cpp +++ b/src/drivers/ins/ilabs/ILabs.cpp @@ -44,6 +44,8 @@ using namespace time_literals; +ModuleBase::Descriptor ILabs::desc{task_spawn, custom_command, print_usage}; + // GPS epoch: 1980-01-06 00:00:00 UTC constexpr uint64_t GPS_EPOCH_SECS = 315964800ULL; @@ -152,8 +154,8 @@ int ILabs::task_spawn(int argc, char *argv[]) { return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); @@ -227,7 +229,7 @@ int ILabs::print_status() { void ILabs::Run() { if (should_exit()) { _sensor.deinit(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -530,5 +532,5 @@ void ILabs::processData(InertialLabs::SensorsData *data) { } extern "C" __EXPORT int ilabs_main(int argc, char *argv[]) { - return ILabs::main(argc, argv); + return ModuleBase::main(ILabs::desc, argc, argv); } diff --git a/src/drivers/ins/ilabs/ILabs.h b/src/drivers/ins/ilabs/ILabs.h index 3754dce982..c0d8b6d0a2 100644 --- a/src/drivers/ins/ilabs/ILabs.h +++ b/src/drivers/ins/ilabs/ILabs.h @@ -59,8 +59,10 @@ #include "sensor.h" -class ILabs : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { +class ILabs : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + ILabs(const char *port); ~ILabs() override; diff --git a/src/drivers/ins/microstrain/MicroStrain.cpp b/src/drivers/ins/microstrain/MicroStrain.cpp index a46639e4ff..cc78f46300 100755 --- a/src/drivers/ins/microstrain/MicroStrain.cpp +++ b/src/drivers/ins/microstrain/MicroStrain.cpp @@ -35,6 +35,8 @@ #include "MicroStrain.hpp" +ModuleBase::Descriptor MicroStrain::desc{task_spawn, custom_command, print_usage}; + device::Serial device_uart{}; MicroStrain::MicroStrain(const char *uart_port) : @@ -1851,7 +1853,7 @@ void MicroStrain::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -1911,8 +1913,8 @@ int MicroStrain::task_spawn(int argc, char *argv[]) if (dev == nullptr || strlen(dev) == 0) { print_usage("no device specified"); - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -1921,8 +1923,8 @@ int MicroStrain::task_spawn(int argc, char *argv[]) MicroStrain *instance = new MicroStrain(dev); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -1933,8 +1935,8 @@ int MicroStrain::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -1995,5 +1997,5 @@ $ microstrain stop extern "C" __EXPORT int microstrain_main(int argc, char *argv[]) { - return MicroStrain::main(argc, argv); + return ModuleBase::main(MicroStrain::desc, argc, argv); } diff --git a/src/drivers/ins/microstrain/MicroStrain.hpp b/src/drivers/ins/microstrain/MicroStrain.hpp index 3cba872793..cad2c2830a 100755 --- a/src/drivers/ins/microstrain/MicroStrain.hpp +++ b/src/drivers/ins/microstrain/MicroStrain.hpp @@ -86,9 +86,11 @@ using matrix::Vector2f; static constexpr float sq(float x) { return x * x; }; -class MicroStrain : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class MicroStrain : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + MicroStrain(const char *_device); ~MicroStrain() override; diff --git a/src/drivers/ins/sbgecom/sbgecom.cpp b/src/drivers/ins/sbgecom/sbgecom.cpp index 17c422839d..2739e73963 100644 --- a/src/drivers/ins/sbgecom/sbgecom.cpp +++ b/src/drivers/ins/sbgecom/sbgecom.cpp @@ -72,6 +72,8 @@ using matrix::Vector2f; +ModuleBase::Descriptor SbgEcom::desc{task_spawn, custom_command, print_usage}; + SbgEcom::SbgEcom(const char *device_name, uint32_t baudrate, const char *config_file, const char *config_string): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device_name)), @@ -952,7 +954,7 @@ void SbgEcom::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -1065,8 +1067,8 @@ int SbgEcom::task_spawn(int argc, char **argv) return PX4_ERROR; } - _task_id = task_id_is_work_queue; - _object.store(instance); + desc.task_id = task_id_is_work_queue; + desc.object.store(instance); instance->ScheduleNow(); return PX4_OK; @@ -1121,5 +1123,5 @@ int SbgEcom::print_status() extern "C" __EXPORT int sbgecom_main(int argc, char **argv) { - return SbgEcom::main(argc, argv); + return ModuleBase::main(SbgEcom::desc, argc, argv); } diff --git a/src/drivers/ins/sbgecom/sbgecom.hpp b/src/drivers/ins/sbgecom/sbgecom.hpp index 332f8def2c..f73ade46f9 100644 --- a/src/drivers/ins/sbgecom/sbgecom.hpp +++ b/src/drivers/ins/sbgecom/sbgecom.hpp @@ -63,10 +63,12 @@ #include #include -class SbgEcom : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class SbgEcom : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + SbgEcom(const char *port, uint32_t baudrate, const char *config_file, const char *config_string); ~SbgEcom() override; diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 0e6877a555..29eed67ca5 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -40,6 +40,8 @@ using matrix::Vector2f; +ModuleBase::Descriptor VectorNav::desc{task_spawn, custom_command, print_usage}; + VectorNav::VectorNav(const char *port) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), @@ -682,7 +684,7 @@ void VectorNav::Run() if (should_exit()) { VnSensor_unregisterAsyncPacketReceivedHandler(&_vs); VnSensor_disconnect(&_vs); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } else if (!_initialized) { @@ -797,8 +799,8 @@ int VectorNav::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); @@ -858,5 +860,5 @@ $ vectornav stop extern "C" __EXPORT int vectornav_main(int argc, char *argv[]) { - return VectorNav::main(argc, argv); + return ModuleBase::main(VectorNav::desc, argc, argv); } diff --git a/src/drivers/ins/vectornav/VectorNav.hpp b/src/drivers/ins/vectornav/VectorNav.hpp index 7984687a6c..0f062c85bd 100644 --- a/src/drivers/ins/vectornav/VectorNav.hpp +++ b/src/drivers/ins/vectornav/VectorNav.hpp @@ -75,9 +75,11 @@ extern "C" { using namespace time_literals; -class VectorNav : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class VectorNav : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + VectorNav(const char *port); ~VectorNav() override; diff --git a/src/drivers/lights/neopixel/neopixel.cpp b/src/drivers/lights/neopixel/neopixel.cpp index f5ebacd634..bd4af6914d 100644 --- a/src/drivers/lights/neopixel/neopixel.cpp +++ b/src/drivers/lights/neopixel/neopixel.cpp @@ -49,9 +49,11 @@ #include #include -class NEOPIXEL : public px4::ScheduledWorkItem, public ModuleBase +class NEOPIXEL : public px4::ScheduledWorkItem, public ModuleBase { public: + static Descriptor desc; + NEOPIXEL(unsigned int number_of_packages); virtual ~NEOPIXEL(); @@ -84,6 +86,8 @@ private: neopixel::NeoLEDData *_leds; }; +ModuleBase::Descriptor NEOPIXEL::desc{task_spawn, custom_command, print_usage}; + NEOPIXEL::NEOPIXEL(unsigned int number_of_packages) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), _number_of_packages(number_of_packages) @@ -131,8 +135,8 @@ int NEOPIXEL::task_spawn(int argc, char *argv[]) NEOPIXEL *instance = new NEOPIXEL(number_of_packages); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -143,8 +147,8 @@ int NEOPIXEL::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -190,7 +194,7 @@ void NEOPIXEL::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -246,5 +250,5 @@ void NEOPIXEL::Run() extern "C" __EXPORT int neopixel_main(int argc, char *argv[]) { - return NEOPIXEL::main(argc, argv); + return ModuleBase::main(NEOPIXEL::desc, argc, argv); } diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 9e2dbf1ab4..f5235f904a 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -40,6 +40,8 @@ using namespace pwm_out; +ModuleBase::Descriptor LinuxPWMOut::desc{task_spawn, custom_command, print_usage}; + LinuxPWMOut::LinuxPWMOut() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), @@ -77,8 +79,8 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[]) LinuxPWMOut *instance = new LinuxPWMOut(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -89,8 +91,8 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -108,7 +110,7 @@ void LinuxPWMOut::Run() ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -166,5 +168,5 @@ Linux PWM output driver with board-specific backend implementation. extern "C" __EXPORT int linux_pwm_out_main(int argc, char *argv[]) { - return LinuxPWMOut::main(argc, argv); + return ModuleBase::main(LinuxPWMOut::desc, argc, argv); } diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp index 14f7201bd6..ab8b41653e 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -49,9 +49,11 @@ using namespace time_literals; -class LinuxPWMOut : public ModuleBase, public OutputModuleInterface +class LinuxPWMOut : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + LinuxPWMOut(); virtual ~LinuxPWMOut(); diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 9bae007a81..de8b39a055 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -66,6 +66,8 @@ #include "MspV1.hpp" +ModuleBase::Descriptor MspOsd::desc{task_spawn, custom_command, print_usage}; + //OSD elements positions //in betaflight configurator set OSD elements to your desired positions and in CLI type "set osd" to retreieve the numbers. //234 -> not visible. Horizontally 2048-2074(spacing 1), vertically 2048-2528(spacing 32). 26 characters X 15 lines @@ -246,7 +248,7 @@ void MspOsd::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -588,8 +590,8 @@ int MspOsd::task_spawn(int argc, char *argv[]) MspOsd *instance = new MspOsd(device); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -600,8 +602,8 @@ int MspOsd::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -700,8 +702,8 @@ int MspOsd::custom_command(int argc, char *argv[]) if (argc == 1) { PX4_INFO("Please provide a channel"); - } else if (is_running() && _object.load()) { - MspOsd *object = _object.load(); + } else if (is_running(desc) && desc.object.load()) { + MspOsd *object = get_instance(desc); int ret = object->set_channel(argv[1]); if (ret == -1) { @@ -748,5 +750,5 @@ $ msp_osd extern "C" __EXPORT int msp_osd_main(int argc, char *argv[]) { - return MspOsd::main(argc, argv); + return ModuleBase::main(MspOsd::desc, argc, argv); } diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index 7b566ca759..8ce42ab999 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -100,9 +100,11 @@ enum SymbolIndex : uint8_t { POWER = 21 }; -class MspOsd : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class MspOsd : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + MspOsd(const char *device); ~MspOsd() override; diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 44464e9511..9093c0831d 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -57,9 +57,11 @@ using namespace drv_pca9685_pwm; using namespace time_literals; -class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface +class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + PCA9685Wrapper(); ~PCA9685Wrapper() override; PCA9685Wrapper(const PCA9685Wrapper &) = delete; @@ -113,6 +115,8 @@ private: int registers_check(); }; +ModuleBase::Descriptor PCA9685Wrapper::desc{task_spawn, custom_command, print_usage}; + PCA9685Wrapper::PCA9685Wrapper() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), @@ -187,7 +191,7 @@ void PCA9685Wrapper::Run() delete pca9685; pca9685 = nullptr; - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -415,8 +419,8 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { auto *instance = new PCA9685Wrapper(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->pca9685 = new PCA9685(iicbus, address); if(instance->pca9685==nullptr){ @@ -438,8 +442,8 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { driverInstanceAllocFailed: delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -470,5 +474,5 @@ void PCA9685Wrapper::updateParams() { } extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ - return PCA9685Wrapper::main(argc, argv); + return ModuleBase::main(PCA9685Wrapper::desc, argc, argv); } diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index 9bb8d47fba..49923baa33 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -45,6 +45,8 @@ #include #include +ModuleBase::Descriptor PPSCapture::desc{task_spawn, custom_command, print_usage}; + PPSCapture::PPSCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -109,7 +111,7 @@ bool PPSCapture::init() void PPSCapture::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -170,8 +172,8 @@ int PPSCapture::task_spawn(int argc, char *argv[]) PPSCapture *instance = new PPSCapture(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -182,8 +184,8 @@ int PPSCapture::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -215,14 +217,14 @@ This implements capturing PPS information from the GNSS module and calculates th void PPSCapture::stop() { - exit_and_cleanup(); + exit_and_cleanup(desc); } extern "C" __EXPORT int pps_capture_main(int argc, char *argv[]) { - if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running()) { + if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running(PPSCapture::desc)) { PPSCapture::stop(); } - return PPSCapture::main(argc, argv); + return ModuleBase::main(PPSCapture::desc, argc, argv); } diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp index 48a0774495..075e198b7e 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -44,9 +44,11 @@ using namespace time_literals; -class PPSCapture : public ModuleBase, public px4::ScheduledWorkItem +class PPSCapture : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + PPSCapture(); virtual ~PPSCapture(); diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index b5df8f3cc6..74c6c47844 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -34,6 +34,8 @@ #include "pwm_input.h" #include +ModuleBase::Descriptor PWMIN::desc{task_spawn, custom_command, print_usage}; + int PWMIN::task_spawn(int argc, char *argv[]) { @@ -44,8 +46,8 @@ PWMIN::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(pwmin); - _task_id = task_id_is_work_queue; + desc.object.store(pwmin); + desc.task_id = task_id_is_work_queue; pwmin->start(); @@ -148,7 +150,7 @@ PWMIN::pwmin_tim_isr(int irq, void *context, void *arg) /* ack the interrupts we just read */ rSR = 0; - auto obj = get_instance(); + auto obj = get_instance(desc); if (obj != nullptr) { obj->publish(status, period, pulse_width); @@ -218,5 +220,5 @@ PWMIN::custom_command(int argc, char *argv[]) extern "C" __EXPORT int pwm_input_main(int argc, char *argv[]) { - return PWMIN::main(argc, argv); + return ModuleBase::main(PWMIN::desc, argc, argv); } diff --git a/src/drivers/pwm_input/pwm_input.h b/src/drivers/pwm_input/pwm_input.h index 71e8f6269a..2272e1d78c 100644 --- a/src/drivers/pwm_input/pwm_input.h +++ b/src/drivers/pwm_input/pwm_input.h @@ -120,9 +120,11 @@ #error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif -class PWMIN : public ModuleBase +class PWMIN : public ModuleBase { public: + static Descriptor desc; + void start(); void publish(uint16_t status, uint32_t period, uint32_t pulse_width); int print_status() override; diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 4db1933a22..bb32fb7152 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -35,6 +35,8 @@ #include +ModuleBase::Descriptor PWMOut::desc{task_spawn, custom_command, print_usage}; + PWMOut::PWMOut() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -158,7 +160,7 @@ void PWMOut::Run() ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -202,8 +204,8 @@ int PWMOut::task_spawn(int argc, char *argv[]) return -1; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); return 0; } @@ -332,5 +334,5 @@ px4io driver is used for main ones. extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]) { - return PWMOut::main(argc, argv); + return ModuleBase::main(PWMOut::desc, argc, argv); } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index da28fa64d5..bae339b511 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -53,9 +53,11 @@ using namespace time_literals; -class PWMOut final : public ModuleBase, public OutputModuleInterface +class PWMOut final : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + PWMOut(); ~PWMOut() override; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0f53b54157..6e61f6e736 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -99,9 +99,11 @@ using namespace time_literals; * * Encapsulates PX4FMU to PX4IO communications modeled as file operations. */ -class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + /** * Constructor. * @@ -339,6 +341,8 @@ private: ) }; +ModuleBase::Descriptor PX4IO::desc{task_spawn, custom_command, print_usage}; + #define PX4IO_DEVICE_PATH "/dev/px4io" PX4IO::PX4IO(device::Device *interface) : @@ -512,7 +516,7 @@ void PX4IO::Run() ScheduleClear(); _mixing_output.unregister(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -1513,7 +1517,8 @@ int PX4IO::bind(int argc, char *argv[]) pulses = atoi(argv[1]); } - get_instance()->ioctl(nullptr, DSM_BIND_START, pulses); + get_instance(desc)->ioctl(nullptr, DSM_BIND_START, pulses); + return 0; } @@ -1529,8 +1534,8 @@ int PX4IO::task_spawn(int argc, char *argv[]) PX4IO *instance = new PX4IO(interface); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init() == PX4_OK) { return PX4_OK; @@ -1541,8 +1546,8 @@ int PX4IO::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -1556,7 +1561,7 @@ int PX4IO::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "checkcrc")) { - if (is_running()) { + if (is_running(desc)) { PX4_ERR("io must be stopped"); return 1; } @@ -1566,7 +1571,7 @@ int PX4IO::custom_command(int argc, char *argv[]) if (!strcmp(verb, "update")) { - if (is_running()) { + if (is_running(desc)) { PX4_ERR("io must be stopped"); return 1; } @@ -1656,7 +1661,7 @@ int PX4IO::custom_command(int argc, char *argv[]) /* commands below here require a started driver */ - if (!is_running()) { + if (!is_running(desc)) { PX4_ERR("not running"); return 1; } @@ -1668,7 +1673,7 @@ int PX4IO::custom_command(int argc, char *argv[]) } uint8_t level = atoi(argv[1]); - int ret = get_instance()->ioctl(nullptr, PX4IO_SET_DEBUG, level); + int ret = get_instance(desc)->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { PX4_ERR("SET_DEBUG failed: %d", ret); @@ -1680,7 +1685,7 @@ int PX4IO::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "bind")) { - if (!is_running()) { + if (!is_running(desc)) { PX4_ERR("io must be running"); return 1; } @@ -1689,7 +1694,7 @@ int PX4IO::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "sbus1_out")) { - int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + int ret = get_instance(desc)->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); if (ret != 0) { PX4_ERR("S.BUS v1 failed (%i)", ret); @@ -1700,7 +1705,7 @@ int PX4IO::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "sbus2_out")) { - int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + int ret = get_instance(desc)->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); if (ret != 0) { PX4_ERR("S.BUS v2 failed (%i)", ret); @@ -1711,12 +1716,12 @@ int PX4IO::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "test_fmu_fail")) { - get_instance()->test_fmu_fail(true); + get_instance(desc)->test_fmu_fail(true); return 0; } if (!strcmp(verb, "test_fmu_ok")) { - get_instance()->test_fmu_fail(false); + get_instance(desc)->test_fmu_fail(false); return 0; } @@ -1764,5 +1769,5 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]) PX4_INFO("PX4IO Not Supported"); return -1; } - return PX4IO::main(argc, argv); + return ModuleBase::main(PX4IO::desc, argc, argv); } diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 95aa9412ed..6f73c3f7d2 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -44,6 +44,8 @@ using namespace time_literals; +ModuleBase::Descriptor CrsfRc::desc{task_spawn, custom_command, print_usage}; + #define CRSF_BAUDRATE 420000 CrsfRc::CrsfRc(const char *device) : @@ -109,8 +111,8 @@ int CrsfRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); @@ -128,7 +130,7 @@ void CrsfRc::Run() _uart = nullptr; } - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -548,7 +550,7 @@ int CrsfRc::custom_command(int argc, char *argv[]) #ifdef CONFIG_RC_CRSF_INJECT if (!strcmp(argv[0], "start")) { - if (is_running()) { + if (is_running(desc)) { return print_usage("already running"); } @@ -559,7 +561,7 @@ int CrsfRc::custom_command(int argc, char *argv[]) } } - if (!is_running()) { + if (!is_running(desc)) { return print_usage("not running"); } @@ -612,5 +614,5 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem extern "C" __EXPORT int crsf_rc_main(int argc, char *argv[]) { - return CrsfRc::main(argc, argv); + return ModuleBase::main(CrsfRc::desc, argc, argv); } diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index a47b361903..85cda4e0ab 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -56,9 +56,11 @@ using namespace device; -class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + CrsfRc(const char *device); ~CrsfRc() override; diff --git a/src/drivers/rc/dsm_rc/DsmRc.cpp b/src/drivers/rc/dsm_rc/DsmRc.cpp index c20538726e..257e90c569 100644 --- a/src/drivers/rc/dsm_rc/DsmRc.cpp +++ b/src/drivers/rc/dsm_rc/DsmRc.cpp @@ -39,6 +39,8 @@ using namespace time_literals; +ModuleBase::Descriptor DsmRc::desc{task_spawn, custom_command, print_usage}; + DsmRc::DsmRc(const char *device) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), @@ -106,8 +108,8 @@ int DsmRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleOnInterval(_current_update_interval); @@ -136,7 +138,7 @@ void DsmRc::Run() dsm_deinit(); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -362,7 +364,7 @@ int DsmRc::custom_command(int argc, char *argv[]) #endif // SPEKTRUM_POWER - if (!is_running()) { + if (!is_running(desc)) { int ret = DsmRc::task_spawn(argc, argv); if (ret) { @@ -419,5 +421,5 @@ This module does Spektrum DSM RC input parsing. extern "C" __EXPORT int dsm_rc_main(int argc, char *argv[]) { - return DsmRc::main(argc, argv); + return ModuleBase::main(DsmRc::desc, argc, argv); } diff --git a/src/drivers/rc/dsm_rc/DsmRc.hpp b/src/drivers/rc/dsm_rc/DsmRc.hpp index 5ce935ec5d..b200271c8b 100644 --- a/src/drivers/rc/dsm_rc/DsmRc.hpp +++ b/src/drivers/rc/dsm_rc/DsmRc.hpp @@ -52,10 +52,12 @@ #include #include -class DsmRc : public ModuleBase, public px4::ScheduledWorkItem +class DsmRc : public ModuleBase, public px4::ScheduledWorkItem { public: + static Descriptor desc; + DsmRc(const char *device); virtual ~DsmRc(); diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp index e113b149f6..bafaf984ae 100644 --- a/src/drivers/rc/ghst_rc/GhstRc.cpp +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -36,6 +36,8 @@ #include #include +ModuleBase::Descriptor GhstRc::desc{task_spawn, custom_command, print_usage}; + GhstRc::GhstRc(const char *device) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), @@ -101,8 +103,8 @@ int GhstRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleOnInterval(_current_update_interval); @@ -129,7 +131,7 @@ void GhstRc::Run() close(_rcs_fd); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -255,7 +257,7 @@ void GhstRc::Run() int GhstRc::custom_command(int argc, char *argv[]) { - if (!is_running()) { + if (!is_running(desc)) { int ret = GhstRc::task_spawn(argc, argv); if (ret) { @@ -307,5 +309,5 @@ This module does Ghost (GHST) RC input parsing. extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) { - return GhstRc::main(argc, argv); + return ModuleBase::main(GhstRc::desc, argc, argv); } diff --git a/src/drivers/rc/ghst_rc/GhstRc.hpp b/src/drivers/rc/ghst_rc/GhstRc.hpp index 0090b216dc..1f7a83d14e 100644 --- a/src/drivers/rc/ghst_rc/GhstRc.hpp +++ b/src/drivers/rc/ghst_rc/GhstRc.hpp @@ -55,10 +55,12 @@ using namespace time_literals; -class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + GhstRc(const char *device); virtual ~GhstRc(); diff --git a/src/drivers/rc/sbus_rc/SbusRc.cpp b/src/drivers/rc/sbus_rc/SbusRc.cpp index 0ac96797e1..c8059ad488 100644 --- a/src/drivers/rc/sbus_rc/SbusRc.cpp +++ b/src/drivers/rc/sbus_rc/SbusRc.cpp @@ -35,6 +35,8 @@ #include +ModuleBase::Descriptor SbusRc::desc{task_spawn, custom_command, print_usage}; + SbusRc::SbusRc(const char *device) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), @@ -98,8 +100,8 @@ int SbusRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleOnInterval(_current_update_interval); @@ -126,7 +128,7 @@ void SbusRc::Run() close(_rcs_fd); - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -265,7 +267,7 @@ void SbusRc::Run() int SbusRc::custom_command(int argc, char *argv[]) { - if (!is_running()) { + if (!is_running(desc)) { int ret = SbusRc::task_spawn(argc, argv); if (ret) { @@ -317,5 +319,5 @@ This module does SBUS RC input parsing. extern "C" __EXPORT int sbus_rc_main(int argc, char *argv[]) { - return SbusRc::main(argc, argv); + return ModuleBase::main(SbusRc::desc, argc, argv); } diff --git a/src/drivers/rc/sbus_rc/SbusRc.hpp b/src/drivers/rc/sbus_rc/SbusRc.hpp index c02d7e3f50..f1c195d049 100644 --- a/src/drivers/rc/sbus_rc/SbusRc.hpp +++ b/src/drivers/rc/sbus_rc/SbusRc.hpp @@ -53,10 +53,12 @@ using namespace time_literals; -class SbusRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class SbusRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + SbusRc(const char *device); virtual ~SbusRc(); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 214853f647..6926621edf 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -40,6 +40,8 @@ using namespace time_literals; +ModuleBase::Descriptor RCInput::desc{task_spawn, custom_command, print_usage}; + constexpr char const *RCInput::RC_SCAN_STRING[]; RCInput::RCInput(const char *device) : @@ -181,8 +183,8 @@ RCInput::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleOnInterval(_current_update_interval); @@ -338,7 +340,7 @@ void RCInput::swap_rx_tx() void RCInput::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -348,7 +350,7 @@ void RCInput::Run() } else { PX4_ERR("init failed"); - exit_and_cleanup(); + exit_and_cleanup(desc); } } else { @@ -919,7 +921,7 @@ int RCInput::custom_command(int argc, char *argv[]) #endif /* SPEKTRUM_POWER */ /* start the FMU if not running */ - if (!is_running()) { + if (!is_running(desc)) { int ret = RCInput::task_spawn(argc, argv); if (ret) { @@ -1033,5 +1035,5 @@ This module does the RC input parsing and auto-selecting the method. Supported m extern "C" __EXPORT int rc_input_main(int argc, char *argv[]) { - return RCInput::main(argc, argv); + return ModuleBase::main(RCInput::desc, argc, argv); } diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 626d84d88c..67e0cf8ae8 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -67,10 +67,12 @@ # include #endif -class RCInput : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class RCInput : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + static Descriptor desc; + RCInput(const char *device); virtual ~RCInput(); diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index ba308b6aa6..bf1b608e91 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -44,6 +44,8 @@ #include "Roboclaw.hpp" #include +ModuleBase::Descriptor Roboclaw::desc{task_spawn, custom_command, print_usage}; + Roboclaw::Roboclaw(const char *device_name, const char *bad_rate_parameter) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -170,7 +172,7 @@ void Roboclaw::Run() { if (should_exit()) { ScheduleClear(); - exit_and_cleanup(); + exit_and_cleanup(desc); _mixing_output.unregister(); return; } @@ -476,8 +478,8 @@ int Roboclaw::task_spawn(int argc, char *argv[]) Roboclaw *instance = new Roboclaw(device_name, baud_rate_parameter_value); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; instance->ScheduleNow(); return OK; @@ -486,8 +488,8 @@ int Roboclaw::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; printf("Ending task_spawn"); @@ -535,5 +537,5 @@ int Roboclaw::print_status() extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]) { - return Roboclaw::main(argc, argv); + return ModuleBase::main(Roboclaw::desc, argc, argv); } diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index 1bb6364649..fd7050fe7a 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -54,9 +54,11 @@ #include #include -class Roboclaw : public ModuleBase, public OutputModuleInterface +class Roboclaw : public ModuleBase, public OutputModuleInterface { public: + static Descriptor desc; + /** * @param device_name Name of the serial port e.g. "/dev/ttyS2" * @param bad_rate_parameter Name of the parameter that holds the baud rate of this serial port diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 1e05471903..e37c646eab 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -39,6 +39,8 @@ #include #include +ModuleBase::Descriptor RPMCapture::desc{task_spawn, custom_command, print_usage}; + RPMCapture::RPMCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), ModuleParams(nullptr) @@ -98,7 +100,7 @@ bool RPMCapture::init() void RPMCapture::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -169,8 +171,8 @@ int RPMCapture::task_spawn(int argc, char *argv[]) RPMCapture *instance = new RPMCapture(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; @@ -181,8 +183,8 @@ int RPMCapture::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; + desc.object.store(nullptr); + desc.task_id = -1; return PX4_ERROR; } @@ -207,14 +209,14 @@ int RPMCapture::print_usage(const char *reason) void RPMCapture::stop() { - exit_and_cleanup(); + exit_and_cleanup(desc); } extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[]) { - if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) { + if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running(RPMCapture::desc)) { RPMCapture::stop(); } - return RPMCapture::main(argc, argv); + return ModuleBase::main(RPMCapture::desc, argc, argv); } diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 63d7426848..7608b81472 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -47,9 +47,11 @@ using namespace time_literals; -class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, public ModuleParams +class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, public ModuleParams { public: + static Descriptor desc; + RPMCapture(); virtual ~RPMCapture(); diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp index aa5c45e8d9..c98e990adf 100644 --- a/src/drivers/safety_button/SafetyButton.cpp +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -40,6 +40,8 @@ using namespace time_literals; +ModuleBase::Descriptor SafetyButton::desc{task_spawn, custom_command, print_usage}; + static constexpr uint8_t CYCLE_COUNT{30}; /* safety switch must be held for 1 second to activate */ // Define the various LED flash sequences for each system state. @@ -150,7 +152,7 @@ void SafetyButton::Run() { if (should_exit()) { - exit_and_cleanup(); + exit_and_cleanup(desc); return; } @@ -183,8 +185,8 @@ SafetyButton::task_spawn(int argc, char *argv[]) return ret; } - _object.store(instance); - _task_id = task_id_is_work_queue; + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; return ret; } @@ -230,5 +232,5 @@ extern "C" __EXPORT int safety_button_main(int argc, char *argv[]); int safety_button_main(int argc, char *argv[]) { - return SafetyButton::main(argc, argv); + return ModuleBase::main(SafetyButton::desc, argc, argv); } diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 0f38b6ba82..8ac6853e73 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -40,9 +40,11 @@ #include #include