diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 15a09a6a16..33f386e472 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -403,8 +403,8 @@ Mission::find_mission_land_start() _land_start_index = i; } - if (_land_start_available && (missionitem.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { - // use the loiter to altitude item of the mission landing as target landing location + if (_land_start_available && i > _land_start_index && item_contains_position(missionitem)) { + // use the position of any waypoint after the land start marker which specifies a position. _landing_lat = missionitem.lat; _landing_lon = missionitem.lon; _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8214669d7a..0c132fb4b8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -564,10 +564,11 @@ Navigator::run() } - if (!rtl_activated && _rtl.initialClimbDone() && get_mission_start_land_available()) { + if (!rtl_activated && _rtl.getClimbAndReturnDone() && get_mission_start_land_available()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - if (!getMissionLandingInProgress()) { + if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED + && !get_land_detected()->landed) { start_mission_landing(); } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 97424a07b3..f46b61e5a3 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -231,8 +231,9 @@ void RTL::on_activation() _rtl_state = RTL_STATE_LANDED; } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { - // RTL straight to RETURN state, but mission will takeover for landing. - _rtl_state = RTL_STATE_RETURN; + // we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode, + // which will continue executing the landing + _rtl_state = RTL_STATE_DESCEND; } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { @@ -246,7 +247,7 @@ void RTL::on_activation() _rtl_state = RTL_STATE_RETURN; } - setInitialClimbDone(_rtl_state != RTL_STATE_CLIMB); + setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN); set_rtl_item(); @@ -273,7 +274,7 @@ void RTL::set_rtl_item() // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination. // After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing. if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { - if (_rtl_state > RTL_STATE_CLIMB) { + if (_rtl_state > RTL_STATE_RETURN) { if (_navigator->start_mission_landing()) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing"); return; @@ -486,11 +487,11 @@ void RTL::advance_rtl() switch (_rtl_state) { case RTL_STATE_CLIMB: - setInitialClimbDone(true); _rtl_state = RTL_STATE_RETURN; break; case RTL_STATE_RETURN: + setClimbAndReturnDone(true); if (vtol_in_fw_mode || descend_and_loiter) { _rtl_state = RTL_STATE_DESCEND; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 56418990de..eff35e4be1 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -83,9 +83,9 @@ public: int rtl_destination(); - void setInitialClimbDone(bool done) { _initial_climb_done = done; } + void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; } - bool initialClimbDone() { return _initial_climb_done; } + bool getClimbAndReturnDone() { return _climb_and_return_done; } private: /** @@ -138,7 +138,7 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; - bool _initial_climb_done{false}; // this flag is set to true if RTL is active and we are past the climb state + bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state DEFINE_PARAMETERS( (ParamFloat) _param_rtl_return_alt,