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

state_machine_helper: automatically initialize to assist mode if using mavlink manual control

Browse Source
This commit is contained in:
Daniel Agar
2021-01-22 10:31:32 -05:00
committed by Matthias Grob
parent b001865e5c
commit 18be1bacdc
2 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/commander_state.msg
View File

@@ -19,3 +19,5 @@ uint8 MAIN_STATE_ORBIT = 14
uint8 MAIN_STATE_MAX = 15 uint8 MAIN_STATE_MAX = 15
uint8 main_state # main state machine uint8 main_state # main state machine
uint16 main_state_changes

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

@@ -398,6 +398,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
if (ret == TRANSITION_CHANGED) { if (ret == TRANSITION_CHANGED) {
if (internal_state->main_state != new_main_state) { if (internal_state->main_state != new_main_state) {
internal_state->main_state = new_main_state; internal_state->main_state = new_main_state;
internal_state->main_state_changes++;
internal_state->timestamp = hrt_absolute_time(); internal_state->timestamp = hrt_absolute_time();
} else { } else {
@@ -493,6 +494,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
case commander_state_s::MAIN_STATE_POSCTL: { case commander_state_s::MAIN_STATE_POSCTL: {
const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed;
if (rc_lost && is_armed) { if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); 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); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
@@ -502,10 +505,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
* this enables POSCTL using e.g. flow. * this enables POSCTL using e.g. flow.
* For fixedwing, a global position is needed. */ * For fixedwing, a global position is needed. */
} else if (is_armed } else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
&& check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, rc_fallback_allowed, status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
!(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE),
status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
// nothing to do - everything done in check_invalid_pos_nav_state // nothing to do - everything done in check_invalid_pos_nav_state
} else { } else {
@@ -783,7 +784,7 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or
if (status_flags.condition_local_position_valid) { if (status_flags.condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) { } else if (status_flags.condition_local_altitude_valid) {
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;