Fix sending first High Latency message

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -103,7 +103,9 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
 	_temperature(SimpleAnalyzer::AVERAGE),
 	_throttle(SimpleAnalyzer::AVERAGE),
 	_windspeed(SimpleAnalyzer::AVERAGE)
-{}
+{
+	reset_last_sent();
+}
 
 bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 {

src/modules/mavlink/mavlink_main.cpp

@@ -2250,7 +2250,7 @@ Mavlink::task_main(int argc, char *argv[])
 				if (_transmitting_enabled &&
 				    !status.high_latency_data_link_active &&
 				    !_transmitting_enabled_commanded &&
-				    (_last_write_success_time > 0u)) { // a first message is written
+				    (_first_heartbeat_sent)) {
 					_transmitting_enabled = false;
 					mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
 
@@ -2425,6 +2425,19 @@ Mavlink::task_main(int argc, char *argv[])
 		MavlinkStream *stream;
 		LL_FOREACH(_streams, stream) {
 			stream->update(t);
+
+			if (!_first_heartbeat_sent) {
+				if (_mode == MAVLINK_MODE_IRIDIUM) {
+					if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
+						_first_heartbeat_sent = stream->first_message_sent();
+					}
+
+				} else {
+					if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
+						_first_heartbeat_sent = stream->first_message_sent();
+					}
+				}
+			}
 		}
 
 		/* pass messages from other UARTs */

src/modules/mavlink/mavlink_main.h

@@ -505,6 +505,7 @@ private:
 	int			_instance_id;
 	bool			_transmitting_enabled;
 	bool			_transmitting_enabled_commanded;
+	bool			_first_heartbeat_sent{false};
 
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;

src/modules/mavlink/mavlink_stream.cpp

@@ -66,6 +66,10 @@ MavlinkStream::update(const hrt_abstime &t)
 		// on the link scheduling
 		if (send(t)) {
 			_last_sent = hrt_absolute_time();
+
+			if (!_first_message_sent) {
+				_first_message_sent = true;
+			}
 		}
 
 		return 0;
@@ -103,6 +107,11 @@ MavlinkStream::update(const hrt_abstime &t)
 		// long time not sending anything, sending multiple messages in a short time is avoided.
 		if (send(t)) {
 			_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
+
+			if (!_first_message_sent) {
+				_first_message_sent = true;
+			}
+
 			return 0;
 
 		} else {

src/modules/mavlink/mavlink_stream.h

@@ -101,6 +101,17 @@ public:
 	 */
 	virtual unsigned get_size_avg() { return get_size(); }
 
+	/**
+	 * @return true if the first message of this stream has been sent
+	 */
+	bool first_message_sent() const { return _first_message_sent; }
+
+	/**
+	 * Reset the time of last sent to 0. Can be used if a message over this
+	 * stream needs to be sent immediately.
+	 */
+	void reset_last_sent() { _last_sent = 0; }
+
 protected:
 	Mavlink      *const _mavlink;
 	int _interval{1000000};		///< if set to negative value = unlimited rate
@@ -117,6 +128,7 @@ protected:
 
 private:
 	hrt_abstime _last_sent{0};
+	bool _first_message_sent{false};
 };