From a593a51f0585d2237159ba84ffb3d1b0a87fa0b9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 1 Nov 2021 18:33:15 +0100 Subject: [PATCH] Commander: fix mode initialization with RC --- msg/commander_state.msg | 1 - src/modules/commander/Commander.cpp | 53 +++++++++++------------------ src/modules/commander/Commander.hpp | 3 ++ 3 files changed, 22 insertions(+), 35 deletions(-) diff --git a/msg/commander_state.msg b/msg/commander_state.msg index df12911f9e..02f4c7e620 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -19,6 +19,5 @@ uint8 MAIN_STATE_ORBIT = 14 uint8 MAIN_STATE_MAX = 15 uint8 main_state -uint8 desired_main_state uint16 main_state_changes diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9eb7932c79..8095e20cac 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -706,8 +706,6 @@ Commander::Commander() : // default for vtol is rotary wing _vtol_status.vtol_in_rw_mode = true; - _internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX; - /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_init(); } @@ -850,21 +848,8 @@ Commander::handle_command(const vehicle_command_s &cmd) } if (desired_main_state != commander_state_s::MAIN_STATE_MAX) { - reset_posvel_validity(); - main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state); - - // If the command is internal (e.g. sent by RC), and we were not (yet) able to switch - // to it, we will try again later. However, we only do that for ALTCTL and POSCTL. - if (!cmd.from_external && main_ret == TRANSITION_DENIED && - (desired_main_state == commander_state_s::MAIN_STATE_ALTCTL || - desired_main_state == commander_state_s::MAIN_STATE_POSCTL)) { - _internal_state.desired_main_state = desired_main_state; - - } else { - _internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX; - } } if (main_ret != TRANSITION_DENIED) { @@ -1567,6 +1552,16 @@ void Commander::executeActionRequest(const action_request_s &action_request) break; case action_request_s::ACTION_SWITCH_MODE: + + // if there's never been a mode change force RC switch as initial state + if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT + && !_armed.armed && (_internal_state.main_state_changes == 0) + && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL + || action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) { + _internal_state.main_state = action_request.mode; + _internal_state.main_state_changes++; + } + main_state_transition(_status, action_request.mode, _status_flags, _internal_state); break; } @@ -2359,6 +2354,15 @@ Commander::run() } } + const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0); + const bool is_mavlink = manual_control_setpoint.chosen_input.data_source > manual_control_input_s::SOURCE_RC; + + if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) { + // if there's never been a mode change force position control as initial state + _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + _internal_state.main_state_changes++; + } + _status.rc_signal_lost = false; _is_throttle_above_center = manual_control_setpoint.chosen_input.z > 0.6f; _is_throttle_low = manual_control_setpoint.chosen_input.z < 0.1f; @@ -2640,25 +2644,6 @@ Commander::run() _imbalanced_propeller_check_triggered = false; } - // If we have an desired state, we should try to reach it but only when still disarmed. - if (_internal_state.desired_main_state != commander_state_s::commander_state_s::MAIN_STATE_MAX && - !_armed.armed) { - const transition_result_t desired_ret = main_state_transition(_status, _internal_state.desired_main_state, - _status_flags, _internal_state); - - if (desired_ret != TRANSITION_DENIED) { - // Reset it for next time. - _internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX; - - tune_positive(_armed.armed); - _status_changed = true; - } - - } else if (_armed.armed) { - // Once armed we reset it. - _internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX; - } - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(_status, _armed, diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e03a15ced8..6e62ad8938 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -223,6 +223,9 @@ private: (ParamFloat) _param_com_wind_warn, + (ParamInt) _param_rc_map_fltmode, + (ParamInt) _param_rc_map_mode_sw, + // Offboard (ParamFloat) _param_com_of_loss_t, (ParamInt) _param_com_obl_act,