diff --git a/platforms/common/apps.cpp.in b/platforms/common/apps.cpp.in index 11d1c50d9f..3ea6eeb485 100644 --- a/platforms/common/apps.cpp.in +++ b/platforms/common/apps.cpp.in @@ -2,6 +2,7 @@ #include #include #include +#include #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[]) diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index 878d99c62e..4474824305 100644 --- a/platforms/common/shutdown.cpp +++ b/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; } diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index db6bfe850b..29e900fe13 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #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; } diff --git a/src/modules/muorb/apps/muorb_main.cpp b/src/modules/muorb/apps/muorb_main.cpp index d3c4d78cec..2193940e4b 100644 --- a/src/modules/muorb/apps/muorb_main.cpp +++ b/src/modules/muorb/apps/muorb_main.cpp @@ -32,12 +32,15 @@ ****************************************************************************/ #include +#include #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;