From 1209520f2bd80b24ea0b678932c2061c5e2b3c9d Mon Sep 17 00:00:00 2001 From: raphaelhunziker1202-stack <250872901+raphaelhunziker1202-stack@users.noreply.github.com> Date: Tue, 1 Sep 2026 19:28:58 +0200 Subject: [PATCH] MAVLink: allow mission upload and clear in flight when the mission is not being executed Aligns the MAVLink mission path with the MSP policy introduced in #10273, where setWaypoint() accepts mission uploads while armed as long as the WP mission is not actively being flown. The MAVLink handlers denied every mission transfer and clear outright while armed. Changes: - New mavlinkMissionEditBlocked() gate used by MISSION_COUNT, MISSION_CLEAR_ALL, MISSION_ITEM and MISSION_ITEM_INT: edits are refused while armed AND (WP mode active OR the mission's own RTH leg is running OR the on-the-fly mission planner is active). The RTH-leg term protects the land/loiter decision at home, which reads the live list; the planner term prevents two writers on the same list. The ARMED term keeps the disarmed path provably unchanged. - When the refused sender owns the receiving transfer, the transfer is aborted via mavlinkAbortMissionUpload(MAV_MISSION_DENIED) so the retry engine stops soliciting items from a partner that was just denied. - Guided fly-to-here / altitude-target items are now dispatched on the item itself (NAV_WAYPOINT with current == 2 or 3) instead of on transfer state, so a guided click can never be absorbed into a running upload. - mavlinkCommitMissionUpload() / mavlinkClearPersistedMission(): while armed the commit or clear applies to RAM only and skips persistence - saveNonVolatileWaypointList() refuses to run while armed, and a flash write mid-flight would stall the main loop. The uploaded mission survives disarm but not a reboot; persisting after landing remains the GCS's responsibility. An in-flight upload also collapses a loaded multi-mission set to the uploaded mission for the rest of the session. - mavlinkResolveUploadedMissionJumps(): armed commits now enforce the same JUMP rules as the arm-time validation they bypass (no JUMP as first item, no self/adjacent targets, sane repeat count, geo-referenced target). Ground uploads are left to the arm-time check. - navigation.c: setWaypoint()'s post-upload clamp of activeWaypointIndex uses >= instead of > (the index is 0-based, so index == waypointCount is already out of range); new public isWpMissionPlannerActive() accessor. - Unit tests: the old MissionCountWhileArmedIsRejected asserts the new policy as MissionCountWhileArmedStartsTransfer; new tests cover the WP-mode and mission-RTH rejections, RAM-only armed commit and clear (persist not called), and the preserved clear rejection during WP mode. The staged upload buffer with atomic commit and snapshot rollback means an in-flight upload never exposes a partially written list to the navigation state machine within a main-loop tick. Note the deliberate MSP-parity semantics carried over from #10273: with nav_wp_mission_restart = RESUME, replacing the mission mid-flight keeps the waypoint index when the new mission is at least as long as the index. --- src/main/mavlink/mavlink_mission.c | 108 ++++++++++++++++-- src/main/navigation/navigation.c | 10 +- src/main/navigation/navigation.h | 1 + src/test/unit/mavlink_unittest.cc | 171 +++++++++++++++++++++++++---- 4 files changed, 255 insertions(+), 35 deletions(-) diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c index 08141b4f086..372a57e3e15 100644 --- a/src/main/mavlink/mavlink_mission.c +++ b/src/main/mavlink/mavlink_mission.c @@ -89,6 +89,19 @@ static bool mavlinkMissionTargetIsLocal(uint8_t targetSystem, uint8_t targetComp (targetComponent == 0 || targetComponent == mavComponentId); } +// A mission edit (upload or clear) is refused while the WP mission is being +// executed: WP mode active, the mission's own RTH leg running (the land/loiter +// decision at home reads the live list), or the on-the-fly mission planner +// writing the same list. This matches the MSP policy from #10273 combined +// with the updateWpMissionPlanner() guard. The ARMED term makes the disarmed +// path provably unchanged, including the short window right after disarming +// before the nav FSM drops out of WP mode. +static bool mavlinkMissionEditBlocked(void) +{ + return ARMING_FLAG(ARMED) && + (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive() || isWpMissionPlannerActive()); +} + static bool mavlinkMissionSenderOwnsTransfer(void) { return mavlinkContext.recvMsg.sysid == mavMissionTransfer.partnerSystem && @@ -194,7 +207,11 @@ static bool mavlinkClearPersistedMission(void) resetWaypointList(); mavlinkContext.missionCompleted = false; - if (mavlinkPersistMission()) { + // While armed the clear applies to RAM only, mirroring the MSP policy: + // saveNonVolatileWaypointList() refuses to run while armed, and a flash + // write mid-flight would stall the main loop anyway. The persisted + // mission is left untouched until the next clear or upload on the ground. + if (ARMING_FLAG(ARMED) || mavlinkPersistMission()) { return true; } @@ -400,6 +417,26 @@ static bool mavlinkResolveUploadedMissionJumps(void) return false; } + // For a mission committed in flight, enforce the same JUMP rules as + // the arm-time validation in navigationIsBlockingArming(), which an + // in-flight upload bypasses entirely: a JUMP cannot be the first + // mission item, cannot target itself or an immediately adjacent + // item, must have a sane repeat count and must target a + // geo-referenced item. Ground uploads are left to the arm-time + // check, keeping disarmed behaviour unchanged. + if (ARMING_FLAG(ARMED)) { + const int targetIndex = targetWaypointNumber - 1; + const navWaypoint_t *target = &mavlinkMissionUploadWaypoints[targetIndex]; + if (i == 0 || + wp->p2 < -1 || + (targetIndex >= (int)i - 1 && targetIndex <= (int)i + 1) || + !(target->action == NAV_WP_ACTION_WAYPOINT || + target->action == NAV_WP_ACTION_HOLD_TIME || + target->action == NAV_WP_ACTION_LAND)) { + return false; + } + } + wp->p1 = targetWaypointNumber; } @@ -427,7 +464,11 @@ static bool mavlinkCommitMissionUpload(void) setWaypoint(i + 1, &mavlinkMissionUploadWaypoints[i]); } - if (!isWaypointListValid() || !mavlinkPersistMission()) { + // While armed the uploaded mission lives in RAM only, mirroring the MSP + // in-flight upload policy (see #10273): saveNonVolatileWaypointList() + // refuses to run while armed, and a flash write mid-flight would stall + // the main loop anyway. On the ground the mission is persisted as before. + if (!isWaypointListValid() || (!ARMING_FLAG(ARMED) && !mavlinkPersistMission())) { mavlinkRestoreMission(&previousMission); return false; } @@ -950,8 +991,16 @@ bool mavlinkHandleIncomingMissionClearAll(void) mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED); return true; } - if (ARMING_FLAG(ARMED)) { - mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + // Clearing is allowed while merely armed (RAM only, see + // mavlinkClearPersistedMission), but not while the WP mission is being + // executed. If the refused sender owns a receiving transfer, abort it so + // the retry engine stops soliciting items from a partner we just denied. + if (mavlinkMissionEditBlocked()) { + if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) { + mavlinkAbortMissionUpload(MAV_MISSION_DENIED); + } else { + mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + } return true; } if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) { @@ -983,8 +1032,17 @@ bool mavlinkHandleIncomingMissionCount(void) } return true; } - if (ARMING_FLAG(ARMED)) { - mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + // Starting an upload is allowed while merely armed (the commit stays in + // RAM, see mavlinkCommitMissionUpload), but not while the WP mission is + // being executed. If the refused sender owns a receiving transfer (e.g. + // a retry after WP mode engaged mid-upload), abort it so the retry + // engine stops soliciting items from a partner we just denied. + if (mavlinkMissionEditBlocked()) { + if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) { + mavlinkAbortMissionUpload(MAV_MISSION_DENIED); + } else { + mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + } return true; } if (mavMissionTransfer.state != MAVLINK_MISSION_TRANSFER_IDLE && !mavlinkMissionSenderOwnsTransfer()) { @@ -1049,14 +1107,27 @@ bool mavlinkHandleIncomingMissionItem(void) } if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { + // Guided fly-to-here (current == 2) and altitude-target (current == 3) + // items are identified by the item itself - no legitimate upload item + // carries these current values - so a guided click is never absorbed + // into a running upload transfer. + if (msg.command == MAV_CMD_NAV_WAYPOINT && (msg.current == 2 || msg.current == 3)) { return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT, (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z); } - mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR); - return true; + // Upload items are accepted while merely armed (in-flight upload, + // matching the MSP policy from #10273), but not while the WP mission + // is being executed. + if (mavlinkMissionEditBlocked()) { + if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) { + mavlinkAbortMissionUpload(MAV_MISSION_DENIED); + } else { + mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + } + return true; + } } return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq, @@ -1252,14 +1323,27 @@ bool mavlinkHandleIncomingMissionItemInt(void) } if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { + // Guided fly-to-here (current == 2) and altitude-target (current == 3) + // items are identified by the item itself - no legitimate upload item + // carries these current values - so a guided click is never absorbed + // into a running upload transfer. + if (msg.command == MAV_CMD_NAV_WAYPOINT && (msg.current == 2 || msg.current == 3)) { return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, msg.x, msg.y, msg.z); } - mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR); - return true; + // Upload items are accepted while merely armed (in-flight upload, + // matching the MSP policy from #10273), but not while the WP mission + // is being executed. + if (mavlinkMissionEditBlocked()) { + if (mavMissionTransfer.state == MAVLINK_MISSION_TRANSFER_RECEIVING && mavlinkMissionSenderOwnsTransfer()) { + mavlinkAbortMissionUpload(MAV_MISSION_DENIED); + } else { + mavlinkSendMissionAckTo(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_DENIED); + } + return true; + } } return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.current, msg.autocontinue, msg.seq, diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7d7cf1629fb..01240ce821a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -5488,8 +5488,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount; if (posControl.waypointListValid) { nonGeoWaypointCount = 0; - // If active WP index is bigger than total mission WP number, reset active WP index (Mission Upload mid flight with interrupted mission) if RESUME is enabled - if (posControl.activeWaypointIndex > posControl.waypointCount) { + // If active WP index is beyond the new mission, reset active WP index (Mission Upload mid flight with interrupted mission) if RESUME is enabled. + // activeWaypointIndex is 0-based, so an index equal to waypointCount is already out of range. + if (posControl.activeWaypointIndex >= posControl.waypointCount) { posControl.activeWaypointIndex = 0; } } @@ -5517,6 +5518,11 @@ bool isWaypointListValid(void) return posControl.waypointListValid; } +bool isWpMissionPlannerActive(void) +{ + return posControl.flags.wpMissionPlannerActive; +} + int getWaypointCount(void) { uint8_t waypointCount = posControl.waypointCount; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d0713d401c5..29c660351ab 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -848,6 +848,7 @@ bool navCanSetHome(void); */ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); +bool isWpMissionPlannerActive(void); #ifdef USE_AUTO_TRANSITION navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void); #endif diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 537b17f45c4..e7530f9caf4 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -131,6 +131,8 @@ static int setWaypointCalls; static int resetWaypointCalls; static int saveWaypointCalls; static bool saveWaypointResult; +static bool testWaypointMissionRTHActive; +static bool testWpMissionPlannerActive; static int mavlinkRxHandleCalls; static bool gcsValid; static int waypointCount; @@ -330,6 +332,8 @@ static void initMavlinkTestState(void) resetWaypointCalls = 0; saveWaypointCalls = 0; saveWaypointResult = true; + testWaypointMissionRTHActive = false; + testWpMissionPlannerActive = false; mavlinkRxHandleCalls = 0; mspCommandCallCount = 0; testReplyPayloadLength = 300; @@ -1548,7 +1552,9 @@ TEST(MavlinkTelemetryTest, MissionCountZeroRestoresPreviousMissionOnPersistFailu EXPECT_EQ(waypointStore[0].flag, NAV_WP_FLAG_LAST); } -TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected) +// In-flight upload: while merely armed (WP mode not active) an upload +// transfer starts normally, matching the MSP policy from #10273. +TEST(MavlinkTelemetryTest, MissionCountWhileArmedStartsTransfer) { initMavlinkTestState(); ENABLE_ARMING_FLAG(ARMED); @@ -1561,28 +1567,141 @@ TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected) pushRxMessage(&msg); handleMAVLinkTelemetry(1000); - mavlink_status_t status; - memset(&status, 0, sizeof(status)); - mavlink_message_t outMsg; - bool sawAck = false; - bool sawRequest = false; + mavlink_message_t requestMsg; + EXPECT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &requestMsg)); + mavlink_message_t ackMsg; + EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ACK, &ackMsg)); +} - 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; - } - } - } +// While the WP mission is actively being flown, an upload is rejected. +TEST(MavlinkTelemetryTest, MissionCountWhileWpModeActiveIsRejected) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + flightModeFlags = NAV_WP_MODE; - EXPECT_TRUE(sawAck); - EXPECT_FALSE(sawRequest); + 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 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_DENIED); + mavlink_message_t requestMsg; + EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_REQUEST_INT, &requestMsg)); +} + +// An in-flight upload commits to RAM but must not touch the EEPROM: +// saveNonVolatileWaypointList() refuses while armed, and a flash write +// mid-flight would stall the main loop. +TEST(MavlinkTelemetryTest, MissionUploadWhileArmedCommitsToRamWithoutPersist) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + 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, 1, 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_ACCEPTED); + EXPECT_EQ(waypointCount, 1); + EXPECT_EQ(waypointStore[0].lat, 375000000); + EXPECT_EQ(waypointStore[0].lon, -1222500000); + EXPECT_EQ(waypointStore[0].flag, NAV_WP_FLAG_LAST); + EXPECT_EQ(saveWaypointCalls, 0); +} + +// An in-flight clear applies to RAM only, without persisting. +TEST(MavlinkTelemetryTest, MissionClearAllWhileArmedClearsRamWithoutPersist) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + 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(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(resetWaypointCalls, 1); + EXPECT_EQ(saveWaypointCalls, 0); +} + +// The mission's own RTH leg still reads the live list (land/loiter decision +// at home), so editing stays forbidden during it even though NAV_WP_MODE is +// no longer asserted. +TEST(MavlinkTelemetryTest, MissionCountDuringMissionRthLegIsRejected) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + testWaypointMissionRTHActive = true; + + 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 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_DENIED); +} + +// Clearing the mission that is actively being flown stays forbidden. +TEST(MavlinkTelemetryTest, MissionClearAllWhileWpModeActiveIsRejected) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + flightModeFlags = NAV_WP_MODE; + + 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(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_DENIED); + EXPECT_EQ(resetWaypointCalls, 0); } TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted) @@ -3896,6 +4015,16 @@ bool saveNonVolatileWaypointList(void) return saveWaypointResult; } +bool isWaypointMissionRTHActive(void) +{ + return testWaypointMissionRTHActive; +} + +bool isWpMissionPlannerActive(void) +{ + return testWpMissionPlannerActive; +} + void resetWaypointList(void) { resetWaypointCalls++;