MAVLink: allow mission upload and clear in flight when the mission is not being executed

docs/Mavlink.md

@@ -22,7 +22,7 @@ INAV builds against the checked-in generated `storm32` MAVLink headers/dialect b
 
 - **No MAVLink parameter API**: INAV sends a single stub parameter and otherwise ignores parameter traffic. Configure the aircraft through the INAV Configurator or CLI instead.
 - **Selective command support**: INAV implements a useful subset of MAVLink commands and ACKs unsupported commands as `UNSUPPORTED`.
-- **Mission handling is partial**: uploads are rejected while armed except for legacy guided waypoint writes, mission frames are validated per command, and MSP mission parity gaps remain.
+- **Mission handling is partial**: uploads and clears are rejected while armed only when the mission is in use (WP mode active, the mission's own RTH leg running, or the on-the-fly mission planner active), mission frames are validated per command, and MSP mission parity gaps remain.
 - **Mode reporting is approximate**: `custom_mode` is mapped to ArduPilot-style modes for compatibility and does not represent every INAV state exactly.
 - **Single local component identity**: INAV always originates as `MAV_COMP_ID_AUTOPILOT1`; attached radios, GCSes, and companions are always remote components, never local per-port FC identities.
 - **Flow control is per-port and opportunistic**: INAV uses remote TX buffer information from `RADIO_STATUS.txbuf`, or from `MLRS_RADIO_LINK_FLOW_CONTROL.txbuf` on MLRS links. Without flow-control input it falls back to blind 20 ms pacing.
@@ -159,10 +159,10 @@ Messages are organized into MAVLink datastream groups. Each group sends one mess
 - `PING`: broadcast requests are echoed to the requesting system/component.
 - `TIMESYNC`: broadcast or locally targeted requests receive the local boot time in nanoseconds.
 - `MISSION_COUNT`: starts an upload transaction. Stored INAV waypoints remain capped at `NAV_MAX_WAYPOINTS`; the upload transaction also allows QGC planned-home and non-storage command items. The owning system/component and ingress port are retained for the transaction.
-- `MISSION_ITEM` / `MISSION_ITEM_INT`: stores mission waypoints; rejects unsupported frames / sequence errors. Upload while armed is rejected except legacy guided waypoint writes.
+- `MISSION_ITEM` / `MISSION_ITEM_INT`: stores mission waypoints; rejects unsupported frames / sequence errors. While armed, an upload is rejected only when the mission is in use; guided waypoint writes (`current` 2 or 3) are always accepted.
 - `MISSION_REQUEST_LIST`, `MISSION_REQUEST`, `MISSION_REQUEST_INT`: stateful mission download with partner checks and one-item retransmission support.
 - `MISSION_ACK`: completes an active mission download.
-- `MISSION_CLEAR_ALL`: clears the runtime and saved mission.
+- `MISSION_CLEAR_ALL`: clears the runtime mission, and the saved mission when disarmed. While armed it clears the runtime mission only.
 - `COMMAND_LONG` / `COMMAND_INT`: command transport for supported `MAV_CMD_*` handlers.
 - `REQUEST_DATA_STREAM`: legacy stream-rate control per stream group.
 - `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported; altitude-only requests are also accepted when X/Y are masked out and GCS navigation is valid.
@@ -228,7 +228,7 @@ The default ArduPilot-compatible path reports modes through `HEARTBEAT.custom_mo
 
 ## MAVLink missions
 
-INAV supports MAVLink mission upload, download, clear, live mission-state reporting, and waypoint-reached notifications. Uploads retry the outstanding request every 1.5 seconds and abort after five unsuccessful retries. Downloads time out after five seconds of inactivity. Only the system/component and ingress port that started a transfer may continue it. Completed uploads and clears update nonvolatile waypoint storage on targets that provide it. Mission downloads always reply with `MISSION_ITEM_INT`, including in response to a legacy float `MISSION_REQUEST`.
+INAV supports MAVLink mission upload, download, clear, live mission-state reporting, and waypoint-reached notifications. Uploads retry the outstanding request every 1.5 seconds and abort after five unsuccessful retries. Downloads time out after five seconds of inactivity. Only the system/component and ingress port that started a transfer may continue it. Completed uploads and clears update nonvolatile waypoint storage on targets that provide it when the aircraft is disarmed. While armed they apply to the runtime mission only: the mission survives disarming but is lost on reboot, and writing it to storage afterwards is the GCS's job. An upload while armed also collapses a loaded multi-mission set to the uploaded mission for the rest of the session. Mission downloads always reply with `MISSION_ITEM_INT`, including in response to a legacy float `MISSION_REQUEST`.
 
 Mission upload is staged before it touches the live INAV waypoint list. The MAVLink stream is translated into a temporary INAV mission, validated, and committed only after the full upload succeeds, so rejected uploads do not leave a half-written mission in the FC. QGC planned home item `0` is skipped because INAV stores home separately; MAVLink sequence `1` becomes INAV waypoint `1`.
 

src/main/mavlink/mavlink_mission.c

@@ -89,6 +89,19 @@ static bool mavlinkMissionTargetIsLocal(uint8_t targetSystem, uint8_t targetComp
         (targetComponent == 0 || targetComponent == mavComponentId);
 }
 
+// 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 makes the disarmed
+// path provably unchanged, including the short window right after disarming
+// before the nav FSM drops out of WP mode.
+static bool mavlinkMissionEditBlocked(void)
+{
+    return ARMING_FLAG(ARMED) &&
+        (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive() || isWpMissionPlannerActive());
+}
+
 static bool mavlinkMissionSenderOwnsTransfer(void)
 {
     return mavlinkContext.recvMsg.sysid == mavMissionTransfer.partnerSystem &&
@@ -194,7 +207,11 @@ static bool mavlinkClearPersistedMission(void)
 
     resetWaypointList();
     mavlinkContext.missionCompleted = false;
-    if (mavlinkPersistMission()) {
+    // While armed the clear applies to RAM only, mirroring the MSP policy:
+    // saveNonVolatileWaypointList() refuses to run while armed, and a flash
+    // write mid-flight would stall the main loop anyway. The persisted
+    // mission is left untouched until the next clear or upload on the ground.
+    if (ARMING_FLAG(ARMED) || mavlinkPersistMission()) {
         return true;
     }
 
@@ -400,6 +417,26 @@ static bool mavlinkResolveUploadedMissionJumps(void)
             return false;
         }
 
+        // For a mission committed in flight, enforce the same JUMP rules as
+        // the arm-time validation in navigationIsBlockingArming(), which an
+        // in-flight upload bypasses entirely: a JUMP cannot be the first
+        // mission item, cannot target itself or an immediately adjacent
+        // item, must have a sane repeat count and must target a
+        // geo-referenced item. Ground uploads are left to the arm-time
+        // check, keeping disarmed behaviour unchanged.
+        if (ARMING_FLAG(ARMED)) {
+            const int targetIndex = targetWaypointNumber - 1;
+            const navWaypoint_t *target = &mavlinkMissionUploadWaypoints[targetIndex];
+            if (i == 0 ||
+                wp->p2 < -1 ||
+                (targetIndex >= (int)i - 1 && targetIndex <= (int)i + 1) ||
+                !(target->action == NAV_WP_ACTION_WAYPOINT ||
+                  target->action == NAV_WP_ACTION_HOLD_TIME ||
+                  target->action == NAV_WP_ACTION_LAND)) {
+                return false;
+            }
+        }
+
         wp->p1 = targetWaypointNumber;
     }
 
@@ -427,7 +464,11 @@ static bool mavlinkCommitMissionUpload(void)
         setWaypoint(i + 1, &mavlinkMissionUploadWaypoints[i]);
     }
 
-    if (!isWaypointListValid() || !mavlinkPersistMission()) {
+    // While armed the uploaded mission lives in RAM only, mirroring the MSP
+    // in-flight upload policy (see #10273): saveNonVolatileWaypointList()
+    // refuses to run while armed, and a flash write mid-flight would stall
+    // the main loop anyway. On the ground the mission is persisted as before.
+    if (!isWaypointListValid() || (!ARMING_FLAG(ARMED) && !mavlinkPersistMission())) {
         mavlinkRestoreMission(&previousMission);
         return false;
     }
@@ -950,8 +991,16 @@ bool mavlinkHandleIncomingMissionClearAll(void)
         mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
         return true;
     }
-    if (ARMING_FLAG(ARMED)) {
-        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+    // Clearing is allowed while merely armed (RAM only, see
+    // mavlinkClearPersistedMission), but not while the WP mission is being
+    // executed. If the refused sender owns a receiving transfer, abort it so
+    // the retry engine stops soliciting items from a partner we just denied.
+    if (mavlinkMissionEditBlocked()) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_DENIED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+        }
         return true;
     }
     if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) {
@@ -983,8 +1032,17 @@ bool mavlinkHandleIncomingMissionCount(void)
         }
         return true;
     }
-    if (ARMING_FLAG(ARMED)) {
-        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+    // Starting an upload is allowed while merely armed (the commit stays in
+    // RAM, see mavlinkCommitMissionUpload), but not while the WP mission is
+    // being executed. If the refused sender owns a receiving transfer (e.g.
+    // a retry after WP mode engaged mid-upload), abort it so the retry
+    // engine stops soliciting items from a partner we just denied.
+    if (mavlinkMissionEditBlocked()) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_DENIED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+        }
         return true;
     }
     if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) {
@@ -1049,14 +1107,27 @@ bool mavlinkHandleIncomingMissionItem(void)
     }
 
     if (ARMING_FLAG(ARMED)) {
-        if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+        // Guided fly-to-here (current == 2) and altitude-target (current == 3)
+        // items are identified by the item itself - no legitimate upload item
+        // carries these current values - so a guided click is never absorbed
+        // into a running upload transfer.
+        if (msg.command == MAV_CMD_NAV_WAYPOINT && (msg.current == 2 || msg.current == 3)) {
             return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame,
                 MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT,
                 (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z);
         }
 
-        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
-        return true;
+        // Upload items are accepted while merely armed (in-flight upload,
+        // matching the MSP policy from #10273), but not while the WP mission
+        // is being executed.
+        if (mavlinkMissionEditBlocked()) {
+            if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+                mavlinkAbortMissionUpload(MAV_MISSION_DENIED);
+            } else {
+                mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+            }
+            return true;
+        }
     }
 
     return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq,
@@ -1252,14 +1323,27 @@ bool mavlinkHandleIncomingMissionItemInt(void)
     }
 
     if (ARMING_FLAG(ARMED)) {
-        if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+        // Guided fly-to-here (current == 2) and altitude-target (current == 3)
+        // items are identified by the item itself - no legitimate upload item
+        // carries these current values - so a guided click is never absorbed
+        // into a running upload transfer.
+        if (msg.command == MAV_CMD_NAV_WAYPOINT && (msg.current == 2 || msg.current == 3)) {
             return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame,
                 MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT,
                 msg.x, msg.y, msg.z);
         }
 
-        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
-        return true;
+        // Upload items are accepted while merely armed (in-flight upload,
+        // matching the MSP policy from #10273), but not while the WP mission
+        // is being executed.
+        if (mavlinkMissionEditBlocked()) {
+            if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+                mavlinkAbortMissionUpload(MAV_MISSION_DENIED);
+            } else {
+                mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+            }
+            return true;
+        }
     }
 
     return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq,

src/main/navigation/navigation.c

@@ -5488,8 +5488,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;
                     }
                 }
@@ -5505,6 +5506,11 @@ void resetWaypointList(void)
     posControl.geoWaypointCount = 0;
     posControl.startWpIndex = 0;
     posControl.wpReachedNotificationPending = false;
+    // The list no longer holds the mission planner's waypoints: an upload or
+    // clear (MSP, MAVLink, EEPROM) replaced it. Otherwise the planner would
+    // resume at its old index inside the new mission when re-enabled.
+    posControl.wpPlannerActiveWPIndex = 0;
+    posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
 #ifdef USE_MULTI_MISSION
     posControl.totalMultiMissionWpCount = 0;
     posControl.loadedMultiMissionIndex = 0;
@@ -5517,6 +5523,11 @@ bool isWaypointListValid(void)
     return posControl.waypointListValid;
 }
 
+bool isWpMissionPlannerActive(void)
+{
+    return posControl.flags.wpMissionPlannerActive;
+}
+
 int getWaypointCount(void)
 {
     uint8_t waypointCount = posControl.waypointCount;

src/main/navigation/navigation.h

@@ -848,6 +848,7 @@ bool navCanSetHome(void);
  */
 bool navigationRTHAllowsLanding(void);
 bool isWaypointMissionRTHActive(void);
+bool isWpMissionPlannerActive(void);
 #ifdef USE_AUTO_TRANSITION
 navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void);
 #endif