Add reboot command for SITL

CMakeLists.txt

@@ -189,6 +189,7 @@ endif()
 
 include(px4_config)
 include(kconfig)
+message(STATUS "PX4 build type: ${CMAKE_BUILD_TYPE}")
 message(STATUS "PX4 config: ${PX4_CONFIG}")
 message(STATUS "PX4 platform: ${PX4_PLATFORM}")
 
@@ -311,6 +312,11 @@ if(${PX4_PLATFORM} STREQUAL "posix")
 		include(coverage)
 	endif()
 
+	if(CMAKE_BUILD_TYPE MATCHES "Debug")
+	set(CMAKE_CXX_STANDARD 23)
+	link_libraries(-lstdc++exp)
+	endif()
+
 	include(sanitizers)
 
 	# Define GNU standard installation directories

Kconfig

@@ -40,6 +40,12 @@ menu "Toolchain"
         help
             forces nolockstep behaviour, despite REPLAY env variable
 
+    config BOARD_REBOOT
+        bool "Enable reboot"
+        depends on PLATFORM_POSIX
+        help
+            enables the reboot command
+
     config BOARD_LINUX_TARGET
         bool "Linux OS Target"
         depends on PLATFORM_POSIX

boards/px4/sitl/default.px4board

@@ -1,4 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
+CONFIG_BOARD_REBOOT=y
 CONFIG_BOARD_TESTING=y
 CONFIG_BOARD_ETHERNET=y
 CONFIG_BOARD_ROOT_PATH="."
@@ -66,6 +67,7 @@ CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SHUTDOWN=y
+CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y

boards/px4/sitl/src/board_shutdown.cpp

@@ -50,7 +50,6 @@ int board_register_power_state_notification_cb(power_button_state_notification_t
 
 int board_power_off(int status)
 {
-	printf("Exiting NOW.\n");
 	fflush(stdout);
 	system_exit(0);
 	return 0;

cmake/px4_add_common_flags.cmake

@@ -101,6 +101,7 @@ function(px4_add_common_flags)
 
 			-Qunused-arguments
 
+      -Wno-vla-cxx-extension
 			-Wno-c99-designator
 			-Wno-unknown-warning-option
 			-Wno-unused-const-variable

disp.py

@@ -0,0 +1,9 @@
+#!/usr/bin/env/python
+import json
+content =open('/tmp/tracer.json', 'r').read()
+for entry in content.split('+++++++'):
+  if not entry.strip(): continue
+  e = json.loads(entry)
+  print(e['op'], e['args'])
+  print(e['bt'])
+

platforms/common/CMakeLists.txt

@@ -47,6 +47,7 @@ add_library(px4_platform STATIC
 	i2c.cpp
 	i2c_spi_buses.cpp
 	module.cpp
+	module_manager.cpp
 	px4_getopt.c
 	px4_cli.cpp
 	shutdown.cpp
@@ -56,6 +57,7 @@ add_library(px4_platform STATIC
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)
+target_compile_definitions(px4_platform PRIVATE MODULE_NAME="px4-platform")
 
 if(NOT "${PX4_BOARD}" MATCHES "io-v2")
 	add_subdirectory(uORB)

platforms/common/apps.cpp.in

@@ -2,6 +2,7 @@
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/log.h>
+#include <px4_platform_common/shutdown.h>
 
 #include "apps.h"
 
@@ -16,7 +17,6 @@
 extern "C" {
 
 ${builtin_apps_decl_string}
-int shutdown_main(int argc, char *argv[]);
 int list_tasks_main(int argc, char *argv[]);
 int list_files_main(int argc, char *argv[]);
 int sleep_main(int argc, char *argv[]);
@@ -40,12 +40,6 @@ void list_builtins(apps_map_type &apps)
 	}
 }
 
-int shutdown_main(int argc, char *argv[])
-{
-	printf("Exiting NOW.\n");
-	system_exit(0);
-}
-
 int list_tasks_main(int argc, char *argv[])
 {
 	px4_show_tasks();

platforms/common/include/px4_platform_common/events.h

@@ -94,6 +94,7 @@ constexpr unsigned sizeofArguments(const T &t, const Args &... args)
  * publish/send an event
  */
 void send(event_s &event);
+void reset_events();
 
 /**
  * Generate event ID from an event name

platforms/common/include/px4_platform_common/init.h

@@ -34,6 +34,7 @@
 __BEGIN_DECLS
 
 int px4_platform_init(void);
+int px4_platform_fini(void);
 void px4_platform_i2c_init(void);
 int px4_platform_console_init(void);
 int px4_platform_configure(void);

platforms/common/include/px4_platform_common/log.h

@@ -56,6 +56,7 @@ __BEGIN_DECLS
  * initialize the orb logging. Logging to console still works without or before calling this.
  */
 __EXPORT extern void px4_log_initialize(void);
+__EXPORT extern void px4_log_finalize(void);
 
 __END_DECLS
 

platforms/common/include/px4_platform_common/module.h

@@ -45,6 +45,9 @@
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/module_manager.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+
 #include <systemlib/px4_macros.h>
 
 #ifdef __cplusplus
@@ -115,7 +118,13 @@ template<class T>
 class ModuleBase
 {
 public:
-	ModuleBase() : _task_should_exit{false} {}
+
+	typedef ModuleBase<T> TBase;
+
+	ModuleBase() : _task_should_exit{false}
+	{
+		static_assert(std::is_base_of<ModuleBase<T>, T>::value, "Should be class T: ModuleBase<T>{...}");
+	}
 	virtual ~ModuleBase() {}
 
 	/**
@@ -127,6 +136,13 @@ class ModuleBase
 	 */
 	static int main(int argc, char *argv[])
 	{
+		ModuleManager::register_module(ModuleEntry {
+			.name = argv[0],
+			.stop_command = &stop_command,
+			.is_running = &is_running,
+			.request_stop = &static_request_stop,
+		});
+
 		if (argc <= 1 ||
 		    strcmp(argv[1], "-h")    == 0 ||
 		    strcmp(argv[1], "help")  == 0 ||
@@ -240,7 +256,7 @@ class ModuleBase
 					px4_usleep(10000); // 10 ms
 					lock_module();
 
-					if (++i > 500 && _task_id != -1) { // wait at most 5 sec
+					if (++i > 500 && _task_id != -1 && _object.load() != nullptr) { // wait at most 5 sec
 						PX4_ERR("timeout, forcing stop");
 
 						if (_task_id != task_id_is_work_queue) {
@@ -270,6 +286,23 @@ class ModuleBase
 		return ret;
 	}
 
+
+	static void static_request_stop()
+	{
+		lock_module();
+
+		if (is_running()) {
+			T *object = _object.load();
+
+			if (object) {
+				object->request_stop();
+			}
+		}
+
+		unlock_module();
+
+	}
+
 	/**
 	 * @brief Handle 'command status': check if running and call print_status() if it is
 	 * @return Returns 0 iff successful, -1 otherwise.
@@ -327,6 +360,12 @@ class ModuleBase
 	virtual void request_stop()
 	{
 		_task_should_exit.store(true);
+
+		if (std::is_base_of<px4::WorkItem, T>::value) {
+			px4::WorkItem *wi = dynamic_cast<px4::WorkItem *>((T *)this);
+			assert(wi != nullptr);
+			wi->ScheduleNow();
+		}
 	}
 
 	/**
@@ -394,7 +433,7 @@ class ModuleBase
 	static px4::atomic<T *> _object;
 
 	/** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */
-	static int _task_id;
+	volatile 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;
@@ -424,7 +463,7 @@ template<class T>
 px4::atomic<T *> ModuleBase<T>::_object{nullptr};
 
 template<class T>
-int ModuleBase<T>::_task_id = -1;
+volatile int ModuleBase<T>::_task_id = -1;
 
 
 #endif /* __cplusplus */

platforms/common/include/px4_platform_common/module_manager.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <pthread.h>
+#include <stdbool.h>
+#include <string>
+#include <unistd.h>
+#include <vector>
+#include <optional>
+
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+#include <systemlib/px4_macros.h>
+
+struct ModuleEntry {
+	std::string name;
+	int (*stop_command)();
+	bool (*is_running)();
+	void (*request_stop)();
+};
+
+class ModuleManager
+{
+public:
+	static void register_module(const ModuleEntry &entry);
+	static std::vector<std::string> get_running();
+	static void cleanup();
+	static const std::vector<ModuleEntry> &get_modules();
+	static const ModuleEntry *get_module(const std::string &name);
+};
+

platforms/common/include/px4_platform_common/posix.h

@@ -121,6 +121,7 @@ __EXPORT int		px4_ioctl(int fd, int cmd, unsigned long arg);
 __EXPORT int		px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout);
 __EXPORT int		px4_access(const char *pathname, int mode);
 __EXPORT px4_task_t	px4_getpid(void);
+__EXPORT void		px4_cleanup(void);
 
 __END_DECLS
 #else

platforms/common/include/px4_platform_common/px4_config.h

@@ -55,3 +55,9 @@
 
 /* PX4 board kconfig symbols */
 #include <px4_boardconfig.h>
+
+
+#if !defined(CONFIG_BOARD_REBOOT) && defined(CONFIG_BOARDCTL_RESET)
+#define CONFIG_BOARD_REBOOT 1
+#endif
+

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -61,6 +61,7 @@ class WorkQueue : public IntrusiveSortedListNode<WorkQueue *>
 
 	bool Attach(WorkItem *item);
 	void Detach(WorkItem *item);
+	void Deinit(WorkItem *item);
 
 	void Add(WorkItem *item);
 	void Remove(WorkItem *item);
@@ -94,6 +95,7 @@ class WorkQueue : public IntrusiveSortedListNode<WorkQueue *>
 	px4_sem_t _qlock;
 #endif
 
+	WorkItem *_current_work_item;
 	IntrusiveQueue<WorkItem *>	_q;
 	px4_sem_t			_process_lock;
 	px4_sem_t			_exit_lock;

platforms/common/include/px4_platform_common/shutdown.h

@@ -88,9 +88,9 @@ typedef enum {
  * @param delay_us optional delay in microseconds
  * @return 0 on success, <0 on error
  */
-#if defined(CONFIG_BOARDCTL_RESET)
+#if defined(CONFIG_BOARD_REBOOT)
 __EXPORT int px4_reboot_request(reboot_request_t request = REBOOT_REQUEST, uint32_t delay_us = 0);
-#endif // CONFIG_BOARDCTL_RESET
+#endif // CONFIG_BOARD_REBOOT
 
 
 /**

platforms/common/include/px4_platform_common/tasks.h

@@ -182,6 +182,7 @@ __EXPORT bool px4_task_is_running(const char *taskname);
 #ifdef __PX4_POSIX
 /** set process (and thread) options */
 __EXPORT int px4_prctl(int option, const char *arg2, px4_task_t pid);
+__EXPORT int px4_running_task_count(void);
 #endif
 
 /** return the name of the current task */