From 4fb6f4313cc9900e8faab65d0e8eaef3f0d25f11 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 23 Jul 2026 12:10:37 +0300 Subject: [PATCH] fix(navigator): resolve DO_JUMP when loading the current mission item An external MISSION_SET_CURRENT sets the current sequence verbatim, with no jump resolution. If it points directly at a DO_JUMP item, the navigator loaded that jump as the current mission item. A DO_JUMP carries no position, so setActiveMissionItems() never overwrote the current setpoint left behind by reset_triplets() - an invalid IDLE setpoint. The vehicle then loitered in place at altitude indefinitely, never resolving the jump and never completing the mission (observed in a real flight). Resolve the jump when loading the current item: follow it to its target without consuming a repetition (a set-current is an out-of-band action, not a normal traversal). For an already-resolved, non-jump current item this is a no-op, so normal mission traversal is unaffected. Also harden two related paths: - getNonJumpItem() now reports failure instead of returning a still-unresolved DO_JUMP when it runs out of iterations (jump loop or a chain longer than MAX_JUMP_ITERATION), which would otherwise be loaded as the current item. - treat a DO_JUMP as reached/completed in the mission block as a safety net, so the navigator advances rather than hanging should one ever reach that point. Signed-off-by: RomanBapst --- src/modules/navigator/mission_base.cpp | 27 +++++++++++++++++++++++-- src/modules/navigator/mission_block.cpp | 5 +++++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0f378a64084..4b79f65d771 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -542,15 +542,30 @@ MissionBase::set_mission_items() bool MissionBase::loadCurrentMissionItem() { - const bool success = loadMissionItemFromCache(_mission.current_seq, _mission_item); + // _mission.current_seq may point directly at a DO_JUMP item: it can be set verbatim by an + // external MISSION_SET_CURRENT (which does no jump resolution), or returned by a jump + // resolution that gave up. A DO_JUMP is not an executable current item, so resolve it to the + // next non-jump item here. We follow the jump to its target but do NOT consume a repetition + // (write_jumps == false), since a set-current is an out-of-band action, not a normal traversal. + // For an already-resolved (non-jump) current item this is a no-op. + int32_t resolved_index = _mission.current_seq; + mission_item_s resolved_item; + + const bool success = getNonJumpItem(resolved_index, resolved_item, MissionTraversalType::FollowMissionControlFlow, + /*write_jumps*/ false, /*mission_direction_backward*/ false) == PX4_OK; if (!success) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t"); events::send(events::ID("mission_item_set_failed"), events::Log::Error, "Mission item could not be set"); + return false; } - return success; + // Persist the resolved index (republishes the mission topic only if it actually changed). + setMissionIndex(resolved_index); + _mission_item = resolved_item; + + return true; } void MissionBase::setEndOfMissionItems() @@ -1047,6 +1062,14 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, } } + if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) { + // Ran out of iterations while still on a DO_JUMP (e.g. a jump loop or a chain longer than + // MAX_JUMP_ITERATION). Report failure instead of returning an unresolved jump as a valid + // item, which would otherwise be loaded as the current mission item. + PX4_ERR("Do Jump could not be resolved within %u iterations.", MAX_JUMP_ITERATION); + return PX4_ERROR; + } + mission_index = new_mission_index; mission = new_mission; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8086ea59861..20d3e671202 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -101,6 +101,11 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_SET_HOME: case NAV_CMD_RETURN_TO_LAUNCH: + // Safety net: a DO_JUMP should never reach here as a current item (it is resolved in + // loadCurrentMissionItem), but if one ever does, treat it as complete so the navigator + // advances instead of hanging on an IDLE setpoint. + case NAV_CMD_DO_JUMP: + return true; // Indefinite Waypoints