ModuleBase add common base type and cleanup

.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"'

boards/aerotenna/ocpoc/default.cmake

@@ -71,6 +71,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf

boards/airmind/mindpx-v2/default.cmake

@@ -88,6 +88,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/atlflight/eagle/default.cmake

@@ -95,6 +95,7 @@ px4_add_board(
 		#hardfault_log
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd

boards/atlflight/excelsior/default.cmake

@@ -93,6 +93,7 @@ px4_add_board(
 		#hardfault_log
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd

boards/av/x-v1/default.cmake

@@ -88,6 +88,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		nshterm

boards/beaglebone/blue/default.cmake

@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf

boards/bitcraze/crazyflie/default.cmake

@@ -44,6 +44,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/emlid/navio2/default.cmake

@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf

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);
+		instance->set_task_id(task_id_is_work_queue);
-		_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;
 }

boards/holybro/durandal-v1/default.cmake

@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/intel/aerofc-v1/default.cmake

@@ -69,6 +69,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/intel/aerofc-v1/rtps.cmake

@@ -69,6 +69,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/modalai/fc-v1/default.cmake

@@ -87,6 +87,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/mro/ctrl-zero-f7/default.cmake

@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/mro/x21/default.cmake

@@ -90,6 +90,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/nxp/fmuk66-v3/default.cmake

@@ -89,6 +89,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v3/default.cmake

@@ -103,6 +103,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v3/rtps.cmake

@@ -99,6 +99,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v3/stackcheck.cmake

@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v4/default.cmake

@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v4/rtps.cmake

@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v4/stackcheck.cmake

@@ -94,6 +94,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v4pro/default.cmake

@@ -96,6 +96,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v4pro/rtps.cmake

@@ -93,6 +93,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/critmonitor.cmake

@@ -97,6 +97,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/debug.cmake

@@ -102,6 +102,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/default.cmake

@@ -103,6 +103,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/fixedwing.cmake

@@ -72,6 +72,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/irqmonitor.cmake

@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/multicopter.cmake

@@ -83,6 +83,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/rover.cmake

@@ -72,6 +72,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/rtps.cmake

@@ -98,6 +98,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5/stackcheck.cmake

@@ -102,6 +102,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/fmu-v5x/default.cmake

@@ -100,6 +100,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

boards/px4/raspberrypi/default.cmake

@@ -67,6 +67,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		param
 		perf

boards/px4/sitl/default.cmake

@@ -63,6 +63,7 @@ px4_add_board(
 		failure
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd

boards/px4/sitl/rtps.cmake

@@ -62,6 +62,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd

boards/px4/sitl/test.cmake

@@ -60,6 +60,7 @@ px4_add_board(
 		esc_calib
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		#mtd

boards/uvify/core/default.cmake

@@ -71,6 +71,7 @@ px4_add_board(
 		i2cdetect
 		led_control
 		mixer
+		modules
 		motor_ramp
 		motor_test
 		mtd

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 <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)
+ *              // 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
- *              // on error return != 0 (and _task_id must be -1)
  *      }
  *
  *      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)
+ *              // instance->set_task_id(task_id_is_work_queue); and return 0
- *              // set _task_id to task_id_is_work_queue and return 0
+ *              // on error return != 0
- *              // on error return != 0 (and _task_id must be -1)
  *      }
  */
 template<class T>
-class ModuleBase
+class ModuleBase : public ModuleBaseInterface
 {
 public:
-	ModuleBase() : _task_should_exit{false} {}
+	ModuleBase() : ModuleBaseInterface(get_name_static())
-	virtual ~ModuleBase() {}
+	{}
+
+	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.
+		return T::custom_command(argc - 1, argv + 1);
-		int ret = T::custom_command(argc - 1, argv + 1);
-		unlock_module();
-
-		return ret;
 	}
 
 	/**
@@ -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()
+	static void exit_and_cleanup() { moduleExitAndCleanup(get_name_static()); }
-	{
-		// Take the lock here:
-		// - if startup fails and we're faster than the parent thread, it will set
-		//   _task_id and subsequently it will look like the task is running.
-		// - deleting the object must take place inside the lock.
-		lock_module();
-
-		delete _object.load();
-		_object.store(nullptr);
-
-		_task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue.
-		unlock_module();
-	}
 
 	/**
 	 * @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn().
 	 * @return Returns 0 iff successful, -1 on timeout or otherwise.
 	 */
-	static int wait_until_running()
+	static int wait_until_running() { return moduleWaitUntilRunning(get_name_static()); }
-	{
-		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;
-	}
 
 	/**
 	 * @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 */
 
 

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 <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
 

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

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);
+		instance->set_task_id(task_id_is_work_queue);
-		_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;
 }

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

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);
+		instance->set_task_id(task_id_is_work_queue);
-		_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;
 }

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);
+		instance->set_task_id(task_id_is_work_queue);
-		_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;
 }

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

src/drivers/heater/heater.cpp

@@ -185,8 +185,7 @@ int Heater::task_spawn(int argc, char *argv[])
 		return PX4_ERROR;
 	}
 
-	_object.store(heater);
+	heater->set_task_id(task_id_is_work_queue);
-	_task_id = task_id_is_work_queue;
 
 	heater->start();
 

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);
+        instance->set_task_id(task_id_is_work_queue);
-        _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;
 }