diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol
index 95c9ce261ec..822d15746de 100644
--- a/posix-configs/SITL/init/ekf2/standard_vtol
+++ b/posix-configs/SITL/init/ekf2/standard_vtol
@@ -79,8 +79,8 @@ mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
 mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
-mavlink start -u 14556 -r 2000000
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540
+mavlink start -u 14556 -r 2000000 -f
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -f
 mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14557
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index f11e974cada..c0acd87bc09 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -520,35 +520,27 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 {
-
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
+			const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
 
-			// Onboard links should pass all messages to the ground
-			// which originate from the same system.
-			// Offboard links should pass all messages from any system.
-			if ((self->_mode == MAVLINK_MODE_NORMAL)
-			    || ((self->_mode != MAVLINK_MODE_NORMAL) && msg->sysid != mavlink_system.sysid)) {
+			// Extract target system and target component if set
+			unsigned target_system_id = (meta->target_system_ofs != 0) ? ((uint8_t *)msg)[meta->target_system_ofs] : 0;
+			unsigned target_component_id = (meta->target_component_ofs != 0) ? ((uint8_t *)msg)[meta->target_component_ofs] : 233;
 
-				const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
+			// Broadcast or addressing this system and not trying to talk
+			// to the autopilot component -> pass on to other components
+			if ((target_system_id == 0 || target_system_id == self->get_system_id())
+				&& (target_component_id == 0 || target_component_id != self->get_component_id())) {
 
-				// Extract target system and target component if set
-				unsigned target_system_id = (meta->target_system_ofs != 0) ? ((uint8_t *)msg)[meta->target_system_ofs] : 0;
-				unsigned target_component_id = (meta->target_component_ofs != 0) ? ((uint8_t *)msg)[meta->target_component_ofs] : 0;
-
-				// Broadcast or addressing this system and not trying to talk
-				// to the autopilot component -> pass on to other components
-				if ((target_system_id == 0 || target_system_id == self->get_system_id())
-				    && (target_component_id != self->get_component_id())) {
-
-					inst->pass_message(msg);
-				}
+				inst->pass_message(msg);
 			}
 		}
 	}
 }
 
+
 int
 Mavlink::get_uart_fd(unsigned index)
 {