diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 3807323c2a..901fa8f057 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -44,8 +44,6 @@
 #include <uORB/uORB.h>
 #include <stdio.h>
 
-#include <systemlib/err.h>
-
 #include "mavlink_orb_subscription.h"
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 		time_topic = 0;
 	}
 
-	if (time_topic != *time) {
-
-		if (orb_copy(_topic, _fd, data)) {
-			/* error copying topic data */
-			memset(data, 0, _topic->o_size);
-			//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
-			return false;
-
-		} else {
-			/* data copied successfully */
-			_published = true;
-			*time = time_topic;
-			return true;
-		}
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
 	} else {
-		return false;
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 }