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++;