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

state_machine_helper: stop mission on RC loss 

This allows to still fly missions completely without RC
but reacts if RC is lost during the mission because
the safety pilot expects to be able to take over.
This commit is contained in:
Matthias Grob 2021-01-21 15:11:02 +01:00
parent f87dbe57c2
commit f13c3a7d44
1 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -514,22 +514,32 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
} else if (status->data_link_lost && data_link_loss_act_configured } else if (status->data_link_lost && data_link_loss_act_configured
&& is_armed && !landed) { && is_armed && !landed) {
// Data link lost, data link loss reaction configured, do configured reaction // Data link lost, data link loss reaction configured -> do configured reaction
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
} else if (status->rc_signal_lost } else if (status->rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once
&& is_armed && !landed) {
// RC link lost, rc loss reaction configured, RC was used before -> RC loss reaction after delay
// Safety pilot expects to be able to take over by RC in case anything unexpected happens
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, param_com_rcl_act_t);
} else if (status->rc_signal_lost && rc_loss_act_configured
&& status->data_link_lost && !data_link_loss_act_configured && status->data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed) { && is_armed && !landed) {
// All links lost, no data link loss reaction configured, immediately do RC loss reaction // All links lost, no data link loss reaction configured -> immediately do RC loss reaction
// Lost all communication, by default it's considered unsafe to continue the mission
// Note this case is reached after the previous one when flying mission completely without RC
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else if (status->rc_signal_lost && !rc_loss_act_configured } else if (status->rc_signal_lost && !rc_loss_act_configured
&& status->data_link_lost && !data_link_loss_act_configured && status->data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed && is_armed && !landed
&& mission_finished) { && mission_finished) {
// All links lost, all link loss reactions disabled, return after mission to prevent stuck vehicle // All links lost, all link loss reactions disabled -> return after mission
// Pilot disabled all reactions, finish mission but then return to avoid lost vehicle
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0); set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);