Skip to content
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

Improve MAVLink behavior with flow control capable links #10222

Merged
merged 3 commits into from
Jul 14, 2024
Merged

Improve MAVLink behavior with flow control capable links #10222

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
442aaad
Quick and dirty attemp at making things play along
mmosca Jul 9, 2024
ff94b77
Improve MAVLink behavior when flow control information is available
MUSTARDTIGERFPV Jul 11, 2024
92d6315
Improve MAVLink behavior with half-duplex links, update default SRs
MUSTARDTIGERFPV Jul 11, 2024
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
71 changes: 50 additions & 21 deletions src/main/telemetry/mavlink.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,14 +162,16 @@ static serialPortConfig_t *portConfig;

static bool mavlinkTelemetryEnabled = false;
static portSharing_e mavlinkPortSharing;
static uint8_t txbuff_free = 100;
static bool txbuff_valid = false;

/* MAVLink datastream rates in Hz */
static uint8_t mavRates[] = {
[MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz
[MAV_DATA_STREAM_RC_CHANNELS] = 5, // 5Hz
MUSTARDTIGERFPV marked this conversation as resolved.
Show resolved Hide resolved
[MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz
[MAV_DATA_STREAM_POSITION] = 2, // 2Hz
[MAV_DATA_STREAM_EXTRA1] = 10, // 10Hz
[MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz
[MAV_DATA_STREAM_EXTRA1] = 3, // 3Hz
[MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz, HEARTBEATs are important
[MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz
};

Expand Down Expand Up @@ -1111,6 +1113,27 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
return true;
}

static bool handleIncoming_PARAM_REQUEST_LIST(void) {
mavlink_param_request_list_t msg;
mavlink_msg_param_request_list_decode(&mavRecvMsg, &msg);

// Respond that we don't have any parameters to force Mission Planner to give up quickly
if (msg.target_system == mavSystemId) {
// mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index);
mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0);
mavlinkSendMessage();
}
return true;
}

static bool handleIncoming_RADIO_STATUS(void) {
mavlink_radio_status_t msg;
mavlink_msg_radio_status_decode(&mavRecvMsg, &msg);
txbuff_valid = true;
txbuff_free = msg.txbuf;
return true;
}

#ifdef USE_ADSB
static bool handleIncoming_ADSB_VEHICLE(void) {
mavlink_adsb_vehicle_t msg;
Expand Down Expand Up @@ -1155,6 +1178,7 @@ static bool handleIncoming_ADSB_VEHICLE(void) {
}
#endif

// Returns whether a message was processed
static bool processMAVLinkIncomingTelemetry(void)
{
while (serialRxBytesWaiting(mavlinkPort) > 0) {
Expand All @@ -1165,6 +1189,8 @@ static bool processMAVLinkIncomingTelemetry(void)
switch (mavRecvMsg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
return handleIncoming_PARAM_REQUEST_LIST();
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
return handleIncoming_MISSION_CLEAR_ALL();
case MAVLINK_MSG_ID_MISSION_COUNT:
Expand All @@ -1176,11 +1202,17 @@ static bool processMAVLinkIncomingTelemetry(void)
case MAVLINK_MSG_ID_MISSION_REQUEST:
return handleIncoming_MISSION_REQUEST();
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
return handleIncoming_RC_CHANNELS_OVERRIDE();
handleIncoming_RC_CHANNELS_OVERRIDE();
MUSTARDTIGERFPV marked this conversation as resolved.
Show resolved Hide resolved
// Don't set that we handled a message, otherwise RC channel packets will block telemetry messages
return false;
#ifdef USE_ADSB
case MAVLINK_MSG_ID_ADSB_VEHICLE:
return handleIncoming_ADSB_VEHICLE();
#endif
case MAVLINK_MSG_ID_RADIO_STATUS:
handleIncoming_RADIO_STATUS();
// Don't set that we handled a message, otherwise radio status packets will block telemetry messages
return false;
default:
return false;
}
Expand All @@ -1192,8 +1224,6 @@ static bool processMAVLinkIncomingTelemetry(void)

void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
{
static bool incomingRequestServed;

if (!mavlinkTelemetryEnabled) {
return;
}
Expand All @@ -1202,24 +1232,23 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
return;
}

// If we did serve data on incoming request - skip next scheduled messages batch to avoid link clogging
if (processMAVLinkIncomingTelemetry()) {
incomingRequestServed = true;
// Process incoming MAVLink - ignore the return indicating whether or not a message was processed
// Very few telemetry links are dynamic uplink/downlink split so uplink telemetry shouldn't reduce downlink bandwidth availability
processMAVLinkIncomingTelemetry();

// Determine whether to send telemetry back
bool shouldSendTelemetry = false;
if (txbuff_valid) {
// Use flow control if available
shouldSendTelemetry = txbuff_free >= 33;
} else {
// If not, use blind frame pacing
shouldSendTelemetry = (currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY;
}

if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
// Only process scheduled data if we didn't serve any incoming request this cycle
if (!incomingRequestServed ||
(
(rxConfig()->receiverType == RX_TYPE_SERIAL) &&
(rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
!tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
)
) {
processMAVLinkTelemetry(currentTimeUs);
}
if (shouldSendTelemetry) {
MUSTARDTIGERFPV marked this conversation as resolved.
Show resolved Hide resolved
processMAVLinkTelemetry(currentTimeUs);
lastMavlinkMessage = currentTimeUs;
incomingRequestServed = false;
}
}

Expand Down
Loading