diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 672fcbc138..a8f2e80a5a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -512,6 +512,109 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason) return arming_res; } +transition_result_t +Commander::try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback) +{ + transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + + if (res == TRANSITION_DENIED) { + + if (desired_mode == commander_state_s::MAIN_STATE_OFFBOARD) { + /* offboard does not have a fallback */ + print_reject_mode("Offboard"); + + return res; + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) { + print_reject_mode("Auto Mission"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_RTL && (res == TRANSITION_DENIED)) { + print_reject_mode("Auto RTL"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LAND && (res == TRANSITION_DENIED)) { + print_reject_mode("Auto Land"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && (res == TRANSITION_DENIED)) { + print_reject_mode("Auto Takeoff"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET && (res == TRANSITION_DENIED)) { + print_reject_mode("Auto Follow"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LOITER && (res == TRANSITION_DENIED)) { + print_reject_mode("Auto Hold"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + /* fall back to position control */ + desired_mode = commander_state_s::MAIN_STATE_POSCTL; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_POSCTL && (res == TRANSITION_DENIED)) { + print_reject_mode("Position"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + /* fall back to altitude control */ + desired_mode = commander_state_s::MAIN_STATE_ALTCTL; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_ALTCTL && (res == TRANSITION_DENIED)) { + print_reject_mode("Altitude"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + /* fall back to stabilized */ + desired_mode = commander_state_s::MAIN_STATE_STAB; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + + if (desired_mode == commander_state_s::MAIN_STATE_STAB && (res == TRANSITION_DENIED)) { + print_reject_mode("Stabilized"); + + if (enable_fallback == FlightModeChange::FallbackEnabled) { + /* fall back to manual */ + desired_mode = commander_state_s::MAIN_STATE_MANUAL; + res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state); + } + } + } + + return res; +} + Commander::Commander() : ModuleParams(nullptr), _failure_detector(this) @@ -2140,6 +2243,13 @@ Commander::run() } } + if (_manual_control.isMavlink()) { + // 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; + } + } + if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { // handle landing gear switch if configured and in a manual mode @@ -2832,13 +2942,9 @@ Commander::set_main_state_rc() /* offboard switch overrides main switch */ if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state); + res = try_mode_change(commander_state_s::MAIN_STATE_OFFBOARD, FlightModeChange::FallbackDisabled); - if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); - /* mode rejected, continue to evaluate the main system mode */ - - } else { + if (res != TRANSITION_DENIED) { /* changed successfully or already in this state */ return res; } @@ -2846,14 +2952,8 @@ Commander::set_main_state_rc() /* RTL switch overrides main switch */ if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, &_internal_state); - if (res == TRANSITION_DENIED) { - print_reject_mode("AUTO RTL"); - - /* fallback to LOITER if home position not set */ - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); - } + res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL, FlightModeChange::FallbackEnabled); if (res != TRANSITION_DENIED) { /* changed successfully or already in this state */ @@ -2865,12 +2965,9 @@ Commander::set_main_state_rc() /* Loiter switch overrides main switch */ if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); + res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_LOITER, FlightModeChange::FallbackDisabled); - if (res == TRANSITION_DENIED) { - print_reject_mode("AUTO HOLD"); - - } else { + if (res != TRANSITION_DENIED) { return res; } } @@ -2890,125 +2987,7 @@ Commander::set_main_state_rc() res = TRANSITION_NOT_CHANGED; } else { - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - /* ensure that the mode selection does not get stuck here */ - int maxcount = 5; - - /* enable the use of break */ - /* fallback strategies, give the user the closest mode to what he wanted */ - while (res == TRANSITION_DENIED && maxcount > 0) { - - maxcount--; - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) { - - /* fall back to loiter */ - new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO MISSION"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) { - - /* fall back to position control */ - new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO RTL"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) { - - /* fall back to position control */ - new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO LAND"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { - - /* fall back to position control */ - new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO TAKEOFF"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) { - - /* fall back to position control */ - new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO FOLLOW"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) { - - /* fall back to position control */ - new_mode = commander_state_s::MAIN_STATE_POSCTL; - print_reject_mode("AUTO HOLD"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_POSCTL) { - - /* fall back to altitude control */ - new_mode = commander_state_s::MAIN_STATE_ALTCTL; - print_reject_mode("POSITION CONTROL"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) { - - /* fall back to stabilized */ - new_mode = commander_state_s::MAIN_STATE_STAB; - print_reject_mode("ALTITUDE CONTROL"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - - if (new_mode == commander_state_s::MAIN_STATE_STAB) { - - /* fall back to manual */ - new_mode = commander_state_s::MAIN_STATE_MANUAL; - print_reject_mode("STABILIZED"); - res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; - } - } - } + res = try_mode_change(new_mode, FlightModeChange::FallbackEnabled); } return res; @@ -3074,13 +3053,11 @@ Commander::set_main_state_rc() case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); + res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL, FlightModeChange::FallbackDisabled); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - - print_reject_mode("POSITION CONTROL"); } // fallback to ALTCTL @@ -3091,7 +3068,7 @@ Commander::set_main_state_rc() } if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) { - print_reject_mode("ALTITUDE CONTROL"); + print_reject_mode("Altitude"); } // fallback to MANUAL @@ -3100,38 +3077,7 @@ Commander::set_main_state_rc() break; case manual_control_switches_s::SWITCH_POS_ON: // AUTO - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode("AUTO MISSION"); - - // fallback to LOITER if home position not set - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to POSCTL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to ALTCTL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to MANUAL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state); - // TRANSITION_DENIED is not possible here + res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION, FlightModeChange::FallbackEnabled); break; default: @@ -3368,7 +3314,7 @@ Commander::print_reject_mode(const char *msg) if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { _last_print_mode_reject_time = t; - mavlink_log_critical(&_mavlink_log_pub, "REJECT %s", msg); + mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ee5dfb3d14..febbc33bfc 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -122,10 +122,17 @@ public: void get_circuit_breaker_params(); private: + + enum class FlightModeChange { + FallbackDisabled = 0, + FallbackEnabled + }; + void answer_command(const vehicle_command_s &cmd, uint8_t result); transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); transition_result_t disarm(arm_disarm_reason_t calling_reason); + transition_result_t try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback); void battery_status_check(); diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index e422565aaa..38a616f5ff 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -64,6 +64,7 @@ public: */ bool update(); bool isRCAvailable() const { return _rc_available; } + bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; } bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, manual_control_switches_s &manual_control_switches, const bool landed);