From 730a520362caf9c9d3e506a31441d9921e008144 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 19 Jul 2014 14:59:13 +0200
Subject: [PATCH] Print mavlink radio module rates

---
 src/modules/mavlink/mavlink_main.cpp     | 51 ++++++++++++++++++++++--
 src/modules/mavlink/mavlink_main.h       | 10 +++++
 src/modules/mavlink/mavlink_receiver.cpp |  3 +-
 3 files changed, 59 insertions(+), 5 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 76b5459a3e..e25851c7d5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -247,6 +247,7 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
+	_rstatus{},
 	_message_buffer{},
 	_message_buffer_mutex{},
 	_param_initialized(false),
@@ -424,6 +425,29 @@ Mavlink::destroy_all_instances()
 	return OK;
 }
 
+int
+Mavlink::get_status_all_instances()
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	warnx("waiting for instances to stop");
+
+	while (inst != nullptr) {
+
+		printf("instance #%u:\n", iterations);
+		inst->display_status();
+
+		/* move on */
+		inst = inst->next;
+		iterations++;
+	}
+
+	/* return an error if there are no instances */
+	return (iterations == 0);
+}
+
 bool
 Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
@@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
 		configure_stream("ATTITUDE", 10.0f * rate_mult);
-		configure_stream("VFR_HUD", 10.0f * rate_mult);
+		configure_stream("VFR_HUD", 8.0f * rate_mult);
 		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
@@ -1665,6 +1689,27 @@ void
 Mavlink::display_status()
 {
 	warnx("running");
+
+	if (_rstatus.timestamp > 0) {
+		printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time);
+
+		switch (_rstatus.type) {
+			case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+			printf("\t3DR RADIO\n");
+			break;
+			default:
+			printf("\tUNKNOWN RADIO\n");
+			break;
+		}
+
+		printf("\trssi:\t%d\t\n", _rstatus.rssi);
+		printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi);
+		printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf);
+		printf("\tnoise:\t%d\tus\n", _rstatus.noise);
+		printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise);
+		printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors);
+		printf("\tfixed:\t%u\tus\n", _rstatus.fixed);
+	}
 }
 
 int
@@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
 
-		// } else if (!strcmp(argv[1], "status")) {
-		// 	mavlink::g_mavlink->status();
+	} else if (!strcmp(argv[1], "status")) {
+		return Mavlink::get_status_all_instances();
 
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index adca0e88f2..d51120462a 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -51,6 +51,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_orb_subscription.h"
@@ -97,6 +98,8 @@ public:
 
 	static int		destroy_all_instances();
 
+	static int		get_status_all_instances();
+
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -229,6 +232,11 @@ public:
 	 */
 	void			count_txerr();
 
+	/**
+	 * Get the receive status of this MAVLink link
+	 */
+	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
+
 protected:
 	Mavlink			*next;
 
@@ -285,6 +293,8 @@ private:
 
 	bool			_flow_control_enabled;
 
+	struct telemetry_status_s	_rstatus;			///< receive status
+
 	struct mavlink_message_buffer {
 		int write_ptr;
 		int read_ptr;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3ee7ec34da..528c8b72a3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		struct telemetry_status_s tstatus;
-		memset(&tstatus, 0, sizeof(tstatus));
+		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
 		tstatus.timestamp = hrt_absolute_time();
 		tstatus.heartbeat_time = _telemetry_heartbeat_time;