ModuleBase add common base type and cleanup

2026-05-24 04:17:34 +08:00 · 2020-11-10 14:03:58 -05:00
96 changed files with 576 additions and 428 deletions
@@ -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"'
@@ -71,6 +71,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf
@@ -88,6 +88,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -95,6 +95,7 @@ px4_add_board(
 		#hardfault_log
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd
@@ -93,6 +93,7 @@ px4_add_board(
 		#hardfault_log
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd
@@ -88,6 +88,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		nshterm
@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf
@@ -44,6 +44,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf
@@ -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;
 }
@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -69,6 +69,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -69,6 +69,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -87,6 +87,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -90,6 +90,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -89,6 +89,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -103,6 +103,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -99,6 +99,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -96,6 +96,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -97,6 +97,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -102,6 +102,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -103,6 +103,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -72,6 +72,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -83,6 +83,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -72,6 +72,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -102,6 +102,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -100,6 +100,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf
@@ -63,6 +63,7 @@ px4_add_board(
 		failure
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd
@@ -62,6 +62,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd
@@ -60,6 +60,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd
@@ -71,6 +71,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd
@@ -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 <px4_platform_common/atomic.h>
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/log.h>
+#include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
 #include <systemlib/px4_macros.h>

 #ifdef __cplusplus

+#include <containers/List.hpp>
+#include <containers/LockGuard.hpp>
+
 #include <cstring>

+/** @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<ModuleBaseInterface *> 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<ModuleBaseInterface *>
+{
+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 T>
-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<T *> _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<class T>
-px4::atomic<T *> ModuleBase<T>::_object{nullptr};
-
-template<class T>
-int ModuleBase<T>::_task_id = -1;
-
-
 #endif /* __cplusplus */


@@ -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 <px4_platform_common/module.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
+#include <containers/LockGuard.hpp>

 pthread_mutex_t px4_modules_mutex = PTHREAD_MUTEX_INITIALIZER;
+List<ModuleBaseInterface *> 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

@@ -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
@@ -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;
 }
@@ -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;
 }

@@ -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;
 }
@@ -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;
 }
@@ -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;
 }

@@ -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();

@@ -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;
 }
@@ -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)
 {
@@ -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;
 }
@@ -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;
 }
@@ -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);

@@ -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;
 }

@@ -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;
 	}

@@ -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;
 }

@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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()
 {
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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;
 	}

@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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:
@@ -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;
 }
@@ -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;
@@ -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;
 }
@@ -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;
 }
@@ -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;
 	}

@@ -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:
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -88,7 +88,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
 public:
 	Navigator();
-	~Navigator() override;
+	virtual ~Navigator() override;

 	Navigator(const Navigator &) = delete;
 	Navigator operator=(const Navigator &) = delete;
@@ -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;
 	}

@@ -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;
 }
@@ -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;
 	}

@@ -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;
 	}
@@ -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;
 }
@@ -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;
 	}

@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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;
 }
@@ -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();
@@ -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
+	)
@@ -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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/posix.h>
+
+__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;
+}
@@ -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;
 	}