From 1acb07c600947271fbc331c458a0ca035affea05 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 17 Feb 2023 15:36:48 +0100 Subject: [PATCH] Navigator: set _land_start_index to first item with a position after the marker _land_start_index is used to to start the mission from this item index, and to avoid to publish a triplet.current.type=IDLE, we need to fill it with the actual position setpoint that the vehicle should go to at the start of a mission landing. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 5 ++--- src/modules/navigator/navigator_main.cpp | 6 +++--- 2 files changed, 5 insertions(+), 6 deletions(-) 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;