From 0fb1751a6a0a28a36e92e1bb6bcf17c207d0b12b Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 25 Sep 2026 13:58:22 -0400 Subject: [PATCH] mavlink: make MISSION_CURRENT a per-port periodic message MISSION_CURRENT was sent by the mission module on a hardcoded 1000 ms timer to every active port. SET_MESSAGE_INTERVAL could not change it, and high-latency ports received it despite only expecting HIGH_LATENCY2. Send it from the per-port periodic scheduler instead, with a fixed 1000 ms default on every port that no data stream rate affects. This makes it configurable via SET/GET_MESSAGE_INTERVAL and REQUEST_MESSAGE, and the high-latency early return now covers it. --- src/main/mavlink/mavlink_internal.h | 1 - src/main/mavlink/mavlink_mission.c | 17 +---- src/main/mavlink/mavlink_mission.h | 1 + src/main/mavlink/mavlink_streams.c | 25 ++++++- src/main/mavlink/mavlink_types.h | 1 + src/test/unit/mavlink_unittest.cc | 111 ++++++++++++++++++++++++++-- 6 files changed, 135 insertions(+), 21 deletions(-) diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h index 513a570f9a1..33d183c375f 100644 --- a/src/main/mavlink/mavlink_internal.h +++ b/src/main/mavlink/mavlink_internal.h @@ -113,7 +113,6 @@ typedef struct mavlinkContext_s { uint8_t componentId; uint8_t recvPortIndex; mavlinkMissionTransfer_t missionTransfer; - timeMs_t lastMissionCurrentMs; bool missionCompleted; bool lastWpModeActive; uint32_t lastArmingDisableFlags; diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c index fb6d179963a..bab5b119065 100644 --- a/src/main/mavlink/mavlink_mission.c +++ b/src/main/mavlink/mavlink_mission.c @@ -471,16 +471,10 @@ void mavlinkMissionUpdate(timeMs_t currentTimeMs) 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; - } - +void mavlinkSendMissionCurrent(void) +{ 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; @@ -492,7 +486,7 @@ void mavlinkMissionUpdate(timeMs_t currentTimeMs) // 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. + // missionCompleted on the WP-mode rising edge in mavlinkMissionUpdate(). missionState = MISSION_STATE_COMPLETE; } else if (active) { missionState = MISSION_STATE_ACTIVE; @@ -500,7 +494,6 @@ void mavlinkMissionUpdate(timeMs_t currentTimeMs) missionState = MISSION_STATE_NOT_STARTED; } - mavSendMask = sendMask; mavlink_msg_mission_current_pack( mavlinkGetCommonConfig()->sysid, MAV_COMP_ID_AUTOPILOT1, @@ -513,8 +506,6 @@ void mavlinkMissionUpdate(timeMs_t currentTimeMs) 0, 0); mavlinkSendMessage(); - mavSendMask = 0; - mavlinkContext.lastMissionCurrentMs = currentTimeMs; } static bool mavlinkHandleArmedGuidedMissionItem( diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h index d01d9408b34..2fb155be337 100644 --- a/src/main/mavlink/mavlink_mission.h +++ b/src/main/mavlink/mavlink_mission.h @@ -31,6 +31,7 @@ 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); +void mavlinkSendMissionCurrent(void); bool mavlinkHandleIncomingMissionClearAll(void); bool mavlinkHandleIncomingMissionCount(void); bool mavlinkHandleIncomingMissionItem(void); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c index aeea48e60e1..5fd65bea924 100644 --- a/src/main/mavlink/mavlink_streams.c +++ b/src/main/mavlink/mavlink_streams.c @@ -2,6 +2,7 @@ #include "common/time.h" +#include "mavlink/mavlink_mission.h" #include "mavlink/mavlink_modes.h" #include "mavlink/mavlink_routing.h" #include "mavlink/mavlink_runtime.h" @@ -140,6 +141,9 @@ bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMess case MAVLINK_MSG_ID_SYSTEM_TIME: *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME; return true; + case MAVLINK_MSG_ID_MISSION_CURRENT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_MISSION_CURRENT; + return true; default: return false; } @@ -195,13 +199,23 @@ static void mavlinkResetMessagesForStream(uint8_t streamNum) } } +static int32_t mavlinkDefaultIntervalUs(mavlinkPeriodicMessage_e periodicMessage, const uint8_t *streamRates) +{ + // MISSION_CURRENT belongs to no data stream: its default is fixed on every port + if (periodicMessage == MAVLINK_PERIODIC_MESSAGE_MISSION_CURRENT) { + return MAVLINK_MISSION_CURRENT_INTERVAL_MS * 1000; + } + + return mavlinkRateToIntervalUs(streamRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); +} + int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage) { if (!mavActivePort) { return -1; } - return mavlinkRateToIntervalUs(mavActivePort->mavRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); + return mavlinkDefaultIntervalUs(periodicMessage, mavActivePort->mavRates); } int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage) @@ -306,7 +320,7 @@ void configureMAVLinkStreamRates(uint8_t portIndex) const int32_t overrideUs = state->mavMessageOverrideIntervalsUs[messageIndex]; const int32_t intervalUs = overrideUs != 0 ? overrideUs - : mavlinkRateToIntervalUs(selectedRates[mavlinkPeriodicMessageBaseStream((mavlinkPeriodicMessage_e)messageIndex)]); + : mavlinkDefaultIntervalUs((mavlinkPeriodicMessage_e)messageIndex, selectedRates); state->mavMessageNextDue[messageIndex] = intervalUs > 0 ? currentTimeUs + intervalUs + 3000 * messageIndex : 0; @@ -1143,6 +1157,9 @@ bool mavlinkSendRequestedMessage(uint16_t messageId) case MAVLINK_MSG_ID_HEARTBEAT: mavlinkSendHeartbeat(); return true; + case MAVLINK_MSG_ID_MISSION_CURRENT: + mavlinkSendMissionCurrent(); + return true; case MAVLINK_MSG_ID_AUTOPILOT_VERSION: if (mavlinkGetProtocolVersion() != 1) { mavlinkSendAutopilotVersion(); @@ -1552,6 +1569,10 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) mavlinkSendSystemTime(); } + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_MISSION_CURRENT, currentTimeUs)) { + mavlinkSendMissionCurrent(); + } + if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) { mavlinkSendStatusText(); } diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h index 8fd6eb56a63..3d90f098afa 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/src/main/mavlink/mavlink_types.h @@ -63,6 +63,7 @@ typedef enum { MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, + MAVLINK_PERIODIC_MESSAGE_MISSION_CURRENT, MAVLINK_PERIODIC_MESSAGE_COUNT } mavlinkPeriodicMessage_e; diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index a22461a8fa3..6cfab91079f 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -3229,7 +3229,7 @@ TEST(MavlinkTelemetryTest, MissionCurrentReportsLoadedMission) initMavlinkTestState(); waypointCount = 2; - handleMAVLinkTelemetry(1000000); + handleMAVLinkTelemetry(1100000); mavlink_message_t currentMsg; ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, ¤tMsg)); @@ -3248,7 +3248,7 @@ TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReen waypointCount = 2; flightModeFlags = NAV_WP_MODE; - handleMAVLinkTelemetry(1000000); + handleMAVLinkTelemetry(1100000); mavlink_message_t currentMsg; mavlink_mission_current_t current; @@ -3261,7 +3261,7 @@ TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReen posControl.wpReachedSeq = 1; posControl.wpReachedNotificationPending = true; resetSerialBuffers(); - handleMAVLinkTelemetry(2000000); + handleMAVLinkTelemetry(2100000); ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, ¤tMsg)); mavlink_msg_mission_current_decode(¤tMsg, ¤t); EXPECT_EQ(current.mission_state, MISSION_STATE_COMPLETE); @@ -3269,7 +3269,7 @@ TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReen // Leaving WP mode keeps COMPLETE. flightModeFlags = 0; resetSerialBuffers(); - handleMAVLinkTelemetry(3000000); + handleMAVLinkTelemetry(3100000); ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, ¤tMsg)); mavlink_msg_mission_current_decode(¤tMsg, ¤t); EXPECT_EQ(current.mission_state, MISSION_STATE_COMPLETE); @@ -3277,12 +3277,113 @@ TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReen // Re-engaging WP mode is a new run: stale COMPLETE must clear. flightModeFlags = NAV_WP_MODE; resetSerialBuffers(); - handleMAVLinkTelemetry(4000000); + handleMAVLinkTelemetry(4100000); ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, ¤tMsg)); mavlink_msg_mission_current_decode(¤tMsg, ¤t); EXPECT_EQ(current.mission_state, MISSION_STATE_ACTIVE); } +static int countTxMessagesById(uint32_t msgid) +{ + int count = 0; + for (const mavlink_message_t &msg : parseTxMessages()) { + if (msg.msgid == msgid) { + count++; + } + } + return count; +} + +static void sendCommandLong(uint16_t command, float param1, float param2) +{ + mavlink_message_t msg; + mavlink_msg_command_long_pack( + 42, 200, &msg, + 1, testTargetComponent, + command, + 0, + param1, param2, 0, 0, 0, 0, 0); + pushRxMessage(&msg); +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalControlsMissionCurrent) +{ + initMavlinkTestState(); + waypointCount = 2; + + sendCommandLong(MAV_CMD_GET_MESSAGE_INTERVAL, (float)MAVLINK_MSG_ID_MISSION_CURRENT, 0); + handleMAVLinkTelemetry(1000); + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_MISSION_CURRENT); + EXPECT_EQ(interval.interval_us, 1000000); + + resetSerialBuffers(); + sendCommandLong(MAV_CMD_SET_MESSAGE_INTERVAL, (float)MAVLINK_MSG_ID_MISSION_CURRENT, 200000.0f); + handleMAVLinkTelemetry(2000); + mavlink_message_t ackMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_COMMAND_ACK, &ackMsg)); + EXPECT_EQ(mavlink_msg_command_ack_get_result(&ackMsg), MAV_RESULT_ACCEPTED); + + resetSerialBuffers(); + for (timeUs_t t = 100000; t <= 1100000; t += 10000) { + handleMAVLinkTelemetry(t); + } + EXPECT_GE(countTxMessagesById(MAVLINK_MSG_ID_MISSION_CURRENT), 4); + + sendCommandLong(MAV_CMD_SET_MESSAGE_INTERVAL, (float)MAVLINK_MSG_ID_MISSION_CURRENT, -1.0f); + handleMAVLinkTelemetry(1110000); + resetSerialBuffers(); + for (timeUs_t t = 1200000; t <= 4200000; t += 10000) { + handleMAVLinkTelemetry(t); + } + EXPECT_EQ(countTxMessagesById(MAVLINK_MSG_ID_MISSION_CURRENT), 0); +} + +TEST(MavlinkTelemetryTest, MissionCurrentDefaultIgnoresDataStreamRates) +{ + initMavlinkTestState(); + + const uint8_t streams[] = { MAV_DATA_STREAM_HEARTBEAT, MAV_DATA_STREAM_ALL }; + for (size_t i = 0; i < ARRAYLEN(streams); i++) { + mavlink_message_t streamMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &streamMsg, + 1, testTargetComponent, + streams[i], 5, 1); + pushRxMessage(&streamMsg); + handleMAVLinkTelemetry(1000); + + resetSerialBuffers(); + sendCommandLong(MAV_CMD_GET_MESSAGE_INTERVAL, (float)MAVLINK_MSG_ID_MISSION_CURRENT, 0); + handleMAVLinkTelemetry(2000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + EXPECT_EQ(interval.interval_us, 1000000); + } +} + +TEST(MavlinkTelemetryTest, HighLatencyPortDoesNotSendMissionCurrent) +{ + initMavlinkTestState(); + waypointCount = 2; + + sendCommandLong(MAV_CMD_CONTROL_HIGH_LATENCY, 1.0f, 0); + handleMAVLinkTelemetry(1000); + + resetSerialBuffers(); + for (timeUs_t t = 100000; t <= 6100000; t += 10000) { + handleMAVLinkTelemetry(t); + } + EXPECT_GT(countTxMessagesById(MAVLINK_MSG_ID_HIGH_LATENCY2), 0); + EXPECT_EQ(countTxMessagesById(MAVLINK_MSG_ID_MISSION_CURRENT), 0); +} + TEST(MavlinkTelemetryTest, MissionItemReachedSurvivesPortlessCycle) { initMavlinkTestState();