fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "commander: remove unused/wrong failsafe handling"

Browse Source 
This reverts commit 9e704a5e121c7516e2133d02b328500d9d66fb67.
This commit is contained in:
Julian Oes 2016-06-06 11:19:26 +02:00 committed by Lorenz Meier
parent d0355cef1f
commit fdff6ea325
1 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/modules/commander/commander.cpp
View File

@ -2629,6 +2629,58 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* Check for failure combinations which lead to flight termination */
if (armed.armed) {
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status_flags.gps_failure) ||
(status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
mavlink_and_console_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
}
}
/* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status_flags.gps_failure) ||
(status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of RC signal loss and GPS failure");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination");
}
}
}
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();