diff --git a/platforms/common/apps.cpp.in b/platforms/common/apps.cpp.in index 11d1c50d9f..36c3ce6bdb 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,6 +44,7 @@ void list_builtins(apps_map_type &apps) int shutdown_main(int argc, char *argv[]) { printf("Exiting NOW.\n"); + uorb_shutdown(); system_exit(0); } diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index 878d99c62e..d1be7ff5a1 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include @@ -173,6 +174,8 @@ static void shutdown_worker(void *arg) const bool delay_elapsed = (now > shutdown_time_us); if (delay_elapsed && ((done && shutdown_lock_counter == 0) || (now > (shutdown_time_us + shutdown_timeout_us)))) { + uorb_shutdown(); + if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index a68f3c5dc8..dd08a8e067 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -51,6 +51,7 @@ #include #endif + static uORB::DeviceMaster *g_dev = nullptr; int uorb_start(void) @@ -113,6 +114,18 @@ int uorb_top(char **topic_filter, int num_filters) return OK; } +void uorb_shutdown(void) +{ +#ifdef CONFIG_ORB_COMMUNICATOR + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch) { + ch->shutdown(); + } + +#endif /* CONFIG_ORB_COMMUNICATOR */ +} + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { return uORB::Manager::get_instance()->orb_advertise(meta, data); diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index ee11923b59..bfdf46fab8 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -121,6 +121,7 @@ __BEGIN_DECLS int uorb_start(void); int uorb_status(void); int uorb_top(char **topic_filter, int num_filters); +void uorb_shutdown(void); /** * ORB topic advertiser handle. diff --git a/platforms/common/uORB/uORBCommunicator.hpp b/platforms/common/uORB/uORBCommunicator.hpp index 8122b34780..64706f29bf 100644 --- a/platforms/common/uORB/uORBCommunicator.hpp +++ b/platforms/common/uORB/uORBCommunicator.hpp @@ -129,6 +129,22 @@ public: virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data) = 0; + + //========================================================================= + // INTERFACES FOR Lifecycle messages + //========================================================================= + + /** + * @brief Interface to notify the remote entity of a shutdown. + * + * @return + * 0 = success; This means the shutdown is successfully sent to the receiver + * Note: This does not mean that the receiver has received it. + * otherwise = failure. + */ + + virtual int16_t shutdown() { return 0; } + }; /** diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index db6bfe850b..87af3b7344 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,7 @@ void sig_int_handler(int sig_num) fflush(stdout); printf("\nPX4 Exiting...\n"); fflush(stdout); + uorb_shutdown(); 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..8db18141e0 100644 --- a/src/modules/muorb/apps/muorb_main.cpp +++ b/src/modules/muorb/apps/muorb_main.cpp @@ -58,7 +58,9 @@ muorb_init() if (channel && channel->Initialize(enable_debug)) { uORB::Manager::get_instance()->set_uorb_communicator(channel); - if (channel->Test()) { return OK; } + if (channel->Test()) { + return OK; + } } return -EINVAL; diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index 437e328156..df2376da27 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -291,6 +291,20 @@ int16_t uORB::AppsProtobufChannel::register_handler(uORBCommunicator::IChannelRx return 0; } +int16_t uORB::AppsProtobufChannel::shutdown() +{ + if (_ShutdownRequested) { + return 0; + } + + _ShutdownRequested = true; + + PX4_ERR("Sending kill command to SLPI!!!"); + fc_sensor_kill_slpi(); + sleep(1); + return 0; +} + int16_t uORB::AppsProtobufChannel::send_message(const char *messageName, int length, uint8_t *data) { // This is done to slow down high rate debug messages. diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp index 7d280a460b..006a8714ef 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp @@ -147,6 +147,16 @@ public: */ int16_t send_message(const char *messageName, int length, uint8_t *data); + /** + * @brief Interface to notify the remote entity of a shutdown. + * + * @return + * 0 = success; This means the shutdown is successfully sent to the receiver + * Note: This does not mean that the receiver has received it. + * otherwise = failure. + */ + int16_t shutdown(); + /** * @brief Interface to test the functions of the protobuf channel. * @@ -169,6 +179,7 @@ private: static bool _Debug; bool _Initialized; + bool _ShutdownRequested{false}; uint32_t _MessageCounter; private: