MAVLink app: Only hold buffers for published topics

src/modules/mavlink/mavlink_main.cpp

@@ -337,7 +337,7 @@ void
 Mavlink::set_proto_version(unsigned version)
 {
 	if ((version == 1 || version == 0) &&
-		   ((_protocol_version_switch == 0) || (_protocol_version_switch == 1))) {
+	    ((_protocol_version_switch == 0) || (_protocol_version_switch == 1))) {
 		get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
 		_protocol_version = 1;
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -51,14 +51,17 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instanc
 	next(nullptr),
 	_topic(topic),
 	_instance(instance),
-	_fd(orb_subscribe_multi(_topic, instance)),
+	_fd(-1),
-	_published(false)
+	_published(false),
+	_last_pub_check(0)
 {
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
-	orb_unsubscribe(_fd);
+	if (_fd >= 0) {
+		orb_unsubscribe(_fd);
+	}
 }
 
 orb_id_t
@@ -76,6 +79,8 @@ MavlinkOrbSubscription::get_instance() const
 bool
 MavlinkOrbSubscription::update(uint64_t *time, void *data)
 {
+
+
 	// TODO this is NOT atomic operation, we can get data newer than time
 	// if topic was published between orb_stat and orb_copy calls.
 
@@ -86,19 +91,10 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
 		time_topic = 0;
 	}
 
-	if (orb_copy(_topic, _fd, data)) {
+	if (update(data)) {
-		if (data != nullptr) {
-			/* error copying topic data */
-			memset(data, 0, _topic->o_size);
-		}
-
-		return false;
-
-	} else {
 		/* data copied successfully */
-		_published = true;
 
-		if (time_topic != *time) {
+		if (time_topic == 0 || (time_topic != *time)) {
 			*time = time_topic;
 			return true;
 
@@ -106,46 +102,76 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
 			return false;
 		}
 	}
+
+	return false;
 }
 
 bool
 MavlinkOrbSubscription::update(void *data)
 {
-	return !orb_copy(_topic, _fd, data);
+	if (!is_published()) {
+		return false;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		if (data != nullptr) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+		}
+
+		return false;
+	}
+
+	return true;
 }
 
 bool
 MavlinkOrbSubscription::update_if_changed(void *data)
 {
-	bool updated;
+	bool prevpub = _published;
 
-	if (orb_check(_fd, &updated)) {
+	if (!is_published()) {
 		return false;
 	}
 
-	if (!updated) {
+	bool updated;
+
+	if (orb_check(_fd, &updated)) {
 		return false;
 	}
 
-	if (orb_copy(_topic, _fd, data)) {
+	// If we didn't update and this topic did not change
-		if (data != nullptr) {
+	// its publication status then nothing really changed
-			/* error copying topic data */
+	if (!updated && prevpub == _published) {
-			memset(data, 0, _topic->o_size);
-		}
-
 		return false;
 	}
 
-	return true;
+	return update(data);
 }
 
 bool
 MavlinkOrbSubscription::is_published()
 {
+	// If we marked it as published no need to check again
 	if (_published) {
 		return true;
 	}
 
+	// Telemetry can sustain an initial published check at 10 Hz
+	hrt_abstime now = hrt_absolute_time();
+
+	if (now - _last_pub_check < 100000) {
+		return false;
+	}
+
+	// We are checking now
+	_last_pub_check = now;
+
+	// If it does not exist its not published
+	if (orb_exists(_topic, _instance)) {
+		return false;
+	}
+
 	bool updated;
 	orb_check(_fd, &updated);
 

src/modules/mavlink/mavlink_orb_subscription.h

@@ -95,6 +95,7 @@ private:
 	const int _instance;		///< get topic instance
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
+	hrt_abstime _last_pub_check;	///< when we checked last
 
 	/* do not allow copying this class */
 	MavlinkOrbSubscription(const MavlinkOrbSubscription &);