mavlink: STATUSTEXT stream use perf count for missed messages

- don't send stale messages
 - process all queued messages until tx buffer is full

src/modules/mavlink/streams/STATUSTEXT.hpp

@@ -36,6 +36,8 @@
 
 #include <uORB/topics/mavlink_log.h>
 
+#include <lib/perf/perf_counter.h>
+
 class MavlinkStreamStatustext : public MavlinkStream
 {
 public:
@@ -55,57 +57,69 @@ public:
 private:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
+	~MavlinkStreamStatustext()
+	{
+		perf_free(_missed_msg_count_perf);
+	}
+
 	uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)};
+	perf_counter_t _missed_msg_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": STATUSTEXT missed messages")};
 	uint16_t _id{0};
-	uint16_t _missed_msg_count{0};
 
 	bool send() override
 	{
-		if (_mavlink->is_connected() && (_mavlink->get_free_tx_buf() >= get_size())) {
+		if (_mavlink->is_connected()) {
+			while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) {
 
-			const unsigned last_generation = _mavlink_log_sub.get_last_generation();
+				const unsigned last_generation = _mavlink_log_sub.get_last_generation();
 
-			mavlink_log_s mavlink_log;
+				mavlink_log_s mavlink_log;
 
-			if (_mavlink_log_sub.update(&mavlink_log)) {
-				if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
-					_missed_msg_count++;
-					PX4_ERR("channel %d has missed %d mavlink log messages", _mavlink->get_channel(), _missed_msg_count);
-				}
+				if (_mavlink_log_sub.update(&mavlink_log)) {
+					// don't send stale messages
+					if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
 
-				mavlink_statustext_t msg{};
-				const char *text = mavlink_log.text;
-				constexpr unsigned max_chunk_size = sizeof(msg.text);
-				msg.severity = mavlink_log.severity;
-				msg.chunk_seq = 0;
-				msg.id = _id++;
-				unsigned text_size;
+						if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
+							perf_count(_missed_msg_count_perf);
+							PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
+								  perf_event_count(_missed_msg_count_perf));
+						}
 
-				while ((text_size = strlen(text)) > 0) {
-					unsigned chunk_size = math::min(text_size, max_chunk_size);
+						mavlink_statustext_t msg{};
+						const char *text = mavlink_log.text;
+						constexpr unsigned max_chunk_size = sizeof(msg.text);
+						msg.severity = mavlink_log.severity;
+						msg.chunk_seq = 0;
+						msg.id = _id++;
+						unsigned text_size;
 
-					if (chunk_size < max_chunk_size) {
-						memcpy(&msg.text[0], &text[0], chunk_size);
-						// pad with zeros
-						memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
+						while ((text_size = strlen(text)) > 0) {
+							unsigned chunk_size = math::min(text_size, max_chunk_size);
 
-					} else {
-						memcpy(&msg.text[0], &text[0], chunk_size);
-					}
+							if (chunk_size < max_chunk_size) {
+								memcpy(&msg.text[0], &text[0], chunk_size);
+								// pad with zeros
+								memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
 
-					mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
+							} else {
+								memcpy(&msg.text[0], &text[0], chunk_size);
+							}
 
-					if (text_size <= max_chunk_size) {
-						break;
+							mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
 
-					} else {
-						text += max_chunk_size;
-					}
+							if (text_size <= max_chunk_size) {
+								break;
 
-					msg.chunk_seq += 1;
-				}
+							} else {
+								text += max_chunk_size;
+							}
+
+							msg.chunk_seq += 1;
+						}
 
-				return true;
+						return true;
+					}
+				}
 			}
 		}