Skip to content

MAVLink: make MISSION_CURRENT settable via SET_MESSAGE_INTERVAL and silent on high-latency ports #12035

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
xznhj8129 wants to merge 1 commit into
base: maintenance-10.x
Choose a base branch
from
+135 −21
Open

MAVLink: make MISSION_CURRENT settable via SET_MESSAGE_INTERVAL and silent on high-latency ports #12035

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion src/main/mavlink/mavlink_internal.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
17 changes: 4 additions & 13 deletions src/main/mavlink/mavlink_mission.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -492,15 +486,14 @@ 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;
} else {
missionState = MISSION_STATE_NOT_STARTED;
}

mavSendMask = sendMask;
mavlink_msg_mission_current_pack(
mavlinkGetCommonConfig()->sysid,
MAV_COMP_ID_AUTOPILOT1,
Expand All @@ -513,8 +506,6 @@ void mavlinkMissionUpdate(timeMs_t currentTimeMs)
0,
0);
mavlinkSendMessage();
mavSendMask = 0;
mavlinkContext.lastMissionCurrentMs = currentTimeMs;
}

static bool mavlinkHandleArmedGuidedMissionItem(
Expand Down
1 change: 1 addition & 0 deletions src/main/mavlink/mavlink_mission.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
25 changes: 23 additions & 2 deletions src/main/mavlink/mavlink_streams.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
}
Expand Down
1 change: 1 addition & 0 deletions src/main/mavlink/mavlink_types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
111 changes: 106 additions & 5 deletions src/test/unit/mavlink_unittest.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, &currentMsg));
Expand All @@ -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;
Expand All @@ -3261,28 +3261,129 @@ TEST(MavlinkTelemetryTest, MissionCurrentCompletionOutranksWpModeAndClearsOnReen
posControl.wpReachedSeq = 1;
posControl.wpReachedNotificationPending = true;
resetSerialBuffers();
handleMAVLinkTelemetry(2000000);
handleMAVLinkTelemetry(2100000);
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);
handleMAVLinkTelemetry(3100000);
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);
handleMAVLinkTelemetry(4100000);
ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_CURRENT, &currentMsg));
mavlink_msg_mission_current_decode(&currentMsg, &current);
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();
Expand Down
Loading