Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

2 files changed, 26 insertions, 28 deletions

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 74089f780..2458dd23a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -258,19 +258,21 @@ protected:
 		struct vehicle_status_s status;
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
-		if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			mavlink_msg_heartbeat_send(_channel,
-						   mavlink_system.type,
-						   MAV_AUTOPILOT_PX4,
-						   mavlink_base_mode,
-						   mavlink_custom_mode,
-						   mavlink_state);
-		}
+		/* always send the heartbeat, independent of the update status of the topics */
+		(void)status_sub->update(&status);
+		(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
+
+		uint8_t mavlink_state = 0;
+		uint8_t mavlink_base_mode = 0;
+		uint32_t mavlink_custom_mode = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+		mavlink_msg_heartbeat_send(_channel,
+					   mavlink_system.type,
+					   MAV_AUTOPILOT_PX4,
+					   mavlink_base_mode,
+					   mavlink_custom_mode,
+					   mavlink_state);
 	}
 };
 
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 3807323c2..901fa8f05 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;
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
-		} else {
-			/* data copied successfully */
-			_published = true;
+	} else {
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
 			*time = time_topic;
 			return true;
-		}
 
-	} else {
-		return false;
+		} else {
+			return false;
+		}
 	}
 }