From 2815c62acff650a0299e8f08f1faf3eec66d45b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 24 Jul 2017 13:05:52 +0200 Subject: [PATCH] fix power button shutdown: use an orb topic instead of a work queue call px4_shutdown_request() was called from the power button IRQ callback, which invoked a work queue callback. But on NuttX, the work queue uses a semaphore, and thus it cannot be called from IRQ context. This patch switches to publishing an uORB msg instead, which is handled in the commander main thread. To increase failure resistance, we could subscribe to the same topic in another module for redundancy, in case commander runs wild. --- msg/CMakeLists.txt | 1 + msg/power_button_state.msg | 8 +++++ src/modules/commander/commander.cpp | 47 ++++++++++++++++++++++++++--- src/platforms/px4_shutdown.h | 3 +- 4 files changed, 53 insertions(+), 6 deletions(-) create mode 100644 msg/power_button_state.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7fc5e6ab91..d28246639a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -81,6 +81,7 @@ set(msg_file_names parameter_update.msg position_setpoint.msg position_setpoint_triplet.msg + power_button_state.msg pwm_input.msg qshell_req.msg rc_channels.msg diff --git a/msg/power_button_state.msg b/msg/power_button_state.msg new file mode 100644 index 0000000000..b941aa820c --- /dev/null +++ b/msg/power_button_state.msg @@ -0,0 +1,8 @@ +# power button state notification message + +uint8 PWR_BUTTON_STATE_IDEL = 0 # Button went up without meeting shutdown button down time (delete event) +uint8 PWR_BUTTON_STATE_DOWN = 1 # Button went Down +uint8 PWR_BUTTON_STATE_UP = 2 # Button went Up +uint8 PWR_BUTTON_STATE_REQUEST_SHUTDOWN = 3 # Button went Up after meeting shutdown button down time + +uint8 event # one of PWR_BUTTON_STATE_* diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index afcd70ff41..cf51f8201f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -104,6 +104,7 @@ #include #include #include +#include #include #include #include @@ -174,6 +175,8 @@ static int64_t lvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = nullptr; +static orb_advert_t power_button_state_pub = nullptr; + /* System autostart ID */ static int autostart_id; @@ -334,11 +337,34 @@ static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub); static int power_button_state_notification_cb(board_power_button_state_notification_e request) { - // Note: this can be called from IRQ handlers - if (request == PWR_BUTTON_REQUEST_SHUT_DOWN) { - px4_shutdown_request(false, false); + // Note: this can be called from IRQ handlers, so we publish a message that will be handled + // on the main thread of commander. + power_button_state_s button_state; + button_state.timestamp = hrt_absolute_time(); + int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; + + switch(request) { + case PWR_BUTTON_IDEL: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL; + break; + case PWR_BUTTON_DOWN: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN; + break; + case PWR_BUTTON_UP: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP; + break; + case PWR_BUTTON_REQUEST_SHUT_DOWN: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN; + break; + default: + PX4_ERR("unhandled power button state: %i", (int)request); + return ret; } - return PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; + + int instance; + orb_publish_auto(ORB_ID(power_button_state), &power_button_state_pub, &button_state, &instance, ORB_PRIO_DEFAULT); + + return ret; } int commander_main(int argc, char *argv[]) @@ -1641,6 +1667,8 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state)); + /* Subscribe to actuator controls (outputs) */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); struct actuator_controls_s actuator_controls; @@ -1878,6 +1906,17 @@ int commander_thread_main(int argc, char *argv[]) param_init_forced = false; } + /* handle power button state */ + orb_check(power_button_state_sub, &updated); + + if (updated) { + power_button_state_s button_state; + orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state); + if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { + px4_shutdown_request(false, false); + } + } + orb_check(sp_man_sub, &updated); if (updated) { diff --git a/src/platforms/px4_shutdown.h b/src/platforms/px4_shutdown.h index 25f21ed940..0248dc927a 100644 --- a/src/platforms/px4_shutdown.h +++ b/src/platforms/px4_shutdown.h @@ -69,8 +69,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); /** - * Request the system to shut down. It's save to call this from an IRQ context, - * but the work queues need to be initialized already. + * Request the system to shut down or reboot. * Note the following: * - The system might not support to shutdown (or reboot). In that case -EINVAL will * be returned.