fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: use explicit routing for message forwarding

Browse Source 
Forward message to other mavlink channels only if it is broadcast or the target component was seen on this link before (at least one message was received)
This commit is contained in:
Oleg Evseev
2021-07-21 10:39:14 +02:00
committed by Julian Oes
parent b1eb762753
commit 440449b85f
4 changed files with 53 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+25 -10
View File
@@ -379,6 +379,20 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self)
return false;
}
bool
Mavlink::component_was_seen(int system_id, int component_id, Mavlink *self)
{
LockGuard lg{mavlink_module_mutex};
for (Mavlink *inst : mavlink_module_instances) {
if (inst && (inst != self) && (inst->_receiver.component_was_seen(system_id, component_id))) {
return true;
}
}
return false;
}
void
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{
@@ -399,7 +413,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
}
}
// 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
if (target_system_id == self->get_system_id() && target_component_id == self->get_component_id()) {
return;
}
@@ -412,8 +426,11 @@ 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)) {
inst->pass_message(msg);
if (inst && (inst != self) && (inst->_forwarding_on)) {
// Pass message only if target component was seen before
if (inst->_receiver.component_was_seen(target_system_id, target_component_id)) {
inst->pass_message(msg);
}
}
}
}
@@ -1362,13 +1379,11 @@ Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
void
Mavlink::pass_message(const mavlink_message_t *msg)
{
if (_forwarding_on) {
/* size is 8 bytes plus variable payload */
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_write(msg, size);
pthread_mutex_unlock(&_message_buffer_mutex);
}
/* size is 8 bytes plus variable payload */
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_write(msg, size);
pthread_mutex_unlock(&_message_buffer_mutex);
}
MavlinkShell *