diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3d0e148e03..16228d72ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1504,6 +1504,7 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; + bool emergency_battery_voltage_actions_done = false; bool status_changed = true; bool param_init_forced = true; @@ -2254,13 +2255,11 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed) { mavlink_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN"); - usleep(200000); - px4_board_pwr(false); } else { - if (low_bat_action == 1) { + if (low_bat_action == 1 || low_bat_action == 3) { // let us send the critical message even if already in RTL - if (warning_action_on || TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { warning_action_on = true; mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURNING TO LAND"); @@ -2269,7 +2268,7 @@ int commander_thread_main(int argc, char *argv[]) } } else if (low_bat_action == 2) { - if (warning_action_on || TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { warning_action_on = true; mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION"); @@ -2278,7 +2277,34 @@ int commander_thread_main(int argc, char *argv[]) } } else { - mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING ADVISED!"); + mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURN TO LAUNCH ADVISED!"); + } + } + + status_changed = true; + + } else if (!status_flags.usb_connected && + battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY && + !emergency_battery_voltage_actions_done) { + emergency_battery_voltage_actions_done = true; + + if (!armed.armed) { + mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN"); + usleep(200000); + px4_board_pwr(false); + + } else { + if (low_bat_action == 2 || low_bat_action == 3) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { + warning_action_on = true; + mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY"); + + } else { + mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING FAILED"); + } + + } else { + mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING ADVISED!"); } } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2de6c6288f..76e001a880 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -315,8 +315,9 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0); * * @group Commander * @value 0 Warning - * @value 1 Return to Land + * @value 1 Return to land * @value 2 Land at current position + * @value 3 Return to land at critically low level, land at current position if reaching dangerously low levels * @decimal 0 * @increment 1 */