diff --git a/.gitignore b/.gitignore
index 201e51a3526..6f653fd9820 100644
--- a/.gitignore
+++ b/.gitignore
@@ -7,6 +7,7 @@
 *.bak
 *.uvgui.*
 *.ubx
+*.log
 .project
 .settings
 .cproject
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 25930f492ce..f4ab6479a5f 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -412,6 +412,8 @@ main_sources(COMMON_SRC
     mavlink/mavlink_guided.c
     mavlink/mavlink_guided.h
     mavlink/mavlink_internal.h
+    mavlink/mavlink_mission.c
+    mavlink/mavlink_mission.h
     mavlink/mavlink_modes.c
     mavlink/mavlink_modes.h
     mavlink/mavlink_types.h
diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c
index 6ccd8831032..de527b67614 100644
--- a/src/main/fc/fc_mavlink.c
+++ b/src/main/fc/fc_mavlink.c
@@ -4,6 +4,7 @@
 
 #include "mavlink/mavlink_command.h"
 #include "mavlink/mavlink_guided.h"
+#include "mavlink/mavlink_mission.h"
 #include "mavlink/mavlink_runtime.h"
 #include "mavlink/mavlink_streams.h"
 
@@ -249,6 +250,22 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn
         return mavlinkHandleIncomingTimesync() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
         return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+        return mavlinkHandleIncomingMissionClearAll() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_COUNT:
+        return mavlinkHandleIncomingMissionCount() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ITEM:
+        return mavlinkHandleIncomingMissionItem() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
+        return mavlinkHandleIncomingMissionItemInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+        return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST:
+        return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
+        return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ACK:
+        return mavlinkHandleIncomingMissionAck() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_COMMAND_LONG:
         return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_COMMAND_INT:
diff --git a/src/main/mavlink/mavlink_guided.h b/src/main/mavlink/mavlink_guided.h
index e18a9c2bf34..e160134d29b 100644
--- a/src/main/mavlink/mavlink_guided.h
+++ b/src/main/mavlink/mavlink_guided.h
@@ -3,16 +3,11 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "mavlink/mavlink_types.h"
-
-typedef enum {
-    MAV_FRAME_SUPPORTED_NONE = 0,
-    MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0),
-    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT = (1 << 1),
-    MAV_FRAME_SUPPORTED_GLOBAL_INT = (1 << 2),
-    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT = (1 << 3),
-} mavFrameSupportMask_e;
+#include "mavlink/mavlink_mission.h"
 
 bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask);
 bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame);
 MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters);
+
+bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void);
+bool mavlinkHandleIncomingSetPositionTargetLocalNed(void);
diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h
index 70d48b6dafd..8dae16494da 100644
--- a/src/main/mavlink/mavlink_internal.h
+++ b/src/main/mavlink/mavlink_internal.h
@@ -74,6 +74,10 @@
 
 #include "scheduler/scheduler.h"
 
+#define MAVLINK_MISSION_UPLOAD_RETRY_MS 1500
+#define MAVLINK_MISSION_UPLOAD_MAX_RETRIES 5
+#define MAVLINK_MISSION_DOWNLOAD_TIMEOUT_MS 5000
+#define MAVLINK_MISSION_CURRENT_INTERVAL_MS 1000
 #define MAVLINK_RX_BYTE_BUDGET 64
 
 typedef struct mavlinkContext_s {
@@ -90,6 +94,10 @@ typedef struct mavlinkContext_s {
     uint8_t autopilotType;
     uint8_t componentId;
     uint8_t recvPortIndex;
+    mavlinkMissionTransfer_t missionTransfer;
+    timeMs_t lastMissionCurrentMs;
+    bool missionCompleted;
+    bool lastWpModeActive;
     uint32_t lastArmingDisableFlags;
     flightModeForTelemetry_e lastFlightMode;
     bool lastGCSNavMode;
@@ -110,5 +118,6 @@ extern mavlinkContext_t mavlinkContext;
 #define mavAutopilotType (mavlinkContext.autopilotType)
 #define mavComponentId (mavlinkContext.componentId)
 #define mavRecvPortIndex (mavlinkContext.recvPortIndex)
+#define mavMissionTransfer (mavlinkContext.missionTransfer)
 
 #endif
diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c
new file mode 100644
index 00000000000..08141b4f086
--- /dev/null
+++ b/src/main/mavlink/mavlink_mission.c
@@ -0,0 +1,1321 @@
+#include "mavlink/mavlink_internal.h"
+
+#include "mavlink/mavlink_guided.h"
+#include "mavlink/mavlink_mission.h"
+#include "mavlink/mavlink_runtime.h"
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK)
+
+/*
+ * Mission transfer retry and partner tracking are adapted from Betaflight's
+ * GPLv3 mavlink_mission.c, commit 87d4bd63, for INAV's routed multi-port runtime.
+ */
+static uint8_t mavlinkActivePortMask(void)
+{
+    uint8_t sendMask = 0;
+    for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) {
+        if (mavPortStates[portIndex].telemetryEnabled && mavPortStates[portIndex].port) {
+            sendMask |= MAVLINK_PORT_MASK(portIndex);
+        }
+    }
+    return sendMask;
+}
+
+#define MAVLINK_MISSION_UPLOAD_MAX_ITEMS ((uint16_t)(NAV_MAX_WAYPOINTS * 4U + 1U))
+
+static navWaypoint_t mavlinkMissionUploadWaypoints[NAV_MAX_WAYPOINTS];
+static uint8_t mavlinkMissionUploadWaypointCount;
+static uint8_t mavlinkMissionUploadSequenceWaypointNumbers[MAVLINK_MISSION_UPLOAD_MAX_ITEMS];
+static int16_t mavlinkMissionCurrentSpeedCmS;
+
+typedef struct mavlinkMissionSnapshot_s {
+    uint8_t waypointCount;
+    bool missionCompleted;
+    navWaypoint_t waypoints[NAV_MAX_WAYPOINTS];
+} mavlinkMissionSnapshot_t;
+
+static void mavlinkClearMissionUploadBuffer(void)
+{
+    memset(mavlinkMissionUploadWaypoints, 0, sizeof(mavlinkMissionUploadWaypoints));
+    memset(mavlinkMissionUploadSequenceWaypointNumbers, 0, sizeof(mavlinkMissionUploadSequenceWaypointNumbers));
+    mavlinkMissionUploadWaypointCount = 0;
+    mavlinkMissionCurrentSpeedCmS = 0;
+}
+
+void mavlinkSendPendingMissionItemReached(void)
+{
+    // Check for a delivery target before consuming the one-slot latch: a
+    // shared port can close on disarm right after landing, and consuming
+    // first would silently destroy the final item's notification.
+    const uint8_t sendMask = mavlinkActivePortMask();
+    if (sendMask == 0) {
+        return;
+    }
+
+    uint16_t seq;
+    if (!navigationConsumeWaypointReached(&seq)) {
+        return;
+    }
+
+    mavlinkContext.missionCompleted = getWaypointCount() > 0 && seq + 1 >= getWaypointCount();
+    mavSendMask = sendMask;
+    mavlink_msg_mission_item_reached_pack(mavlinkGetCommonConfig()->sysid, MAV_COMP_ID_AUTOPILOT1, &mavSendMsg, seq);
+    mavlinkSendMessage();
+    mavSendMask = 0;
+}
+
+uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages)
+{
+    switch (wp->action) {
+        case NAV_WP_ACTION_RTH:
+        case NAV_WP_ACTION_JUMP:
+        case NAV_WP_ACTION_SET_HEAD:
+            return MAV_FRAME_MISSION;
+        default:
+            break;
+    }
+
+    if ((wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE) {
+        return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL;
+    }
+
+    return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT;
+}
+
+
+static bool mavlinkMissionTargetIsLocal(uint8_t targetSystem, uint8_t targetComponent)
+{
+    return (targetSystem == 0 || targetSystem == mavSystemId) &&
+        (targetComponent == 0 || targetComponent == mavComponentId);
+}
+
+static bool mavlinkMissionSenderOwnsTransfer(void)
+{
+    return mavlinkContext.recvMsg.sysid == mavMissionTransfer.partnerSystem &&
+        mavlinkContext.recvMsg.compid == mavMissionTransfer.partnerComponent &&
+        mavRecvPortIndex == mavMissionTransfer.ingressPortIndex;
+}
+
+static void mavlinkSendMissionAckTo(uint8_t targetSystem, uint8_t targetComponent, MAV_MISSION_RESULT result)
+{
+    mavlink_msg_mission_ack_pack(
+        mavSystemId,
+        mavComponentId,
+        &mavSendMsg,
+        targetSystem,
+        targetComponent,
+        result,
+        MAV_MISSION_TYPE_MISSION,
+        0
+    );
+    mavlinkSendMessage();
+}
+
+static void mavlinkResetMissionTransfer(void)
+{
+    memset(&mavMissionTransfer, 0, sizeof(mavMissionTransfer));
+    mavMissionTransfer.state = MAVLINK_MISSION_TRANSFER_IDLE;
+    mavlinkClearMissionUploadBuffer();
+}
+
+static void mavlinkAbortMissionUpload(MAV_MISSION_RESULT result)
+{
+    mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, result);
+    mavlinkResetMissionTransfer();
+}
+
+static void mavlinkStartMissionTransfer(mavlinkMissionTransferState_e state, uint16_t count)
+{
+    mavlinkResetMissionTransfer();
+    mavMissionTransfer.state = state;
+    mavMissionTransfer.count = count;
+    mavMissionTransfer.partnerSystem = mavlinkContext.recvMsg.sysid;
+    mavMissionTransfer.partnerComponent = mavlinkContext.recvMsg.compid;
+    mavMissionTransfer.ingressPortIndex = mavRecvPortIndex;
+    mavMissionTransfer.useIntMessages = true;
+    mavMissionTransfer.lastActivityMs = millis();
+}
+
+static void mavlinkSendMissionRequest(void)
+{
+    if (mavMissionTransfer.useIntMessages) {
+        mavlink_msg_mission_request_int_pack(
+            mavSystemId,
+            mavComponentId,
+            &mavSendMsg,
+            mavMissionTransfer.partnerSystem,
+            mavMissionTransfer.partnerComponent,
+            mavMissionTransfer.nextSequence,
+            MAV_MISSION_TYPE_MISSION);
+    } else {
+        mavlink_msg_mission_request_pack(
+            mavSystemId,
+            mavComponentId,
+            &mavSendMsg,
+            mavMissionTransfer.partnerSystem,
+            mavMissionTransfer.partnerComponent,
+            mavMissionTransfer.nextSequence,
+            MAV_MISSION_TYPE_MISSION);
+    }
+    mavlinkSendMessage();
+}
+
+static bool mavlinkPersistMission(void)
+{
+#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
+    return saveNonVolatileWaypointList();
+#else
+    return true;
+#endif
+}
+
+static void mavlinkSnapshotMission(mavlinkMissionSnapshot_t *snapshot)
+{
+    snapshot->waypointCount = getWaypointCount();
+    snapshot->missionCompleted = mavlinkContext.missionCompleted;
+    for (uint8_t i = 0; i < snapshot->waypointCount; i++) {
+        getWaypoint(i + 1, &snapshot->waypoints[i]);
+    }
+}
+
+static void mavlinkRestoreMission(const mavlinkMissionSnapshot_t *snapshot)
+{
+    resetWaypointList();
+    for (uint8_t i = 0; i < snapshot->waypointCount; i++) {
+        setWaypoint(i + 1, &snapshot->waypoints[i]);
+    }
+    mavlinkContext.missionCompleted = snapshot->missionCompleted;
+}
+
+static bool mavlinkClearPersistedMission(void)
+{
+    mavlinkMissionSnapshot_t previousMission;
+    mavlinkSnapshotMission(&previousMission);
+
+    resetWaypointList();
+    mavlinkContext.missionCompleted = false;
+    if (mavlinkPersistMission()) {
+        return true;
+    }
+
+    mavlinkRestoreMission(&previousMission);
+    return false;
+}
+
+static bool mavlinkMissionCoordinateIsValid(int32_t lat, int32_t lon)
+{
+    return lat >= -900000000 && lat <= 900000000 &&
+        lon >= -1800000000 && lon <= 1800000000;
+}
+
+static bool mavlinkMissionAltitudeIsValid(float altMeters)
+{
+    if (!isfinite(altMeters) ||
+        altMeters < (float)INT32_MIN / 100.0f ||
+        altMeters > (float)INT32_MAX / 100.0f) {
+        return false;
+    }
+
+    const int64_t altCentimeters = (int64_t)llrintf(altMeters * 100.0f);
+    return altCentimeters >= INT32_MIN && altCentimeters <= INT32_MAX;
+}
+
+static int32_t mavlinkMissionAltitudeToCentimeters(float altMeters)
+{
+    return (int32_t)llrintf(altMeters * 100.0f);
+}
+
+static bool mavlinkMissionItemIsQgcPlannedHome(
+    uint8_t frame,
+    uint16_t command,
+    uint8_t current,
+    uint16_t seq,
+    float param1,
+    float param2,
+    float param3,
+    float param4)
+{
+    if (seq != 0 || current != 0 || command != MAV_CMD_NAV_WAYPOINT || mavMissionTransfer.count <= 1) {
+        return false;
+    }
+
+    if (!mavlinkFrameIsSupported(frame, MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_INT)) {
+        return false;
+    }
+
+    // QGC-style planned home has no command parameters; a real first waypoint is usually current.
+    return (isnan(param1) || param1 == 0.0f) &&
+        (isnan(param2) || param2 == 0.0f) &&
+        (isnan(param3) || param3 == 0.0f) &&
+        (isnan(param4) || param4 == 0.0f);
+}
+
+static void mavlinkApplyCurrentSpeed(navWaypoint_t *wp)
+{
+    if (mavlinkMissionCurrentSpeedCmS <= 0) {
+        return;
+    }
+
+    switch (wp->action) {
+        case NAV_WP_ACTION_WAYPOINT:
+        case NAV_WP_ACTION_LAND:
+            wp->p1 = mavlinkMissionCurrentSpeedCmS;
+            break;
+        case NAV_WP_ACTION_HOLD_TIME:
+            wp->p2 = mavlinkMissionCurrentSpeedCmS;
+            break;
+        default:
+            break;
+    }
+}
+
+static bool mavlinkMissionApplyDelayToPreviousWaypoint(float delaySeconds)
+{
+    if (!isfinite(delaySeconds) || delaySeconds < 0.0f || delaySeconds > (float)INT16_MAX) {
+        return false;
+    }
+
+    const int32_t roundedDelay = (int32_t)lrintf(delaySeconds);
+    if (roundedDelay < 0 || roundedDelay > INT16_MAX) {
+        return false;
+    }
+
+    const int16_t delay = (int16_t)roundedDelay;
+
+    for (uint8_t i = mavlinkMissionUploadWaypointCount; i > 0; i--) {
+        navWaypoint_t *wp = &mavlinkMissionUploadWaypoints[i - 1];
+
+        if (!(wp->action == NAV_WP_ACTION_WAYPOINT || wp->action == NAV_WP_ACTION_HOLD_TIME)) {
+            continue;
+        }
+
+        if (wp->action == NAV_WP_ACTION_WAYPOINT) {
+            wp->action = NAV_WP_ACTION_HOLD_TIME;
+            wp->p2 = wp->p1;
+            wp->p1 = delay;
+            return true;
+        }
+
+        const int32_t combinedDelay = (int32_t)wp->p1 + delay;
+        if (combinedDelay < 0 || combinedDelay > INT16_MAX) {
+            return false;
+        }
+
+        wp->p1 = (int16_t)combinedDelay;
+        return true;
+    }
+
+    return false;
+}
+
+static bool mavlinkMissionAltitudeFrameIsSupported(uint8_t frame)
+{
+    return mavlinkFrameIsSupported(frame,
+        MAV_FRAME_SUPPORTED_GLOBAL |
+        MAV_FRAME_SUPPORTED_GLOBAL_INT |
+        MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+        MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT);
+}
+
+static void mavlinkMissionApplyAltitudeMode(navWaypoint_t *wp, uint8_t frame)
+{
+    if (mavlinkFrameUsesAbsoluteAltitude(frame)) {
+        wp->p3 = (int16_t)(wp->p3 | NAV_WP_ALTMODE);
+    } else {
+        wp->p3 = (int16_t)(wp->p3 & (int16_t)~NAV_WP_ALTMODE);
+    }
+}
+
+static bool mavlinkMissionApplyAltitudeToPreviousWaypoint(float altitudeMeters, uint8_t frame, bool updateAltitudeMode)
+{
+    if (!mavlinkMissionAltitudeIsValid(altitudeMeters)) {
+        return false;
+    }
+
+    if (updateAltitudeMode && !mavlinkMissionAltitudeFrameIsSupported(frame)) {
+        return false;
+    }
+
+    for (uint8_t i = mavlinkMissionUploadWaypointCount; i > 0; i--) {
+        navWaypoint_t *wp = &mavlinkMissionUploadWaypoints[i - 1];
+
+        if (!(wp->action == NAV_WP_ACTION_WAYPOINT || wp->action == NAV_WP_ACTION_HOLD_TIME || wp->action == NAV_WP_ACTION_LAND)) {
+            continue;
+        }
+
+        wp->alt = mavlinkMissionAltitudeToCentimeters(altitudeMeters);
+        if (updateAltitudeMode) {
+            mavlinkMissionApplyAltitudeMode(wp, frame);
+        }
+
+        return true;
+    }
+
+    return false;
+}
+
+static bool mavlinkStageUploadedMissionItem(uint16_t seq, navWaypoint_t *wp)
+{
+    if (seq >= MAVLINK_MISSION_UPLOAD_MAX_ITEMS || mavlinkMissionUploadWaypointCount >= NAV_MAX_WAYPOINTS) {
+        return false;
+    }
+
+    const uint8_t waypointNumber = mavlinkMissionUploadWaypointCount + 1;
+    mavlinkMissionUploadSequenceWaypointNumbers[seq] = waypointNumber;
+
+    mavlinkApplyCurrentSpeed(wp);
+    wp->flag = (seq + 1 >= mavMissionTransfer.count) ? NAV_WP_FLAG_LAST : 0;
+
+    mavlinkMissionUploadWaypoints[mavlinkMissionUploadWaypointCount] = *wp;
+    mavlinkMissionUploadWaypointCount++;
+
+    return true;
+}
+
+static bool mavlinkStageSkippedMissionItem(uint16_t seq)
+{
+    if (seq >= MAVLINK_MISSION_UPLOAD_MAX_ITEMS) {
+        return false;
+    }
+
+    mavlinkMissionUploadSequenceWaypointNumbers[seq] = 0;
+    return true;
+}
+
+static bool mavlinkResolveUploadedMissionJumps(void)
+{
+    for (uint8_t i = 0; i < mavlinkMissionUploadWaypointCount; i++) {
+        navWaypoint_t *wp = &mavlinkMissionUploadWaypoints[i];
+        if (wp->action != NAV_WP_ACTION_JUMP) {
+            continue;
+        }
+
+        const uint16_t targetSeq = (uint16_t)wp->p1;
+        if (targetSeq >= MAVLINK_MISSION_UPLOAD_MAX_ITEMS) {
+            return false;
+        }
+
+        const uint8_t targetWaypointNumber = mavlinkMissionUploadSequenceWaypointNumbers[targetSeq];
+        if (targetWaypointNumber == 0 || targetWaypointNumber > mavlinkMissionUploadWaypointCount) {
+            return false;
+        }
+
+        wp->p1 = targetWaypointNumber;
+    }
+
+    return true;
+}
+
+static bool mavlinkCommitMissionUpload(void)
+{
+    if (!mavlinkResolveUploadedMissionJumps()) {
+        return false;
+    }
+
+    if (mavlinkMissionUploadWaypointCount == 0) {
+        return mavlinkClearPersistedMission();
+    }
+
+    mavlinkMissionUploadWaypoints[mavlinkMissionUploadWaypointCount - 1].flag = NAV_WP_FLAG_LAST;
+
+    mavlinkMissionSnapshot_t previousMission;
+    mavlinkSnapshotMission(&previousMission);
+
+    resetWaypointList();
+
+    for (uint8_t i = 0; i < mavlinkMissionUploadWaypointCount; i++) {
+        setWaypoint(i + 1, &mavlinkMissionUploadWaypoints[i]);
+    }
+
+    if (!isWaypointListValid() || !mavlinkPersistMission()) {
+        mavlinkRestoreMission(&previousMission);
+        return false;
+    }
+
+    mavlinkContext.missionCompleted = false;
+    return true;
+}
+
+void mavlinkMissionUpdate(timeMs_t currentTimeMs)
+{
+    // A fresh WP-mode engagement starts a new mission run: forget the
+    // previous run's completion so a re-flown mission reports ACTIVE, not a
+    // stale COMPLETE.
+    const bool wpModeActive = FLIGHT_MODE(NAV_WP_MODE);
+    if (wpModeActive && !mavlinkContext.lastWpModeActive) {
+        mavlinkContext.missionCompleted = false;
+    }
+    mavlinkContext.lastWpModeActive = wpModeActive;
+
+    if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING &&
+        currentTimeMs - mavMissionTransfer.lastActivityMs >= MAVLINK_MISSION_UPLOAD_RETRY_MS) {
+        mavSendMask = MAVLINK_PORT_MASK(mavMissionTransfer.ingressPortIndex);
+        if (mavMissionTransfer.retries >= MAVLINK_MISSION_UPLOAD_MAX_RETRIES) {
+            mavlinkSendMissionAckTo(
+                mavMissionTransfer.partnerSystem,
+                mavMissionTransfer.partnerComponent,
+                MAV_MISSION_OPERATION_CANCELLED);
+            mavlinkResetMissionTransfer();
+        } else {
+            mavMissionTransfer.retries++;
+            mavMissionTransfer.lastActivityMs = currentTimeMs;
+            mavlinkSendMissionRequest();
+        }
+        mavSendMask = 0;
+    }
+
+    if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_SENDING &&
+        currentTimeMs - mavMissionTransfer.lastActivityMs >= MAVLINK_MISSION_DOWNLOAD_TIMEOUT_MS) {
+        mavlinkResetMissionTransfer();
+    }
+
+    if (currentTimeMs - mavlinkContext.lastMissionCurrentMs < MAVLINK_MISSION_CURRENT_INTERVAL_MS) {
+        return;
+    }
+
+    const uint8_t sendMask = mavlinkActivePortMask();
+    if (sendMask == 0) {
+        return;
+    }
+
+    const uint16_t total = getWaypointCount();
+    const bool active = FLIGHT_MODE(NAV_WP_MODE);
+    const uint16_t seq = total > 0 && active && getActiveWpNumber() > 0 ? getActiveWpNumber() - 1 : 0;
+    MISSION_STATE missionState;
+    if (total == 0) {
+        missionState = MISSION_STATE_NO_MISSION;
+    } else if (mavlinkContext.missionCompleted) {
+        // Completion outranks WP-mode activity: after the final item (e.g. a
+        // LAND) the FSM parks in NAV_STATE_WAYPOINT_FINISHED, which still
+        // maps to NAV_WP_MODE - a landed vehicle must not report ACTIVE
+        // until the pilot flips the mode switch. A re-flight clears
+        // missionCompleted on the WP-mode rising edge above.
+        missionState = MISSION_STATE_COMPLETE;
+    } else if (active) {
+        missionState = MISSION_STATE_ACTIVE;
+    } else {
+        missionState = MISSION_STATE_NOT_STARTED;
+    }
+
+    mavSendMask = sendMask;
+    mavlink_msg_mission_current_pack(
+        mavlinkGetCommonConfig()->sysid,
+        MAV_COMP_ID_AUTOPILOT1,
+        &mavSendMsg,
+        seq,
+        total,
+        missionState,
+        active ? 1 : 2,
+        0,
+        0,
+        0);
+    mavlinkSendMessage();
+    mavSendMask = 0;
+    mavlinkContext.lastMissionCurrentMs = currentTimeMs;
+}
+
+static bool mavlinkHandleArmedGuidedMissionItem(
+    uint8_t current, 
+    uint8_t frame, 
+    mavFrameSupportMask_e allowedFrames, 
+    int32_t latitudeE7, 
+    int32_t longitudeE7, 
+    float altitudeMeters)
+{
+    if (!isGCSValid()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
+        return true;
+    }
+
+    if (!mavlinkFrameIsSupported(frame, allowedFrames)) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME);
+        return true;
+    }
+    if (!mavlinkMissionAltitudeIsValid(altitudeMeters) ||
+        latitudeE7 < -900000000 || latitudeE7 > 900000000 ||
+        longitudeE7 < -1800000000 || longitudeE7 > 1800000000) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID);
+        return true;
+    }
+
+    if (current == 2) {
+        navWaypoint_t wp = {0};
+        wp.action = NAV_WP_ACTION_WAYPOINT;
+        wp.lat = latitudeE7;
+        wp.lon = longitudeE7;
+        wp.alt = mavlinkMissionAltitudeToCentimeters(altitudeMeters);
+        wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+
+        setWaypoint(255, &wp);
+
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED);
+        return true;
+    }
+
+    if (current == 3) {
+        const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, altitudeMeters);
+        MAV_MISSION_RESULT response = MAV_MISSION_ERROR;
+        if (result == MAV_RESULT_ACCEPTED) {response = MAV_MISSION_ACCEPTED;}
+        else if (result == MAV_RESULT_UNSUPPORTED) {response = MAV_MISSION_UNSUPPORTED;}
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, response);
+        return true;
+    }
+
+    mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
+    return true;
+}
+
+static void mavlinkFinishAcceptedUploadSequence(void)
+{
+    mavMissionTransfer.nextSequence++;
+
+    if (mavMissionTransfer.nextSequence >= mavMissionTransfer.count) {
+        if (mavlinkCommitMissionUpload()) {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID);
+        }
+        mavlinkResetMissionTransfer();
+    } else {
+        mavlinkSendMissionRequest();
+    }
+}
+
+static bool mavlinkHandleMissionItemCommon(
+    bool useIntMessages,
+    uint8_t frame,
+    uint16_t command,
+    uint8_t current,
+    uint8_t autocontinue,
+    uint16_t seq,
+    float param1,
+    float param2,
+    float param3,
+    float param4,
+    int32_t lat,
+    int32_t lon,
+    float altMeters)
+{
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_RECEIVING || !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE);
+        return true;
+    }
+
+    if (!mavMissionTransfer.formatSelected) {
+        mavMissionTransfer.useIntMessages = useIntMessages;
+        mavMissionTransfer.formatSelected = true;
+    } else if (mavMissionTransfer.useIntMessages != useIntMessages) {
+        mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        return true;
+    }
+
+    mavMissionTransfer.lastActivityMs = millis();
+    mavMissionTransfer.retries = 0;
+
+    if (seq != mavMissionTransfer.nextSequence) {
+        mavlinkSendMissionRequest();
+        return true;
+    }
+
+    if (seq >= MAVLINK_MISSION_UPLOAD_MAX_ITEMS) {
+        mavlinkAbortMissionUpload(MAV_MISSION_NO_SPACE);
+        return true;
+    }
+
+    if (mavlinkMissionItemIsQgcPlannedHome(frame, command, current, seq, param1, param2, param3, param4)) {
+        UNUSED(current);
+        UNUSED(autocontinue);
+        UNUSED(param1);
+        UNUSED(param2);
+        UNUSED(param3);
+        UNUSED(param4);
+        UNUSED(lat);
+        UNUSED(lon);
+        UNUSED(altMeters);
+
+        if (!mavlinkStageSkippedMissionItem(seq)) {
+            mavlinkAbortMissionUpload(MAV_MISSION_NO_SPACE);
+            return true;
+        }
+
+        mavlinkFinishAcceptedUploadSequence();
+        return true;
+    }
+
+    const bool lastMissionItem = seq + 1 >= mavMissionTransfer.count;
+
+    if (autocontinue == 0 && !lastMissionItem) {
+        mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        return true;
+    }
+
+    UNUSED(current);
+
+    navWaypoint_t wp = {0};
+    bool storeWaypoint = true;
+
+    switch (command) {
+        case MAV_CMD_NAV_WAYPOINT:
+            if (!mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!mavlinkMissionCoordinateIsValid(lat, lon)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID);
+                return true;
+            }
+            if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_WAYPOINT;
+            if (isfinite(param1) && param1 > 0.0f) {
+                if (param1 > INT16_MAX) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                    return true;
+                }
+                wp.action = NAV_WP_ACTION_HOLD_TIME;
+                wp.p1 = (int16_t)lrintf(param1);
+            }
+            wp.lat = lat;
+            wp.lon = lon;
+            wp.alt = mavlinkMissionAltitudeToCentimeters(altMeters);
+            wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+            break;
+
+        case MAV_CMD_NAV_LOITER_TIME:
+            if (!mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!mavlinkMissionCoordinateIsValid(lat, lon)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID);
+                return true;
+            }
+            if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                return true;
+            }
+            if (!isfinite(param1) || param1 < 0.0f || param1 > INT16_MAX) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_HOLD_TIME;
+            wp.lat = lat;
+            wp.lon = lon;
+            wp.alt = mavlinkMissionAltitudeToCentimeters(altMeters);
+            wp.p1 = (int16_t)lrintf(param1);
+            wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+            break;
+
+        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+            if (frame != MAV_FRAME_MISSION && !mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_RTH;
+            if (isfinite(param1) && param1 > 0.0f) {
+                wp.p1 = 1;
+            }
+            if (mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                    return true;
+                }
+                wp.alt = mavlinkMissionAltitudeToCentimeters(altMeters);
+                wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+            }
+            break;
+
+        case MAV_CMD_NAV_LAND:
+            if (!mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!mavlinkMissionCoordinateIsValid(lat, lon)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID);
+                return true;
+            }
+            if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_LAND;
+            wp.lat = lat;
+            wp.lon = lon;
+            wp.alt = mavlinkMissionAltitudeToCentimeters(altMeters);
+            wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+            break;
+
+        case MAV_CMD_DO_JUMP:
+            if (frame != MAV_FRAME_MISSION) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!isfinite(param1) || param1 < 0.0f || param1 >= MAVLINK_MISSION_UPLOAD_MAX_ITEMS) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                return true;
+            }
+            if (!isfinite(param2) || param2 < INT16_MIN || param2 > INT16_MAX) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM2);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_JUMP;
+            wp.p1 = (int16_t)lrintf(param1);
+            wp.p2 = (int16_t)lrintf(param2);
+            break;
+
+        case MAV_CMD_CONDITION_DELAY:
+            if (frame != MAV_FRAME_MISSION) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!isfinite(param1) || param1 < 0.0f || param1 > (float)INT16_MAX) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                return true;
+            }
+            if ((isfinite(param2) && param2 != 0.0f) ||
+                (isfinite(param3) && param3 != 0.0f) ||
+                (isfinite(param4) && param4 != 0.0f)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+            if (!mavlinkMissionApplyDelayToPreviousWaypoint(param1)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+            storeWaypoint = false;
+            break;
+
+        case MAV_CMD_CONDITION_CHANGE_ALT:
+            if (frame != MAV_FRAME_MISSION && !mavlinkMissionAltitudeFrameIsSupported(frame)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                return true;
+            }
+            if (!mavlinkMissionApplyAltitudeToPreviousWaypoint(altMeters, frame, frame != MAV_FRAME_MISSION)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+            storeWaypoint = false;
+            break;
+
+        case MAV_CMD_DO_CHANGE_ALTITUDE:
+            if (!mavlinkMissionAltitudeIsValid(param1)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                return true;
+            }
+            if (isfinite(param2)) {
+                if (param2 < 0.0f || param2 > (float)UINT8_MAX) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM2);
+                    return true;
+                }
+
+                const uint8_t altitudeFrame = (uint8_t)lrintf(param2);
+                if (!mavlinkMissionApplyAltitudeToPreviousWaypoint(param1, altitudeFrame, true)) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                    return true;
+                }
+            } else {
+                if (!mavlinkMissionApplyAltitudeToPreviousWaypoint(param1, 0, false)) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                    return true;
+                }
+            }
+            storeWaypoint = false;
+            break;
+
+        case MAV_CMD_DO_CHANGE_SPEED:
+            if (!isfinite(param1) || !isfinite(param2)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID);
+                return true;
+            }
+            if (param1 > 1.0f) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+            if (param2 >= 0.0f) {
+                if (param2 > (float)INT16_MAX / 100.0f) {
+                    mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM2);
+                    return true;
+                }
+                mavlinkMissionCurrentSpeedCmS = (int16_t)lrintf(param2 * 100.0f);
+            }
+            storeWaypoint = false;
+            break;
+
+        case MAV_CMD_DO_SET_ROI:
+            if (!isfinite(param1) || param1 != MAV_ROI_LOCATION) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+            if (!mavlinkFrameIsSupported(frame,
+                MAV_FRAME_SUPPORTED_GLOBAL |
+                MAV_FRAME_SUPPORTED_GLOBAL_INT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+                MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!mavlinkMissionCoordinateIsValid(lat, lon)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID);
+                return true;
+            }
+            if (!mavlinkMissionAltitudeIsValid(altMeters)) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM7);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_SET_POI;
+            wp.lat = lat;
+            wp.lon = lon;
+            wp.alt = mavlinkMissionAltitudeToCentimeters(altMeters);
+            wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+            break;
+
+        case MAV_CMD_CONDITION_YAW:
+            if (frame != MAV_FRAME_MISSION) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED_FRAME);
+                return true;
+            }
+            if (!isfinite(param1) || param1 < -1.0f || param1 >= 360.0f) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM1);
+                return true;
+            }
+            if (isfinite(param4) && param4 != 0.0f) {
+                mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+                return true;
+            }
+
+            wp.action = NAV_WP_ACTION_SET_HEAD;
+            wp.p1 = (int16_t)lrintf(param1);
+            break;
+
+        default:
+            mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+            return true;
+    }
+
+    if (storeWaypoint) {
+        if (!mavlinkStageUploadedMissionItem(seq, &wp)) {
+            mavlinkAbortMissionUpload(MAV_MISSION_NO_SPACE);
+            return true;
+        }
+    } else if (!mavlinkStageSkippedMissionItem(seq)) {
+        mavlinkAbortMissionUpload(MAV_MISSION_NO_SPACE);
+        return true;
+    }
+
+    mavlinkFinishAcceptedUploadSequence();
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionClearAll(void)
+{
+    mavlink_mission_clear_all_t msg;
+    mavlink_msg_mission_clear_all_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION && msg.mission_type != MAV_MISSION_TYPE_ALL) {
+        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);
+        return true;
+    }
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+        return true;
+    }
+
+    mavlinkResetMissionTransfer();
+    mavlinkSendMissionAckTo(
+        mavlinkContext.recvMsg.sysid,
+        mavlinkContext.recvMsg.compid,
+        mavlinkClearPersistedMission() ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionCount(void)
+{
+    mavlink_mission_count_t msg;
+    mavlink_msg_mission_count_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        } else {
+            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);
+        return true;
+    }
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+        return true;
+    }
+    if (msg.count > MAVLINK_MISSION_UPLOAD_MAX_ITEMS) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_NO_SPACE);
+        return true;
+    }
+    if (msg.count == 0) {
+        mavlinkResetMissionTransfer();
+        mavlinkSendMissionAckTo(
+            mavlinkContext.recvMsg.sysid,
+            mavlinkContext.recvMsg.compid,
+            mavlinkClearPersistedMission() ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
+        return true;
+    }
+
+    mavlinkStartMissionTransfer(MAVLINK_MISSION_TRANSFER_RECEIVING, msg.count);
+    mavlinkSendMissionRequest();
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionItem(void)
+{
+    mavlink_mission_item_t msg;
+    mavlink_msg_mission_item_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
+        }
+        return true;
+    }
+    const bool coordinateCommand = msg.command == MAV_CMD_NAV_WAYPOINT ||
+        msg.command == MAV_CMD_NAV_LOITER_TIME ||
+        msg.command == MAV_CMD_NAV_LAND ||
+        msg.command == MAV_CMD_DO_SET_ROI;
+    if (coordinateCommand) {
+        if (!isfinite(msg.x) || msg.x < -90.0f || msg.x > 90.0f) {
+            if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM5_X);
+            } else {
+                mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_PARAM5_X);
+            }
+            return true;
+        }
+        if (!isfinite(msg.y) || msg.y < -180.0f || msg.y > 180.0f) {
+            if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+                mavlinkAbortMissionUpload(MAV_MISSION_INVALID_PARAM6_Y);
+            } else {
+                mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_PARAM6_Y);
+            }
+            return true;
+        }
+    }
+
+    if (ARMING_FLAG(ARMED)) {
+        if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+            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;
+    }
+
+    return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq,
+        msg.param1, msg.param2, msg.param3, msg.param4,
+        coordinateCommand ? (int32_t)lrintf(msg.x * 1e7f) : 0,
+        coordinateCommand ? (int32_t)lrintf(msg.y * 1e7f) : 0,
+        msg.z);
+}
+
+bool mavlinkHandleIncomingMissionRequestList(void)
+{
+    mavlink_mission_request_list_t msg;
+    mavlink_msg_mission_request_list_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
+        }
+        return true;
+    }
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED);
+        return true;
+    }
+
+    const uint16_t count = getWaypointCount();
+    mavlinkStartMissionTransfer(MAVLINK_MISSION_TRANSFER_SENDING, count);
+    mavlink_msg_mission_count_pack(
+        mavSystemId,
+        mavComponentId,
+        &mavSendMsg,
+        mavMissionTransfer.partnerSystem,
+        mavMissionTransfer.partnerComponent,
+        count,
+        MAV_MISSION_TYPE_MISSION,
+        0);
+    mavlinkSendMessage();
+    if (count == 0) {
+        mavlinkResetMissionTransfer();
+    }
+    return true;
+}
+
+bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item)
+{
+    mavlinkMissionItemData_t data = {0};
+
+    data.frame = mavlinkWaypointFrame(wp, useIntMessages);
+
+    switch (wp->action) {
+        case NAV_WP_ACTION_WAYPOINT:
+            data.command = MAV_CMD_NAV_WAYPOINT;
+            data.lat = wp->lat;
+            data.lon = wp->lon;
+            data.alt = wp->alt / 100.0f;
+            break;
+
+        case NAV_WP_ACTION_HOLD_TIME:
+            data.command = MAV_CMD_NAV_LOITER_TIME;
+            data.param1 = wp->p1;
+            data.lat = wp->lat;
+            data.lon = wp->lon;
+            data.alt = wp->alt / 100.0f;
+            break;
+
+        case NAV_WP_ACTION_RTH:
+            data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH;
+            break;
+
+        case NAV_WP_ACTION_LAND:
+            data.command = MAV_CMD_NAV_LAND;
+            data.lat = wp->lat;
+            data.lon = wp->lon;
+            data.alt = wp->alt / 100.0f;
+            break;
+
+        case NAV_WP_ACTION_JUMP:
+            data.command = MAV_CMD_DO_JUMP;
+            data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f;
+            data.param2 = wp->p2;
+            break;
+
+        case NAV_WP_ACTION_SET_POI:
+            data.command = MAV_CMD_DO_SET_ROI;
+            data.param1 = MAV_ROI_LOCATION;
+            data.lat = wp->lat;
+            data.lon = wp->lon;
+            data.alt = wp->alt / 100.0f;
+            break;
+
+        case NAV_WP_ACTION_SET_HEAD:
+            data.command = MAV_CMD_CONDITION_YAW;
+            data.param1 = wp->p1;
+            break;
+
+        default:
+            return false;
+    }
+
+    *item = data;
+    return true;
+}
+
+// Downloads always answer with MISSION_ITEM_INT, even for a legacy (float)
+// MISSION_REQUEST: MAVLink deprecated MISSION_REQUEST (2020-06) with the
+// explicit guidance to treat it as MISSION_REQUEST_INT. A float MISSION_ITEM
+// encoder used to live here but was unreachable and has been removed.
+static bool mavlinkSendMissionItemResponse(uint16_t seq)
+{
+    navWaypoint_t wp;
+    getWaypoint(seq + 1, &wp);
+
+    mavlinkMissionItemData_t item;
+    if (!mavlinkFillMissionItemFromWaypoint(&wp, true, &item)) {
+        return false;
+    }
+
+    mavlink_msg_mission_item_int_pack(
+        mavSystemId,
+        mavComponentId,
+        &mavSendMsg,
+        mavlinkContext.recvMsg.sysid,
+        mavlinkContext.recvMsg.compid,
+        seq,
+        item.frame,
+        item.command,
+        FLIGHT_MODE(NAV_WP_MODE) && getActiveWpNumber() == seq + 1,
+        1,
+        item.param1, item.param2, item.param3, item.param4,
+        item.lat,
+        item.lon,
+        item.alt,
+        MAV_MISSION_TYPE_MISSION);
+
+    mavlinkSendMessage();
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionRequest(void)
+{
+    mavlink_mission_request_t msg;
+    mavlink_msg_mission_request_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
+        return true;
+    }
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_SENDING || !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE);
+        return true;
+    }
+    if (msg.seq < mavMissionTransfer.count) {
+        if (mavlinkSendMissionItemResponse(msg.seq)) {
+            if (msg.seq >= mavMissionTransfer.nextSequence) {
+                mavMissionTransfer.nextSequence = msg.seq + 1;
+            }
+            mavMissionTransfer.lastActivityMs = millis();
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
+            mavlinkResetMissionTransfer();
+        }
+    } else {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE);
+        mavlinkResetMissionTransfer();
+    }
+
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionItemInt(void)
+{
+    mavlink_mission_item_int_t msg;
+    mavlink_msg_mission_item_int_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) {
+            mavlinkAbortMissionUpload(MAV_MISSION_UNSUPPORTED);
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
+        }
+        return true;
+    }
+
+    if (ARMING_FLAG(ARMED)) {
+        if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+            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;
+    }
+
+    return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq,
+        msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z);
+}
+
+bool mavlinkHandleIncomingMissionRequestInt(void)
+{
+    mavlink_mission_request_int_t msg;
+    mavlink_msg_mission_request_int_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (msg.mission_type != MAV_MISSION_TYPE_MISSION) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED);
+        return true;
+    }
+    if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_SENDING || !mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE);
+        return true;
+    }
+    if (msg.seq < mavMissionTransfer.count) {
+        if (mavlinkSendMissionItemResponse(msg.seq)) {
+            if (msg.seq >= mavMissionTransfer.nextSequence) {
+                mavMissionTransfer.nextSequence = msg.seq + 1;
+            }
+            mavMissionTransfer.lastActivityMs = millis();
+        } else {
+            mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR);
+            mavlinkResetMissionTransfer();
+        }
+    } else {
+        mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE);
+        mavlinkResetMissionTransfer();
+    }
+
+    return true;
+}
+
+bool mavlinkHandleIncomingMissionAck(void)
+{
+    mavlink_mission_ack_t msg;
+    mavlink_msg_mission_ack_decode(&mavlinkContext.recvMsg, &msg);
+
+    if (!mavlinkMissionTargetIsLocal(msg.target_system, msg.target_component)) {
+        return false;
+    }
+    if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING &&
+        mavlinkMissionSenderOwnsTransfer() &&
+        msg.type == MAV_MISSION_OPERATION_CANCELLED) {
+        mavlinkResetMissionTransfer();
+    } else if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_SENDING && mavlinkMissionSenderOwnsTransfer()) {
+        mavlinkResetMissionTransfer();
+    }
+    return true;
+}
+
+#endif
diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h
new file mode 100644
index 00000000000..d01d9408b34
--- /dev/null
+++ b/src/main/mavlink/mavlink_mission.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "navigation/navigation.h"
+
+#include "mavlink/mavlink_types.h"
+
+typedef enum {
+    MAV_FRAME_SUPPORTED_NONE = 0,
+    MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0),
+    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT = (1 << 1),
+    MAV_FRAME_SUPPORTED_GLOBAL_INT = (1 << 2),
+    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT = (1 << 3),
+} mavFrameSupportMask_e;
+
+typedef struct mavlinkMissionItemData_s {
+    uint8_t frame;
+    uint16_t command;
+    float param1;
+    float param2;
+    float param3;
+    float param4;
+    int32_t lat;
+    int32_t lon;
+    float alt;
+} mavlinkMissionItemData_t;
+
+uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages);
+bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item);
+void mavlinkSendPendingMissionItemReached(void);
+void mavlinkMissionUpdate(timeMs_t currentTimeMs);
+bool mavlinkHandleIncomingMissionClearAll(void);
+bool mavlinkHandleIncomingMissionCount(void);
+bool mavlinkHandleIncomingMissionItem(void);
+bool mavlinkHandleIncomingMissionRequestList(void);
+bool mavlinkHandleIncomingMissionRequest(void);
+bool mavlinkHandleIncomingMissionItemInt(void);
+bool mavlinkHandleIncomingMissionRequestInt(void);
+bool mavlinkHandleIncomingMissionAck(void);
diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c
index 333f9d4fd54..94551728ef4 100644
--- a/src/main/mavlink/mavlink_runtime.c
+++ b/src/main/mavlink/mavlink_runtime.c
@@ -2,6 +2,7 @@
 
 #include "fc/fc_mavlink.h"
 
+#include "mavlink/mavlink_mission.h"
 #include "mavlink/mavlink_ports.h"
 #include "mavlink/mavlink_routing.h"
 #include "mavlink/mavlink_runtime.h"
@@ -279,6 +280,8 @@ static bool isMAVLinkTelemetryHalfDuplex(uint8_t portIndex)
 
 void mavlinkRuntimeHandle(timeUs_t currentTimeUs)
 {
+    mavlinkSendPendingMissionItemReached();
+    mavlinkMissionUpdate(currentTimeUs / 1000);
     mavlinkSendModeStatusText();
     mavlinkSendArmingStatusText();
 
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 111d660f674..ca0f74face7 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2078,6 +2078,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
 }
 
+static void navMarkWaypointReached(int8_t waypointIndex);
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -2106,12 +2108,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
         case NAV_WP_ACTION_HOLD_TIME:
             // Save the current time for the time the waypoint was reached
             posControl.wpReachedTime = millis();
+            navMarkWaypointReached(posControl.activeWaypointIndex);
             return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
     }
 
     UNREACHABLE();
 }
 
+static void navMarkWaypointReached(int8_t waypointIndex)
+{
+    if (waypointIndex < posControl.startWpIndex) {
+        return;
+    }
+
+    posControl.wpReachedSeq = (uint16_t)(waypointIndex - posControl.startWpIndex);
+    posControl.wpReachedNotificationPending = true;
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -2129,6 +2142,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
 
         if (posControl.wpAltitudeReached) {
             posControl.wpReachedTime = millis();
+            navMarkWaypointReached(posControl.activeWaypointIndex);
         } else {
             return NAV_FSM_EVENT_NONE;
         }
@@ -2156,7 +2170,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
     const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
 
     if (landEvent == NAV_FSM_EVENT_SUCCESS) {
-        // Landing controller returned success - invoke RTH finish states and finish the waypoint
+        // Landing controller returned success - invoke RTH finish states and finish the waypoint.
+        // Success maps straight to NAV_STATE_WAYPOINT_FINISHED, bypassing NAV_STATE_WAYPOINT_NEXT,
+        // so the LAND item must be marked reached here or it never reports completion.
+        navMarkWaypointReached(posControl.activeWaypointIndex);
         navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
         navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState);
     }
@@ -2168,6 +2185,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio
 {
     UNUSED(previousState);
 
+    if (!posControl.wpReachedNotificationPending) {
+        navMarkWaypointReached(posControl.activeWaypointIndex);
+    }
+
     if (isLastMissionWaypoint()) {      // Last waypoint reached
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
     }
@@ -2554,6 +2575,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(nav
 {
     UNUSED(previousState);
 
+    // A mission LAND item handed off to the autoland FSM finishes here, not
+    // through NAV_STATE_WAYPOINT_RTH_LAND's success branch - credit the item
+    // or the mission never reports completion. landState guards re-entry
+    // (this state self-loops on timeout).
+    if (posControl.fwLandState.landWp && posControl.fwLandState.landState != FW_AUTOLAND_STATE_IDLE) {
+        navMarkWaypointReached(posControl.activeWaypointIndex);
+    }
+
     posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
 
     return NAV_FSM_EVENT_NONE;
@@ -4021,6 +4050,7 @@ void resetWaypointList(void)
     posControl.waypointListValid = false;
     posControl.geoWaypointCount = 0;
     posControl.startWpIndex = 0;
+    posControl.wpReachedNotificationPending = false;
 #ifdef USE_MULTI_MISSION
     posControl.totalMultiMissionWpCount = 0;
     posControl.loadedMultiMissionIndex = 0;
@@ -5358,6 +5388,17 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) {
     return &posControl.pids;
 }
 
+bool navigationConsumeWaypointReached(uint16_t *seq)
+{
+    if (!posControl.wpReachedNotificationPending) {
+        return false;
+    }
+
+    *seq = posControl.wpReachedSeq;
+    posControl.wpReachedNotificationPending = false;
+    return true;
+}
+
 bool isAdjustingPosition(void) {
     return posControl.flags.isAdjustingPosition;
 }
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 398781fa596..65b66d778ce 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -802,6 +802,7 @@ bool isProbablyStillFlying(void);
 void resetLandingDetectorActiveState(void);
 
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
+bool navigationConsumeWaypointReached(uint16_t *seq);
 
 int32_t navigationGetHeadingError(void);
 float navigationGetCrossTrackError(void);
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index da4d27c6710..9f4d546f98c 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -498,6 +498,8 @@ typedef struct {
     float                       wpInitialDistance;          // Distance when starting flight to WP
     float                       wpDistance;                 // Distance to active WP
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached
+    uint16_t                    wpReachedSeq;               // Last reached mission item sequence relative to startWpIndex
+    bool                        wpReachedNotificationPending;
     bool                        wpAltitudeReached;          // WP altitude achieved
 
 #ifdef USE_FW_AUTOLAND
diff --git a/src/test/mavlink/missions/README_mavlink_mission_tester.md b/src/test/mavlink/missions/README_mavlink_mission_tester.md
new file mode 100644
index 00000000000..fddd45ee834
--- /dev/null
+++ b/src/test/mavlink/missions/README_mavlink_mission_tester.md
@@ -0,0 +1,63 @@
+# MAVLink mission tester
+
+This is a non-interactive mission upload/regression harness for MAVLink mission translation.
+
+It uses:
+
+- `pymavlink` for raw MAVLink mission upload through MAVProxy UDP `14550`
+- `mspapi2` for MSP mission readback through SITL UART1 TCP `5760`
+
+It does not start SITL or MAVProxy by default.
+
+For a self-contained run that starts SITL, starts MAVProxy, runs the mission test, prints log errors, and shuts everything down:
+
+```sh
+src/test/mavlink/missions/run_mavlink_mission_tester.sh
+```
+
+## Manual setup
+
+From `inav`:
+
+```sh
+./cmake/build_SITL/inav_9.1.0_SITL --serialport=/dev/ttyUSB0 --serialuart=3 --baudrate=460800 --path="../mydev/branch/mavlink_multiport2/eeprom.bin" --chanmap=M01-01,S02-02,S01-03,S04-04
+```
+
+From `inav/src/test/mavlink/missions/results`:
+
+```sh
+mavproxy.py --master=tcp:127.0.0.1:5763 --force-connected --nowait --daemon --out=udp:127.0.0.1:14550
+```
+
+Then from the INAV repo root:
+
+```sh
+conda run -n drone python src/test/mavlink/missions/mavlink_mission_tester.py
+```
+
+List cases without connecting:
+
+```sh
+conda run -n drone python src/test/mavlink/missions/mavlink_mission_tester.py --list-cases
+```
+
+Run one case:
+
+```sh
+conda run -n drone python src/test/mavlink/missions/mavlink_mission_tester.py --case qgc_planned_home_waypoints_land
+```
+
+The JSON report is written to:
+
+```text
+src/test/mavlink/missions/results/mission_tester_report.json
+```
+
+## Current cases
+
+- QGC planned home + waypoints + land
+- waypoint hold time plus `MAV_CMD_CONDITION_DELAY`
+- `MAV_CMD_DO_CHANGE_SPEED` applying to following legs
+- altitude modifier commands preserving INAV P3 bits
+- `MAV_CMD_DO_JUMP` remap after QGC planned-home skip
+- unsupported `MAV_CMD_NAV_TAKEOFF` rejection without MSP verification
diff --git a/src/test/mavlink/missions/mavlink_mission_tester.ini b/src/test/mavlink/missions/mavlink_mission_tester.ini
new file mode 100644
index 00000000000..987c2a2c732
--- /dev/null
+++ b/src/test/mavlink/missions/mavlink_mission_tester.ini
@@ -0,0 +1,18 @@
+[connection]
+mavlink_endpoint = udpin:127.0.0.1:14550
+msp_tcp_endpoint = 127.0.0.1:5760
+target_component = 1
+source_system = 252
+source_component = 191
+
+[paths]
+log_dir = src/test/mavlink/missions/results
+report_path = src/test/mavlink/missions/results/mission_tester_report.json
+
+[timeouts]
+heartbeat_timeout_s = 15.0
+mission_timeout_s = 20.0
+clear_timeout_s = 8.0
+msp_read_timeout_ms = 20.0
+msp_write_timeout_ms = 1200.0
+msp_verify_settle_s = 2.0
diff --git a/src/test/mavlink/missions/mavlink_mission_tester.py b/src/test/mavlink/missions/mavlink_mission_tester.py
new file mode 100644
index 00000000000..7d7cb6e6ab0
--- /dev/null
+++ b/src/test/mavlink/missions/mavlink_mission_tester.py
@@ -0,0 +1,853 @@
+#!/usr/bin/env python3
+"""
+Usage:
+  conda run -n drone python src/test/mavlink/missions/mavlink_mission_tester.py
+  conda run -n drone python src/test/mavlink/missions/mavlink_mission_tester.py --config src/test/mavlink/missions/mavlink_mission_tester.ini
+
+Expected external setup:
+  ./cmake/build_SITL/inav_9.1.0_SITL --serialport=/dev/ttyUSB0 --serialuart=3 --baudrate=460800 --path="../mydev/branch/mavlink_multiport2/eeprom.bin" --chanmap=M01-01,S02-02,S01-03,S04-04
+  cd src/test/mavlink/missions/results
+  mavproxy.py --master=tcp:127.0.0.1:5763 --force-connected --nowait --daemon --out=udp:127.0.0.1:14550
+"""
+
+from __future__ import annotations
+
+import argparse
+import configparser
+import json
+import os
+import sys
+import time
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Any, Iterable
+
+os.environ["MAVLINK20"] = "1"
+
+from pymavlink import mavutil
+
+try:
+    from mspapi2.msp_api import MSPApi
+    from mspapi2.lib import InavEnums
+except ModuleNotFoundError:
+    workspace_root_guess = Path(__file__).resolve().parents[5]
+    mspapi2_repo = workspace_root_guess / "mspapi2"
+    sys.path.insert(0, str(mspapi2_repo))
+    from mspapi2.msp_api import MSPApi
+    from mspapi2.lib import InavEnums
+
+
+INAV_ROOT = Path(__file__).resolve().parents[4]
+WORKSPACE_ROOT = INAV_ROOT.parent
+DEFAULT_CONFIG_PATH = Path(__file__).resolve().with_suffix(".ini")
+
+BASE_LAT_E7 = 365304400
+BASE_LON_E7 = -832163830
+LAT_STEP_E7 = 2500
+LON_STEP_E7 = 3500
+
+NAV_WP_ACTION_WAYPOINT = int(InavEnums.navWaypointActions_e.NAV_WP_ACTION_WAYPOINT)
+NAV_WP_ACTION_HOLD_TIME = int(InavEnums.navWaypointActions_e.NAV_WP_ACTION_HOLD_TIME)
+NAV_WP_ACTION_JUMP = int(InavEnums.navWaypointActions_e.NAV_WP_ACTION_JUMP)
+NAV_WP_ACTION_LAND = int(InavEnums.navWaypointActions_e.NAV_WP_ACTION_LAND)
+NAV_WP_FLAG_LAST = int(InavEnums.navWaypointFlags_e.NAV_WP_FLAG_LAST)
+NAV_WP_ALTMODE = int(InavEnums.navWaypointP3Flags_e.NAV_WP_ALTMODE)
+
+MAV_MISSION_ACCEPTED = int(mavutil.mavlink.MAV_MISSION_ACCEPTED)
+MAV_MISSION_UNSUPPORTED = int(mavutil.mavlink.MAV_MISSION_UNSUPPORTED)
+MAV_MISSION_TYPE_MISSION = int(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
+MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID)
+MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS)
+
+LIVE_STATUS_ERROR_WORDS = (
+    "error",
+    "fail",
+    "failed",
+    "invalid",
+    "unsupported",
+    "denied",
+    "mission",
+    "waypoint",
+)
+
+
+@dataclass(frozen=True)
+class RuntimeConfig:
+    mavlink_endpoint: str
+    msp_tcp_endpoint: str
+    target_component: int
+    source_system: int
+    source_component: int
+    log_dir: Path
+    report_path: Path
+    heartbeat_timeout_s: float
+    mission_timeout_s: float
+    clear_timeout_s: float
+    msp_read_timeout_ms: float
+    msp_write_timeout_ms: float
+    msp_verify_settle_s: float
+
+
+@dataclass(frozen=True)
+class MissionItem:
+    seq: int
+    frame: int
+    command: int
+    current: int
+    autocontinue: int
+    param1: float
+    param2: float
+    param3: float
+    param4: float
+    x: int
+    y: int
+    z: float
+
+
+@dataclass(frozen=True)
+class ExpectedWaypoint:
+    waypointIndex: int
+    action: int
+    latitudeE7: int
+    longitudeE7: int
+    altitudeCm: int
+    param1: int
+    param2: int
+    param3: int
+    flag: int
+
+
+@dataclass(frozen=True)
+class MissionCase:
+    name: str
+    description: str
+    items: tuple[MissionItem, ...]
+    expected_upload_result: int
+    expected_waypoints: tuple[ExpectedWaypoint, ...]
+    preload_items: tuple[MissionItem, ...] = ()
+    verify_legacy_download: bool = False
+
+
+def resolve_inav_path(path_text: str) -> Path:
+    path = Path(path_text).expanduser()
+    if path.is_absolute():
+        return path
+    return INAV_ROOT / path
+
+
+def load_config(config_path: Path) -> RuntimeConfig:
+    parser = configparser.ConfigParser()
+    parser.read(config_path)
+    return RuntimeConfig(
+        mavlink_endpoint=parser["connection"]["mavlink_endpoint"],
+        msp_tcp_endpoint=parser["connection"]["msp_tcp_endpoint"],
+        target_component=parser["connection"].getint("target_component"),
+        source_system=parser["connection"].getint("source_system"),
+        source_component=parser["connection"].getint("source_component"),
+        log_dir=resolve_inav_path(parser["paths"]["log_dir"]),
+        report_path=resolve_inav_path(parser["paths"]["report_path"]),
+        heartbeat_timeout_s=parser["timeouts"].getfloat("heartbeat_timeout_s"),
+        mission_timeout_s=parser["timeouts"].getfloat("mission_timeout_s"),
+        clear_timeout_s=parser["timeouts"].getfloat("clear_timeout_s"),
+        msp_read_timeout_ms=parser["timeouts"].getfloat("msp_read_timeout_ms"),
+        msp_write_timeout_ms=parser["timeouts"].getfloat("msp_write_timeout_ms"),
+        msp_verify_settle_s=parser["timeouts"].getfloat("msp_verify_settle_s"),
+    )
+
+
+def lat_e7(offset: int) -> int:
+    return BASE_LAT_E7 + offset * LAT_STEP_E7
+
+
+def lon_e7(offset: int) -> int:
+    return BASE_LON_E7 + offset * LON_STEP_E7
+
+
+def meters_to_centimeters(meters: float) -> int:
+    return int(round(meters * 100.0))
+
+
+def mission_item(
+    seq: int,
+    frame: int,
+    command: int,
+    *,
+    current: int = 0,
+    autocontinue: int = 1,
+    param1: float = 0.0,
+    param2: float = 0.0,
+    param3: float = 0.0,
+    param4: float = 0.0,
+    latitudeE7: int = 0,
+    longitudeE7: int = 0,
+    altitudeMeters: float = 0.0,
+) -> MissionItem:
+    return MissionItem(
+        seq=seq,
+        frame=frame,
+        command=command,
+        current=current,
+        autocontinue=autocontinue,
+        param1=param1,
+        param2=param2,
+        param3=param3,
+        param4=param4,
+        x=latitudeE7,
+        y=longitudeE7,
+        z=altitudeMeters,
+    )
+
+
+def expected_waypoint(
+    waypoint_index: int,
+    action: int,
+    latitudeE7: int,
+    longitudeE7: int,
+    altitudeMeters: float,
+    *,
+    param1: int = 0,
+    param2: int = 0,
+    param3: int = 0,
+    flag: int = 0,
+) -> ExpectedWaypoint:
+    return ExpectedWaypoint(
+        waypointIndex=waypoint_index,
+        action=action,
+        latitudeE7=latitudeE7,
+        longitudeE7=longitudeE7,
+        altitudeCm=meters_to_centimeters(altitudeMeters),
+        param1=param1,
+        param2=param2,
+        param3=param3,
+        flag=flag,
+    )
+
+
+def mission_result_name(result: int) -> str:
+    return mavutil.mavlink.enums["MAV_MISSION_RESULT"][int(result)].name
+
+
+def command_name(command: int) -> str:
+    return mavutil.mavlink.enums["MAV_CMD"][int(command)].name
+
+
+def frame_name(frame: int) -> str:
+    return mavutil.mavlink.enums["MAV_FRAME"][int(frame)].name
+
+
+def waypoint_action_name(action: int) -> str:
+    return InavEnums.navWaypointActions_e(int(action)).name
+
+
+def make_cases() -> tuple[MissionCase, ...]:
+    global_int = int(mavutil.mavlink.MAV_FRAME_GLOBAL_INT)
+    global_relative_alt_int = int(mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
+    mission_frame = int(mavutil.mavlink.MAV_FRAME_MISSION)
+
+    nav_waypoint = int(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT)
+    nav_loiter_time = int(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME)
+    nav_land = int(mavutil.mavlink.MAV_CMD_NAV_LAND)
+    nav_takeoff = int(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF)
+    do_jump = int(mavutil.mavlink.MAV_CMD_DO_JUMP)
+    do_change_speed = int(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED)
+    condition_delay = int(mavutil.mavlink.MAV_CMD_CONDITION_DELAY)
+    condition_change_alt = int(mavutil.mavlink.MAV_CMD_CONDITION_CHANGE_ALT)
+    do_change_altitude = int(mavutil.mavlink.MAV_CMD_DO_CHANGE_ALTITUDE)
+
+    return (
+        MissionCase(
+            name="qgc_planned_home_waypoints_land",
+            description="QGC planned home item is skipped; MAVLink seq 1 becomes INAV WP1.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, latitudeE7=lat_e7(0), longitudeE7=lon_e7(0), altitudeMeters=0.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(1), longitudeE7=lon_e7(1), altitudeMeters=50.0),
+                mission_item(2, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(2), longitudeE7=lon_e7(2), altitudeMeters=70.0),
+                mission_item(3, global_relative_alt_int, nav_land, latitudeE7=lat_e7(3), longitudeE7=lon_e7(3), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(1), lon_e7(1), 50.0),
+                expected_waypoint(2, NAV_WP_ACTION_WAYPOINT, lat_e7(2), lon_e7(2), 70.0),
+                expected_waypoint(3, NAV_WP_ACTION_LAND, lat_e7(3), lon_e7(3), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+        MissionCase(
+            name="waypoint_hold_time_and_condition_delay",
+            description="Waypoint hold time plus CONDITION_DELAY folds into one INAV HOLD_TIME item.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, latitudeE7=lat_e7(0), longitudeE7=lon_e7(0), altitudeMeters=0.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, param1=5.0, latitudeE7=lat_e7(4), longitudeE7=lon_e7(4), altitudeMeters=55.0),
+                mission_item(2, mission_frame, condition_delay, param1=7.0),
+                mission_item(3, global_relative_alt_int, nav_land, latitudeE7=lat_e7(5), longitudeE7=lon_e7(5), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_HOLD_TIME, lat_e7(4), lon_e7(4), 55.0, param1=12),
+                expected_waypoint(2, NAV_WP_ACTION_LAND, lat_e7(5), lon_e7(5), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+        MissionCase(
+            name="absolute_first_waypoint_is_not_planned_home",
+            description="A current first absolute waypoint is kept; only QGC-style non-current planned home is skipped.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, current=1, latitudeE7=lat_e7(16), longitudeE7=lon_e7(16), altitudeMeters=95.0),
+                mission_item(1, global_relative_alt_int, nav_land, latitudeE7=lat_e7(17), longitudeE7=lon_e7(17), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(16), lon_e7(16), 95.0, param3=NAV_WP_ALTMODE),
+                expected_waypoint(2, NAV_WP_ACTION_LAND, lat_e7(17), lon_e7(17), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+        MissionCase(
+            name="speed_change_applies_to_following_legs",
+            description="DO_CHANGE_SPEED is a pending modifier applied to later geographic legs.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, latitudeE7=lat_e7(0), longitudeE7=lon_e7(0), altitudeMeters=0.0),
+                mission_item(1, mission_frame, do_change_speed, param1=1.0, param2=12.5),
+                mission_item(2, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(6), longitudeE7=lon_e7(6), altitudeMeters=60.0),
+                mission_item(3, global_relative_alt_int, nav_loiter_time, param1=4.0, latitudeE7=lat_e7(7), longitudeE7=lon_e7(7), altitudeMeters=60.0),
+                mission_item(4, global_relative_alt_int, nav_land, latitudeE7=lat_e7(8), longitudeE7=lon_e7(8), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(6), lon_e7(6), 60.0, param1=1250),
+                expected_waypoint(2, NAV_WP_ACTION_HOLD_TIME, lat_e7(7), lon_e7(7), 60.0, param1=4, param2=1250),
+                expected_waypoint(3, NAV_WP_ACTION_LAND, lat_e7(8), lon_e7(8), 0.0, param1=1250, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+        MissionCase(
+            name="altitude_modifiers_preserve_p3_bits",
+            description="Altitude modifier items update the previous geographic waypoint and preserve P3 semantics.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, latitudeE7=lat_e7(0), longitudeE7=lon_e7(0), altitudeMeters=0.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(9), longitudeE7=lon_e7(9), altitudeMeters=40.0),
+                mission_item(2, mission_frame, do_change_altitude, param1=65.0, param2=float(global_relative_alt_int)),
+                mission_item(3, global_int, condition_change_alt, latitudeE7=0, longitudeE7=0, altitudeMeters=80.0),
+                mission_item(4, global_relative_alt_int, nav_land, latitudeE7=lat_e7(10), longitudeE7=lon_e7(10), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(9), lon_e7(9), 80.0, param3=NAV_WP_ALTMODE),
+                expected_waypoint(2, NAV_WP_ACTION_LAND, lat_e7(10), lon_e7(10), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+        MissionCase(
+            name="jump_target_remap_after_home_skip",
+            description="DO_JUMP target seq is remapped after QGC planned-home skip.",
+            expected_upload_result=MAV_MISSION_ACCEPTED,
+            items=(
+                mission_item(0, global_int, nav_waypoint, latitudeE7=lat_e7(0), longitudeE7=lon_e7(0), altitudeMeters=0.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(11), longitudeE7=lon_e7(11), altitudeMeters=45.0),
+                mission_item(2, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(12), longitudeE7=lon_e7(12), altitudeMeters=45.0),
+                mission_item(3, mission_frame, do_jump, param1=1.0, param2=2.0),
+                mission_item(4, global_relative_alt_int, nav_land, latitudeE7=lat_e7(13), longitudeE7=lon_e7(13), altitudeMeters=0.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(11), lon_e7(11), 45.0),
+                expected_waypoint(2, NAV_WP_ACTION_WAYPOINT, lat_e7(12), lon_e7(12), 45.0),
+                expected_waypoint(3, NAV_WP_ACTION_JUMP, 0, 0, 0.0, param1=1, param2=2),
+                expected_waypoint(4, NAV_WP_ACTION_LAND, lat_e7(13), lon_e7(13), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+            verify_legacy_download=True,
+        ),
+        MissionCase(
+            name="takeoff_is_rejected_without_partial_commit",
+            description="MAV_CMD_NAV_TAKEOFF remains unsupported and must not half-write a mission.",
+            expected_upload_result=MAV_MISSION_UNSUPPORTED,
+            items=(
+                mission_item(0, global_relative_alt_int, nav_takeoff, latitudeE7=lat_e7(14), longitudeE7=lon_e7(14), altitudeMeters=20.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(15), longitudeE7=lon_e7(15), altitudeMeters=50.0),
+            ),
+            expected_waypoints=(),
+        ),
+        MissionCase(
+            name="failed_upload_preserves_existing_mission",
+            description="A rejected upload after a valid mission leaves the stored INAV mission unchanged.",
+            expected_upload_result=MAV_MISSION_UNSUPPORTED,
+            preload_items=(
+                mission_item(0, global_relative_alt_int, nav_waypoint, current=1, latitudeE7=lat_e7(18), longitudeE7=lon_e7(18), altitudeMeters=55.0),
+                mission_item(1, global_relative_alt_int, nav_land, latitudeE7=lat_e7(19), longitudeE7=lon_e7(19), altitudeMeters=0.0),
+            ),
+            items=(
+                mission_item(0, global_relative_alt_int, nav_takeoff, latitudeE7=lat_e7(20), longitudeE7=lon_e7(20), altitudeMeters=20.0),
+                mission_item(1, global_relative_alt_int, nav_waypoint, latitudeE7=lat_e7(21), longitudeE7=lon_e7(21), altitudeMeters=50.0),
+            ),
+            expected_waypoints=(
+                expected_waypoint(1, NAV_WP_ACTION_WAYPOINT, lat_e7(18), lon_e7(18), 55.0),
+                expected_waypoint(2, NAV_WP_ACTION_LAND, lat_e7(19), lon_e7(19), 0.0, flag=NAV_WP_FLAG_LAST),
+            ),
+        ),
+    )
+
+
+class MissionTester:
+    def __init__(self, config: RuntimeConfig) -> None:
+        self.config = config
+        self.connection = mavutil.mavlink_connection(
+            config.mavlink_endpoint,
+            source_system=config.source_system,
+            source_component=config.source_component,
+            dialect="common",
+            autoreconnect=True,
+        )
+        self.connection.mav.srcSystem = config.source_system
+        self.connection.mav.srcComponent = config.source_component
+        self.target_system = 0
+        self.target_component = config.target_component
+        self.next_heartbeat_at = 0.0
+        self.case_statustext: list[dict[str, Any]] = []
+
+    def close(self) -> None:
+        self.connection.close()
+
+    def send_heartbeat(self) -> None:
+        self.connection.mav.heartbeat_send(
+            MAV_TYPE_GCS,
+            MAV_AUTOPILOT_INVALID,
+            0,
+            0,
+            mavutil.mavlink.MAV_STATE_ACTIVE,
+        )
+
+    def maybe_send_heartbeat(self) -> None:
+        now = time.monotonic()
+        if now >= self.next_heartbeat_at:
+            self.send_heartbeat()
+            self.next_heartbeat_at = now + 1.0
+
+    def learn_target_from_heartbeat(self, message: Any) -> None:
+        if int(message.autopilot) == MAV_AUTOPILOT_INVALID:
+            return
+        self.target_system = int(message.get_srcSystem())
+        if self.target_component == 0:
+            self.target_component = int(message.get_srcComponent())
+
+    def wait_for_target(self) -> None:
+        deadline = time.monotonic() + self.config.heartbeat_timeout_s
+        while time.monotonic() < deadline:
+            self.maybe_send_heartbeat()
+            message = self.connection.recv_match(blocking=True, timeout=0.1)
+            if message is None:
+                continue
+            if message.get_type() == "HEARTBEAT":
+                self.learn_target_from_heartbeat(message)
+                if self.target_system != 0:
+                    print(
+                        f"target_discovered system={self.target_system} component={self.target_component}",
+                        flush=True,
+                    )
+                    return
+        raise TimeoutError(f"mavlink_endpoint={self.config.mavlink_endpoint} heartbeat_timeout_s={self.config.heartbeat_timeout_s}")
+
+    def handle_background_message(self, message: Any) -> None:
+        message_type = message.get_type()
+        if message_type == "HEARTBEAT":
+            self.learn_target_from_heartbeat(message)
+        elif message_type == "STATUSTEXT":
+            text = bytes(message.text).decode("utf-8", errors="ignore").rstrip("\x00") if isinstance(message.text, list) else str(message.text).rstrip("\x00")
+            self.case_statustext.append(
+                {
+                    "severity": int(message.severity),
+                    "text": text,
+                }
+            )
+
+    def drain(self, duration_s: float) -> None:
+        deadline = time.monotonic() + duration_s
+        while time.monotonic() < deadline:
+            self.maybe_send_heartbeat()
+            message = self.connection.recv_match(blocking=False)
+            if message is None:
+                time.sleep(0.01)
+                continue
+            self.handle_background_message(message)
+
+    def clear_mission(self) -> int:
+        self.drain(0.1)
+        self.connection.mav.mission_clear_all_send(
+            self.target_system,
+            self.target_component,
+        )
+        deadline = time.monotonic() + self.config.clear_timeout_s
+        while time.monotonic() < deadline:
+            self.maybe_send_heartbeat()
+            message = self.connection.recv_match(blocking=True, timeout=0.1)
+            if message is None:
+                continue
+            self.handle_background_message(message)
+            if message.get_type() == "MISSION_ACK" and int(message.get_srcSystem()) == self.target_system:
+                return int(message.type)
+        raise TimeoutError(f"mission_clear_timeout_s={self.config.clear_timeout_s}")
+
+    def send_mission_item(self, item: MissionItem) -> None:
+        self.connection.mav.mission_item_int_send(
+            self.target_system,
+            self.target_component,
+            item.seq,
+            item.frame,
+            item.command,
+            item.current,
+            item.autocontinue,
+            item.param1,
+            item.param2,
+            item.param3,
+            item.param4,
+            item.x,
+            item.y,
+            item.z,
+        )
+
+    def upload_mission(self, mission_items: tuple[MissionItem, ...]) -> int:
+        self.connection.mav.mission_count_send(
+            self.target_system,
+            self.target_component,
+            len(mission_items),
+        )
+
+        items_by_seq = {item.seq: item for item in mission_items}
+        deadline = time.monotonic() + self.config.mission_timeout_s
+        while time.monotonic() < deadline:
+            self.maybe_send_heartbeat()
+            message = self.connection.recv_match(blocking=True, timeout=0.1)
+            if message is None:
+                continue
+            self.handle_background_message(message)
+            if int(message.get_srcSystem()) != self.target_system:
+                continue
+
+            message_type = message.get_type()
+            if message_type in ("MISSION_REQUEST", "MISSION_REQUEST_INT"):
+                seq = int(message.seq)
+                self.send_mission_item(items_by_seq[seq])
+            elif message_type == "MISSION_ACK":
+                return int(message.type)
+
+        raise TimeoutError(f"mission_upload_timeout_s={self.config.mission_timeout_s}")
+
+    def verify_legacy_download_uses_item_int(self, expected_count: int) -> dict[str, Any]:
+        self.drain(0.1)
+        self.connection.mav.mission_request_list_send(
+            self.target_system,
+            self.target_component,
+        )
+
+        mismatches = []
+        downloaded_items = []
+        deadline = time.monotonic() + self.config.mission_timeout_s
+        count = None
+        while time.monotonic() < deadline:
+            self.maybe_send_heartbeat()
+            message = self.connection.recv_match(blocking=True, timeout=0.1)
+            if message is None:
+                continue
+            self.handle_background_message(message)
+            if int(message.get_srcSystem()) != self.target_system:
+                continue
+            if message.get_type() == "MISSION_COUNT":
+                count = int(message.count)
+                break
+
+        if count is None:
+            raise TimeoutError(f"mission_download_count_timeout_s={self.config.mission_timeout_s}")
+        if count != expected_count:
+            mismatches.append(f"download_count: expected={expected_count} actual={count}")
+
+        for seq in range(count):
+            self.connection.mav.mission_request_send(
+                self.target_system,
+                self.target_component,
+                seq,
+            )
+            item_deadline = time.monotonic() + self.config.mission_timeout_s
+            item_message = None
+            while time.monotonic() < item_deadline:
+                self.maybe_send_heartbeat()
+                message = self.connection.recv_match(blocking=True, timeout=0.1)
+                if message is None:
+                    continue
+                self.handle_background_message(message)
+                if int(message.get_srcSystem()) != self.target_system:
+                    continue
+                if message.get_type() in ("MISSION_ITEM", "MISSION_ITEM_INT", "MISSION_ACK"):
+                    item_message = message
+                    break
+
+            if item_message is None:
+                raise TimeoutError(f"mission_download_item_timeout_s={self.config.mission_timeout_s} seq={seq}")
+
+            message_type = item_message.get_type()
+            downloaded_items.append(
+                {
+                    "seq": int(getattr(item_message, "seq", -1)),
+                    "message_type": message_type,
+                }
+            )
+            if message_type != "MISSION_ITEM_INT":
+                mismatches.append(f"download_seq={seq}: expected MISSION_ITEM_INT actual={message_type}")
+                continue
+            if int(item_message.seq) != seq:
+                mismatches.append(f"download_seq: expected={seq} actual={int(item_message.seq)}")
+
+        self.connection.mav.mission_ack_send(
+            self.target_system,
+            self.target_component,
+            MAV_MISSION_ACCEPTED,
+        )
+
+        return {
+            "items": downloaded_items,
+            "mismatches": mismatches,
+        }
+
+    def upload_case(self, case: MissionCase) -> dict[str, Any]:
+        self.case_statustext = []
+        clear_result = self.clear_mission()
+        preload_result = None
+        if case.preload_items:
+            preload_result = self.upload_mission(case.preload_items)
+        upload_result = self.upload_mission(case.items)
+        return {
+            "clear_result": clear_result,
+            "clear_result_name": mission_result_name(clear_result),
+            "preload_result": preload_result,
+            "preload_result_name": mission_result_name(preload_result) if preload_result is not None else None,
+            "upload_result": upload_result,
+            "upload_result_name": mission_result_name(upload_result),
+            "statustext": self.case_statustext,
+        }
+
+
+def actual_waypoint_from_msp(waypoint: dict[str, Any]) -> ExpectedWaypoint:
+    return ExpectedWaypoint(
+        waypointIndex=int(waypoint["waypointIndex"]),
+        action=int(waypoint["action"]),
+        latitudeE7=int(round(float(waypoint["latitude"]) * 1e7)),
+        longitudeE7=int(round(float(waypoint["longitude"]) * 1e7)),
+        altitudeCm=int(round(float(waypoint["altitude"]) * 100.0)),
+        param1=int(waypoint["param1"]),
+        param2=int(waypoint["param2"]),
+        param3=int(waypoint["param3"]),
+        flag=int(waypoint["flag"]),
+    )
+
+
+def compare_waypoints(expected: ExpectedWaypoint, actual: ExpectedWaypoint) -> list[str]:
+    mismatches = []
+    for field_name in expected.__dataclass_fields__:
+        expected_value = getattr(expected, field_name)
+        actual_value = getattr(actual, field_name)
+        if actual_value != expected_value:
+            mismatches.append(f"{field_name}: expected={expected_value} actual={actual_value}")
+    return mismatches
+
+
+def verify_msp_mission(config: RuntimeConfig, case: MissionCase) -> dict[str, Any]:
+    with MSPApi(
+        tcp_endpoint=config.msp_tcp_endpoint,
+        read_timeout_ms=config.msp_read_timeout_ms,
+        write_timeout_ms=config.msp_write_timeout_ms,
+    ) as api:
+        waypoint_info = api.get_waypoint_info()
+        actual_count = int(waypoint_info["waypointCount"])
+        actual_waypoints = [
+            actual_waypoint_from_msp(api.get_waypoint(waypoint_index))
+            for waypoint_index in range(1, actual_count + 1)
+        ]
+
+    expected_count = len(case.expected_waypoints)
+    mismatches = []
+    if actual_count != expected_count:
+        mismatches.append(f"waypointCount: expected={expected_count} actual={actual_count}")
+
+    for expected, actual in zip(case.expected_waypoints, actual_waypoints):
+        waypoint_mismatches = compare_waypoints(expected, actual)
+        if waypoint_mismatches:
+            mismatches.append(f"WP{expected.waypointIndex}: " + "; ".join(waypoint_mismatches))
+
+    return {
+        "waypoint_info": waypoint_info,
+        "expected_waypoints": [asdict(waypoint) for waypoint in case.expected_waypoints],
+        "actual_waypoints": [asdict(waypoint) for waypoint in actual_waypoints],
+        "mismatches": mismatches,
+    }
+
+
+def interesting_statustext(messages: Iterable[dict[str, Any]]) -> list[dict[str, Any]]:
+    interesting = []
+    for message in messages:
+        lower_text = str(message["text"]).lower()
+        if any(word in lower_text for word in LIVE_STATUS_ERROR_WORDS):
+            interesting.append(message)
+    return interesting
+
+
+def find_latest_mavproxy_log(log_dir: Path) -> Path | None:
+    candidates = list(log_dir.glob("*.tlog")) + list(log_dir.glob("**/*.tlog"))
+    if not candidates:
+        return None
+    return max(candidates, key=lambda path: path.stat().st_mtime)
+
+
+def scan_mavproxy_log(log_dir: Path) -> dict[str, Any]:
+    log_path = find_latest_mavproxy_log(log_dir)
+    if log_path is None:
+        return {
+            "path": None,
+            "interesting_statustext": [],
+        }
+
+    connection = mavutil.mavlink_connection(str(log_path), dialect="common")
+    messages = []
+    while True:
+        message = connection.recv_match(type="STATUSTEXT", blocking=False)
+        if message is None:
+            break
+        text = bytes(message.text).decode("utf-8", errors="ignore").rstrip("\x00") if isinstance(message.text, list) else str(message.text).rstrip("\x00")
+        messages.append(
+            {
+                "severity": int(message.severity),
+                "text": text,
+            }
+        )
+    connection.close()
+
+    return {
+        "path": str(log_path.relative_to(INAV_ROOT)),
+        "interesting_statustext": interesting_statustext(messages),
+    }
+
+
+def mission_item_report(item: MissionItem) -> dict[str, Any]:
+    payload = asdict(item)
+    payload["frame_name"] = frame_name(item.frame)
+    payload["command_name"] = command_name(item.command)
+    return payload
+
+
+def expected_waypoint_report(waypoint: ExpectedWaypoint) -> dict[str, Any]:
+    payload = asdict(waypoint)
+    payload["action_name"] = waypoint_action_name(waypoint.action)
+    return payload
+
+
+def run_cases(config: RuntimeConfig, cases: tuple[MissionCase, ...]) -> dict[str, Any]:
+    tester = MissionTester(config)
+    results = []
+    try:
+        tester.wait_for_target()
+        for case in cases:
+            print(f"case_start name={case.name} items={len(case.items)}", flush=True)
+            upload = tester.upload_case(case)
+            preload_matches = upload["preload_result"] is None or int(upload["preload_result"]) == MAV_MISSION_ACCEPTED
+            upload_matches = preload_matches and int(upload["upload_result"]) == case.expected_upload_result
+            msp_report = None
+            legacy_download_report = None
+            if upload_matches:
+                time.sleep(config.msp_verify_settle_s)
+                msp_report = verify_msp_mission(config, case)
+                passed = not msp_report["mismatches"]
+                if case.verify_legacy_download:
+                    legacy_download_report = tester.verify_legacy_download_uses_item_int(len(case.expected_waypoints))
+                    passed = passed and not legacy_download_report["mismatches"]
+            else:
+                passed = False
+
+            case_report = {
+                "name": case.name,
+                "description": case.description,
+                "passed": passed,
+                "expected_upload_result": case.expected_upload_result,
+                "expected_upload_result_name": mission_result_name(case.expected_upload_result),
+                "upload": upload,
+                "mission_items": [mission_item_report(item) for item in case.items],
+                "preload_items": [mission_item_report(item) for item in case.preload_items],
+                "expected_waypoints": [expected_waypoint_report(waypoint) for waypoint in case.expected_waypoints],
+                "msp": msp_report,
+                "legacy_download": legacy_download_report,
+                "interesting_statustext": interesting_statustext(upload["statustext"]),
+            }
+            results.append(case_report)
+            print(
+                f"case_done name={case.name} passed={int(passed)} "
+                f"upload_result={upload['upload_result_name']}",
+                flush=True,
+            )
+    finally:
+        tester.close()
+
+    return {
+        "config": {
+            "mavlink_endpoint": config.mavlink_endpoint,
+            "msp_tcp_endpoint": config.msp_tcp_endpoint,
+            "log_dir": str(config.log_dir.relative_to(INAV_ROOT)),
+            "report_path": str(config.report_path.relative_to(INAV_ROOT)),
+        },
+        "cases": results,
+        "mavproxy_log": scan_mavproxy_log(config.log_dir),
+        "passed": all(case["passed"] for case in results),
+    }
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Upload MAVLink mission cases and verify INAV's stored MSP mission.")
+    parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="INI config path")
+    parser.add_argument("--case", action="append", dest="case_names", help="Run one case name; may be supplied more than once")
+    parser.add_argument("--list-cases", action="store_true", help="Print available mission cases without connecting")
+    args = parser.parse_args()
+
+    cases = make_cases()
+    if args.list_cases:
+        for case in cases:
+            print(f"{case.name}: {case.description}")
+        return
+
+    if args.case_names:
+        selected_names = set(args.case_names)
+        cases = tuple(case for case in cases if case.name in selected_names)
+        missing_names = selected_names - {case.name for case in cases}
+        if missing_names:
+            raise ValueError(f"unknown_case_names={sorted(missing_names)}")
+
+    config = load_config(resolve_inav_path(args.config))
+    report = run_cases(config, cases)
+    config.report_path.parent.mkdir(parents=True, exist_ok=True)
+    config.report_path.write_text(json.dumps(report, indent=2, sort_keys=True) + "\n", encoding="utf-8")
+    print(f"report_path={config.report_path.relative_to(INAV_ROOT)} passed={int(report['passed'])}", flush=True)
+    print("mission_test_summary_start", flush=True)
+    for case_report in report["cases"]:
+        print(
+            f"case={case_report['name']} passed={int(case_report['passed'])} "
+            f"upload={case_report['upload']['upload_result_name']} "
+            f"expected_upload={case_report['expected_upload_result_name']}",
+            flush=True,
+        )
+        msp_report = case_report["msp"]
+        if msp_report is not None:
+            print(
+                f"case={case_report['name']} msp_mismatches={len(msp_report['mismatches'])} "
+                f"waypoint_info={msp_report['waypoint_info']}",
+                flush=True,
+            )
+            for mismatch in msp_report["mismatches"]:
+                print(f"case={case_report['name']} mismatch={mismatch}", flush=True)
+        for message in case_report["interesting_statustext"]:
+            print(
+                f"case={case_report['name']} statustext_severity={message['severity']} "
+                f"statustext={message['text']}",
+                flush=True,
+            )
+        legacy_download_report = case_report["legacy_download"]
+        if legacy_download_report is not None:
+            print(
+                f"case={case_report['name']} legacy_download_mismatches={len(legacy_download_report['mismatches'])}",
+                flush=True,
+            )
+            for mismatch in legacy_download_report["mismatches"]:
+                print(f"case={case_report['name']} legacy_download_mismatch={mismatch}", flush=True)
+    print("mission_test_summary_end", flush=True)
+    raise SystemExit(0 if report["passed"] else 1)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/test/mavlink/missions/results/.gitignore b/src/test/mavlink/missions/results/.gitignore
new file mode 100644
index 00000000000..a5baada18fd
--- /dev/null
+++ b/src/test/mavlink/missions/results/.gitignore
@@ -0,0 +1,3 @@
+*
+!.gitignore
+
diff --git a/src/test/mavlink/missions/run_mavlink_mission_tester.sh b/src/test/mavlink/missions/run_mavlink_mission_tester.sh
new file mode 100755
index 00000000000..2dbb9eb405f
--- /dev/null
+++ b/src/test/mavlink/missions/run_mavlink_mission_tester.sh
@@ -0,0 +1,204 @@
+#!/usr/bin/env bash
+set -Eeuo pipefail
+
+SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
+INAV_DIR="$(cd -- "${SCRIPT_DIR}/../../../.." && pwd)"
+WORKSPACE_ROOT="$(cd -- "${INAV_DIR}/.." && pwd)"
+BRANCH_DIR="${WORKSPACE_ROOT}/mydev/branch/mavlink_multiport2"
+LOG_DIR="${SCRIPT_DIR}/results"
+
+SITL_BINARY="${INAV_DIR}/cmake/build_SITL/inav_9.1.0_SITL"
+EEPROM_PATH="../mydev/branch/mavlink_multiport2/eeprom.bin"
+CONFIG_PATH="${SCRIPT_DIR}/mavlink_mission_tester.ini"
+REPORT_PATH="${LOG_DIR}/mission_tester_report.json"
+SITL_LOG="${LOG_DIR}/mission_tester_sitl.log"
+MAVPROXY_LOG="${LOG_DIR}/mission_tester_mavproxy.log"
+TESTER_LOG="${LOG_DIR}/mission_tester_stdout.log"
+
+SITL_PID=""
+MAVPROXY_PID=""
+
+require_file() {
+    local path="$1"
+    local label="$2"
+    if [[ ! -f "${path}" ]]; then
+        echo "missing_${label}=${path}" >&2
+        exit 1
+    fi
+}
+
+wait_tcp() {
+    local host="$1"
+    local port="$2"
+    local label="$3"
+    local timeout_s="$4"
+    local start_s
+    start_s="$(date +%s)"
+
+    while (( "$(date +%s)" - start_s < timeout_s )); do
+        if (exec 3<>"/dev/tcp/${host}/${port}") 2>/dev/null; then
+            exec 3<&-
+            exec 3>&-
+            echo "tcp_ready label=${label} endpoint=${host}:${port}"
+            return 0
+        fi
+        sleep 0.2
+    done
+
+    echo "tcp_timeout label=${label} endpoint=${host}:${port} timeout_s=${timeout_s}" >&2
+    return 1
+}
+
+stop_process_group() {
+    local pid="$1"
+    local label="$2"
+
+    if [[ -z "${pid}" ]]; then
+        return 0
+    fi
+
+    if ! kill -0 "${pid}" 2>/dev/null; then
+        return 0
+    fi
+
+    echo "stopping_process label=${label} pid=${pid}"
+    kill -TERM -- "-${pid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true
+
+    for _ in {1..30}; do
+        if ! kill -0 "${pid}" 2>/dev/null; then
+            wait "${pid}" 2>/dev/null || true
+            return 0
+        fi
+        sleep 0.1
+    done
+
+    echo "killing_process label=${label} pid=${pid}"
+    kill -KILL -- "-${pid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+    wait "${pid}" 2>/dev/null || true
+}
+
+tail_log() {
+    local path="$1"
+    local label="$2"
+    if [[ -f "${path}" ]]; then
+        echo "${label}_tail_start"
+        tail -80 "${path}" || true
+        echo "${label}_tail_end"
+    fi
+}
+
+scan_log_errors() {
+    local path="$1"
+    local label="$2"
+    if [[ -f "${path}" ]]; then
+        echo "${label}_error_scan_start"
+        grep -Ein "error|fail|failed|invalid|unsupported|denied|timeout|traceback" "${path}" || true
+        echo "${label}_error_scan_end"
+    fi
+}
+
+start_mavproxy_master() {
+    local master_endpoint="$1"
+    local port_label="${master_endpoint##*:}"
+    local attempt_log="${LOG_DIR}/mission_tester_mavproxy_${port_label}.log"
+
+    : > "${attempt_log}"
+    (
+        cd "${LOG_DIR}"
+        exec setsid conda run --no-capture-output -n drone mavproxy.py \
+            --master="${master_endpoint}" \
+            --force-connected \
+            --nowait \
+            --daemon \
+            --out=udp:127.0.0.1:14550
+    ) >"${attempt_log}" 2>&1 &
+    MAVPROXY_PID=$!
+    MAVPROXY_LOG="${attempt_log}"
+    echo "mavproxy_started pid=${MAVPROXY_PID} master=${master_endpoint} log=src/test/mavlink/missions/results/mission_tester_mavproxy_${port_label}.log"
+
+    sleep 5
+    if kill -0 "${MAVPROXY_PID}" 2>/dev/null; then
+        echo "mavproxy_ready pid=${MAVPROXY_PID} master=${master_endpoint}"
+        return 0
+    fi
+
+    echo "mavproxy_exited_early master=${master_endpoint}" >&2
+    tail_log "${attempt_log}" "mavproxy_${port_label}"
+    MAVPROXY_PID=""
+    return 1
+}
+
+cleanup() {
+    local status=$?
+    set +e
+    stop_process_group "${MAVPROXY_PID}" "mavproxy"
+    stop_process_group "${SITL_PID}" "sitl"
+    if [[ "${status}" -ne 0 ]]; then
+        tail_log "${TESTER_LOG}" "tester"
+        tail_log "${MAVPROXY_LOG}" "mavproxy"
+        tail_log "${SITL_LOG}" "sitl"
+    fi
+    exit "${status}"
+}
+
+trap cleanup EXIT INT TERM
+
+require_file "${SITL_BINARY}" "sitl_binary"
+require_file "${CONFIG_PATH}" "config"
+require_file "${BRANCH_DIR}/eeprom.bin" "eeprom"
+
+mkdir -p "${LOG_DIR}"
+rm -f "${REPORT_PATH}"
+: > "${SITL_LOG}"
+: > "${TESTER_LOG}"
+
+echo "mission_tester_start"
+echo "inav_dir=."
+echo "sitl_binary=cmake/build_SITL/inav_9.1.0_SITL"
+echo "results_dir=src/test/mavlink/missions/results"
+
+(
+    cd "${INAV_DIR}"
+    exec setsid "${SITL_BINARY}" \
+        --serialport=/dev/ttyUSB0 \
+        --serialuart=3 \
+        --baudrate=460800 \
+        --path="${EEPROM_PATH}" \
+        --chanmap=M01-01,S02-02,S01-03,S04-04
+) >"${SITL_LOG}" 2>&1 &
+SITL_PID=$!
+echo "sitl_started pid=${SITL_PID} log=src/test/mavlink/missions/results/mission_tester_sitl.log"
+
+wait_tcp 127.0.0.1 5760 "sitl_msp_uart1" 20
+wait_tcp 127.0.0.1 5763 "sitl_mavlink_uart4" 20
+
+start_mavproxy_master tcp:127.0.0.1:5763
+
+set +e
+(
+    cd "${INAV_DIR}"
+    conda run --no-capture-output -n drone python src/test/mavlink/missions/mavlink_mission_tester.py \
+        --config src/test/mavlink/missions/mavlink_mission_tester.ini
+) 2>&1 | tee "${TESTER_LOG}"
+TEST_STATUS=${PIPESTATUS[0]}
+set -e
+
+echo "mission_tester_process_status=${TEST_STATUS}"
+scan_log_errors "${TESTER_LOG}" "tester"
+scan_log_errors "${MAVPROXY_LOG}" "mavproxy"
+scan_log_errors "${SITL_LOG}" "sitl"
+
+if [[ -f "${REPORT_PATH}" ]]; then
+    echo "mission_tester_report=src/test/mavlink/missions/results/mission_tester_report.json"
+fi
+
+if [[ "${TEST_STATUS}" -ne 0 ]]; then
+    if [[ -f "${REPORT_PATH}" ]]; then
+        echo "mission_tester_report_json_start"
+        cat "${REPORT_PATH}"
+        echo "mission_tester_report_json_end"
+    fi
+    exit "${TEST_STATUS}"
+fi
+
+echo "mission_tester_passed"
diff --git a/src/test/mavlink/routing/mavlink_routing_compliance_test.py b/src/test/mavlink/routing/mavlink_routing_compliance_test.py
new file mode 100644
index 00000000000..0ea996ec1b5
--- /dev/null
+++ b/src/test/mavlink/routing/mavlink_routing_compliance_test.py
@@ -0,0 +1,499 @@
+#!/usr/bin/env python3
+"""
+Usage:
+  conda run -n drone python src/utils/mavlink_tests/mavlink_routing_compliance_test.py --config src/utils/mavlink_tests/routing_test_config.yaml
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import time
+from pathlib import Path
+from typing import Any, Dict, List
+
+import yaml
+from pymavlink import mavutil
+
+
+MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID)
+MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = int(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
+MAV_STATE_ACTIVE = int(mavutil.mavlink.MAV_STATE_ACTIVE)
+MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS)
+
+ROUTING_RULE_REFERENCES = {
+    "broadcast_forward": "routing.md:25-27,48-49",
+    "known_target_forward": "routing.md:49-50; MAVLink_routing.cpp:66-80",
+    "unknown_target_blocked": "routing.md:26-27,53",
+    "no_ingress_loop": "MAVLink_routing.cpp:197-209",
+    "no_repack": "routing.md:29-31",
+}
+
+
+def load_config(config_path: Path) -> Dict[str, Any]:
+    return yaml.safe_load(config_path.read_text(encoding="utf-8"))
+
+
+def open_mavlink_connection(endpoint: str, source_system: int, source_component: int, timeout_s: float) -> Any:
+    deadline = time.monotonic() + timeout_s
+    while True:
+        try:
+            return mavutil.mavlink_connection(
+                endpoint,
+                source_system=source_system,
+                source_component=source_component,
+                autoreconnect=True,
+            )
+        except OSError as error:
+            if time.monotonic() >= deadline:
+                raise TimeoutError(
+                    f"endpoint={endpoint} source_system={source_system} source_component={source_component} timeout_s={timeout_s}"
+                ) from error
+            time.sleep(0.2)
+
+
+def set_source_identity(connection: Any, system_id: int, component_id: int) -> None:
+    connection.mav.srcSystem = system_id
+    connection.mav.srcComponent = component_id
+
+
+def send_heartbeat(connection: Any, system_id: int, component_id: int, mav_type: int) -> None:
+    set_source_identity(connection, system_id, component_id)
+    connection.mav.heartbeat_send(
+        mav_type,
+        MAV_AUTOPILOT_INVALID,
+        MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+        0,
+        MAV_STATE_ACTIVE,
+    )
+
+
+def drain_link(connection: Any, duration_s: float) -> None:
+    deadline = time.monotonic() + duration_s
+    while time.monotonic() < deadline:
+        message = connection.recv_match(blocking=False)
+        if message is None:
+            time.sleep(0.002)
+
+
+def drain_all_links(connections: Dict[str, Any], duration_s: float) -> None:
+    for connection in connections.values():
+        drain_link(connection, duration_s)
+
+
+def collect_command_observations(connections: Dict[str, Any], command_id: int, observation_window_s: float) -> Dict[str, List[Dict[str, Any]]]:
+    observations: Dict[str, List[Dict[str, Any]]] = {link_name: [] for link_name in connections}
+    deadline = time.monotonic() + observation_window_s
+    while time.monotonic() < deadline:
+        saw_message = False
+        for link_name, connection in connections.items():
+            while True:
+                message = connection.recv_match(type=["COMMAND_LONG"], blocking=False)
+                if message is None:
+                    break
+                if int(message.command) != command_id:
+                    continue
+                observations[link_name].append(
+                    {
+                        "src_system": int(message.get_srcSystem()),
+                        "src_component": int(message.get_srcComponent()),
+                        "target_system": int(message.target_system),
+                        "target_component": int(message.target_component),
+                        "command": int(message.command),
+                        "confirmation": int(message.confirmation),
+                        "params": [
+                            float(message.param1),
+                            float(message.param2),
+                            float(message.param3),
+                            float(message.param4),
+                            float(message.param5),
+                            float(message.param6),
+                            float(message.param7),
+                        ],
+                    }
+                )
+                saw_message = True
+        if not saw_message:
+            time.sleep(0.002)
+    return observations
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Run MAVLink routing compliance checks against INAV multiport routing.")
+    parser.add_argument("--config", required=True, type=Path, help="YAML config path")
+    args = parser.parse_args()
+
+    config = load_config(args.config)
+    timing = config["timing"]
+    network = config["network"]
+    tests = config["tests"]
+    components_cfg = config["components"]
+    vehicle = config["vehicle"]
+
+    gcs_link_name = network["gcs_link"]
+    fc_system_id = int(vehicle["fc_system_id"])
+    unknown_component_id = int(tests["unknown_component_id"])
+    unknown_system_id = int(tests["unknown_system_id"])
+    marker_command_base = int(tests["marker_command_base"])
+    report_path = Path(tests["report_markdown"])
+
+    link_cfgs = network["links"]
+    if gcs_link_name not in link_cfgs:
+        raise ValueError(f"gcs_link={gcs_link_name} missing from network.links")
+
+    components: List[Dict[str, Any]] = []
+    for component in components_cfg:
+        if component["link"] not in link_cfgs:
+            raise ValueError(f"component={component['name']} link={component['link']} missing from network.links")
+        components.append(
+            {
+                "name": component["name"],
+                "link": component["link"],
+                "system_id": int(component["system_id"]),
+                "component_id": int(component["component_id"]),
+                "mav_type": int(component["mav_type"]),
+            }
+        )
+
+    component_ids = {component["component_id"] for component in components}
+    component_system_ids = {component["system_id"] for component in components}
+    if unknown_component_id in component_ids:
+        raise ValueError(f"unknown_component_id={unknown_component_id} collides with configured component IDs")
+    if unknown_system_id in component_system_ids or unknown_system_id == fc_system_id:
+        raise ValueError(f"unknown_system_id={unknown_system_id} collides with configured systems")
+
+    gcs_actor = {
+        "name": "gcs",
+        "link": gcs_link_name,
+        "system_id": int(link_cfgs[gcs_link_name]["source_system"]),
+        "component_id": int(link_cfgs[gcs_link_name]["source_component"]),
+        "mav_type": MAV_TYPE_GCS,
+    }
+
+    inter_source = components[0]
+    inter_target = None
+    for component in components:
+        if component["link"] != inter_source["link"]:
+            inter_target = component
+            break
+    if inter_target is None:
+        raise ValueError("Need at least one component on a different link for inter-component routing test")
+
+    connections: Dict[str, Any] = {}
+    fc_heartbeats: Dict[str, Dict[str, int]] = {}
+
+    try:
+        for link_name, link_cfg in link_cfgs.items():
+            connection = open_mavlink_connection(
+                endpoint=link_cfg["endpoint"],
+                source_system=int(link_cfg["source_system"]),
+                source_component=int(link_cfg["source_component"]),
+                timeout_s=float(timing["connect_timeout_s"]),
+            )
+            connections[link_name] = connection
+
+        for link_name, connection in connections.items():
+            heartbeat = connection.wait_heartbeat(timeout=float(timing["connect_timeout_s"]))
+            if heartbeat is None:
+                raise TimeoutError(f"link={link_name} wait_heartbeat timed out")
+            fc_heartbeats[link_name] = {
+                "system_id": int(heartbeat.get_srcSystem()),
+                "component_id": int(heartbeat.get_srcComponent()),
+            }
+            print(
+                f"fc_heartbeat link={link_name} system_id={fc_heartbeats[link_name]['system_id']} "
+                f"component_id={fc_heartbeats[link_name]['component_id']}",
+                flush=True,
+            )
+
+        time.sleep(float(timing["settle_after_connect_s"]))
+        drain_all_links(connections, float(timing["drain_window_s"]))
+
+        send_heartbeat(
+            connections[gcs_actor["link"]],
+            gcs_actor["system_id"],
+            gcs_actor["component_id"],
+            gcs_actor["mav_type"],
+        )
+        for component in components:
+            send_heartbeat(
+                connections[component["link"]],
+                component["system_id"],
+                component["component_id"],
+                component["mav_type"],
+            )
+            print(
+                f"route_learn_heartbeat component={component['name']} link={component['link']} "
+                f"system_id={component['system_id']} component_id={component['component_id']} mav_type={component['mav_type']}",
+                flush=True,
+            )
+        time.sleep(float(timing["route_learn_settle_s"]))
+        drain_all_links(connections, float(timing["drain_window_s"]))
+
+        cases: List[Dict[str, Any]] = []
+        all_other_than_gcs = sorted([link_name for link_name in connections if link_name != gcs_link_name])
+
+        cases.append(
+            {
+                "name": "gcs_broadcast",
+                "source": gcs_actor,
+                "target_system": 0,
+                "target_component": 0,
+                "expected_links": all_other_than_gcs,
+                "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+            }
+        )
+
+        cases.append(
+            {
+                "name": "gcs_target_local_system_component_broadcast",
+                "source": gcs_actor,
+                "target_system": fc_system_id,
+                "target_component": 0,
+                "expected_links": all_other_than_gcs,
+                "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+            }
+        )
+
+        for component in components:
+            expected_links = [component["link"]]
+            if component["link"] == gcs_link_name:
+                expected_links = []
+            cases.append(
+                {
+                    "name": f"gcs_target_{component['name']}",
+                    "source": gcs_actor,
+                    "target_system": component["system_id"],
+                    "target_component": component["component_id"],
+                    "expected_links": sorted(expected_links),
+                    "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+                }
+            )
+
+        cases.append(
+            {
+                "name": "gcs_target_unknown_component",
+                "source": gcs_actor,
+                "target_system": fc_system_id,
+                "target_component": unknown_component_id,
+                "expected_links": [],
+                "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]],
+            }
+        )
+
+        cases.append(
+            {
+                "name": "gcs_target_unknown_system",
+                "source": gcs_actor,
+                "target_system": unknown_system_id,
+                "target_component": 1,
+                "expected_links": [],
+                "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]],
+            }
+        )
+
+        component_broadcast_expected = sorted([link_name for link_name in connections if link_name != inter_source["link"]])
+        cases.append(
+            {
+                "name": f"{inter_source['name']}_broadcast",
+                "source": inter_source,
+                "target_system": 0,
+                "target_component": 0,
+                "expected_links": component_broadcast_expected,
+                "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+            }
+        )
+
+        cases.append(
+            {
+                "name": f"{inter_source['name']}_to_{inter_target['name']}",
+                "source": inter_source,
+                "target_system": inter_target["system_id"],
+                "target_component": inter_target["component_id"],
+                "expected_links": [inter_target["link"]],
+                "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+            }
+        )
+
+        cases.append(
+            {
+                "name": f"{inter_source['name']}_to_gcs",
+                "source": inter_source,
+                "target_system": gcs_actor["system_id"],
+                "target_component": gcs_actor["component_id"],
+                "expected_links": [gcs_actor["link"]],
+                "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]],
+            }
+        )
+
+        if marker_command_base + len(cases) > 65535:
+            raise ValueError(f"marker_command_base={marker_command_base} too high for case_count={len(cases)}")
+
+        results: List[Dict[str, Any]] = []
+        for case_index, case in enumerate(cases, start=1):
+            drain_all_links(connections, float(timing["drain_window_s"]))
+            command_id = marker_command_base + case_index
+            confirmation = case_index % 256
+            params = [
+                float(1000 + case_index),
+                float(2000 + case_index),
+                float(3000 + case_index),
+                float(4000 + case_index),
+                float(5000 + case_index),
+                float(6000 + case_index),
+                float(7000 + case_index),
+            ]
+
+            source = case["source"]
+            source_connection = connections[source["link"]]
+            set_source_identity(source_connection, source["system_id"], source["component_id"])
+            source_connection.mav.command_long_send(
+                int(case["target_system"]),
+                int(case["target_component"]),
+                command_id,
+                confirmation,
+                params[0],
+                params[1],
+                params[2],
+                params[3],
+                params[4],
+                params[5],
+                params[6],
+            )
+
+            observations = collect_command_observations(
+                connections=connections,
+                command_id=command_id,
+                observation_window_s=float(timing["observation_window_s"]),
+            )
+            observed_links = sorted([link_name for link_name, msgs in observations.items() if len(msgs) > 0])
+            expected_links = sorted(case["expected_links"])
+            no_ingress_loop = source["link"] not in observed_links
+            routing_match = observed_links == expected_links
+
+            payload_intact = True
+            payload_issues: List[str] = []
+            for link_name, messages in observations.items():
+                for message in messages:
+                    if message["src_system"] != int(source["system_id"]) or message["src_component"] != int(source["component_id"]):
+                        payload_intact = False
+                        payload_issues.append(
+                            f"link={link_name} src_mismatch expected=({source['system_id']},{source['component_id']}) "
+                            f"observed=({message['src_system']},{message['src_component']})"
+                        )
+                    if message["target_system"] != int(case["target_system"]) or message["target_component"] != int(case["target_component"]):
+                        payload_intact = False
+                        payload_issues.append(
+                            f"link={link_name} target_mismatch expected=({case['target_system']},{case['target_component']}) "
+                            f"observed=({message['target_system']},{message['target_component']})"
+                        )
+                    if message["command"] != command_id or message["confirmation"] != confirmation:
+                        payload_intact = False
+                        payload_issues.append(
+                            f"link={link_name} cmd_mismatch expected=(command={command_id},confirmation={confirmation}) "
+                            f"observed=(command={message['command']},confirmation={message['confirmation']})"
+                        )
+                    for index, observed_value in enumerate(message["params"]):
+                        expected_value = params[index]
+                        if not math.isclose(observed_value, expected_value, rel_tol=0.0, abs_tol=1e-3):
+                            payload_intact = False
+                            payload_issues.append(
+                                f"link={link_name} param{index + 1}_mismatch expected={expected_value} observed={observed_value}"
+                            )
+
+            case_pass = routing_match and no_ingress_loop and payload_intact
+            results.append(
+                {
+                    "name": case["name"],
+                    "source": source["name"],
+                    "source_link": source["link"],
+                    "target_system": int(case["target_system"]),
+                    "target_component": int(case["target_component"]),
+                    "command_id": command_id,
+                    "expected_links": expected_links,
+                    "observed_links": observed_links,
+                    "routing_match": routing_match,
+                    "no_ingress_loop": no_ingress_loop,
+                    "payload_intact": payload_intact,
+                    "pass": case_pass,
+                    "rules": case["rules"],
+                    "payload_issues": payload_issues,
+                }
+            )
+
+            print(
+                f"case={case['name']} command_id={command_id} source={source['name']} source_link={source['link']} "
+                f"target=({case['target_system']},{case['target_component']}) expected_links={expected_links} observed_links={observed_links} "
+                f"routing_match={routing_match} no_ingress_loop={no_ingress_loop} payload_intact={payload_intact} pass={case_pass}",
+                flush=True,
+            )
+            time.sleep(float(timing["inter_case_pause_s"]))
+
+    finally:
+        for connection in connections.values():
+            connection.close()
+
+    pass_count = sum(1 for result in results if result["pass"])
+    fail_count = len(results) - pass_count
+
+    markdown_lines: List[str] = []
+    markdown_lines.append("# MAVLink Routing Compliance Test")
+    markdown_lines.append("")
+    markdown_lines.append(
+        f"- fc_system_id: `{fc_system_id}`"
+    )
+    markdown_lines.append(
+        f"- links: `{sorted(list(link_cfgs.keys()))}`"
+    )
+    markdown_lines.append(
+        f"- gcs_link: `{gcs_link_name}`"
+    )
+    markdown_lines.append(
+        f"- components: `{[(component['name'], component['system_id'], component['component_id'], component['link']) for component in components]}`"
+    )
+    markdown_lines.append("")
+    markdown_lines.append("## Rule References")
+    markdown_lines.append("")
+    markdown_lines.append(f"- broadcast_forward: `{ROUTING_RULE_REFERENCES['broadcast_forward']}`")
+    markdown_lines.append(f"- known_target_forward: `{ROUTING_RULE_REFERENCES['known_target_forward']}`")
+    markdown_lines.append(f"- unknown_target_blocked: `{ROUTING_RULE_REFERENCES['unknown_target_blocked']}`")
+    markdown_lines.append(f"- no_ingress_loop: `{ROUTING_RULE_REFERENCES['no_ingress_loop']}`")
+    markdown_lines.append(f"- no_repack: `{ROUTING_RULE_REFERENCES['no_repack']}`")
+    markdown_lines.append("")
+    markdown_lines.append("## Results")
+    markdown_lines.append("")
+    markdown_lines.append("| Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass |")
+    markdown_lines.append("| --- | --- | --- | --- | --- | --- | --- | --- | --- |")
+    for result in results:
+        markdown_lines.append(
+            f"| `{result['name']}` | `{result['source']} ({result['source_link']})` | "
+            f"`({result['target_system']},{result['target_component']})` | "
+            f"`{result['expected_links']}` | `{result['observed_links']}` | "
+            f"`{result['routing_match']}` | `{result['no_ingress_loop']}` | `{result['payload_intact']}` | `{result['pass']}` |"
+        )
+    markdown_lines.append("")
+    markdown_lines.append(f"summary pass_count={pass_count} fail_count={fail_count} total={len(results)}")
+    markdown_lines.append("")
+
+    failures = [result for result in results if not result["pass"]]
+    if failures:
+        markdown_lines.append("## Failure Details")
+        markdown_lines.append("")
+        for failure in failures:
+            markdown_lines.append(f"### {failure['name']}")
+            markdown_lines.append("")
+            markdown_lines.append(
+                f"- expected_links: `{failure['expected_links']}` observed_links: `{failure['observed_links']}` "
+                f"routing_match: `{failure['routing_match']}` no_ingress_loop: `{failure['no_ingress_loop']}` "
+                f"payload_intact: `{failure['payload_intact']}`"
+            )
+            if failure["payload_issues"]:
+                markdown_lines.append(f"- payload_issues: `{failure['payload_issues']}`")
+            markdown_lines.append(f"- rule_refs: `{failure['rules']}`")
+            markdown_lines.append("")
+
+    report_path.write_text("\n".join(markdown_lines) + "\n", encoding="utf-8")
+    print(f"report_path={report_path} pass_count={pass_count} fail_count={fail_count} total={len(results)}", flush=True)
+
+    if fail_count > 0:
+        raise SystemExit(1)
diff --git a/src/test/mavlink/routing/routing_test_config.yaml b/src/test/mavlink/routing/routing_test_config.yaml
new file mode 100644
index 00000000000..d4677ba4ebf
--- /dev/null
+++ b/src/test/mavlink/routing/routing_test_config.yaml
@@ -0,0 +1,58 @@
+timing:
+  connect_timeout_s: 10.0
+  settle_after_connect_s: 0.5
+  route_learn_settle_s: 0.5
+  drain_window_s: 0.15
+  observation_window_s: 0.8
+  inter_case_pause_s: 0.15
+
+network:
+  gcs_link: link_radio
+  links:
+    link_radio:
+      endpoint: tcp:127.0.0.1:5761
+      source_system: 1
+      source_component: 68
+    link_companion:
+      endpoint: tcp:127.0.0.1:5762
+      source_system: 1
+      source_component: 191
+    link_camera:
+      endpoint: tcp:127.0.0.1:5763
+      source_system: 1
+      source_component: 100
+    link_gimbal:
+      endpoint: tcp:127.0.0.1:5764
+      source_system: 1
+      source_component: 154
+
+vehicle:
+  fc_system_id: 1
+
+tests:
+  marker_command_base: 31000
+  unknown_component_id: 199
+  unknown_system_id: 42
+  report_markdown: src/utils/mavlink_tests/ROUTING_TEST.md
+
+components:
+  - name: mav2_companion_computer
+    link: link_companion
+    system_id: 1
+    component_id: 191
+    mav_type: 18
+  - name: mav1_elrs_receiver
+    link: link_radio
+    system_id: 1
+    component_id: 68
+    mav_type: 49
+  - name: mav3_camera
+    link: link_camera
+    system_id: 1
+    component_id: 100
+    mav_type: 30
+  - name: mav4_gimbal
+    link: link_gimbal
+    system_id: 1
+    component_id: 154
+    mav_type: 26
diff --git a/src/test/mavlink/sanity/fc_diff.txt b/src/test/mavlink/sanity/fc_diff.txt
new file mode 100644
index 00000000000..f2edb8a7f1f
--- /dev/null
+++ b/src/test/mavlink/sanity/fc_diff.txt
@@ -0,0 +1,231 @@
+# Building AutoComplete Cache ... Done!
+# 
+# diff all 
+
+# version
+# INAV/SITL 9.1.0 Jul 10 2026 / 12:06:40 (abfc1f39) 
+# GCC-13.4.0
+
+# start the command batch
+batch start
+
+# reset configuration to default settings
+defaults noreboot
+
+# resources
+
+# Timer overrides
+
+# Outputs [servo]
+
+# safehome
+
+# Fixed Wing Approach
+
+# geozone
+
+# geozone vertices
+
+# features
+feature TELEMETRY
+feature BLACKBOX
+feature PWM_OUTPUT_ENABLE
+
+# blackbox
+blackbox -NAV_ACC
+blackbox NAV_POS
+blackbox NAV_PID
+blackbox MAG
+blackbox ACC
+blackbox ATTI
+blackbox RC_DATA
+blackbox RC_COMMAND
+blackbox MOTORS
+blackbox -GYRO_RAW
+blackbox -PEAKS_R
+blackbox -PEAKS_P
+blackbox -PEAKS_Y
+blackbox SERVOS
+
+# Receiver: Channel map
+
+# Ports
+serial 1 1 115200 115200 0 2470000
+serial 2 256 115200 115200 460800 115200
+serial 3 256 115200 115200 115200 115200
+serial 6 33554432 115200 115200 0 115200
+serial 7 2 115200 115200 0 115200
+
+# Modes [aux]
+aux 0 0 0 1300 2100
+aux 1 1 1 1300 1700
+aux 2 53 3 1325 1725
+aux 3 11 1 1700 2100
+aux 4 10 2 1700 2100
+aux 5 28 3 1700 2100
+aux 6 31 2 1300 1700
+aux 7 3 6 1700 2100
+aux 8 27 4 1300 2100
+
+# Adjustments [adjrange]
+
+# Receiver rxrange
+
+# temp_sensor
+
+# Mission Control Waypoints [wp]
+#wp 7 valid
+wp 0 1 456392480 -743752489 5000 0 0 0 0
+wp 1 1 456445849 -743652167 5000 0 0 0 0
+wp 2 1 456443268 -743791597 5000 2100 0 0 0
+wp 3 1 456412128 -743773235 5000 1500 0 0 0
+wp 4 3 456400630 -743818396 15000 30 1500 0 0
+wp 5 1 456392518 -743756609 15000 1500 0 0 0
+wp 6 4 0 0 0 0 0 0 165
+
+# OSD [osd_layout]
+osd_layout 0 0 0 12 V
+osd_layout 0 1 2 1 V
+osd_layout 0 2 0 0 V
+osd_layout 0 3 8 6 V
+osd_layout 0 9 2 5 V
+osd_layout 0 13 23 7 V
+osd_layout 0 15 22 6 V
+osd_layout 0 23 24 1 V
+osd_layout 0 26 23 8 V
+osd_layout 0 32 2 2 V
+osd_layout 0 110 0 9 V
+osd_layout 0 129 10 1 V
+osd_layout 0 144 11 14 V
+osd_layout 0 145 22 2 V
+osd_layout 0 159 0 10 V
+
+# Programming: logic
+
+# Programming: global variables
+
+# Programming: PID controllers
+
+# OSD: custom elements
+
+# master
+set gyro_main_lpf_hz = 25
+set ins_gravity_cmss =  980.443
+set acc_hardware = FAKE
+set align_mag = CW270FLIP
+set mag_hardware = FAKE
+set baro_hardware = FAKE
+set pitot_hardware = VIRTUAL
+set receiver_type = SERIAL
+set serialrx_provider = MAVLINK
+set blackbox_rate_denom = 2
+set motor_pwm_protocol = STANDARD
+set small_angle = 180
+set applied_defaults = 3
+set airmode_type = STICK_CENTER_ONCE
+set nav_wp_radius = 800
+set nav_wp_max_safe_distance = 500
+set nav_rth_altitude = 5000
+set nav_fw_control_smoothness = 2
+set nav_fw_launch_motor_delay = 100
+set nav_fw_launch_max_altitude = 5000
+set nav_fw_launch_climb_angle = 25
+set mavlink_port1_ext_status_rate = 0
+set mavlink_autopilot_type = ARDUPILOT
+set mavlink_port1_rc_chan_rate = 0
+set mavlink_port1_pos_rate = 0
+set mavlink_port1_extra1_rate = 0
+set mavlink_port1_extra2_rate = 0
+set mavlink_port1_extra3_rate = 0
+set mavlink_port1_min_txbuffer = 0
+set mavlink_port2_min_txbuffer = 0
+set mavlink_port3_min_txbuffer = 0
+set mavlink_port4_min_txbuffer = 0
+set osd_video_system = PAL
+
+# control_profile
+control_profile 1
+
+set fw_p_pitch = 15
+set fw_i_pitch = 5
+set fw_d_pitch = 5
+set fw_ff_pitch = 80
+set fw_p_roll = 15
+set fw_i_roll = 3
+set fw_d_roll = 7
+set fw_p_yaw = 50
+set fw_i_yaw = 0
+set fw_d_yaw = 20
+set fw_ff_yaw = 255
+set dterm_lpf_hz = 10
+set fw_turn_assist_pitch_gain =  0.400
+set nav_fw_pos_z_p = 22
+set nav_fw_pos_z_i = 6
+set nav_fw_pos_z_d = 2
+set nav_fw_pos_xy_p = 55
+set d_boost_min =  1.000
+set d_boost_max =  1.000
+set tpa_rate = 80
+set rc_expo = 30
+set rc_yaw_expo = 30
+set roll_rate = 18
+set pitch_rate = 9
+set yaw_rate = 3
+
+# control_profile
+control_profile 2
+
+
+# control_profile
+control_profile 3
+
+
+# mixer_profile
+mixer_profile 1
+
+set platform_type = AIRPLANE
+set has_flaps = ON
+set model_preview_type = 14
+
+# Mixer: motor mixer
+
+mmix reset
+
+mmix 0  1.000  0.000  0.000  0.000
+
+# Mixer: servo mixer
+smix reset
+
+smix 0 1 1 100 0 -1
+smix 1 2 0 100 0 -1
+smix 2 3 0 100 0 -1
+smix 3 4 2 100 0 -1
+
+# mixer_profile
+mixer_profile 2
+
+
+# Mixer: motor mixer
+
+# Mixer: servo mixer
+
+# battery_profile
+battery_profile 1
+
+set throttle_idle =  5.000
+
+# battery_profile
+battery_profile 2
+
+
+# battery_profile
+battery_profile 3
+
+
+# restore original profile selection
+control_profile 1
+mixer_profile 1
+battery_profile 1
+
+# save configuration
+save
\ No newline at end of file
diff --git a/src/test/mavlink/sanity/mavlink_sitl_sanity.py b/src/test/mavlink/sanity/mavlink_sitl_sanity.py
new file mode 100644
index 00000000000..12216fb7bb6
--- /dev/null
+++ b/src/test/mavlink/sanity/mavlink_sitl_sanity.py
@@ -0,0 +1,868 @@
+#!/usr/bin/env python3
+"""
+Usage:
+  conda run -n drone python tools/mavlink_sitl_sanity.py
+  conda run -n drone python tools/mavlink_sitl_sanity.py --config tools/mavlink_sitl_sanity.yaml
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+from pathlib import Path
+import queue
+import re
+import shutil
+import signal
+import socket
+import subprocess
+import sys
+import threading
+import time
+
+from pymavlink import mavutil
+import yaml
+
+try:
+    from mspapi2.msp_api import MSPApi
+except ModuleNotFoundError:
+    SCRIPT_DIR = Path(__file__).resolve().parent
+    WORKSPACE_ROOT = SCRIPT_DIR.parents[3]
+    MSPAPI2_REPO = WORKSPACE_ROOT / "mspapi2"
+    if MSPAPI2_REPO.exists():
+        sys.path.insert(0, str(MSPAPI2_REPO))
+    from mspapi2.msp_api import MSPApi
+
+
+SCRIPT_DIR = Path(__file__).resolve().parent
+WORKSPACE_ROOT = SCRIPT_DIR.parents[3]
+DEFAULT_CONFIG_PATH = SCRIPT_DIR / "mavlink_sitl_sanity.yaml"
+AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID)
+MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS)
+
+
+class ProcessOutput:
+    def __init__(self, process: subprocess.Popen[str], log_path: Path):
+        self.process = process
+        self.log_path = log_path
+        self.lines: list[str] = []
+        self.events: queue.Queue[str] = queue.Queue()
+        self.log_file = log_path.open("w", encoding="utf-8")
+        self.thread = threading.Thread(target=self._reader, daemon=True)
+        self.thread.start()
+
+    def _reader(self) -> None:
+        try:
+            for line in self.process.stdout:
+                self.lines.append(line)
+                self.log_file.write(line)
+                self.log_file.flush()
+                self.events.put(line)
+        finally:
+            self.log_file.flush()
+
+    def mark(self) -> int:
+        return len(self.lines)
+
+    def wait_for(self, pattern: str, timeout_s: float, start_index: int = 0) -> str:
+        compiled = re.compile(pattern)
+        deadline = time.monotonic() + timeout_s
+        while True:
+            for line in self.lines[start_index:]:
+                if compiled.search(line):
+                    return line.strip()
+            remaining = deadline - time.monotonic()
+            if remaining <= 0:
+                break
+            try:
+                line = self.events.get(timeout=min(remaining, 0.5))
+                if compiled.search(line):
+                    return line.strip()
+            except queue.Empty:
+                continue
+        recent = "".join(self.lines[max(0, len(self.lines) - 20):]).strip()
+        raise TimeoutError(f'Pattern "{pattern}" not found in {self.log_path}: {recent}')
+
+    def close(self) -> None:
+        self.thread.join(timeout=1.0)
+        self.log_file.close()
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Run a small SITL sanity check across MSP, MAVLink RC, and MAVLink GCS links.")
+    parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML config path")
+    return parser.parse_args()
+
+
+def load_config(config_path: Path) -> dict:
+    return yaml.safe_load(config_path.read_text(encoding="utf-8"))
+
+
+def relpath(path: Path) -> str:
+    return path.resolve().relative_to(WORKSPACE_ROOT.resolve()).as_posix()
+
+
+def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None:
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        try:
+            with socket.create_connection((host, port), timeout=1.0):
+                return
+        except (ConnectionRefusedError, socket.timeout, OSError):
+            time.sleep(0.2)
+    raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s:.1f}s")
+
+
+def wait_for_tcp_port_cycle(host: str, port: int, timeout_s: float) -> None:
+    deadline = time.monotonic() + timeout_s
+    saw_closed = False
+    while time.monotonic() < deadline:
+        try:
+            with socket.create_connection((host, port), timeout=1.0):
+                if saw_closed:
+                    return
+        except (ConnectionRefusedError, socket.timeout, OSError):
+            saw_closed = True
+        time.sleep(0.2)
+    if saw_closed:
+        raise TimeoutError(f"TCP port {host}:{port} did not return after reboot within {timeout_s:.1f}s")
+    wait_for_tcp_port(host, port, timeout_s)
+
+
+def wait_for_configured_ports(config: dict) -> None:
+    tests = config["tests"]
+    ports = config["ports"]
+    time.sleep(1.0)
+    wait_for_tcp_port("127.0.0.1", int(ports["msp"]), float(tests["save_reboot_timeout_s"]))
+    wait_for_tcp_port("127.0.0.1", int(ports["rc"]), float(tests["save_reboot_timeout_s"]))
+    wait_for_tcp_port("127.0.0.1", int(ports["gcs"]), float(tests["save_reboot_timeout_s"]))
+
+
+def cli_read_until_prompt(cli_socket: socket.socket) -> str:
+    data = b""
+    while b"\n# " not in data:
+        chunk = cli_socket.recv(65536)
+        if not chunk:
+            raise ConnectionError("CLI socket closed before prompt")
+        data += chunk
+    return data.decode("utf-8", errors="replace")
+
+
+def run_cli_commands(host: str, port: int, commands: list[str]) -> bool:
+    sent_save = False
+    with socket.create_connection((host, port), timeout=5.0) as cli_socket:
+        cli_socket.settimeout(3.0)
+        cli_socket.sendall(b"#\n")
+        cli_read_until_prompt(cli_socket)
+        for command in commands:
+            cli_socket.sendall(command.encode("utf-8") + b"\n")
+            if command == "save":
+                sent_save = True
+                break
+            cli_read_until_prompt(cli_socket)
+    return sent_save
+
+
+def build_serial_command(index: int, function_mask: int, baud: int) -> str:
+    return f"serial {index} {function_mask} {baud} {baud} 0 {baud}"
+
+
+def load_cli_batch_commands(path: Path) -> list[str]:
+    commands: list[str] = []
+    for raw_line in path.read_text(encoding="utf-8").splitlines():
+        line = raw_line.strip()
+        if not line or line.startswith("#"):
+            continue
+        if line in ("batch start", "batch end"):
+            continue
+        commands.append(line)
+    return commands
+
+
+def build_cli_config_commands(config: dict) -> list[str]:
+    cli_cfg = config["cli"]
+    if "diff_path" in cli_cfg:
+        return load_cli_batch_commands(WORKSPACE_ROOT / cli_cfg["diff_path"])
+    rc_baud = int(cli_cfg["rc_baud"])
+    telemetry_baud = int(cli_cfg["telemetry_baud"])
+    commands = [
+        "feature TELEMETRY",
+        "set receiver_type = SERIAL",
+        "set serialrx_provider = MAVLINK",
+        "set mavlink_version = 2",
+        "set mavlink_port1_high_latency = OFF",
+        "set mavlink_port2_high_latency = OFF",
+        "set mavlink_port3_high_latency = OFF",
+        "set mavlink_port4_high_latency = OFF",
+        build_serial_command(0, 1, 115200),
+        build_serial_command(1, 320, rc_baud),
+        build_serial_command(2, 256, telemetry_baud),
+        build_serial_command(3, 0, telemetry_baud),
+        build_serial_command(4, 0, telemetry_baud),
+    ]
+    commands.extend(cli_cfg["mode_ranges"])
+    commands.append("save")
+    return commands
+
+
+def start_process(command: list[str], cwd: Path, log_path: Path) -> tuple[subprocess.Popen[str], ProcessOutput]:
+    log_path.parent.mkdir(parents=True, exist_ok=True)
+    env = os.environ.copy()
+    env.pop("LD_LIBRARY_PATH", None)
+    env["PYTHONUNBUFFERED"] = "1"
+    process = subprocess.Popen(
+        command,
+        cwd=str(cwd),
+        env=env,
+        stdin=subprocess.PIPE,
+        stdout=subprocess.PIPE,
+        stderr=subprocess.STDOUT,
+        text=True,
+        bufsize=1,
+        start_new_session=True,
+    )
+    if process.stdout is None:
+        raise RuntimeError("stdout pipe was not created")
+    return process, ProcessOutput(process, log_path)
+
+
+def stop_process(process: subprocess.Popen[str] | None) -> None:
+    if process is None or process.poll() is not None:
+        return
+    os.killpg(process.pid, signal.SIGINT)
+    try:
+        process.wait(timeout=10.0)
+    except subprocess.TimeoutExpired:
+        os.killpg(process.pid, signal.SIGTERM)
+        try:
+            process.wait(timeout=5.0)
+        except subprocess.TimeoutExpired:
+            os.killpg(process.pid, signal.SIGKILL)
+            process.wait(timeout=5.0)
+
+
+def open_mavlink_tcp_connection(port: int, source_system: int, source_component: int, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    while True:
+        try:
+            return mavutil.mavlink_connection(
+                f"tcp:127.0.0.1:{port}",
+                source_system=source_system,
+                source_component=source_component,
+                autoreconnect=True,
+            )
+        except OSError:
+            if time.monotonic() >= deadline:
+                raise
+            time.sleep(0.2)
+
+
+def open_mavlink_udp_listener(port: int, source_system: int, source_component: int):
+    return mavutil.mavlink_connection(
+        f"udpin:127.0.0.1:{port}",
+        source_system=source_system,
+        source_component=source_component,
+        autoreconnect=True,
+    )
+
+
+def is_fc_heartbeat(message) -> bool:
+    return int(message.autopilot) != AUTOPILOT_INVALID and int(message.type) != MAV_TYPE_GCS
+
+
+def wait_for_fc_heartbeat(master, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        message = master.recv_match(type="HEARTBEAT", blocking=True, timeout=min(1.0, deadline - time.monotonic()))
+        if message is None:
+            continue
+        if is_fc_heartbeat(message):
+            return message
+    raise TimeoutError(f"No FC heartbeat received within {timeout_s:.1f}s")
+
+
+def wait_for_message(master, message_type: str, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        message = master.recv_match(blocking=True, timeout=min(1.0, deadline - time.monotonic()))
+        if message is None:
+            continue
+        if message.get_type() == message_type:
+            return message
+    raise TimeoutError(f"No {message_type} received within {timeout_s:.1f}s")
+
+
+def wait_for_command_ack(master, command: int, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        message = master.recv_match(blocking=True, timeout=min(1.0, deadline - time.monotonic()))
+        if message is None:
+            continue
+        if message.get_type() == "COMMAND_ACK" and int(message.command) == command:
+            return message
+    raise TimeoutError(f"No COMMAND_ACK for {command} within {timeout_s:.1f}s")
+
+
+def wait_for_fc_mode(master, custom_mode: int, armed: bool | None, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    observed_modes: list[str] = []
+    while time.monotonic() < deadline:
+        heartbeat = master.recv_match(type="HEARTBEAT", blocking=True, timeout=min(1.0, deadline - time.monotonic()))
+        if heartbeat is None or not is_fc_heartbeat(heartbeat):
+            continue
+        is_armed = (int(heartbeat.base_mode) & int(mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)) != 0
+        observed_modes.append(f"{int(heartbeat.custom_mode)}:{int(is_armed)}")
+        if len(observed_modes) > 8:
+            observed_modes.pop(0)
+        if int(heartbeat.custom_mode) != custom_mode:
+            continue
+        if armed is None:
+            return heartbeat
+        if is_armed == armed:
+            return heartbeat
+    raise TimeoutError(
+        f"No FC heartbeat for custom_mode={custom_mode} armed={armed} within {timeout_s:.1f}s "
+        f"observed={observed_modes}"
+    )
+
+
+def wait_for_fc_armed_state(master, armed: bool, timeout_s: float):
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        heartbeat = master.recv_match(type="HEARTBEAT", blocking=True, timeout=min(1.0, deadline - time.monotonic()))
+        if heartbeat is None or not is_fc_heartbeat(heartbeat):
+            continue
+        is_armed = (int(heartbeat.base_mode) & int(mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)) != 0
+        if is_armed == armed:
+            return heartbeat
+    raise TimeoutError(f"No FC heartbeat for armed={armed} within {timeout_s:.1f}s")
+
+
+def measure_message_rate(master, message_type: str, window_s: float) -> float:
+    deadline = time.monotonic() + window_s
+    count = 0
+    while time.monotonic() < deadline:
+        message = master.recv_match(blocking=True, timeout=min(0.5, deadline - time.monotonic()))
+        if message is None:
+            continue
+        if message.get_type() == message_type:
+            count += 1
+    return count / window_s
+
+
+def drain_messages(master, drain_s: float) -> None:
+    deadline = time.monotonic() + drain_s
+    while time.monotonic() < deadline:
+        timeout_s = deadline - time.monotonic()
+        if timeout_s <= 0:
+            return
+        message = master.recv_match(blocking=True, timeout=min(0.1, timeout_s))
+        if message is None:
+            continue
+
+
+def mavproxy_send(process: subprocess.Popen[str], command: str) -> None:
+    if process.stdin is None:
+        raise RuntimeError("MAVProxy stdin pipe was not created")
+    process.stdin.write(command + "\n")
+    process.stdin.flush()
+
+
+def readback_waypoint_count(path: Path) -> int:
+    lines = [line for line in path.read_text(encoding="utf-8").splitlines() if line.strip()]
+    if not lines or lines[0] != "QGC WPL 110":
+        raise ValueError(f"Unexpected waypoint file header in {path}")
+    return len(lines) - 1
+
+
+def msp_check(config: dict) -> str:
+    tests = config["tests"]
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        status = api.get_inav_status()
+        attitude = api.get_attitude()
+        rx_config = api.get_rx_config()
+    receiver_type = rx_config["receiverType"].name
+    serial_provider = rx_config["serialRxProvider"].name
+    return (
+        f"cycleTime={status['cycleTime']} cpuLoad={status['cpuLoad']} "
+        f"roll={attitude['roll']:.1f} pitch={attitude['pitch']:.1f} "
+        f"receiverType={receiver_type} serialRxProvider={serial_provider}"
+    )
+
+
+def msp_land_command_check(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        status = api.get_inav_status()
+        try:
+            api.set_land()
+            result = "ack=ok"
+        except Exception as exc:
+            detail = str(exc)
+            if "unsupported (! response)" not in detail:
+                raise
+            result = f"expected_unarmed_error={detail}"
+    active_modes = [mode.name for mode in status.get("activeModes", [])]
+    return f"{result} activeModes={active_modes}"
+
+
+def msp_rth_command_check(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        status = api.get_inav_status()
+        try:
+            api.set_rth()
+            result = "ack=ok"
+        except Exception as exc:
+            detail = str(exc)
+            if "unsupported (! response)" not in detail:
+                raise
+            result = f"expected_idle_error={detail}"
+    active_modes = [mode.name for mode in status.get("activeModes", [])]
+    return f"{result} activeModes={active_modes}"
+
+
+def msp_set_home_check(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        status = api.get_inav_status()
+        try:
+            api.set_home(
+                latitude_deg=37.5001,
+                longitude_deg=-122.2499,
+                altitude_m=12.34,
+                altitude_datum=0,
+            )
+            result = "ack=ok"
+        except Exception as exc:
+            detail = str(exc)
+            if "unsupported (! response)" not in detail:
+                raise
+            result = f"expected_gate_error={detail}"
+    active_modes = [mode.name for mode in status.get("activeModes", [])]
+    return f"{result} activeModes={active_modes}"
+
+
+def msp_arm_disarm_check(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        api.set_arm_state(False)
+        status = api.get_inav_status()
+        try:
+            api.set_arm_state(True)
+            arm_result = "arm_ack=ok"
+        except Exception as exc:
+            detail = str(exc)
+            if "unsupported (! response)" not in detail:
+                raise
+            arm_result = f"expected_arm_error={detail}"
+    arming_flags = [flag.name for flag in status["armingFlags"]]
+    active_modes = [mode.name for mode in status.get("activeModes", [])]
+    return f"disarm_ack=ok {arm_result} armingFlags={arming_flags} activeModes={active_modes}"
+
+
+def msp_nav_roi_check(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        api.set_nav_roi(
+            latitude_deg=37.5001,
+            longitude_deg=-122.2499,
+            altitude_m=12.34,
+            p1=123,
+            p2=-45,
+            alt_datum=0,
+            action=7,
+            flag=1,
+        )
+        roi = api.get_nav_roi()
+        api.set_nav_roi(
+            latitude_deg=0.0,
+            longitude_deg=0.0,
+            altitude_m=0.0,
+            p1=0,
+            p2=0,
+            alt_datum=0,
+            action=0,
+            flag=0,
+        )
+        cleared = api.get_nav_roi()
+    if abs(roi["latitude_deg"] - 37.5001) > 1e-7 or abs(roi["longitude_deg"] + 122.2499) > 1e-7:
+        raise RuntimeError(f"ROI readback mismatch lat={roi['latitude_deg']} lon={roi['longitude_deg']}")
+    if abs(roi["altitude_m"] - 12.34) > 0.01 or roi["p1"] != 123 or roi["p2"] != -45:
+        raise RuntimeError(f"ROI payload mismatch altitude_m={roi['altitude_m']} p1={roi['p1']} p2={roi['p2']}")
+    if roi["alt_datum"] != 0 or roi["action"] != 7 or roi["flag"] != 1:
+        raise RuntimeError(f"ROI metadata mismatch alt_datum={roi['alt_datum']} action={roi['action']} flag={roi['flag']}")
+    if (
+        cleared["latitude_deg"] != 0.0
+        or cleared["longitude_deg"] != 0.0
+        or cleared["altitude_m"] != 0.0
+        or cleared["p1"] != 0
+        or cleared["p2"] != 0
+        or cleared["alt_datum"] != 0
+        or cleared["action"] != 0
+        or cleared["flag"] != 0
+    ):
+        raise RuntimeError(f"ROI clear mismatch cleared={cleared}")
+    return (
+        f"lat={roi['latitude_deg']:.7f} lon={roi['longitude_deg']:.7f} "
+        f"altitude_m={roi['altitude_m']:.2f} action={roi['action']} cleared_flag={cleared['flag']}"
+    )
+
+
+def read_arming_status(config: dict) -> str:
+    ports = config["ports"]
+    with MSPApi(tcp_endpoint=f"127.0.0.1:{int(ports['msp'])}") as api:
+        status = api.get_inav_status()
+        nav_status = api.get_nav_status()
+    arming_flags = [flag.name for flag in status["armingFlags"]]
+    if "activeModes" in nav_status:
+        active_modes = [mode.name for mode in nav_status["activeModes"]]
+    else:
+        active_modes = [mode.name for mode in status.get("activeModes", [])]
+    return f"armingFlags={arming_flags} activeModes={active_modes}"
+
+
+def receiver_telemetry_check(config: dict) -> str:
+    tests = config["tests"]
+    ports = config["ports"]
+    master = open_mavlink_tcp_connection(int(ports["rc"]), 241, 191, float(tests["port_ready_timeout_s"]))
+    try:
+        heartbeat = wait_for_fc_heartbeat(master, float(tests["heartbeat_timeout_s"]))
+        target_system = heartbeat.get_srcSystem()
+        target_component = heartbeat.get_srcComponent()
+        master.mav.request_data_stream_send(
+            target_system,
+            target_component,
+            mavutil.mavlink.MAV_DATA_STREAM_ALL,
+            int(tests["receiver_request_rate_hz"]),
+            1,
+        )
+        wait_for_message(master, "ATTITUDE", float(tests["telemetry_timeout_s"]))
+        return f"heartbeat_sysid={target_system} heartbeat_compid={target_component} attitude=seen"
+    finally:
+        master.close()
+
+
+def run_rc_script(config: dict) -> str:
+    tests = config["tests"]
+    paths = config["paths"]
+    rc_script = WORKSPACE_ROOT / paths["rc_script"]
+    command = build_rc_script_command(config, float(tests["rc_duration_s"]), "MID")
+    env = os.environ.copy()
+    env.pop("LD_LIBRARY_PATH", None)
+    completed = subprocess.run(command, cwd=str(WORKSPACE_ROOT), env=env, capture_output=True, text=True, check=True)
+    summary_lines = [line for line in completed.stdout.splitlines() if line.startswith("summary ")]
+    if not summary_lines:
+        raise RuntimeError(f"No summary line from {relpath(rc_script)}:\n{completed.stdout}")
+    summary = summary_lines[-1]
+    match = re.search(r"rx=(\d+).*mismatch_count=(\d+).*rx_hz=([0-9.]+)", summary)
+    if match is None:
+        raise ValueError(f"Unexpected RC summary format: {summary}")
+    rx_count = int(match.group(1))
+    mismatch_count = int(match.group(2))
+    rx_hz = float(match.group(3))
+    if rx_count < int(tests["rc_min_rx_count"]):
+        raise RuntimeError(f"RC rx count too low: {rx_count}")
+    if mismatch_count != 0:
+        raise RuntimeError(f"RC mismatch count is not zero: {mismatch_count}")
+    return f"rx={rx_count} mismatch_count={mismatch_count} rx_hz={rx_hz:.2f}"
+
+
+def build_rc_script_command(config: dict, duration_s: float, throttle_level: str) -> list[str]:
+    paths = config["paths"]
+    rc_script = WORKSPACE_ROOT / paths["rc_script"]
+    return [
+        sys.executable,
+        str(rc_script),
+        "--master",
+        f"tcp:127.0.0.1:{int(config['ports']['rc'])}",
+        "--duration",
+        str(duration_s),
+        "--tx-hz",
+        str(float(config["tests"]["rc_tx_hz"])),
+        "--roll",
+        "MID",
+        "--pitch",
+        "MID",
+        "--yaw",
+        "MID",
+        "--throttle",
+        throttle_level,
+    ]
+
+
+def start_rc_stream(config: dict, duration_s: float) -> tuple[subprocess.Popen[str], ProcessOutput]:
+    paths = config["paths"]
+    return start_process(
+        build_rc_script_command(config, duration_s, "LOW"),
+        WORKSPACE_ROOT,
+        WORKSPACE_ROOT / paths["control_plane_rc_log"],
+    )
+
+
+def start_mavproxy(config: dict) -> tuple[subprocess.Popen[str], ProcessOutput]:
+    paths = config["paths"]
+    ports = config["ports"]
+    tests = config["tests"]
+    mavproxy = shutil.which("mavproxy.py")
+    if mavproxy is None:
+        raise FileNotFoundError("mavproxy.py not found in PATH")
+    state_dir = WORKSPACE_ROOT / paths["mavproxy_state_dir"]
+    state_dir.mkdir(parents=True, exist_ok=True)
+    command = [
+        mavproxy,
+        f"--master=tcp:127.0.0.1:{int(ports['gcs'])}",
+        f"--out=udp:127.0.0.1:{int(ports['mavproxy_out'])}",
+        "--streamrate=-1",
+        "--state-basedir",
+        str(state_dir),
+        "--aircraft",
+        "mavlink_sitl_sanity",
+        "--target-system",
+        str(int(tests["target_system"])),
+        "--target-component",
+        str(int(tests["target_component"])),
+    ]
+    return start_process(command, WORKSPACE_ROOT, WORKSPACE_ROOT / paths["mavproxy_log"])
+
+
+def gcs_heartbeat_check(config: dict, observer) -> str:
+    tests = config["tests"]
+    heartbeat = wait_for_fc_heartbeat(observer, float(tests["heartbeat_timeout_s"]))
+    return f"heartbeat_sysid={heartbeat.get_srcSystem()} heartbeat_compid={heartbeat.get_srcComponent()}"
+
+
+def send_gcs_heartbeat(master) -> None:
+    master.mav.heartbeat_send(
+        mavutil.mavlink.MAV_TYPE_GCS,
+        mavutil.mavlink.MAV_AUTOPILOT_INVALID,
+        0,
+        0,
+        mavutil.mavlink.MAV_STATE_ACTIVE,
+    )
+
+
+def control_plane_check(config: dict) -> str:
+    tests = config["tests"]
+    ports = config["ports"]
+    control_cfg = config["control_plane"]
+    rc_process = None
+    rc_output = None
+    master = open_mavlink_tcp_connection(
+        int(ports["gcs"]),
+        int(control_cfg["source_system"]),
+        int(control_cfg["source_component"]),
+        float(tests["port_ready_timeout_s"]),
+    )
+    try:
+        rc_process, rc_output = start_rc_stream(config, float(control_cfg["rc_hold_duration_s"]))
+        time.sleep(float(control_cfg["rc_warmup_s"]))
+        heartbeat = wait_for_fc_heartbeat(master, float(control_cfg["heartbeat_timeout_s"]))
+        target_system = heartbeat.get_srcSystem()
+        target_component = heartbeat.get_srcComponent()
+        send_gcs_heartbeat(master)
+        drain_messages(master, 0.5)
+
+        requester_ts = time.time_ns()
+        master.mav.timesync_send(0, requester_ts)
+        timesync = wait_for_message(master, "TIMESYNC", float(control_cfg["timesync_timeout_s"]))
+        if int(timesync.ts1) != requester_ts:
+            raise RuntimeError(f"TIMESYNC ts1 mismatch: expected {requester_ts} got {int(timesync.ts1)}")
+        if int(timesync.tc1) <= 0:
+            raise RuntimeError(f"TIMESYNC tc1 not populated: {int(timesync.tc1)}")
+
+        send_gcs_heartbeat(master)
+        master.mav.set_mode_send(
+            target_system,
+            int(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
+            int(control_cfg["fbwa_custom_mode"]),
+        )
+        try:
+            wait_for_fc_mode(master, int(control_cfg["fbwa_custom_mode"]), False, float(control_cfg["mode_timeout_s"]))
+            mode_probe = f"fbwa_mode={int(control_cfg['fbwa_custom_mode'])}"
+        except TimeoutError as exc:
+            mode_probe = f"fbwa_headless_masked={exc}"
+
+        send_gcs_heartbeat(master)
+        master.mav.set_mode_send(
+            target_system,
+            int(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
+            int(control_cfg["manual_custom_mode"]),
+        )
+        wait_for_fc_mode(master, int(control_cfg["manual_custom_mode"]), False, float(control_cfg["mode_timeout_s"]))
+
+        send_gcs_heartbeat(master)
+        master.mav.command_long_send(
+            target_system,
+            target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            1.0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+        )
+        arm_ack = wait_for_command_ack(master, int(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM), float(control_cfg["ack_timeout_s"]))
+        if int(arm_ack.result) != int(mavutil.mavlink.MAV_RESULT_DENIED):
+            raise RuntimeError(f"Headless arm expected DENIED but got result {int(arm_ack.result)}")
+        arming_status = read_arming_status(config)
+        if "ARMING_DISABLED_SENSORS_CALIBRATING" not in arming_status or \
+            "ARMING_DISABLED_NAVIGATION_UNSAFE" not in arming_status or \
+            "ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED" not in arming_status:
+            raise RuntimeError(f"Headless arm deny did not expose expected blockers: {arming_status}")
+
+        return (
+            f"timesync_tc1={int(timesync.tc1)} {mode_probe} "
+            f"manual_mode={int(control_cfg['manual_custom_mode'])} arm_result={int(arm_ack.result)} "
+            f"{arming_status}"
+        )
+    finally:
+        master.close()
+        stop_process(rc_process)
+        if rc_output is not None:
+            rc_output.close()
+
+
+def mission_upload_check(config: dict, mavproxy_process: subprocess.Popen[str], mavproxy_output: ProcessOutput) -> str:
+    tests = config["tests"]
+    paths = config["paths"]
+    mission_path = WORKSPACE_ROOT / paths["mission_file"]
+    start_index = mavproxy_output.mark()
+    mavproxy_send(mavproxy_process, f"wp load {mission_path}")
+    line = mavproxy_output.wait_for(rf"Loaded {int(tests['mission_waypoint_count'])} waypoints in ", float(tests["mission_timeout_s"]), start_index)
+    return f"mission={relpath(mission_path)} {line}"
+
+
+def mission_readback_check(config: dict, mavproxy_process: subprocess.Popen[str], mavproxy_output: ProcessOutput) -> str:
+    tests = config["tests"]
+    paths = config["paths"]
+    readback_path = WORKSPACE_ROOT / paths["mission_readback_file"]
+    if readback_path.exists():
+        readback_path.unlink()
+    start_index = mavproxy_output.mark()
+    mavproxy_send(mavproxy_process, f"wp save {readback_path}")
+    mavproxy_output.wait_for(rf"Saved {int(tests['mission_waypoint_count'])} waypoints to ", float(tests["mission_timeout_s"]), start_index)
+    count = readback_waypoint_count(readback_path)
+    if count != int(tests["mission_waypoint_count"]):
+        raise RuntimeError(f"Readback waypoint count mismatch: expected {tests['mission_waypoint_count']} got {count}")
+    return f"readback={relpath(readback_path)} waypoint_count={count}"
+
+
+def stream_rate_check(config: dict, observer, mavproxy_process: subprocess.Popen[str]) -> str:
+    tests = config["tests"]
+    mavproxy_send(mavproxy_process, f"set streamrate {int(tests['streamrate_hz'])}")
+    wait_for_message(observer, "ATTITUDE", float(tests["telemetry_timeout_s"]))
+    drain_messages(observer, 0.5)
+    rate_hz = measure_message_rate(observer, "ATTITUDE", float(tests["rate_measure_window_s"]))
+    if rate_hz < float(tests["streamrate_min_hz"]):
+        raise RuntimeError(f"ATTITUDE rate too low after streamrate set: {rate_hz:.2f}Hz")
+    return f"attitude_rate_hz={rate_hz:.2f}"
+
+
+def message_rate_check(config: dict, observer, mavproxy_process: subprocess.Popen[str], mavproxy_output: ProcessOutput) -> str:
+    tests = config["tests"]
+    target_rate_hz = float(tests["message_rate_hz"])
+    start_index = mavproxy_output.mark()
+    mavproxy_send(mavproxy_process, "module load messagerate")
+    mavproxy_send(mavproxy_process, f"messagerate set ATTITUDE {target_rate_hz}")
+    time.sleep(1.0)
+    mavproxy_send(mavproxy_process, "messagerate get ATTITUDE")
+    line = mavproxy_output.wait_for(r"Msg:ATTITUDE  rate:([0-9.]+)Hz", float(tests["telemetry_timeout_s"]), start_index)
+    reported_match = re.search(r"rate:([0-9.]+)Hz", line)
+    if reported_match is None:
+        raise RuntimeError(f"Unexpected messagerate output: {line}")
+    reported_rate_hz = float(reported_match.group(1))
+    if abs(reported_rate_hz - target_rate_hz) > float(tests["message_rate_report_tolerance_hz"]):
+        raise RuntimeError(f"Reported ATTITUDE rate {reported_rate_hz:.2f}Hz does not match target {target_rate_hz:.2f}Hz")
+    drain_messages(observer, 0.5)
+    observed_rate_hz = measure_message_rate(observer, "ATTITUDE", float(tests["rate_measure_window_s"]))
+    if abs(observed_rate_hz - target_rate_hz) > float(tests["message_rate_observed_tolerance_hz"]):
+        raise RuntimeError(f"Observed ATTITUDE rate {observed_rate_hz:.2f}Hz does not match target {target_rate_hz:.2f}Hz")
+    return f"reported_rate_hz={reported_rate_hz:.2f} observed_rate_hz={observed_rate_hz:.2f}"
+
+
+def print_report(results: list[tuple[str, bool, str]]) -> None:
+    for name, ok, detail in results:
+        status = "PASS" if ok else "FAIL"
+        print(f"{name}: {status} {detail}", flush=True)
+    overall = all(ok for _, ok, _ in results)
+    print(f"OVERALL: {'PASS' if overall else 'FAIL'}", flush=True)
+
+
+def main() -> int:
+    args = parse_args()
+    config_path = Path(args.config).resolve()
+    config = load_config(config_path)
+    sitl_process = None
+    sitl_output = None
+    mavproxy_process = None
+    mavproxy_output = None
+    observer = None
+    results: list[tuple[str, bool, str]] = []
+
+    def record(name: str, action) -> None:
+        try:
+            detail = action()
+            results.append((name, True, detail))
+        except Exception as exc:
+            results.append((name, False, str(exc)))
+            raise
+
+    try:
+        sitl_cfg = config["sitl"]
+        tests = config["tests"]
+        eeprom_path = WORKSPACE_ROOT / sitl_cfg["eeprom_path"]
+        if eeprom_path.exists():
+            eeprom_path.unlink()
+        sitl_command = [str(WORKSPACE_ROOT / sitl_cfg["binary"]), "--path", str(WORKSPACE_ROOT / sitl_cfg["eeprom_path"])]
+        sitl_process, sitl_output = start_process(sitl_command, WORKSPACE_ROOT / sitl_cfg["workdir"], WORKSPACE_ROOT / sitl_cfg["runtime_log"])
+        wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(tests["port_ready_timeout_s"]))
+        for _ in range(int(config["cli"]["apply_count"])):
+            run_cli_commands("127.0.0.1", int(config["ports"]["msp"]), build_cli_config_commands(config))
+            wait_for_configured_ports(config)
+        wait_for_tcp_port("127.0.0.1", int(config["ports"]["rc"]), float(tests["port_ready_timeout_s"]))
+        wait_for_tcp_port("127.0.0.1", int(config["ports"]["gcs"]), float(tests["port_ready_timeout_s"]))
+
+        record("MSP_UART1", lambda: msp_check(config))
+        record("MSP_ARM_DISARM_UART1", lambda: msp_arm_disarm_check(config))
+        record("MSP_LAND_UART1", lambda: msp_land_command_check(config))
+        record("MSP_RTH_UART1", lambda: msp_rth_command_check(config))
+        record("MSP_SET_HOME_UART1", lambda: msp_set_home_check(config))
+        record("MSP_NAV_ROI_UART1", lambda: msp_nav_roi_check(config))
+        record("MAVLINK_TELEMETRY_UART2", lambda: receiver_telemetry_check(config))
+        record("RC_QUICK_CHECK_UART2", lambda: run_rc_script(config))
+        record("CONTROL_PLANE_UART3", lambda: control_plane_check(config))
+
+        mavproxy_process, mavproxy_output = start_mavproxy(config)
+        observer = open_mavlink_udp_listener(int(config["ports"]["mavproxy_out"]), 244, 191)
+        record("MAVLINK_GCS_UART3", lambda: gcs_heartbeat_check(config, observer))
+        record("MISSION_UPLOAD_UART3", lambda: mission_upload_check(config, mavproxy_process, mavproxy_output))
+        record("MISSION_READBACK_UART3", lambda: mission_readback_check(config, mavproxy_process, mavproxy_output))
+        record("STREAM_RATE_UART3", lambda: stream_rate_check(config, observer, mavproxy_process))
+        record("MESSAGE_RATE_UART3", lambda: message_rate_check(config, observer, mavproxy_process, mavproxy_output))
+    except Exception:
+        exc = sys.exc_info()[1]
+        if exc is not None and (not results or results[-1][1]):
+            results.append(("UNHANDLED", False, str(exc)))
+        print_report(results)
+        return 1
+    finally:
+        if observer is not None:
+            observer.close()
+        stop_process(mavproxy_process)
+        if mavproxy_output is not None:
+            mavproxy_output.close()
+        stop_process(sitl_process)
+        if sitl_output is not None:
+            sitl_output.close()
+
+    print_report(results)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/src/test/mavlink/sanity/mavlink_sitl_sanity.yaml b/src/test/mavlink/sanity/mavlink_sitl_sanity.yaml
new file mode 100644
index 00000000000..0d83a6b133b
--- /dev/null
+++ b/src/test/mavlink/sanity/mavlink_sitl_sanity.yaml
@@ -0,0 +1,61 @@
+sitl:
+  binary: cmake/build_SITL/inav_9.1.0_SITL
+  workdir: inav/cmake
+  eeprom_path: src/test/mavlink/sanity/results/eeprom.bin
+  runtime_log: src/test/mavlink/sanity/results/sitl_runtime_sanity.log
+
+ports:
+  msp: 5760
+  rc: 5761
+  gcs: 5762
+  mavproxy_out: 14550
+
+cli:
+  apply_count: 2
+  diff_path: src/test/mavlink/sanity/fc_diff.txt
+  rc_baud: 460800
+  telemetry_baud: 115200
+  mode_ranges:
+    - aux 0 9 0 1300 2100
+    - aux 1 22 1 1300 2100
+    - aux 2 10 2 1300 2100
+
+tests:
+  port_ready_timeout_s: 30
+  save_reboot_timeout_s: 45
+  heartbeat_timeout_s: 15
+  telemetry_timeout_s: 15
+  receiver_request_rate_hz: 5
+  rc_duration_s: 5
+  rc_tx_hz: 100
+  rc_min_rx_count: 20
+  target_system: 1
+  target_component: 1
+  mission_waypoint_count: 2
+  mission_timeout_s: 20
+  streamrate_hz: 10
+  streamrate_min_hz: 4
+  message_rate_hz: 7
+  message_rate_report_tolerance_hz: 0.2
+  message_rate_observed_tolerance_hz: 2.5
+  rate_measure_window_s: 3
+
+control_plane:
+  source_system: 245
+  source_component: 190
+  heartbeat_timeout_s: 15
+  timesync_timeout_s: 10
+  ack_timeout_s: 10
+  mode_timeout_s: 12
+  fbwa_custom_mode: 5
+  manual_custom_mode: 0
+  rc_hold_duration_s: 30
+  rc_warmup_s: 2
+
+paths:
+  rc_script: src/test/mavlink/sanity/mavlink_test_rc.py
+  control_plane_rc_log: src/test/mavlink/sanity/results/control_plane_rc.log
+  mission_file: tools/mavlink_sitl_sanity_mission.waypoints
+  mission_readback_file: src/test/mavlink/sanity/results/mavlink_sitl_sanity_readback.waypoints
+  mavproxy_log: src/test/mavlink/sanity/results/mavproxy_sanity.log
+  mavproxy_state_dir: src/test/mavlink/sanity/results/mavproxy_state
diff --git a/src/test/mavlink/sanity/mavlink_sitl_sanity_mission.waypoints b/src/test/mavlink/sanity/mavlink_sitl_sanity_mission.waypoints
new file mode 100644
index 00000000000..8bdef9c5986
--- /dev/null
+++ b/src/test/mavlink/sanity/mavlink_sitl_sanity_mission.waypoints
@@ -0,0 +1,3 @@
+QGC WPL 110
+0	1	0	16	0	0	0	0	45.3493856	-74.0727489	48	1
+1	0	3	16	0	0	0	0	45.3472065	-74.0777807	50	1
diff --git a/src/test/mavlink/sanity/mavlink_test_rc.py b/src/test/mavlink/sanity/mavlink_test_rc.py
new file mode 100644
index 00000000000..4862ec7aad2
--- /dev/null
+++ b/src/test/mavlink/sanity/mavlink_test_rc.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python3
+"""
+Usage:
+  conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761
+  conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761 --duration 20 --tx-hz 100 --roll MID --pitch MID --yaw MID --throttle LOW
+"""
+
+from __future__ import annotations
+
+import argparse
+import time
+from collections import Counter
+from typing import List
+
+from pymavlink import mavutil
+
+
+CHANNEL_ORDER = [
+    "roll",
+    "pitch",
+    "throttle",
+    "yaw",
+    "ch5",
+    "ch6",
+    "ch7",
+    "ch8",
+    "ch9",
+    "ch10",
+    "ch11",
+    "ch12",
+    "ch13",
+    "ch14",
+    "ch15",
+    "ch16",
+    "ch17",
+    "ch18",
+]
+DEFAULT_CHANNELS = [900] * 18
+DEFAULT_CHANNELS[0] = 1500
+DEFAULT_CHANNELS[1] = 1500
+DEFAULT_CHANNELS[3] = 1500
+LEVEL_TO_PWM = {"LOW": 900, "MID": 1500, "HIGH": 2100}
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Send MAVLink RC_CHANNELS_OVERRIDE stream and monitor RC echo.")
+    parser.add_argument("--master", required=True, help='pymavlink master endpoint, e.g. "tcp:127.0.0.1:5761"')
+    parser.add_argument("--tx-hz", type=float, default=100.0, help="RC override transmit rate in Hz")
+    parser.add_argument("--duration", type=float, default=0.0, help="Test duration in seconds, 0 means run forever")
+    parser.add_argument("--source-system", type=int, default=240, help="MAVLink source system ID")
+    parser.add_argument("--source-component", type=int, default=191, help="MAVLink source component ID")
+    parser.add_argument("--echo-tolerance", type=int, default=20, help="Allowed absolute PWM error for RC_CHANNELS echo checks")
+    for channel_name in CHANNEL_ORDER:
+        parser.add_argument(f"--{channel_name}", default=None, help="LOW/MID/HIGH or integer PWM")
+    args = parser.parse_args()
+
+    channels: List[int] = list(DEFAULT_CHANNELS)
+    for idx, channel_name in enumerate(CHANNEL_ORDER):
+        raw_value = getattr(args, channel_name)
+        if raw_value is None:
+            continue
+        level_name = raw_value.upper()
+        if level_name in LEVEL_TO_PWM:
+            channels[idx] = LEVEL_TO_PWM[level_name]
+        else:
+            channels[idx] = int(raw_value)
+
+    print(
+        f"master={args.master} tx_hz={args.tx_hz} duration={args.duration} "
+        f"source_system={args.source_system} source_component={args.source_component} channels={channels}",
+        flush=True,
+    )
+
+    master = mavutil.mavlink_connection(
+        args.master,
+        source_system=args.source_system,
+        source_component=args.source_component,
+        autoreconnect=True,
+    )
+    heartbeat = master.wait_heartbeat(timeout=10)
+    if heartbeat is None:
+        raise TimeoutError("No heartbeat received from FC")
+    target_system = heartbeat.get_srcSystem()
+    target_component = heartbeat.get_srcComponent()
+    print(f"target_system={target_system} target_component={target_component}", flush=True)
+
+    period_s = 1.0 / args.tx_hz
+    tx_count = 0
+    rx_count = 0
+    mismatch_count = 0
+    decode_errors = 0
+    last_rx_time = 0.0
+    loop_start = time.monotonic()
+    next_tx_time = loop_start
+    next_report_time = loop_start + 1.0
+    mismatch_hist = Counter()
+
+    while True:
+        now = time.monotonic()
+        if args.duration > 0 and (now - loop_start) >= args.duration:
+            break
+
+        if now >= next_tx_time:
+            master.mav.rc_channels_override_send(target_system, target_component, *channels[:8])
+            tx_count += 1
+            next_tx_time += period_s
+            if next_tx_time < now:
+                next_tx_time = now + period_s
+
+        message = master.recv_match(type=["RC_CHANNELS", "RC_CHANNELS_RAW"], blocking=False)
+        if message is not None:
+            rx_count += 1
+            last_rx_time = now
+            if message.get_type() == "RC_CHANNELS":
+                for i in range(4):
+                    key = f"chan{i + 1}_raw"
+                    observed = int(getattr(message, key))
+                    err = abs(observed - channels[i])
+                    if err > args.echo_tolerance:
+                        mismatch_count += 1
+                        mismatch_hist[i + 1] += 1
+            else:
+                decode_errors += 1
+
+        if now >= next_report_time:
+            rx_age = -1.0 if last_rx_time == 0.0 else (now - last_rx_time)
+            print(
+                f"status_s={now - loop_start:.1f} tx={tx_count} rx={rx_count} mismatches={mismatch_count} "
+                f"decode_errors={decode_errors} rx_age_s={rx_age:.3f} mismatch_hist={dict(mismatch_hist)}",
+                flush=True,
+            )
+            next_report_time += 1.0
+
+        time.sleep(0.001)
+
+    elapsed = max(time.monotonic() - loop_start, 1e-6)
+    print(
+        f"summary elapsed_s={elapsed:.2f} tx={tx_count} rx={rx_count} mismatch_count={mismatch_count} "
+        f"mismatch_rate={mismatch_count / max(rx_count, 1):.6f} tx_hz={tx_count / elapsed:.2f} rx_hz={rx_count / elapsed:.2f}",
+        flush=True,
+    )
diff --git a/src/test/mavlink/sanity/results/.gitignore b/src/test/mavlink/sanity/results/.gitignore
new file mode 100644
index 00000000000..d6b7ef32c84
--- /dev/null
+++ b/src/test/mavlink/sanity/results/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index b85d610cd94..96b15243567 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -71,7 +71,7 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial
 set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER)
 
 set_property(SOURCE mavlink_unittest.cc PROPERTY depends
-    "fc/fc_mavlink.c" "mavlink/mavlink_command.c" "mavlink/mavlink_guided.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c"
+    "fc/fc_mavlink.c" "mavlink/mavlink_command.c" "mavlink/mavlink_guided.c" "mavlink/mavlink_mission.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c"
     "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" "mavlink/mavlink_streams.c" "telemetry/mavlink.c"
     "common/crc.c" "common/maths.c" "common/streambuf.c" "common/string_light.c" "msp/msp_serial.c")
 set_property(SOURCE mavlink_unittest.cc PROPERTY definitions USE_TELEMETRY USE_TELEMETRY_MAVLINK)
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 5cd18fa2fb1..df3fd1b9923 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -330,6 +330,8 @@ static void initMavlinkTestState(void)
     memset(&GPS_home, 0, sizeof(GPS_home));
     memset(waypointStore, 0, sizeof(waypointStore));
     memset(&rxLinkStatistics, 0, sizeof(rxLinkStatistics));
+    posControl.wpReachedSeq = 0;
+    posControl.wpReachedNotificationPending = false;
 
     telemetryConfigMutable()->mavlink_common.sysid = 1;
     telemetryConfigMutable()->mavlink_common.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT;
@@ -771,19 +773,402 @@ TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates)
 }
 
 
+TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets)
+{
+    initMavlinkTestState();
 
+    mavlink_message_t msg;
+    mavlink_msg_mission_clear_all_pack(
+        42, 200, &msg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
 
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
 
+    EXPECT_EQ(resetWaypointCalls, 1);
 
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
 
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
 
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+    EXPECT_EQ(ack.mission_type, MAV_MISSION_TYPE_MISSION);
+}
 
+TEST(MavlinkTelemetryTest, MissionClearAllRestoresPreviousMissionOnPersistFailure)
+{
+    initMavlinkTestState();
 
+    waypointCount = 1;
+    waypointStore[0].action = NAV_WP_ACTION_WAYPOINT;
+    waypointStore[0].lat = 365304400;
+    waypointStore[0].lon = -832163830;
+    waypointStore[0].alt = 1234;
+    waypointStore[0].flag = NAV_WP_FLAG_LAST;
+    saveWaypointResult = false;
 
+    mavlink_message_t msg;
+    mavlink_msg_mission_clear_all_pack(
+        42, 200, &msg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
 
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
 
+    EXPECT_EQ(ack.type, MAV_MISSION_ERROR);
+    EXPECT_EQ(waypointCount, 1);
+    EXPECT_EQ(waypointStore[0].lat, 365304400);
+    EXPECT_EQ(waypointStore[0].lon, -832163830);
+    EXPECT_EQ(waypointStore[0].alt, 1234);
+    EXPECT_EQ(waypointStore[0].flag, NAV_WP_FLAG_LAST);
+}
 
+TEST(MavlinkTelemetryTest, MissionCountRequestsFirstItem)
+{
+    initMavlinkTestState();
 
+    mavlink_message_t msg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t reqMsg;
+    ASSERT_TRUE(popTxMessage(&reqMsg));
+    ASSERT_EQ(reqMsg.msgid, MAVLINK_MSG_ID_MISSION_REQUEST_INT);
+
+    mavlink_mission_request_int_t req;
+    mavlink_msg_mission_request_int_decode(&reqMsg, &req);
+
+    EXPECT_EQ(req.seq, 0);
+    EXPECT_EQ(req.mission_type, MAV_MISSION_TYPE_MISSION);
+}
+
+TEST(MavlinkTelemetryTest, MissionCountZeroRestoresPreviousMissionOnPersistFailure)
+{
+    initMavlinkTestState();
+
+    waypointCount = 1;
+    waypointStore[0].action = NAV_WP_ACTION_LAND;
+    waypointStore[0].lat = 365304400;
+    waypointStore[0].lon = -832163830;
+    waypointStore[0].flag = NAV_WP_FLAG_LAST;
+    saveWaypointResult = false;
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 0, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+    EXPECT_EQ(ack.type, MAV_MISSION_ERROR);
+    EXPECT_EQ(waypointCount, 1);
+    EXPECT_EQ(waypointStore[0].action, NAV_WP_ACTION_LAND);
+    EXPECT_EQ(waypointStore[0].lat, 365304400);
+    EXPECT_EQ(waypointStore[0].lon, -832163830);
+    EXPECT_EQ(waypointStore[0].flag, NAV_WP_FLAG_LAST);
+}
+
+TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected)
+{
+    initMavlinkTestState();
+    ENABLE_ARMING_FLAG(ARMED);
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_status_t status;
+    memset(&status, 0, sizeof(status));
+    mavlink_message_t outMsg;
+    bool sawAck = false;
+    bool sawRequest = false;
+
+    for (size_t i = 0; i < serialTxLen; i++) {
+        if (mavlink_parse_char(0, serialTxBuffer[i], &outMsg, &status) == MAVLINK_FRAMING_OK) {
+            if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_ACK) {
+                mavlink_mission_ack_t ack;
+                mavlink_msg_mission_ack_decode(&outMsg, &ack);
+                EXPECT_EQ(ack.type, MAV_MISSION_DENIED);
+                sawAck = true;
+            }
+            if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {
+                sawRequest = true;
+            }
+        }
+    }
+
+    EXPECT_TRUE(sawAck);
+    EXPECT_FALSE(sawRequest);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+
+    serialTxLen = 0;
+
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+    EXPECT_EQ(setWaypointCalls, 1);
+    EXPECT_EQ(lastWaypoint.lat, 375000000);
+    EXPECT_EQ(lastWaypoint.lon, -1222500000);
+    EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+}
+
+TEST(MavlinkTelemetryTest, MissionLandRetainsSuppliedPosition)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+    serialTxLen = 0;
+
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_LAND, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+    EXPECT_EQ(lastWaypoint.action, NAV_WP_ACTION_LAND);
+    EXPECT_EQ(lastWaypoint.lat, 375000000);
+    EXPECT_EQ(lastWaypoint.lon, -1222500000);
+    EXPECT_EQ(lastWaypoint.alt, 1230);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemIntSingleFinalItemAllowsAutocontinueZero)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+
+    serialTxLen = 0;
+
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 0,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemIntNonFinalAutocontinueZeroIsRejected)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+
+    serialTxLen = 0;
+
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 0,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+    EXPECT_EQ(ack.type, MAV_MISSION_UNSUPPORTED);
+}
+
+
+
+TEST(MavlinkTelemetryTest, MissionRequestListSendsCount)
+{
+    initMavlinkTestState();
+    waypointCount = 2;
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_request_list_pack(
+        42, 200, &msg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&msg);
+    handleMavlinkUntilRxEmpty(1000);
+
+    mavlink_message_t countMsg;
+    ASSERT_TRUE(popTxMessage(&countMsg));
+    ASSERT_EQ(countMsg.msgid, MAVLINK_MSG_ID_MISSION_COUNT);
+
+    mavlink_mission_count_t count;
+    mavlink_msg_mission_count_decode(&countMsg, &count);
+
+    EXPECT_EQ(count.count, 2);
+    EXPECT_EQ(count.mission_type, MAV_MISSION_TYPE_MISSION);
+}
+
+TEST(MavlinkTelemetryTest, MissionRequestSendsWaypointInt)
+{
+    initMavlinkTestState();
+    waypointCount = 1;
+    waypointStore[0].action = NAV_WP_ACTION_WAYPOINT;
+    waypointStore[0].lat = 375000000;
+    waypointStore[0].lon = -1222500000;
+    waypointStore[0].alt = 1234;
+    waypointStore[0].p3 = 0;
+
+    mavlink_message_t listMsg;
+    mavlink_msg_mission_request_list_pack(
+        42, 200, &listMsg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&listMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_request_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 0, MAV_MISSION_TYPE_MISSION);
+
+    pushRxMessage(&msg);
+    handleMavlinkUntilRxEmpty(1000);
+
+    mavlink_message_t itemMsg;
+    ASSERT_TRUE(popTxMessage(&itemMsg));
+    ASSERT_EQ(itemMsg.msgid, MAVLINK_MSG_ID_MISSION_ITEM_INT);
+
+    mavlink_mission_item_int_t item;
+    mavlink_msg_mission_item_int_decode(&itemMsg, &item);
+
+    EXPECT_EQ(item.seq, 0);
+    EXPECT_EQ(item.command, MAV_CMD_NAV_WAYPOINT);
+    EXPECT_EQ(item.frame, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT);
+    EXPECT_EQ(item.x, 375000000);
+    EXPECT_EQ(item.y, -1222500000);
+    EXPECT_NEAR(item.z, 12.34f, 1e-4f);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemReachedIsBroadcastOnceWhenPending)
+{
+    initMavlinkTestState();
+
+    posControl.wpReachedSeq = 3;
+    posControl.wpReachedNotificationPending = true;
+
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t reachedMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg));
+
+    mavlink_mission_item_reached_t reached;
+    mavlink_msg_mission_item_reached_decode(&reachedMsg, &reached);
+
+    EXPECT_EQ(reached.seq, 3);
+
+    resetSerialBuffers();
+    handleMAVLinkTelemetry(1000);
+
+    EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg));
+}
 
 
 TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam)
@@ -1613,13 +1998,490 @@ TEST(MavlinkTelemetryTest, RequestMessageRejectsAvailableModeWhenStandardModesDi
 }
 #endif
 
+TEST(MavlinkTelemetryTest, MissionUploadPersistsAndRetriesMissingItem)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
 
+    resetSerialBuffers();
+    fakeMillis = testMissionUploadRetryMs;
+    handleMAVLinkTelemetry((timeUs_t)testMissionUploadRetryMs * 1000);
 
+    mavlink_message_t retryMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &retryMsg));
+    mavlink_mission_request_int_t retry;
+    mavlink_msg_mission_request_int_decode(&retryMsg, &retry);
+    EXPECT_EQ(retry.seq, 0);
 
+    resetSerialBuffers();
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
 
+    EXPECT_EQ(saveWaypointCalls, 1);
+}
+
+TEST(MavlinkTelemetryTest, MissionUploadOutOfSequenceRerequestsExpectedItem)
+{
+    initMavlinkTestState();
 
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
 
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 1,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t requestMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &requestMsg));
+
+    mavlink_mission_request_int_t request;
+    mavlink_msg_mission_request_int_decode(&requestMsg, &request);
+    EXPECT_EQ(request.seq, 0);
+
+    mavlink_message_t ackMsg;
+    EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ACK, &ackMsg));
+    EXPECT_EQ(setWaypointCalls, 0);
+}
+
+TEST(MavlinkTelemetryTest, MissionUploadCancelAckResetsTransfer)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t cancelMsg;
+    mavlink_msg_mission_ack_pack(
+        42, 200, &cancelMsg,
+        1, testTargetComponent,
+        MAV_MISSION_OPERATION_CANCELLED,
+        MAV_MISSION_TYPE_MISSION,
+        0);
+    pushRxMessage(&cancelMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ACK, &ackMsg));
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+    EXPECT_EQ(ack.type, MAV_MISSION_INVALID_SEQUENCE);
+    EXPECT_EQ(setWaypointCalls, 0);
+}
+
+TEST(MavlinkTelemetryTest, MissionUploadRestoresPreviousMissionOnPersistFailure)
+{
+    initMavlinkTestState();
+
+    waypointCount = 1;
+    waypointStore[0].action = NAV_WP_ACTION_WAYPOINT;
+    waypointStore[0].lat = 365304400;
+    waypointStore[0].lon = -832163830;
+    waypointStore[0].alt = 1234;
+    waypointStore[0].flag = NAV_WP_FLAG_LAST;
+    saveWaypointResult = false;
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
 
+    mavlink_message_t itemMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &itemMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&itemMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ACK, &ackMsg));
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+    EXPECT_EQ(ack.type, MAV_MISSION_INVALID);
+    EXPECT_EQ(waypointCount, 1);
+    EXPECT_EQ(waypointStore[0].lat, 365304400);
+    EXPECT_EQ(waypointStore[0].lon, -832163830);
+    EXPECT_EQ(waypointStore[0].alt, 1234);
+    EXPECT_EQ(waypointStore[0].flag, NAV_WP_FLAG_LAST);
+}
+
+TEST(MavlinkTelemetryTest, MissionUploadSkipsQgcPlannedHomeAndAcceptsWaypointNanParams)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+
+    resetSerialBuffers();
+    mavlink_message_t homeMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &homeMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        NAN, NAN, NAN, NAN,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&homeMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t requestMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &requestMsg));
+    mavlink_mission_request_int_t request;
+    mavlink_msg_mission_request_int_decode(&requestMsg, &request);
+    EXPECT_EQ(request.seq, 1);
+
+    resetSerialBuffers();
+    mavlink_message_t waypointMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &waypointMsg,
+        1, testTargetComponent, 1,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        NAN, NAN, NAN, NAN,
+        375001000, -1222501000, 45.6f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&waypointMsg);
+    handleMAVLinkTelemetry(1000);
+
+    EXPECT_EQ(setWaypointCalls, 1);
+    EXPECT_EQ(lastWaypointNumber, 1);
+    EXPECT_EQ(lastWaypoint.action, NAV_WP_ACTION_WAYPOINT);
+    EXPECT_EQ(lastWaypoint.lat, 375001000);
+    EXPECT_EQ(lastWaypoint.lon, -1222501000);
+    EXPECT_EQ(lastWaypoint.alt, 4560);
+    EXPECT_EQ(saveWaypointCalls, 1);
+}
+
+TEST(MavlinkTelemetryTest, MissionUploadPreservesCurrentAbsoluteFirstWaypoint)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t countMsg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &countMsg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&countMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t firstMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &firstMsg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_INT,
+        MAV_CMD_NAV_WAYPOINT, 1, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 123.4f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&firstMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t secondMsg;
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &secondMsg,
+        1, testTargetComponent, 1,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375001000, -1222501000, 45.6f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&secondMsg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ACK, &ackMsg));
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+    EXPECT_EQ(setWaypointCalls, 2);
+    EXPECT_EQ(waypointCount, 2);
+    EXPECT_EQ(waypointStore[0].lat, 375000000);
+    EXPECT_EQ(waypointStore[0].lon, -1222500000);
+    EXPECT_EQ(waypointStore[0].alt, 12340);
+    EXPECT_EQ(waypointStore[0].p3, NAV_WP_ALTMODE);
+    EXPECT_EQ(waypointStore[1].lat, 375001000);
+    EXPECT_EQ(waypointStore[1].lon, -1222501000);
+    EXPECT_EQ(waypointStore[1].alt, 4560);
+}
+
+TEST(MavlinkTelemetryTest, MissionDownloadServesValidOutOfOrderRequests)
+{
+    initMavlinkTestState();
+
+    waypointCount = 2;
+    waypointStore[0].action = NAV_WP_ACTION_WAYPOINT;
+    waypointStore[0].lat = 375000000;
+    waypointStore[0].lon = -1222500000;
+    waypointStore[0].alt = 1234;
+    waypointStore[1].action = NAV_WP_ACTION_LAND;
+    waypointStore[1].lat = 375001000;
+    waypointStore[1].lon = -1222501000;
+    waypointStore[1].alt = 0;
+    waypointStore[1].flag = NAV_WP_FLAG_LAST;
+
+    mavlink_message_t listMsg;
+    mavlink_msg_mission_request_list_pack(
+        42, 200, &listMsg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&listMsg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_message_t requestMsg;
+    mavlink_msg_mission_request_int_pack(
+        42, 200, &requestMsg,
+        1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&requestMsg);
+    handleMavlinkUntilRxEmpty(1000);
+
+    mavlink_message_t itemMsg;
+    ASSERT_TRUE(popTxMessage(&itemMsg));
+    ASSERT_EQ(itemMsg.msgid, MAVLINK_MSG_ID_MISSION_ITEM_INT);
+
+    mavlink_mission_item_int_t item;
+    mavlink_msg_mission_item_int_decode(&itemMsg, &item);
+    EXPECT_EQ(item.seq, 1);
+    EXPECT_EQ(item.command, MAV_CMD_NAV_LAND);
+
+    resetSerialBuffers();
+    mavlink_msg_mission_request_int_pack(
+        42, 200, &requestMsg,
+        1, testTargetComponent, 0, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&requestMsg);
+    handleMavlinkUntilRxEmpty(1000);
+
+    ASSERT_TRUE(popTxMessage(&itemMsg));
+    ASSERT_EQ(itemMsg.msgid, MAVLINK_MSG_ID_MISSION_ITEM_INT);
+    mavlink_msg_mission_item_int_decode(&itemMsg, &item);
+    EXPECT_EQ(item.seq, 0);
+    EXPECT_EQ(item.command, MAV_CMD_NAV_WAYPOINT);
+}
+
+TEST(MavlinkTelemetryTest, MissionCurrentReportsLoadedMission)
+{
+    initMavlinkTestState();
+    waypointCount = 2;
+
+    handleMAVLinkTelemetry(1000000);
+
+    mavlink_message_t currentMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
+
+    mavlink_mission_current_t current;
+    mavlink_msg_mission_current_decode(&currentMsg, &current);
+    EXPECT_EQ(current.seq, 0);
+    EXPECT_EQ(current.total, 2);
+    EXPECT_EQ(current.mission_state, MISSION_STATE_NOT_STARTED);
+    EXPECT_EQ(current.mission_mode, 2);
+}
+
+TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReengage)
+{
+    initMavlinkTestState();
+    waypointCount = 2;
+    flightModeFlags = NAV_WP_MODE;
+
+    handleMAVLinkTelemetry(1000000);
+
+    mavlink_message_t currentMsg;
+    mavlink_mission_current_t current;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
+    mavlink_msg_mission_current_decode(&currentMsg, &current);
+    EXPECT_EQ(current.mission_state, MISSION_STATE_ACTIVE);
+
+    // Final item reached; the FSM parks in WAYPOINT_FINISHED which still maps
+    // to NAV_WP_MODE - the landed vehicle must report COMPLETE, not ACTIVE.
+    posControl.wpReachedSeq = 1;
+    posControl.wpReachedNotificationPending = true;
+    resetSerialBuffers();
+    handleMAVLinkTelemetry(2000000);
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
+    mavlink_msg_mission_current_decode(&currentMsg, &current);
+    EXPECT_EQ(current.mission_state, MISSION_STATE_COMPLETE);
+
+    // Leaving WP mode keeps COMPLETE.
+    flightModeFlags = 0;
+    resetSerialBuffers();
+    handleMAVLinkTelemetry(3000000);
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
+    mavlink_msg_mission_current_decode(&currentMsg, &current);
+    EXPECT_EQ(current.mission_state, MISSION_STATE_COMPLETE);
+
+    // Re-engaging WP mode is a new run: stale COMPLETE must clear.
+    flightModeFlags = NAV_WP_MODE;
+    resetSerialBuffers();
+    handleMAVLinkTelemetry(4000000);
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
+    mavlink_msg_mission_current_decode(&currentMsg, &current);
+    EXPECT_EQ(current.mission_state, MISSION_STATE_ACTIVE);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemReachedSurvivesPortlessCycle)
+{
+    initMavlinkTestState();
+    waypointCount = 2;
+    posControl.wpReachedSeq = 1;
+    posControl.wpReachedNotificationPending = true;
+
+    // No active port: the latch must not be consumed and discarded (a shared
+    // port closing on disarm right after landing would otherwise eat the
+    // final item's notification).
+    mavlinkRuntimeFreePorts();
+    handleMAVLinkTelemetry(1000);
+    EXPECT_TRUE(posControl.wpReachedNotificationPending);
+
+    checkMAVLinkTelemetryState();
+    resetSerialBuffers();
+    handleMAVLinkTelemetry(2000);
+
+    mavlink_message_t reachedMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg));
+
+    mavlink_mission_item_reached_t reached;
+    mavlink_msg_mission_item_reached_decode(&reachedMsg, &reached);
+    EXPECT_EQ(reached.seq, 1);
+}
+
+TEST(MavlinkTelemetryTest, MissionClearAllDeniedForNonOwningSender)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+
+    mavlink_message_t reqMsg;
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &reqMsg));
+    resetSerialBuffers();
+
+    // A different GCS may not cancel another partner's transfer.
+    mavlink_msg_mission_clear_all_pack(
+        43, 200, &msg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(2000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+    EXPECT_EQ(ack.type, MAV_MISSION_DENIED);
+    EXPECT_EQ(resetWaypointCalls, 0);
+
+    // The owning sender's transfer is still alive: item 0 is accepted and
+    // item 1 requested.
+    resetSerialBuffers();
+    mavlink_msg_mission_item_int_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+        MAV_CMD_NAV_WAYPOINT, 0, 1,
+        0, 0, 0, 0,
+        375000000, -1222500000, 12.3f,
+        MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(3000);
+
+    ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &reqMsg));
+    mavlink_mission_request_int_t req;
+    mavlink_msg_mission_request_int_decode(&reqMsg, &req);
+    EXPECT_EQ(req.seq, 1);
+}
+
+TEST(MavlinkTelemetryTest, MissionClearAllFromOwnerCancelsOwnTransfer)
+{
+    initMavlinkTestState();
+
+    mavlink_message_t msg;
+    mavlink_msg_mission_count_pack(
+        42, 200, &msg,
+        1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0);
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(1000);
+    resetSerialBuffers();
+
+    mavlink_msg_mission_clear_all_pack(
+        42, 200, &msg,
+        1, testTargetComponent, MAV_MISSION_TYPE_MISSION);
+    pushRxMessage(&msg);
+    handleMAVLinkTelemetry(2000);
+
+    mavlink_message_t ackMsg;
+    ASSERT_TRUE(popTxMessage(&ackMsg));
+    ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+    mavlink_mission_ack_t ack;
+    mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+    EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+    EXPECT_EQ(resetWaypointCalls, 1);
+}
 
 TEST(MavlinkTelemetryTest, ArmingDisableChangeSendsStatusTextOnce)
 {
@@ -2066,6 +2928,17 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3
     return altitudeTargetSetResult;
 }
 
+bool navigationConsumeWaypointReached(uint16_t *seq)
+{
+    if (!posControl.wpReachedNotificationPending) {
+        return false;
+    }
+
+    *seq = posControl.wpReachedSeq;
+    posControl.wpReachedNotificationPending = false;
+    return true;
+}
+
 navigationFSMStateFlags_t navGetCurrentStateFlags(void)
 {
     return (navigationFSMStateFlags_t)0;