diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake
index 778cfab3ecf0..3d7178c68eb7 100644
--- a/platforms/posix/cmake/sitl_tests.cmake
+++ b/platforms/posix/cmake/sitl_tests.cmake
@@ -50,17 +50,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")
@@ -71,14 +72,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")
@@ -88,14 +87,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")
@@ -104,15 +101,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}
+)
 set_tests_properties(dyn PROPERTIES PASS_REGULAR_EXPRESSION "1: PASSED")
 sanitizer_fail_test_on_error(dyn)
 
@@ -124,17 +119,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")
diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp
index 25da2c5c6206..ba57343d045a 100644
--- a/platforms/posix/src/main.cpp
+++ b/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,15 @@ 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;
+
+	system_exit(0);
+}
diff --git a/platforms/posix/src/px4_layer/px4_posix_impl.cpp b/platforms/posix/src/px4_layer/px4_posix_impl.cpp
index 9c1cf6504214..ad1e3ad92557 100644
--- a/platforms/posix/src/px4_layer/px4_posix_impl.cpp
+++ b/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()
diff --git a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp
index bb428eae64fa..8b477b1c7675 100644
--- a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp
+++ b/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[])
 {
diff --git a/src/platforms/apps.cpp.in b/src/platforms/apps.cpp.in
index 9f32d1505c0d..a1eef9a31b23 100644
--- a/src/platforms/apps.cpp.in
+++ b/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();