mavlink: don't miss first vehicle_command_ack

This fixes a corner case where the first advertise/publish of a
vehicle_command_ack was missed. What happened was that the
orb_subscribe_multi was not called until the topic had been published
and therefore orb_exists was happy. This means that by the time
orb_subscribe_multi was finally called, the first vehicle_command_ack
was already history and not detected by orb_check.

This changed adds a flag to the MavlinkOrbSubscription which tells it to
subscribe to a topic from the beginning.

src/modules/mavlink/mavlink_main.cpp

@@ -1899,6 +1899,10 @@ Mavlink::task_main(int argc, char *argv[])
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
 	uint64_t status_time = 0;
 	MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
+	/* We don't want to miss the first advertise of an ACK, so we subscribe from the
+	 * beginning and not just when the topic exists. */
+	ack_sub->subscribe_from_beginning(true);
+
 	uint64_t ack_time = 0;
 	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -53,7 +53,8 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instanc
 	_instance(instance),
 	_fd(-1),
 	_published(false),
-	_last_pub_check(0)
+	_last_pub_check(0),
+	_subscribe_from_beginning(false)
 {
 }
 
@@ -157,15 +158,6 @@ MavlinkOrbSubscription::is_published()
 		return true;
 	}
 
-	// This is a workaround for this issue:
-	// https://github.com/PX4/Firmware/issues/5438
-#if defined(__PX4_LINUX) || defined(__PX4_QURT)
-
-	if (_fd < 0) {
-		_fd = orb_subscribe_multi(_topic, _instance);
-	}
-
-#else
 	// Telemetry can sustain an initial published check at 10 Hz
 	hrt_abstime now = hrt_absolute_time();
 
@@ -176,14 +168,22 @@ MavlinkOrbSubscription::is_published()
 	// We are checking now
 	_last_pub_check = now;
 
-	// If it does not exist its not published
-	if (orb_exists(_topic, _instance)) {
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
+	if (_fd < 0) {
+		_fd = orb_subscribe_multi(_topic, _instance);
+	}
+#else
+	// We don't want to subscribe to anything that does not exist
+	// in order to save memory and file descriptors.
+	// However, for some topics like vehicle_command_ack, we want to subscribe
+	// from the beginning in order not to miss the first publish respective advertise.
+	if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
 		return false;
+	}
 
-	} else if (_fd < 0) {
+	if (_fd < 0) {
 		_fd = orb_subscribe_multi(_topic, _instance);
 	}
-
 #endif
 
 	bool updated;
@@ -195,3 +195,9 @@ MavlinkOrbSubscription::is_published()
 
 	return _published;
 }
+
+void
+MavlinkOrbSubscription::subscribe_from_beginning(bool subscribe_from_beginning)
+{
+	_subscribe_from_beginning = subscribe_from_beginning;
+}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -87,6 +87,9 @@ public:
 	 * If no data is available the buffer will be filled with zeros.
 	 */
 	bool is_published();
+
+	void subscribe_from_beginning(bool from_beginning);
+
 	orb_id_t get_topic() const;
 	int get_instance() const;
 
@@ -96,6 +99,7 @@ private:
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
 	hrt_abstime _last_pub_check;	///< when we checked last
+	bool _subscribe_from_beginning; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
 
 	/* do not allow copying this class */
 	MavlinkOrbSubscription(const MavlinkOrbSubscription &);