From 849f4531a3900512ab2c86e4a823e5532ccfe508 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Jan 2021 10:31:32 -0500 Subject: [PATCH] commander: automatically initialize to assist mode if using mavlink manual control --- msg/commander_state.msg | 2 ++ src/modules/commander/Commander.cpp | 10 +++++++++- src/modules/commander/state_machine_helper.cpp | 11 ++++++----- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 38c94931f1..f22f36864a 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -19,3 +19,5 @@ uint8 MAIN_STATE_ORBIT = 14 uint8 MAIN_STATE_MAX = 15 uint8 main_state # main state machine + +uint16 main_state_changes diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ef1198902d..f974b1c239 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1950,7 +1950,15 @@ Commander::run() } // update manual_control_setpoint before geofence (which might check sticks or switches) - _manual_control_setpoint_sub.update(&_manual_control_setpoint); + if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { + // manual control from mavlink + if (_manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC) { + // if there's never been a mode change force position control as initial state + if (!_armed.armed && (_internal_state.main_state_changes == 0)) { + _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + } + } + } /* start geofence result check */ _geofence_result_sub.update(&_geofence_result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ee99c0a8e5..8770a28fc7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -369,6 +369,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai if (ret == TRANSITION_CHANGED) { if (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(); } else { @@ -462,6 +463,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ 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) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); @@ -472,10 +475,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ * this enables POSCTL using e.g. flow. * For fixedwing, a global position is needed. */ - } else if (is_armed - && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, - !(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE), - status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + } else if (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)) { // nothing to do - everything done in check_invalid_pos_nav_state } else { @@ -739,7 +740,7 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or if (status_flags.condition_local_position_valid) { 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) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;