diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 899a0d4a6e..6bd0c7bcea 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_ONBOARD || msg->sysid == mavlink_system.sysid ) {
+			if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
 				inst->pass_message(msg);
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 06153fdac7..4d7b35f039 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -44,6 +44,7 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <pthread.h>
+#include <stdio.h>
 #include <math.h>
 #include <stdbool.h>
 #include <fcntl.h>
@@ -1458,11 +1459,6 @@ MavlinkReceiver::receive_thread(void *arg)
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
 				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
-
-					printf("\n");
-					printf("HANDLE MESSAGE\n");
-					printf("MSGID:%i\n",msg.msgid);
-
 					/* handle generic messages and commands */
 					handle_message(&msg);