diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index fb9f65cf50..b997198211 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -392,7 +392,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 
 			/* if not in normal mode, we are an onboard link
 			 * onboard links should only pass on messages from the same system ID */
-			if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
+			if ( self->_mode != MAVLINK_MODE_ONBOARD || msg->sysid == mavlink_system.sysid ) {
 				inst->pass_message(msg);
 			}
 		}