yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

state_machine_helper: explicitly notify about rca and data link loss

Browse Source
This commit is contained in:
Matthias Grob
2021-06-23 15:28:35 +02:00
parent bec9eef5b0
commit 15096ead5f
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -58,6 +58,7 @@ static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offbo
static constexpr const char reason_no_local_position[] = "no local position"; static constexpr const char reason_no_local_position[] = "no local position";
static constexpr const char reason_no_global_position[] = "no global position"; static constexpr const char reason_no_global_position[] = "no global position";
static constexpr const char reason_no_datalink[] = "no datalink"; static constexpr const char reason_no_datalink[] = "no datalink";
static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink";
// This array defines the arming state transitions. The rows are the new state, and the columns // This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which // are the current state. Using new state and current state you can index into the array which
@@ -558,7 +559,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
&& mission_finished) { && mission_finished) {
// All links lost, all link loss reactions disabled -> return after mission finished // All links lost, all link loss reactions disabled -> return after mission finished
// Pilot disabled all reactions, finish mission but then return to avoid lost vehicle // 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_rc_and_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);
} else if (!stay_in_failsafe) { } else if (!stay_in_failsafe) {
@@ -602,7 +603,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
&& is_armed && !landed) { && is_armed && !landed) {
// All links lost, all link loss reactions disabled -> return // All links lost, all link loss reactions disabled -> return
// Pilot disabled all reactions, return to avoid lost vehicle // Pilot disabled all reactions, 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_rc_and_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);
} else { } else {
@@ -720,7 +721,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
&& is_armed && !landed) { && is_armed && !landed) {
// All links lost, all link loss reactions disabled -> return // All links lost, all link loss reactions disabled -> return
// Pilot disabled all reactions, return to avoid lost vehicle // Pilot disabled all reactions, 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_rc_and_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);
} else { } else {