From 6fa6360aef492297a23c8ae164d5da26b541c311 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 3 Sep 2024 18:10:37 +0200 Subject: [PATCH] Commander: always allow to switch to LAND mode (#23580) Special handling for LAND mode: always allow to switch into it such that if used as emergency mode it is always available. When triggering it the user generally wants the vehicle to descend immediately, and if that means to switch to DESCEND it is fine. Signed-off-by: Silvan Fuhrer --- src/modules/commander/Commander.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fe0b89e8b2..ff29ad0481 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -887,7 +887,14 @@ Commander::handle_command(const vehicle_command_s &cmd) } if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) { - if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) { + + // Special handling for LAND mode: always allow to switch into it such that if used + // as emergency mode it is always available. When triggering it the user generally wants + // the vehicle to descend immediately, and if that means to switch to DESCEND it is fine. + + const bool force = desired_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd), false, force)) { main_ret = TRANSITION_CHANGED; } else { @@ -1062,7 +1069,13 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) { + // Special handling for LAND mode: always allow to switch into it such that if used + // as emergency mode it is always available. When triggering it the user generally wants + // the vehicle to descend immediately, and if that means to switch to DESCEND it is fine. + const bool force = true; + + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd), false, + force)) { mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t"); events::send(events::ID("commander_landing_current_pos"), events::Log::Info, "Landing at current position");