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

@@ -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 *

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; }

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",

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