dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: fix orbit failsafes for data link & rc loss

This commit is contained in:
Matthias Grob 2018-12-03 17:15:41 +01:00
parent 5b5d5ac6d5
commit e979d24fff
1 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/commander/state_machine_helper.cpp
View File

@ -588,16 +588,31 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
break;
case commander_state_s::MAIN_STATE_ORBIT:
/* require local position */
if (status->engine_failure) {
// failsafe: on engine failure
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// failsafe: necessary position estimate lost (nothing to do - everything done in check_invalid_pos_nav_state)
} else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// failsafe: just datalink is lost and we're in air
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (rc_lost && !data_link_loss_act_configured && is_armed) {
// failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
} else {
// no failsafe, RC is not mandatory for orbit
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
}
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: