fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 16:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Differentiate between emergency battery level and critical battery level

Browse Source
This commit is contained in:
Lorenz Meier
2017-02-22 22:04:04 +01:00
parent 029c6de4a9
commit 16530e15db
2 changed files with 34 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+32 -6
View File
@@ -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!");
}
}