From 0de5a511b7439c87e5f418892288adffdd3d4332 Mon Sep 17 00:00:00 2001 From: raphaelhunziker1202-stack <250872901+raphaelhunziker1202-stack@users.noreply.github.com> Date: Tue, 1 Sep 2026 19:11:18 +0200 Subject: [PATCH] MAVLink: allow mission upload and clear in flight when the mission is not being executed Aligns the MAVLink mission path with the MSP policy introduced in #10273, where setWaypoint() accepts mission uploads while armed as long as the WP mission is not actively being flown. The MAVLink handlers still carried the blanket ARMING_FLAG(ARMED) rejection from 2017 (1076fc13c), so the two transports enforced different rules in front of the same shared sink. Changes: - New mavlinkMissionEditBlocked() gate used by MISSION_ITEM, MISSION_COUNT and MISSION_CLEAR_ALL: edits are refused while armed AND (WP mode active OR the mission's own RTH leg running OR the on-the-fly mission planner active). The RTH-leg term protects the land/loiter decision at home, which reads the live list; the planner term prevents two writers on the same list. The ARMED term keeps the disarmed path unchanged. setWaypoint() enforces the WP-mode rule too but returns void, so the handler-level check turns a silent drop into a proper MAV_MISSION_ERROR NACK. - While armed, MISSION_ITEMs are only accepted inside a transfer opened by MISSION_COUNT (incomingMissionWpSequence < incomingMissionWpCount); without this a single stray or duplicated item could rewrite the mission in flight. Refusals drop the half-open transaction so a stray item cannot resume it later. The Mission Planner guided-waypoint carve-out (current == 2, #11061) is preserved unchanged. - MISSION_COUNT with count == 0 now clears the mission and replies MISSION_ACK(ACCEPTED) per the mission protocol instead of requesting a nonexistent item; the oversized-count reply is NO_SPACE in both armed and disarmed state (previously ERROR while armed). - navigation.c: setWaypoint()'s post-upload clamp of activeWaypointIndex uses >= instead of > (the index is 0-based, so index == waypointCount is already out of range); new public isWpMissionPlannerActive() accessor. Known limitation, unchanged from the MSP path: the 9.x upload writes items one at a time into the live list, so a transfer aborted mid-way leaves an invalid partial mission until a new upload completes - the same semantics a Configurator upload over a radio link has today. The maintenance-10.x mission stack stages uploads and commits atomically; this change deliberately ports only the policy to 9.x. --- src/main/navigation/navigation.c | 10 +++- src/main/navigation/navigation.h | 1 + src/main/telemetry/mavlink.c | 90 +++++++++++++++++++++++++++----- 3 files changed, 87 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eda064bd68b..1241b4a92a7 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4003,8 +4003,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount; if (posControl.waypointListValid) { nonGeoWaypointCount = 0; - // If active WP index is bigger than total mission WP number, reset active WP index (Mission Upload mid flight with interrupted mission) if RESUME is enabled - if (posControl.activeWaypointIndex > posControl.waypointCount) { + // If active WP index is beyond the new mission, reset active WP index (Mission Upload mid flight with interrupted mission) if RESUME is enabled. + // activeWaypointIndex is 0-based, so an index equal to waypointCount is already out of range. + if (posControl.activeWaypointIndex >= posControl.waypointCount) { posControl.activeWaypointIndex = 0; } } @@ -4031,6 +4032,11 @@ bool isWaypointListValid(void) return posControl.waypointListValid; } +bool isWpMissionPlannerActive(void) +{ + return posControl.flags.wpMissionPlannerActive; +} + int getWaypointCount(void) { uint8_t waypointCount = posControl.waypointCount; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index eb1621e9f8d..3328a8e9fe7 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -786,6 +786,7 @@ bool navigationIsControllingAltitude(void); */ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); +bool isWpMissionPlannerActive(void); bool rthClimbStageActiveAndComplete(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 6c7e130d06a..50f541e916f 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1016,6 +1016,22 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) } } +// Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT) +static int incomingMissionWpCount = 0; +static int incomingMissionWpSequence = 0; + +// A mission edit (upload or clear) is refused while the WP mission is being +// executed: WP mode active, the mission's own RTH leg running (the +// land/loiter decision at home reads the live list), or the on-the-fly +// mission planner writing the same list. This matches the MSP policy from +// #10273 combined with the updateWpMissionPlanner() guard. The ARMED term +// keeps the disarmed path provably unchanged. +static bool mavlinkMissionEditBlocked(void) +{ + return ARMING_FLAG(ARMED) && + (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive() || isWpMissionPlannerActive()); +} + static bool handleIncoming_MISSION_CLEAR_ALL(void) { mavlink_mission_clear_all_t msg; @@ -1023,6 +1039,16 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) // Check if this message is for us if (msg.target_system == mavSystemId) { + // Clearing the mission is not allowed while it is being executed, + // consistent with the policy enforced by setWaypoint() for uploads + if (mavlinkMissionEditBlocked()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + // A clear ends any upload transaction that may be in progress + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; resetWaypointList(); mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); @@ -1032,10 +1058,6 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) return false; } -// Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT) -static int incomingMissionWpCount = 0; -static int incomingMissionWpSequence = 0; - static bool handleIncoming_MISSION_COUNT(void) { mavlink_mission_count_t msg; @@ -1044,18 +1066,36 @@ static bool handleIncoming_MISSION_COUNT(void) // Check if this message is for us if (msg.target_system == mavSystemId) { if (msg.count <= NAV_MAX_WAYPOINTS) { + // Reject the transfer up front while the mission is being + // executed - every MISSION_ITEM would be refused anyway (see + // handleIncoming_MISSION_ITEM). Also drop any half-open + // transaction so a stray item cannot resume it later. + if (mavlinkMissionEditBlocked()) { + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + // Per the mission protocol, count == 0 clears the mission and is + // acknowledged directly - there are no items to request. + if (msg.count == 0) { + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + resetWaypointList(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } incomingMissionWpCount = msg.count; // We need to know how many items to request incomingMissionWpSequence = 0; mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } - else if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } else { + // Oversized mission: NO_SPACE is the accurate diagnostic in + // both armed and disarmed state mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; @@ -1075,7 +1115,12 @@ static bool handleIncoming_MISSION_ITEM(void) // Check supported values first if (ARMING_FLAG(ARMED)) { // Legacy Mission Planner BS for GUIDED - if (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) { + if ((msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) { + if (!isGCSValid()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } if (!(msg.frame == MAV_FRAME_GLOBAL)) { mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, @@ -1086,7 +1131,7 @@ static bool handleIncoming_MISSION_ITEM(void) navWaypoint_t wp; wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = (int32_t)(msg.x * 1e7f); + wp.lat = (int32_t)(msg.x * 1e7f); wp.lon = (int32_t)(msg.y * 1e7f); wp.alt = (int32_t)(msg.z * 100.0f); wp.p1 = 0; @@ -1099,7 +1144,28 @@ static bool handleIncoming_MISSION_ITEM(void) MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; - } else { + } + + // Mission upload while the mission is being executed is not + // allowed. Otherwise fall through: uploading while merely armed + // is permitted, matching the MSP policy in setWaypoint() (see + // PR #10273). setWaypoint() enforces the same rule but returns + // void, so this check is what turns a silent drop into a proper + // NACK. The half-open transaction is dropped so a stray item + // cannot resume it after the mission finishes. + if (mavlinkMissionEditBlocked()) { + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + // While armed, only accept items that belong to a transfer + // started by MISSION_COUNT. Without this, a single stray or + // duplicated MISSION_ITEM could rewrite the mission in flight + // (the disarmed path keeps its historical behaviour). + if (!(incomingMissionWpCount > 0 && incomingMissionWpSequence < incomingMissionWpCount)) { mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true;