From 5784f1d95106f6fa6be3fc7ebb187473425e89fa Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 21 Apr 2021 15:45:21 +0200
Subject: [PATCH] mavlink: add detailed stats about rx messages

---
 src/modules/mavlink/mavlink_main.cpp     |  1 +
 src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  2 ++
 3 files changed, 23 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a1596284f1..1b175a5231 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2814,6 +2814,7 @@ Mavlink::display_status()
 	printf("\t  tx rate max: %i B/s\n", _datarate);
 	printf("\t  rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg);
 	printf("\t  rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate);
+	_receiver.print_detailed_rx_stats();
 
 	if (_mavlink_ulog) {
 		printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index fc59ccb5c6..403baa674c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -3151,6 +3151,26 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message)
 	}
 }
 
+void MavlinkReceiver::print_detailed_rx_stats() const
+{
+	const uint32_t now_ms = hrt_absolute_time() / 1000;
+
+	// TODO: add mutex around shared data.
+	for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) {
+		if (_component_states[i].received_messages > 0) {
+			printf("\t  received from %u/%u: %lu, lost: %lu, last %lu ms ago\n",
+			       _component_states[i].system_id,
+			       _component_states[i].component_id,
+			       _component_states[i].received_messages,
+			       _component_states[i].missed_messages,
+			       now_ms - _component_states[i].last_time_received_ms);
+
+		} else {
+			break;
+		}
+	}
+}
+
 void MavlinkReceiver::start()
 {
 	pthread_attr_t receiveloop_attr;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 47868c2238..106426d7b0 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -124,6 +124,8 @@ public:
 	void start();
 	void stop();
 
+	void print_detailed_rx_stats() const;
+
 private:
 	static void *start_trampoline(void *context);
 	void run();