diff options
Diffstat (limited to 'src/modules/mavlink')
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 2 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 6 |
2 files changed, 6 insertions, 2 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6bd0c7bce..899a0d4a6 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); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4d7b35f03..06153fdac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -44,7 +44,6 @@ #include <nuttx/config.h> #include <unistd.h> #include <pthread.h> -#include <stdio.h> #include <math.h> #include <stdbool.h> #include <fcntl.h> @@ -1459,6 +1458,11 @@ 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); |