diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index c5ae9e2ffe..6fa4b21baa 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -887,6 +887,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mount"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "modules status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' @@ -965,6 +966,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mount"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "modules status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param status"' @@ -989,7 +991,7 @@ void statusSEGGER() { } void cleanupFTDI() { - // wipe sdcard + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "modules stop-all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' @@ -1019,7 +1021,7 @@ void cleanupFTDI() { } void cleanupSEGGER() { - // wipe sdcard + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "modules stop-all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander stop"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink stop-all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "navigator stop"' diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index fbedfc2307..7401ed77dc 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -71,6 +71,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp param perf diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 9814a4ed74..abb109a4bf 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -88,6 +88,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 777f863dc3..c197d1bddc 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -95,6 +95,7 @@ px4_add_board( #hardfault_log led_control mixer + modules motor_ramp motor_test #mtd diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index e299119c36..689e0b7e76 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -93,6 +93,7 @@ px4_add_board( #hardfault_log led_control mixer + modules motor_ramp motor_test #mtd diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 5e7747ba34..44cc9e37a5 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -88,6 +88,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test nshterm diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 2331d8721d..c157bd953c 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -67,6 +67,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp param perf diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index d779e0cae7..010a9daf99 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -44,6 +44,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index 0b319eb392..dd86b1dfe7 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -67,6 +67,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp param perf diff --git a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp index 4cc8213537..c8a9423c28 100644 --- a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp +++ b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp @@ -130,8 +130,7 @@ int NavioRGBLed::task_spawn(int argc, char *argv[]) NavioRGBLed *instance = new NavioRGBLed(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init() == PX4_OK) { return PX4_OK; @@ -142,8 +141,6 @@ int NavioRGBLed::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index a049d2d35e..317d16e081 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -93,6 +93,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 586653708f..7cbc875ae5 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -69,6 +69,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 86893f003f..f1d4e274a4 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -69,6 +69,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 481d2472cd..930efc7099 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -87,6 +87,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index adfe04e9c0..c17888960c 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -93,6 +93,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index c48b8bee2c..14424788bc 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -90,6 +90,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index a426d2977b..504c925251 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -89,6 +89,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index b3ff1af803..7ec38a6723 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -103,6 +103,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 241afc0d34..3394519d23 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -99,6 +99,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 2fd123e4a1..9c9cfd1493 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -94,6 +94,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 4375d2ab10..9726611810 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -98,6 +98,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 0a0268b194..b6389e8973 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -94,6 +94,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index a62f74615c..59c991c591 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -94,6 +94,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index b80e684953..f5b3c9c4a1 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -96,6 +96,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index b2f60bc9a5..3fa12264a2 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -93,6 +93,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 173740450c..bceae80b95 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -97,6 +97,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index 14636aba14..2042a6327c 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -102,6 +102,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index d8c7c152d4..6a6f2b21da 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -103,6 +103,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 71d42e7702..b8778db6bc 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -72,6 +72,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index ea60335d56..b9981ba978 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -98,6 +98,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 37e266a095..6da9f5b49d 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -83,6 +83,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 0c4b6b6868..e7c7ac4c29 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -72,6 +72,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index bdb56b5a8e..e483f0562b 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -98,6 +98,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 11951eac4f..1fbd73cef3 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -102,6 +102,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index b37add5bf4..fb1bc5bc1f 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -100,6 +100,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 46f93e8c1f..3cf1a5ccff 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -67,6 +67,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp param perf diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index dc58fac57b..b0ca5655eb 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -63,6 +63,7 @@ px4_add_board( failure led_control mixer + modules motor_ramp motor_test #mtd diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index fa97dbcf2a..55f69aa02a 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -62,6 +62,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp motor_test #mtd diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 6d00a84e5e..be06985ff1 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -60,6 +60,7 @@ px4_add_board( esc_calib led_control mixer + modules motor_ramp motor_test #mtd diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index d73ab53a23..7d92ad7485 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -71,6 +71,7 @@ px4_add_board( i2cdetect led_control mixer + modules motor_ramp motor_test mtd diff --git a/platforms/common/include/px4_platform_common/module.h b/platforms/common/include/px4_platform_common/module.h index d9e9061365..bb54d425a4 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-2020 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 @@ -44,13 +44,23 @@ #include #include #include +#include #include #include #ifdef __cplusplus +#include +#include + #include +/** @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; + +class ModuleBaseInterface; +extern List px4_modules_list; + /** * @brief This mutex protects against race conditions during startup & shutdown of modules. * There could be one mutex per module instantiation, but to reduce the memory footprint @@ -59,6 +69,123 @@ */ extern pthread_mutex_t px4_modules_mutex; +class ModuleBaseInterface : public ListNode +{ +public: + ModuleBaseInterface(const char *name) : + _module_name(name) + { + px4_modules_list.add(this); + } + + virtual ~ModuleBaseInterface() + { + px4_modules_list.remove(this); + } + + const char *get_name() const { return _module_name; } + + /** + * @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_RAW("[%s] running", _module_name); + return 0; + } + + /** + * @brief Tells the module to stop (used from outside or inside the module thread). + */ + virtual void request_stop() { _task_should_exit.store(true); } + + int task_id() const { return _task_id.load(); } + void set_task_id(int id) { _task_id.store(id); } + + /** + * @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() {}; + + bool running() { return (task_id() != -1); } + + /** + * @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(); } + +protected: + + /** + * @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); } + + const char *_module_name; + + /** @var _task_should_exit Boolean flag to indicate if the task should exit. */ + px4::atomic_bool _task_should_exit{false}; + + /** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */ + px4::atomic_int _task_id{-1}; + +}; + +/** + * @brief Get module address by name. + * Caller responsible for locking. + * @param name The module name. + * @return Returns address if found, nullptr otherwise. + */ +ModuleBaseInterface *moduleInstance(const char *name); + +/** + * @brief Check if module is running (by name). + * Caller responsible for locking. + * @param name The module name. + * @return Returns true if running, false otherwise. + */ +bool moduleRunning(const char *name); + +/** + * @brief Request module stop by name. + * @param name The module name. + * @return Returns 0 on success, -1 otherwise. + */ +int moduleStop(const char *name); + +/** + * @brief Check if module is running (by name). + * @param name The module name. + * @return Returns 0 on success, -1 otherwise. + */ +int moduleStatus(const char *name); + +/** + * @brief Delete module by name. + * @param name The module name. + */ +void moduleExitAndCleanup(const char *name); + +/** + * @brief Wait until module is running (by name). + * @param name The module name. + * @return Returns true if running, false otherwise. + */ +bool moduleWaitUntilRunning(const char *name); + +void modulesStatusAll(); +void modulesStopAll(); + /** * @class ModuleBase * Base class for modules, implementing common functionality, @@ -75,9 +202,8 @@ extern pthread_mutex_t px4_modules_mutex; * 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) + * // optional: wait until object is not null, which means the task got initialized (use a timeout) + * // on error return != 0 * } * * static T *instantiate(int argc, char *argv[]) @@ -104,19 +230,24 @@ extern pthread_mutex_t px4_modules_mutex; * - 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) + * // 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) + * // instance->set_task_id(task_id_is_work_queue); and return 0 + * // on error return != 0 * } */ template -class ModuleBase +class ModuleBase : public ModuleBaseInterface { public: - ModuleBase() : _task_should_exit{false} {} - virtual ~ModuleBase() {} + ModuleBase() : ModuleBaseInterface(get_name_static()) + {} + + virtual ~ModuleBase() = default; + +#if defined(MODULE_NAME) + static constexpr const char *get_name_static() { return MODULE_NAME; } +#endif // MODULE_NAME /** * @brief main Main entry point to the module that should be @@ -132,6 +263,7 @@ public: strcmp(argv[1], "help") == 0 || strcmp(argv[1], "info") == 0 || strcmp(argv[1], "usage") == 0) { + return T::print_usage(); } @@ -141,18 +273,14 @@ public: } if (strcmp(argv[1], "status") == 0) { - return status_command(); + return moduleStatus(get_name_static()); } if (strcmp(argv[1], "stop") == 0) { - return stop_command(); + return moduleStop(get_name_static()); } - lock_module(); // Lock here, as the method could access _object. - int ret = T::custom_command(argc - 1, argv + 1); - unlock_module(); - - return ret; + return T::custom_command(argc - 1, argv + 1); } /** @@ -175,15 +303,19 @@ public: argv += 1; #endif + // lock module, instantiate will create module object and add to list + lock_module(); T *object = T::instantiate(argc, argv); - _object.store(object); if (object) { + object->set_task_id(px4_getpid()); + unlock_module(); object->run(); } else { PX4_ERR("failed to instantiate object"); ret = -1; + unlock_module(); } exit_and_cleanup(); @@ -201,9 +333,9 @@ public: static int start_command_base(int argc, char *argv[]) { int ret = 0; - lock_module(); + LockGuard lg(px4_modules_mutex); - if (is_running()) { + if (moduleRunning(get_name_static())) { ret = -1; PX4_ERR("Task already running"); @@ -215,102 +347,14 @@ public: } } - unlock_module(); return ret; } - /** - * @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. - */ - 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; - } - - /** - * @brief Handle 'command status': check if running and call print_status() if it is - * @return Returns 0 iff successful, -1 otherwise. - */ - 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; - } - - /** - * @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() - { - } + virtual void run() {} /** * @brief Returns the status of the module. @@ -318,118 +362,36 @@ public: */ static bool is_running() { - return _task_id != -1; + LockGuard lg(px4_modules_mutex); + return moduleRunning(get_name_static()); } 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(); - } + static void exit_and_cleanup() { moduleExitAndCleanup(get_name_static()); } /** * @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 i = 0; - - do { - /* Wait up to 1s. */ - px4_usleep(2500); - - } while (!_object.load() && ++i < 400); - - if (i == 400) { - PX4_ERR("Timed out while waiting for thread to start"); - return -1; - } - - return 0; - } + static int wait_until_running() { return moduleWaitUntilRunning(get_name_static()); } /** * @brief Get the module's object instance, (this is null if it's not running). */ static T *get_instance() { - return (T *)_object.load(); + LockGuard lg(px4_modules_mutex); + return (T *)moduleInstance(get_name_static()); } - /** - * @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/common/module.cpp b/platforms/common/module.cpp index 3ca0b6a748..ae53ac875f 100644 --- a/platforms/common/module.cpp +++ b/platforms/common/module.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2017-2020 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 @@ -43,8 +43,149 @@ #include #include #include +#include pthread_mutex_t px4_modules_mutex = PTHREAD_MUTEX_INITIALIZER; +List px4_modules_list; + +ModuleBaseInterface *moduleInstance(const char *name) +{ + // search list + for (ModuleBaseInterface *module : px4_modules_list) { + if (strcmp(module->get_name(), name) == 0) { + return module; + } + } + + return nullptr; +} + +bool moduleRunning(const char *name) +{ + // search list + ModuleBaseInterface *module = moduleInstance(name); + + if (module != nullptr) { + return module->running(); + } + + return false; +} + +int moduleStop(const char *name) +{ + bool ret = 0; + pthread_mutex_lock(&px4_modules_mutex); + + if (moduleRunning(name)) { + + ModuleBaseInterface *object = nullptr; + unsigned int i = 0; + + do { + // search for module again to request stop + object = moduleInstance(name); + + if (object != nullptr) { + object->request_stop(); + + pthread_mutex_unlock(&px4_modules_mutex); + px4_usleep(20000); // 20 ms + pthread_mutex_lock(&px4_modules_mutex); + + // search for module again to check status + object = moduleInstance(name); + + if (++i > 100 && (object != nullptr)) { // wait at most 2 sec + const int task_id = object->task_id(); + + if (task_id != task_id_is_work_queue) { + px4_task_delete(task_id); + } + + delete object; + object = nullptr; + + ret = -1; + break; + } + } + } while (object != nullptr); + } + + pthread_mutex_unlock(&px4_modules_mutex); + return ret; +} + +void moduleExitAndCleanup(const char *name) +{ + // 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. + LockGuard lg(px4_modules_mutex); + delete moduleInstance(name); +} + +bool moduleWaitUntilRunning(const char *name) +{ + int i = 0; + + do { + // Wait up to 1s. + px4_usleep(2500); + + } while (!moduleInstance(name) && ++i < 400); + + if (i == 400) { + PX4_ERR("Timed out while waiting for %s thread to start", name); + return false; + } + + return true; +} + +int moduleStatus(const char *name) +{ + LockGuard lg(px4_modules_mutex); + ModuleBaseInterface *object = moduleInstance(name); + + if (object && object->running()) { + return object->print_status(); + + } else { + PX4_INFO("%s not running", name); + } + + return -1; +} + +void modulesStatusAll() +{ + LockGuard lg(px4_modules_mutex); + + for (ModuleBaseInterface *module : px4_modules_list) { + if (module->task_id() == task_id_is_work_queue) { + PX4_INFO("Running: %s (WQ)", module->get_name()); + + } else if (module->task_id() > 0) { + PX4_INFO("Running: %s (PID: %d)", module->get_name(), module->task_id()); + + } else { + PX4_ERR("Invalid task id: %s (ID: %d)", module->get_name(), module->task_id()); + } + } +} + +void modulesStopAll() +{ + LockGuard lg(px4_modules_mutex); + + for (ModuleBaseInterface *module : px4_modules_list) { + PX4_INFO("Stopping: %s", module->get_name()); + module->request_stop(); + } +} #ifndef __PX4_NUTTX diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 81f474a0a9..fcb1fcd91f 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -68,24 +68,15 @@ uorb status # stop all echo "Stopping all modules" +modules stop-all logger stop -pwm_out_sim stop -mc_rate_control stop -mc_att_control stop -fw_att_control stop -mc_pos_control stop -fw_pos_control_l1 stop -navigator stop commander stop -land_detector stop -ekf2 stop -airspeed_selector stop -sensors stop simulator stop tone_alarm stop dataman stop +sleep 2 #uorb stop shutdown diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 63cad5f147..27b848618b 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -278,7 +278,7 @@ int ADC::custom_command(int argc, char *argv[]) if (!strcmp(verb, "test")) { if (is_running()) { - return _object.load()->test(); + return get_instance()->test(); } return PX4_ERROR; @@ -292,8 +292,7 @@ int ADC::task_spawn(int argc, char *argv[]) ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init() == PX4_OK) { return PX4_OK; @@ -304,8 +303,6 @@ int ADC::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/drivers/distance_sensor/pga460/pga460.cpp b/src/drivers/distance_sensor/pga460/pga460.cpp index 3d54d74926..a30f5afc99 100644 --- a/src/drivers/distance_sensor/pga460/pga460.cpp +++ b/src/drivers/distance_sensor/pga460/pga460.cpp @@ -750,12 +750,9 @@ int PGA460::task_spawn(int argc, char *argv[]) entry_point, (char *const *)argv); if (task_id < 0) { - task_id = -1; return -errno; } - _task_id = task_id; - return PX4_OK; } diff --git a/src/drivers/distance_sensor/srf05/SRF05.cpp b/src/drivers/distance_sensor/srf05/SRF05.cpp index 36d3ed66fd..92aedbf63a 100644 --- a/src/drivers/distance_sensor/srf05/SRF05.cpp +++ b/src/drivers/distance_sensor/srf05/SRF05.cpp @@ -193,8 +193,7 @@ 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; + instance->set_task_id(task_id_is_work_queue); if (instance->init() == PX4_OK) { return PX4_OK; @@ -205,8 +204,6 @@ int SRF05::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 1d760da467..8075b51eb8 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -471,8 +471,7 @@ DShotOutput::task_spawn(int argc, char *argv[]) DShotOutput *instance = new DShotOutput(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init() == PX4_OK) { return PX4_OK; @@ -483,8 +482,6 @@ DShotOutput::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7d5c30a114..d3f8699da8 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1131,10 +1131,6 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) return -errno; } - if (instance == Instance::Main) { - _task_id = task_id; - } - return 0; } diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index e7a545c3f4..81af105144 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -185,8 +185,7 @@ int Heater::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(heater); - _task_id = task_id_is_work_queue; + heater->set_task_id(task_id_is_work_queue); heater->start(); diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index bd49b00266..a5f29fc1d5 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -494,8 +494,7 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) { auto *instance = new PWMDriverWrapper(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); int ch; int address=PCA9685_DEFAULT_ADDRESS; @@ -541,8 +540,6 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) { driverInstanceAllocFailed: delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index bdb798c291..f3eab942fc 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -43,8 +43,7 @@ PWMIN::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(pwmin); - _task_id = task_id_is_work_queue; + pwmin->set_task_id(task_id_is_work_queue); pwmin->start(); @@ -61,7 +60,6 @@ PWMIN::start() timer_init(); } - void PWMIN::timer_init(void) { diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 27df7483f1..c399ed08f8 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -507,8 +507,7 @@ int PWMOut::task_spawn(int argc, char *argv[]) PWMOut *instance = new PWMOut(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init() == PX4_OK) { return PX4_OK; @@ -519,8 +518,6 @@ int PWMOut::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 86d2f57cdd..e42be11336 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -259,8 +259,7 @@ PWMSim::task_spawn(int argc, char *argv[]) return -1; } - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); instance->ScheduleNow(); return 0; } diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 11a6ad6d1b..83f9902a74 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -151,8 +151,7 @@ RCInput::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); instance->ScheduleOnInterval(_current_update_interval); diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp index 8c7babd4f9..becea1656a 100644 --- a/src/drivers/safety_button/SafetyButton.cpp +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -234,6 +234,7 @@ SafetyButton::task_spawn(int argc, char *argv[]) return -1; } + instance->set_task_id(task_id_is_work_queue); int ret = instance->Start(); if (ret != PX4_OK) { @@ -241,9 +242,6 @@ SafetyButton::task_spawn(int argc, char *argv[]) return ret; } - _object.store(instance); - _task_id = task_id_is_work_queue; - return ret; } diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index ce782f81c3..c737308b92 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -721,22 +721,20 @@ void TAP_ESC::run() int TAP_ESC::task_spawn(int argc, char *argv[]) { /* start the task */ - _task_id = px4_task_spawn_cmd("tap_esc", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1180, - (px4_main_t)&run_trampoline, - argv); + int task_id = px4_task_spawn_cmd("tap_esc", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 1180, + (px4_main_t)&run_trampoline, + argv); - if (_task_id < 0) { + if (task_id < 0) { PX4_ERR("task start failed"); - _task_id = -1; return PX4_ERROR; } // wait until task is up & running if (wait_until_running() < 0) { - _task_id = -1; return -1; } diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 5dc7ef81c3..c22b6b0eb7 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -228,14 +228,13 @@ int ToneAlarm::task_spawn(int argc, char *argv[]) return -1; } + instance->set_task_id(task_id_is_work_queue); + if (!instance->Init()) { delete instance; return PX4_ERROR; } - _object.store(instance); - _task_id = task_id_is_work_queue; - return PX4_OK; } diff --git a/src/examples/fake_gyro/FakeGyro.cpp b/src/examples/fake_gyro/FakeGyro.cpp index 3de0245521..bbe100b3db 100644 --- a/src/examples/fake_gyro/FakeGyro.cpp +++ b/src/examples/fake_gyro/FakeGyro.cpp @@ -84,8 +84,7 @@ int FakeGyro::task_spawn(int argc, char *argv[]) FakeGyro *instance = new FakeGyro(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -96,8 +95,6 @@ int FakeGyro::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 12547cd853..ffe5d77b60 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -98,8 +98,7 @@ int FakeMagnetometer::task_spawn(int argc, char *argv[]) FakeMagnetometer *instance = new FakeMagnetometer(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -110,8 +109,6 @@ int FakeMagnetometer::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/examples/gyro_fft/GyroFFT.cpp b/src/examples/gyro_fft/GyroFFT.cpp index 17d8ef4043..871867453e 100644 --- a/src/examples/gyro_fft/GyroFFT.cpp +++ b/src/examples/gyro_fft/GyroFFT.cpp @@ -371,8 +371,7 @@ int GyroFFT::task_spawn(int argc, char *argv[]) GyroFFT *instance = new GyroFFT(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -383,8 +382,6 @@ int GyroFFT::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/examples/work_item/WorkItemExample.cpp b/src/examples/work_item/WorkItemExample.cpp index 674c174228..275815d20e 100644 --- a/src/examples/work_item/WorkItemExample.cpp +++ b/src/examples/work_item/WorkItemExample.cpp @@ -96,8 +96,7 @@ int WorkItemExample::task_spawn(int argc, char *argv[]) WorkItemExample *instance = new WorkItemExample(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -108,8 +107,6 @@ int WorkItemExample::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index db4f81338c..a8cb4934dc 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -146,8 +146,7 @@ int AirshipAttitudeControl::task_spawn(int argc, char *argv[]) AirshipAttitudeControl *instance = new AirshipAttitudeControl(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -158,8 +157,6 @@ int AirshipAttitudeControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index cc7f8b9f2a..233f024dbd 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -205,13 +205,12 @@ AirspeedModule::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - _object.store(dev); + dev->set_task_id(task_id_is_work_queue); dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); - _task_id = task_id_is_work_queue; return PX4_OK; - } + void AirspeedModule::init() { diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index c4153f9dff..0b34996624 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -581,8 +581,7 @@ AttitudeEstimatorQ::task_spawn(int argc, char *argv[]) AttitudeEstimatorQ *instance = new AttitudeEstimatorQ(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -593,8 +592,6 @@ AttitudeEstimatorQ::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index a3aaf1b5b2..ee4b934dd6 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -260,8 +260,7 @@ BatteryStatus::task_spawn(int argc, char *argv[]) BatteryStatus *instance = new BatteryStatus(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -272,8 +271,6 @@ BatteryStatus::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/camera_feedback/CameraFeedback.cpp b/src/modules/camera_feedback/CameraFeedback.cpp index d1487ab478..984a8b1184 100644 --- a/src/modules/camera_feedback/CameraFeedback.cpp +++ b/src/modules/camera_feedback/CameraFeedback.cpp @@ -125,8 +125,7 @@ CameraFeedback::task_spawn(int argc, char *argv[]) CameraFeedback *instance = new CameraFeedback(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -137,8 +136,6 @@ CameraFeedback::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9e5b86f617..5ec720d356 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3712,15 +3712,14 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 40, - 3250, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 40, + 3250, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; } diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 9ad04b0c45..c37ed2aa4c 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -121,8 +121,7 @@ int EscBattery::task_spawn(int argc, char *argv[]) EscBattery *instance = new EscBattery(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -133,8 +132,6 @@ int EscBattery::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index bc92d44117..4eadc85c3f 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -49,17 +49,16 @@ static constexpr uint32_t SEND_EVENT_INTERVAL_US{1_s / 30}; int SendEvent::task_spawn(int argc, char *argv[]) { - SendEvent *send_event = new SendEvent(); + SendEvent *instance = new SendEvent(); - if (!send_event) { + if (!instance) { PX4_ERR("alloc failed"); return PX4_ERROR; } - _object.store(send_event); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); - send_event->start(); + instance->start(); return 0; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 6498b97e1c..a799d69976 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -731,8 +731,7 @@ int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -743,8 +742,6 @@ int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 1372e070ab..bf57e043d7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -88,6 +88,8 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + void request_stop() override { _task_should_exit.store(true); ScheduleNow(); } + bool init(); private: diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a16ffcae80..82ec57d541 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1925,8 +1925,7 @@ int FixedwingPositionControl::task_spawn(int argc, char *argv[]) FixedwingPositionControl *instance = new FixedwingPositionControl(vtol); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -1937,8 +1936,6 @@ int FixedwingPositionControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index a64bdc9e00..b4127858eb 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -95,9 +95,7 @@ int LandDetector::task_spawn(int argc, char *argv[]) strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1); _currentMode[sizeof(_currentMode) - 1] = '\0'; - _object.store(obj); - _task_id = task_id_is_work_queue; - + obj->set_task_id(task_id_is_work_queue); obj->start(); return PX4_OK; diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index ea22b60f59..7e4d9c9cb0 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -63,18 +63,17 @@ LoadMon::~LoadMon() int LoadMon::task_spawn(int argc, char *argv[]) { - LoadMon *obj = new LoadMon(); + LoadMon *instance = new LoadMon(); - if (!obj) { + if (!instance) { PX4_ERR("alloc failed"); return -1; } - _object.store(obj); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); /* Schedule a cycle to start things. */ - obj->start(); + instance->start(); return 0; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index fb4878e7ab..4b32c159fe 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -1026,8 +1026,7 @@ BlockLocalPositionEstimator::task_spawn(int argc, char *argv[]) BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -1038,8 +1037,6 @@ BlockLocalPositionEstimator::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ca86b2c2e1..1b00f84be8 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -159,15 +159,14 @@ int Logger::custom_command(int argc, char *argv[]) int Logger::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("logger", - SCHED_DEFAULT, - SCHED_PRIORITY_LOG_CAPTURE, - 3700, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("logger", + SCHED_DEFAULT, + SCHED_PRIORITY_LOG_CAPTURE, + 3700, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; } diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 589ee046c7..0c32aaee16 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -79,6 +79,8 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + void request_stop() override { ModuleBaseInterface::request_stop(); ScheduleNow(); } + bool init(); private: diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 08ab125356..645f256261 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -368,8 +368,7 @@ int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -380,8 +379,6 @@ int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 6851e9f41a..c63d94d5c5 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -227,8 +227,7 @@ int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[]) MulticopterHoverThrustEstimator *instance = new MulticopterHoverThrustEstimator(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -239,8 +238,6 @@ int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 7abc8be07f..8777def6d4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -798,8 +798,7 @@ int MulticopterPositionControl::task_spawn(int argc, char *argv[]) MulticopterPositionControl *instance = new MulticopterPositionControl(vtol); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -810,8 +809,6 @@ int MulticopterPositionControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 5974a86962..d13c4bd749 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -328,8 +328,7 @@ int MulticopterRateControl::task_spawn(int argc, char *argv[]) MulticopterRateControl *instance = new MulticopterRateControl(vtol); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -340,8 +339,6 @@ int MulticopterRateControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 31b86237e5..820d598d22 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -88,7 +88,7 @@ class Navigator : public ModuleBase, public ModuleParams { public: Navigator(); - ~Navigator() override; + virtual ~Navigator() override; Navigator(const Navigator &) = delete; Navigator operator=(const Navigator &) = delete; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2277ff23c7..8ea76424b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -740,15 +740,14 @@ Navigator::run() int Navigator::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("navigator", - SCHED_DEFAULT, - SCHED_PRIORITY_NAVIGATION, - 1800, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_NAVIGATION, + 1800, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; } diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 9c9469e4f1..ab6cf98799 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -559,8 +559,7 @@ RCUpdate::task_spawn(int argc, char *argv[]) RCUpdate *instance = new RCUpdate(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -571,8 +570,6 @@ RCUpdate::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 8869b0a153..9edf19a258 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -1011,15 +1011,14 @@ Replay::task_spawn(int argc, char *argv[]) return -1; } - _task_id = px4_task_spawn_cmd("replay", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("replay", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a19d9b1989..db7c9f7506 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -539,14 +539,14 @@ RoverPositionControl::run() int RoverPositionControl::task_spawn(int argc, char *argv[]) { /* start the task */ - _task_id = px4_task_spawn_cmd("rover_pos_ctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_POSITION_CONTROL, - 1700, - (px4_main_t)&RoverPositionControl::run_trampoline, - nullptr); + int task_id = px4_task_spawn_cmd("rover_pos_ctrl", + SCHED_DEFAULT, + SCHED_PRIORITY_POSITION_CONTROL, + 1700, + (px4_main_t)&RoverPositionControl::run_trampoline, + nullptr); - if (_task_id < 0) { + if (task_id < 0) { warn("task start failed"); return -errno; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 326e9f4d63..0a91f89021 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -684,8 +684,7 @@ int Sensors::task_spawn(int argc, char *argv[]) Sensors *instance = new Sensors(hil_enabled); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -696,8 +695,6 @@ int Sensors::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 3d8082fd7e..62cb435f26 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -64,15 +64,14 @@ int Sih::custom_command(int argc, char *argv[]) int Sih::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("sih", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1024, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("sih", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 4faa956791..14fc253e9d 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -110,8 +110,7 @@ int BatterySimulator::task_spawn(int argc, char *argv[]) BatterySimulator *instance = new BatterySimulator(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -122,8 +121,6 @@ int BatterySimulator::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 4c6181e224..4d8e84775e 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -262,8 +262,7 @@ int TemperatureCompensationModule::task_spawn(int argc, char *argv[]) TemperatureCompensationModule *instance = new TemperatureCompensationModule(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -274,8 +273,6 @@ int TemperatureCompensationModule::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index dfd93722fb..fc2c1cf05b 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -272,8 +272,7 @@ int UUVAttitudeControl::task_spawn(int argc, char *argv[]) UUVAttitudeControl *instance = new UUVAttitudeControl(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -284,8 +283,6 @@ int UUVAttitudeControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0a745f211d..1e473ccb59 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -499,8 +499,7 @@ VtolAttitudeControl::task_spawn(int argc, char *argv[]) VtolAttitudeControl *instance = new VtolAttitudeControl(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + instance->set_task_id(task_id_is_work_queue); if (instance->init()) { return PX4_OK; @@ -511,8 +510,6 @@ VtolAttitudeControl::task_spawn(int argc, char *argv[]) } delete instance; - _object.store(nullptr); - _task_id = -1; return PX4_ERROR; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 25259d54d2..c7c9340d4e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -102,6 +102,8 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + void request_stop() override { _task_should_exit.store(true); ScheduleNow(); } + bool init(); bool is_fixed_wing_requested(); diff --git a/src/systemcmds/modules/CMakeLists.txt b/src/systemcmds/modules/CMakeLists.txt new file mode 100644 index 0000000000..aabf23402e --- /dev/null +++ b/src/systemcmds/modules/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__modules + MAIN modules + COMPILE_FLAGS + SRCS + modules.cpp + DEPENDS + ) diff --git a/src/systemcmds/modules/modules.cpp b/src/systemcmds/modules/modules.cpp new file mode 100644 index 0000000000..6b91bb9b0f --- /dev/null +++ b/src/systemcmds/modules/modules.cpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 modules.cpp + * + * Modules tool. + */ + +#include +#include +#include +#include + +__BEGIN_DECLS +__EXPORT int modules_main(int argc, char *argv[]); +__END_DECLS + +static void print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("modules", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status of all running modules in the system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop-all", "Stop all modules"); +} + +int +modules_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "status")) { + modulesStatusAll(); + return PX4_OK; + } + + if (!strcmp(argv[1], "stop-all")) { + modulesStopAll(); + return PX4_OK; + } + } + + print_usage(); + return 1; +} diff --git a/src/templates/template_module/template_module.cpp b/src/templates/template_module/template_module.cpp index ad087d40ea..8b98435fdf 100644 --- a/src/templates/template_module/template_module.cpp +++ b/src/templates/template_module/template_module.cpp @@ -70,15 +70,14 @@ int TemplateModule::custom_command(int argc, char *argv[]) int TemplateModule::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("module", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (px4_main_t)&run_trampoline, - (char *const *)argv); + int task_id = px4_task_spawn_cmd("module", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; + if (task_id < 0) { return -errno; }