From 6b9d86680b791eee823006fd6706b9b35b32314b Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 10 Nov 2022 15:06:24 +0100 Subject: [PATCH] commander: fix hold after mission logic previous change in logic to hold after mission clear also broke rtl, as non-mission takeoff still published a mission result which allowed entering the mission finished condition and always changing state to loiter (ignoring rtl). new logic only switches navigation states if mission is finish and the nav state is explicitly in takeoff state, or in mission state --- src/modules/commander/Commander.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 24ee73913b..cf023dd3a2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1862,17 +1862,22 @@ void Commander::checkForMissionUpdate() } } - // handle navigation state auto-switching based on mission_result if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) { - if ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) - && (_param_takeoff_finished_action.get() == 1) && auto_mission_available) { - _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) { + // Transition mode to loiter or auto-mission after takeoff is completed. + if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) { + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); - } else { + } else { + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); + } + + } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + // Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode. _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); } }