From 440449b85fe05c6a1089f8cc54db70db131d3299 Mon Sep 17 00:00:00 2001 From: Oleg Evseev Date: Mon, 12 Jul 2021 19:01:17 +0300 Subject: [PATCH] mavlink: use explicit routing for message forwarding 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) --- src/modules/mavlink/mavlink_main.cpp | 35 +++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 26 ++++++++++++++++-- src/modules/mavlink/mavlink_receiver.h | 4 ++- 4 files changed, 53 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 551d5f30e2..74d9b99f09 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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 * diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e6d4e92e15..e22d6b5ce9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -162,6 +162,8 @@ public: static bool serial_instance_exists(const char *device_name, Mavlink *self); + static bool component_was_seen(int system_id, int component_id, Mavlink *self = nullptr); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); int get_uart_fd() const { return _uart_fd; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4261617497..7324530d49 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -462,8 +462,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; if (!target_ok) { - if (!_mavlink->get_forwarding_on()) { - // Reject alien commands only if there is no forwarding enabled + // Reject alien commands only if there is no forwarding or we've never seen target component before + if (!_mavlink->get_forwarding_on() + || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED); } @@ -3146,6 +3147,23 @@ MavlinkReceiver::run() } } +bool MavlinkReceiver::component_was_seen(int system_id, int component_id) +{ + // For system broadcast messages return true if at least one component was seen before + if (system_id == 0) { + return _component_states_count > 0; + } + + for (unsigned i = 0; i < _component_states_count; ++i) { + if (_component_states[i].system_id == system_id + && (component_id == 0 || _component_states[i].component_id == component_id)) { + return true; + } + } + + return false; +} + void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) { const bool component_states_has_still_space = [this, &message]() { @@ -3183,6 +3201,8 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; _component_states[i].last_sequence = message.seq; + _component_states_count = i + 1; + // Also update overall stats ++_total_received_counter; @@ -3204,7 +3224,7 @@ void MavlinkReceiver::print_detailed_rx_stats() const const uint32_t now_ms = hrt_absolute_time() / 1000; // TODO: add mutex around shared data. - for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) { + for (unsigned i = 0; i < _component_states_count; ++i) { if (_component_states[i].received_messages > 0) { printf("\t received from sysid: %" PRIu8 " compid: %" PRIu8 ": %" PRIu32 ", lost: %" PRIu32 ", last %" PRIu32 " ms ago\n", diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 2f9da22783..9f898e4711 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -124,6 +124,7 @@ public: void start(); void stop(); + bool component_was_seen(int system_id, int component_id); void print_detailed_rx_stats() const; private: @@ -241,7 +242,7 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - static constexpr int MAX_REMOTE_COMPONENTS{8}; + static constexpr unsigned MAX_REMOTE_COMPONENTS{8}; struct ComponentState { uint32_t last_time_received_ms{0}; uint32_t received_messages{0}; @@ -251,6 +252,7 @@ private: uint8_t last_sequence{0}; }; ComponentState _component_states[MAX_REMOTE_COMPONENTS] {}; + unsigned _component_states_count{0}; bool _warned_component_states_full_once{false}; uint64_t _total_received_counter{0}; ///< The total number of successfully received messages