diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index c27cd38b56..6abf4cf10f 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -251,6 +251,11 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _mission_item.lon = _home_pos_sub.get().lon; _mission_item.yaw = NAN; + // make previous and next setpoints invalid, such that there will be no line following. + // If the vehicle drifted off the path during back-transition it should just go straight to the landing point. + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + _navigator->reset_position_setpoint(pos_sp_triplet->next); + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && do_need_move_to_item()) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; @@ -261,15 +266,9 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; - // make previous setpoint invalid, such that there will be no prev-current line following. - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - } else { _mission_item.altitude = _home_pos_sub.get().alt; _mission_item.altitude_is_relative = false; - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - _navigator->reset_position_setpoint(pos_sp_triplet->next); _mission_item.land_precision = _param_rtl_pld_md.get();