posix shutdown investigation/hacks [take 2]

platforms/posix/cmake/sitl_tests.cmake

@@ -54,17 +54,18 @@ if (CMAKE_SYSTEM_NAME STREQUAL "CYGWIN")
 endif()
 
 foreach(test_name ${tests})
-	configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated)
+	configure_file(
+		${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in
+		${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated
+		)
 
 	add_test(NAME ${test_name}
-		COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-			$<TARGET_FILE:px4>
-			none
-			none
-			test_${test_name}_generated
-			${PX4_SOURCE_DIR}
-			${PX4_BINARY_DIR}
-		WORKING_DIRECTORY ${SITL_WORKING_DIR})
+		COMMAND $<TARGET_FILE:px4> -d
+			-s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated
+			-t ${PX4_SOURCE_DIR}/test_data/
+			${PX4_SOURCE_DIR}/ROMFS/px4fmu_test
+		WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	)
 
 	set_tests_properties(${test_name} PROPERTIES FAIL_REGULAR_EXPRESSION "${test_name} FAILED")
 	set_tests_properties(${test_name} PROPERTIES PASS_REGULAR_EXPRESSION "${test_name} PASSED")
@@ -75,14 +76,12 @@ endforeach()
 
 # Mavlink test requires mavlink running
 add_test(NAME mavlink
-	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-		$<TARGET_FILE:px4>
-		none
-		none
-		test_mavlink
-		${PX4_SOURCE_DIR}
-		${PX4_BINARY_DIR}
-	WORKING_DIRECTORY ${SITL_WORKING_DIR})
+	COMMAND $<TARGET_FILE:px4> -d
+		-s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_mavlink
+		-t ${PX4_SOURCE_DIR}/test_data/
+		${PX4_SOURCE_DIR}/ROMFS/px4fmu_test
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}
+)
 
 set_tests_properties(mavlink PROPERTIES FAIL_REGULAR_EXPRESSION "mavlink FAILED")
 set_tests_properties(mavlink PROPERTIES PASS_REGULAR_EXPRESSION "mavlink PASSED")
@@ -92,14 +91,12 @@ sanitizer_fail_test_on_error(mavlink)
 if(NOT CMAKE_SYSTEM_NAME STREQUAL "CYGWIN")
 	# Shutdown test
 	add_test(NAME shutdown
-		COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-			$<TARGET_FILE:px4>
-			none
-			none
-			test_shutdown
-			${PX4_SOURCE_DIR}
-			${PX4_BINARY_DIR}
-		WORKING_DIRECTORY ${SITL_WORKING_DIR})
+		COMMAND $<TARGET_FILE:px4> -d
+			-s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_shutdown
+			-t ${PX4_SOURCE_DIR}/test_data/
+			${PX4_SOURCE_DIR}/ROMFS/px4fmu_test
+		WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	)
 
 	#set_tests_properties(shutdown PROPERTIES FAIL_REGULAR_EXPRESSION "shutdown FAILED")
 	set_tests_properties(shutdown PROPERTIES PASS_REGULAR_EXPRESSION "Shutting down")
@@ -108,15 +105,13 @@ endif()
 
 # Dynamic module loading test
 add_test(NAME dyn
-	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-		$<TARGET_FILE:px4>
-		none
-		none
-		test_dyn_hello
-		${PX4_SOURCE_DIR}
-		${PX4_BINARY_DIR}
+	COMMAND $<TARGET_FILE:px4> -d
+		-s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_dyn_hello
+		-t ${PX4_SOURCE_DIR}/test_data/
+		${PX4_SOURCE_DIR}/ROMFS/px4fmu_test
 		$<TARGET_FILE:examples__dyn_hello>
-	WORKING_DIRECTORY ${SITL_WORKING_DIR})
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}/../src/examples/dyn_hello
+)
 set_tests_properties(dyn PROPERTIES PASS_REGULAR_EXPRESSION "1: PASSED")
 sanitizer_fail_test_on_error(dyn)
 
@@ -128,17 +123,18 @@ set(test_cmds
 	)
 
 foreach(cmd_name ${test_cmds})
-	configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/cmd_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/cmd_${cmd_name}_generated)
+	configure_file(
+		${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/cmd_template.in
+		${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/cmd_${cmd_name}_generated
+		)
 
 	add_test(NAME posix_${cmd_name}
-		COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-			$<TARGET_FILE:px4>
-			none
-			none
-			cmd_${cmd_name}_generated
-			${PX4_SOURCE_DIR}
-			${PX4_BINARY_DIR}
-		WORKING_DIRECTORY ${SITL_WORKING_DIR})
+		COMMAND $<TARGET_FILE:px4> -d
+			-s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/cmd_${cmd_name}_generated
+			-t ${PX4_SOURCE_DIR}/test_data/
+			${PX4_SOURCE_DIR}/ROMFS/px4fmu_test
+		WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	)
 
 	sanitizer_fail_test_on_error(posix_${cmd_name})
 	set_tests_properties(posix_${cmd_name} PROPERTIES PASS_REGULAR_EXPRESSION "Shutting down")

platforms/posix/src/main.cpp

@@ -84,15 +84,8 @@ static const char *LOCK_FILE_PATH = "/tmp/px4_lock";
 #define PATH_MAX 1024
 #endif
 
-
 static volatile bool _exit_requested = false;
 
-
-namespace px4
-{
-void init_once();
-}
-
 static void sig_int_handler(int sig_num);
 static void sig_fpe_handler(int sig_num);
 static void sig_segv_handler(int sig_num);
@@ -271,7 +264,6 @@ int main(int argc, char **argv)
 
 		DriverFramework::Framework::initialize();
 
-		px4::init_once();
 		px4::init(argc, argv, "px4");
 
 		ret = run_startup_script(commands_file, absolute_binary_path, instance);
@@ -297,6 +289,9 @@ int main(int argc, char **argv)
 		std::string cmd("shutdown");
 		px4_daemon::Pxh::process_line(cmd, true);
 
+		// remove any file lock
+		const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
+		remove(file_lock_path.c_str());
 	}
 
 	return PX4_OK;
@@ -426,8 +421,8 @@ void sig_int_handler(int sig_num)
 	fflush(stdout);
 	printf("\nExiting...\n");
 	fflush(stdout);
-	px4_daemon::Pxh::stop();
-	_exit_requested = true;
+
+	px4_systemreset(false);
 }
 
 void sig_fpe_handler(int sig_num)
@@ -436,8 +431,8 @@ void sig_fpe_handler(int sig_num)
 	printf("\nfloating point exception\n");
 	PX4_BACKTRACE();
 	fflush(stdout);
-	px4_daemon::Pxh::stop();
-	_exit_requested = true;
+
+	px4_systemreset(false);
 }
 
 void sig_segv_handler(int sig_num)
@@ -642,3 +637,16 @@ std::string pwd()
 	char temp[PATH_MAX];
 	return (getcwd(temp, PATH_MAX) ? std::string(temp) : std::string(""));
 }
+
+void
+px4_systemreset(bool to_bootloader)
+{
+	printf("Shutting down\n");
+
+	px4_daemon::Pxh::stop();
+
+	_exit_requested = true;
+
+	// Don't use system_exit so that all threads can return and be joined.
+	// system_exit(0);
+}

platforms/posix/src/px4_daemon/server.cpp

@@ -206,9 +206,7 @@ Server::_server_main()
 				auto thread = _fd_to_thread.find(poll_fds[i].fd);
 
 				if (thread != _fd_to_thread.end()) {
-					// Thread is still running, so we cancel it.
-					// TODO: use a more graceful exit method to avoid resource leaks
-					pthread_cancel(thread->second);
+					pthread_join(thread->second, nullptr);
 					_fd_to_thread.erase(thread);
 				}
 

platforms/posix/src/px4_layer/px4_posix_impl.cpp

@@ -53,8 +53,6 @@
 #include <pthread.h>
 #include <px4_init.h>
 
-extern pthread_t _shell_task_id;
-
 __BEGIN_DECLS
 
 long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
@@ -64,19 +62,6 @@ __END_DECLS
 namespace px4
 {
 
-void init_once();
-
-void init_once()
-{
-	_shell_task_id = pthread_self();
-	//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
-
-	work_queues_init();
-	hrt_work_queue_init();
-
-	px4_platform_init();
-}
-
 void init(int argc, char *argv[], const char *app_name)
 {
 	printf("\n");
@@ -96,6 +81,11 @@ void init(int argc, char *argv[], const char *app_name)
 #else
 	(void)pthread_setname_np(pthread_self(), app_name);
 #endif
+
+	work_queues_init();
+	hrt_work_queue_init();
+
+	px4_platform_init();
 }
 
 uint64_t get_time_micros()

platforms/posix/src/px4_layer/px4_posix_tasks.cpp

@@ -65,7 +65,6 @@
 #define PX4_MAX_TASKS 50
 #define SHELL_TASK_ID (PX4_MAX_TASKS+1)
 
-pthread_t _shell_task_id = 0;
 pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 struct task_entry {
@@ -111,13 +110,6 @@ static void *entry_adapter(void *ptr)
 	return nullptr;
 }
 
-void
-px4_systemreset(bool to_bootloader)
-{
-	PX4_WARN("Called px4_system_reset");
-	system_exit(0);
-}
-
 px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
 			      char *const argv[])
 {
@@ -303,7 +295,7 @@ int px4_task_delete(px4_task_t id)
 		pthread_exit(nullptr);
 
 	} else {
-		rv = pthread_cancel(pid);
+		rv = pthread_join(pid, nullptr);
 	}
 
 	taskmap[id].isused = false;

src/modules/simulator/simulator.cpp

@@ -248,7 +248,10 @@ extern "C" {
 				PX4_WARN("Simulator not running");
 
 			} else {
-				px4_task_delete(g_sim_task);
+				if (Simulator::getInstance()) {
+					Simulator::getInstance()->stop();
+				}
+
 				g_sim_task = -1;
 			}
 

src/modules/simulator/simulator.h

@@ -68,6 +68,7 @@
 #include <v2.0/mavlink_types.h>
 #include <v2.0/common/mavlink.h>
 #include <lib/ecl/geo/geo.h>
+#include <atomic>
 
 namespace simulator
 {
@@ -238,6 +239,8 @@ class Simulator : public ModuleParams
 	void set_ip(InternetProtocol ip);
 	void set_port(unsigned port);
 
+	void stop() { _should_exit = true; }
+
 private:
 	Simulator() :
 		ModuleParams(nullptr)
@@ -281,6 +284,9 @@ class Simulator : public ModuleParams
 
 	static Simulator *_instance;
 
+	// TODO: should replace this boilerplate with ModuleBase.
+	std::atomic_bool _should_exit{false};
+
 	// simulated sensor instances
 	simulator::Report<simulator::RawAirspeedData>	_airspeed{1};
 	simulator::Report<simulator::RawAccelData>	_accel{1};

src/modules/simulator/simulator_mavlink.cpp

@@ -612,7 +612,7 @@ void Simulator::send()
 	fds[0].fd = _actuator_outputs_sub;
 	fds[0].events = POLLIN;
 
-	while (true) {
+	while (!_should_exit) {
 		// Wait for up to 100ms for data.
 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
@@ -715,7 +715,7 @@ void Simulator::poll_for_MAVLink_messages()
 
 		PX4_INFO("Waiting for simulator to connect on UDP port %u", _port);
 
-		while (true) {
+		while (!_should_exit) {
 			// Once we receive something, we're most probably good and can carry on.
 			int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0,
 					     (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);
@@ -734,7 +734,7 @@ void Simulator::poll_for_MAVLink_messages()
 
 		PX4_INFO("Waiting for simulator to connect on TCP port %u", _port);
 
-		while (true) {
+		while (!_should_exit) {
 			if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 				PX4_ERR("Creating TCP socket failed: %s", strerror(errno));
 				return;
@@ -818,7 +818,7 @@ void Simulator::poll_for_MAVLink_messages()
 
 	_initialized = true;
 
-	while (true) {
+	while (!_should_exit) {
 
 		// wait for new mavlink messages to arrive
 		int pret = ::poll(&fds[0], fd_count, 1000);

src/platforms/apps.cpp.in

@@ -16,7 +16,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 list_devices_main(int argc, char *argv[]);
@@ -31,7 +30,6 @@ extern void px4_show_devices(void);
 void init_app_map(apps_map_type &apps)
 {
 ${builtin_apps_string}
-	apps["shutdown"] = shutdown_main;
 	apps["list_tasks"] = list_tasks_main;
 	apps["list_files"] = list_files_main;
 	apps["list_devices"] = list_devices_main;
@@ -48,12 +46,6 @@ void list_builtins(apps_map_type &apps)
 	}
 }
 
-int shutdown_main(int argc, char *argv[])
-{
-	printf("Shutting down\n");
-	system_exit(0);
-}
-
 int list_tasks_main(int argc, char *argv[])
 {
 	px4_show_tasks();

src/platforms/px4_tasks.h

@@ -153,7 +153,11 @@ __BEGIN_DECLS
 
 /** Reboots the board (without waiting for clean shutdown). Modules should use px4_shutdown_request() in most cases.
  */
+#ifdef __PX4_NUTTX
 __EXPORT void px4_systemreset(bool to_bootloader) noreturn_function;
+#else
+__EXPORT void px4_systemreset(bool to_bootloader); //noreturn_function;
+#endif
 
 /** Starts a task and performs any specific accounting, scheduler setup, etc. */
 __EXPORT px4_task_t px4_task_spawn_cmd(const char *name,