diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e4836828e8..d87b1eaac2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -433,11 +433,9 @@ Mission::find_mission_land_start() if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { found_land_start_marker = true; - _land_start_index = i; } - if (found_land_start_marker && !_land_start_available && i > _land_start_index - && item_contains_position(missionitem)) { + if (found_land_start_marker && !_land_start_available && item_contains_position(missionitem)) { // use the position of any waypoint after the land start marker which specifies a position. _landing_start_lat = missionitem.lat; _landing_start_lon = missionitem.lon; @@ -447,6 +445,7 @@ Mission::find_mission_land_start() && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) : _navigator->get_loiter_radius(); _land_start_available = true; + _land_start_index = i; // set it to the first item containing a position after the land start marker was found } if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ec5596ee82..76f633254d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -545,9 +545,9 @@ void Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { - /* find NAV_CMD_DO_LAND_START in the mission and - * use MAV_CMD_MISSION_START to start the mission there - */ + // find NAV_CMD_DO_LAND_START in the mission and + // use MAV_CMD_MISSION_START to start the mission from the next item containing a position setpoint + if (_mission.land_start()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;