Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

mavlink: optimize message forwarding

Browse Source
This commit is contained in:
Oleg
2021-07-09 12:36:06 +03:00
committed by Daniel Agar
parent 64448e17ca
commit 76dedfec5d
1 changed files with 28 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+11 -16
View File
@@ -382,10 +382,6 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self)
void void
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{ {
LockGuard lg{mavlink_module_mutex};
for (Mavlink *inst : mavlink_module_instances) {
if (inst && (inst != self)) {
const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid); const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
int target_system_id = 0; int target_system_id = 0;
@@ -395,30 +391,29 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
if (meta) { if (meta) {
// Extract target system and target component if set // Extract target system and target component if set
if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
if (meta->target_system_ofs < msg->len) {
target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs]; target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
} }
}
if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
if (meta->target_component_ofs < msg->len) {
target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs]; target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
} }
} }
}
// If it's a message only for us, we keep it, otherwise, we forward it. // If it's a message only for us, we keep it, otherwise, we forward it.
const bool targeted_only_at_us = if (target_system_id == self->get_system_id() && target_component_id == self->get_component_id()) {
(target_system_id == self->get_system_id() && return;
target_component_id == self->get_component_id()); }
// We don't forward heartbeats unless it's specifically enabled. // We don't forward heartbeats unless it's specifically enabled.
const bool heartbeat_check_ok = if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT && !self->forward_heartbeats_enabled()) {
(msg->msgid != MAVLINK_MSG_ID_HEARTBEAT || self->forward_heartbeats_enabled()); return;
if (!targeted_only_at_us && heartbeat_check_ok) {
inst->pass_message(msg);
} }
LockGuard lg{mavlink_module_mutex};
for (Mavlink *inst : mavlink_module_instances) {
if (inst && (inst != self)) {
inst->pass_message(msg);
} }
} }
} }