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

---
 src/modules/mavlink/mavlink_main.cpp | 51 +++++++++++++++++++++++++++++++++---
 1 file changed, 48 insertions(+), 3 deletions(-)

(limited to 'src/modules/mavlink/mavlink_main.cpp')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 76b5459a3..e25851c7d 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);
-- 
cgit v1.2.3