diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 556bddbd18..2085995d6c 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -77,7 +77,8 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource // Special case termination state: even though this mode prevents arming, // still don't switch out of it after disarm and thus store it in _nav_state_after_disarming. - if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state) + if ((!_health_and_arming_checks.modePreventsArming(user_intended_nav_state) + && !isTakeOffIntended(user_intended_nav_state)) || user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { _nav_state_after_disarming = user_intended_nav_state; } diff --git a/src/modules/commander/UserModeIntention.hpp b/src/modules/commander/UserModeIntention.hpp index cfb4e3998b..e37bf467b1 100644 --- a/src/modules/commander/UserModeIntention.hpp +++ b/src/modules/commander/UserModeIntention.hpp @@ -91,6 +91,7 @@ public: private: bool isArmed() const { return _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; } + bool isTakeOffIntended(uint8_t user_intented_nav_state) const {return user_intented_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || user_intented_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;} const vehicle_status_s &_vehicle_status; const HealthAndArmingChecks &_health_and_arming_checks;