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