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

delete Outback Challenge (OBC) AUTO_RTGS (datalink loss) and AUTO_RCRECOVER (rc loss)

Browse Source
This commit is contained in:
Daniel Agar
2020-03-11 22:45:55 -04:00
committed by GitHub
parent 0ce9e113ff
commit 5d33b9e999
22 changed files with 40 additions and 848 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -85,8 +85,7 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state);
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act);
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
@@ -421,8 +420,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
if (rc_lost && is_armed) {
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);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
} else {
switch (internal_state->main_state) {
@@ -459,8 +457,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
if (rc_lost && is_armed) {
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);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
/* A local position estimate is enough for POSCTL for multirotors,
@@ -500,21 +497,19 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
} else if (data_link_loss_act_configured && status->data_link_lost && is_armed) {
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
* check for datalink lost: this should always trigger RTL */
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,
vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
&& mission_finished && is_armed) {
/* datalink loss DISABLED:
* check if both, RC and datalink are lost during the mission
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
* or all links are lost after the mission finishes in air: this should always trigger RTL */
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,
vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
} else if (!stay_in_failsafe) {
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
@@ -533,8 +528,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
// 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) {
/* also go into failsafe if just datalink is lost, and we're actually in air */
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
@@ -542,8 +536,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* go into failsafe if RC is lost and 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);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
} else if (status->rc_signal_lost) {
/* don't bother if RC is lost if datalink is connected */
@@ -607,8 +600,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
} 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);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
@@ -620,8 +612,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
// 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);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
@@ -768,8 +759,7 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or
}
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state)
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act)
{
// do the best you can according to the action set
@@ -778,13 +768,6 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
// If datalink loss failsafe is disabled then no action must be taken.
break;
case link_loss_actions_t::AUTO_RECOVER:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
status->nav_state = auto_recovery_nav_state;
return;
}
// FALLTHROUGH
case link_loss_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);