From 52c290e2346455a3928312312111b9671bacde28 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Wed, 30 May 2018 11:28:21 +0200 Subject: [PATCH] commander: moved emergency shutdown to end of cycle --- src/modules/commander/commander.cpp | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c3f94f8fa6..3ebdac48de 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1308,6 +1308,7 @@ Commander::run() bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; bool emergency_battery_voltage_actions_done = false; + bool dangerous_battery_level_requests_poweroff = false; bool status_changed = true; bool param_init_forced = true; @@ -1967,17 +1968,9 @@ Commander::run() emergency_battery_voltage_actions_done = true; if (!armed.armed) { - mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN"); - usleep(200000); - int ret_val = px4_shutdown_request(false, false); - - if (ret_val) { - mavlink_log_critical(&mavlink_log_pub, "SYSTEM DOES NOT SUPPORT SHUTDOWN"); - - } else { - while (1) { usleep(1); } - } - + // Request shutdown at the end of the cycle. This allows + // the vehicle state to be published after emergency landing + dangerous_battery_level_requests_poweroff = true; } else { if (low_bat_action == 2 || low_bat_action == 3) { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state)) { @@ -2747,6 +2740,20 @@ Commander::run() arm_auth_update(now, params_updated || param_init_forced); + // Handle shutdown request from emergency battery action + if(dangerous_battery_level_requests_poweroff){ + mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN"); + usleep(200000); + int ret_val = px4_shutdown_request(false, false); + + if (ret_val) { + mavlink_log_critical(&mavlink_log_pub, "SYSTEM DOES NOT SUPPORT SHUTDOWN"); + + } else { + while (1) { usleep(1); } + } + } + usleep(COMMANDER_MONITORING_INTERVAL); }