Merge branch 'main' into pr-kill-slpi-on-exit

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"
 
@@ -43,7 +44,9 @@ void list_builtins(apps_map_type &apps)
 int shutdown_main(int argc, char *argv[])
 {
 	printf("Exiting NOW.\n");
-	system_exit(0);
+	PX4_DEBUG("Exiting in shutdown_main");
+	px4_shutdown_request(0);
+	return 0;
 }
 
 int list_tasks_main(int argc, char *argv[])

platforms/common/shutdown.cpp

@@ -111,7 +111,7 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor
 #define SHUTDOWN_ARG_TO_ISP (1<<3)
 static uint8_t shutdown_args = 0;
 
-static constexpr int max_shutdown_hooks = 1;
+static constexpr int max_shutdown_hooks = 2;
 static shutdown_hook_t shutdown_hooks[max_shutdown_hooks] = {};
 
 static hrt_abstime shutdown_time_us = 0;
@@ -157,6 +157,7 @@ int px4_unregister_shutdown_hook(shutdown_hook_t hook)
 static void shutdown_worker(void *arg)
 {
 	PX4_DEBUG("shutdown worker (%i)", shutdown_counter);
+	shutdown_counter++;
 	bool done = true;
 
 	pthread_mutex_lock(&shutdown_mutex);
@@ -265,7 +266,7 @@ int px4_shutdown_request(uint32_t delay_us)
 		shutdown_time_us += delay_us;
 	}
 
-	work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, 1);
+	work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, 0);
 	pthread_mutex_unlock(&shutdown_mutex);
 	return 0;
 }

platforms/posix/src/px4/common/main.cpp

@@ -73,6 +73,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/posix.h>
+#include <px4_platform_common/shutdown.h>
 
 #include "apps.h"
 #include "px4_daemon/client.h"
@@ -505,6 +506,8 @@ void sig_int_handler(int sig_num)
 	fflush(stdout);
 	printf("\nPX4 Exiting...\n");
 	fflush(stdout);
+	PX4_DEBUG("PX4 Exiting in sig_int_handler\n");
+	px4_shutdown_request(0);
 	px4_daemon::Pxh::stop();
 	_exit_requested = true;
 }

src/modules/muorb/apps/muorb_main.cpp

@@ -32,12 +32,15 @@
  ****************************************************************************/
 
 #include <string.h>
+#include <px4_platform_common/shutdown.h>
 #include "uORBAppsProtobufChannel.hpp"
 #include "uORB/uORBManager.hpp"
+#include "fc_sensor.h"
 
 extern "C" {
 	__EXPORT int muorb_main(int argc, char *argv[]);
 	__EXPORT int muorb_init();
+	__EXPORT bool muorb_kill_slpi();
 }
 
 static bool enable_debug = false;
@@ -48,6 +51,15 @@ muorb_main(int argc, char *argv[])
 	return muorb_init();
 }
 
+bool
+muorb_kill_slpi(void)
+{
+	PX4_ERR("Sending kill command to SLPI!!!");
+	fc_sensor_kill_slpi();
+	sleep(1);
+	return true;
+}
+
 int
 muorb_init()
 {
@@ -58,7 +70,11 @@ muorb_init()
 	if (channel && channel->Initialize(enable_debug)) {
 		uORB::Manager::get_instance()->set_uorb_communicator(channel);
 
-		if (channel->Test()) { return OK; }
+		px4_register_shutdown_hook(&muorb_kill_slpi);
+
+		if (channel->Test()) {
+			return OK;
+		}
 	}
 
 	return -EINVAL;