diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9bd719651e..30ee768eb3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1879,18 +1879,24 @@ void Commander::run() // this nice pattern stolen from handle_command // if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK, ModeChangeSource::ModeExecutor, false)) { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK)) { + _prev_nav_state = _vehicle_status.nav_state; + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK); + + // no error handling like this for now + // if (ret) { // return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; // PX4_INFO("mode intention changed"); - } else { + // } else { // printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); // return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; // PX4_INFO("mode intention not changed"); - } - } // else { + // } + + } else { // leave the mode again somehow... - // } + _user_mode_intention.change(_prev_nav_state); + } modeManagementUpdate(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 18955393fd..6265bce8c9 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -286,6 +286,8 @@ private: vehicle_land_detected_s _vehicle_land_detected{}; + uint8_t _prev_nav_state; + // commander publications actuator_armed_s _actuator_armed{}; vehicle_control_mode_s _vehicle_control_mode{};