From 371586be2c0416996e9b1da8d0bbd774d303c514 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 18 May 2017 14:04:14 +0200
Subject: [PATCH] MAVLink: Add verbose command

---
 src/modules/mavlink/mavlink_main.cpp     | 29 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_main.h       | 19 ++++++++++++++--
 src/modules/mavlink/mavlink_receiver.cpp |  8 +++++++
 3 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 903c09f645..9ff55251b9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -499,6 +499,32 @@ Mavlink::get_status_all_instances()
 	return (iterations == 0);
 }
 
+void
+Mavlink::set_verbose(bool v)
+{
+	_verbose = v;
+}
+
+int
+Mavlink::set_verbose_all_instances()
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	while (inst != nullptr) {
+
+		inst->set_verbose(true);
+
+		/* 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)
 {
@@ -2807,6 +2833,9 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "status")) {
 		return Mavlink::get_status_all_instances();
 
+	} else if (!strcmp(argv[1], "verbose")) {
+		return Mavlink::set_verbose_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 629346bf6b..111151f956 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -132,6 +132,14 @@ public:
 
 	static int		get_status_all_instances();
 
+	/**
+	 * Set all instances to verbose mode
+	 *
+	 * This is primarily intended for analysis and
+	 * not intended for normal operation
+	 */
+	static int		set_verbose_all_instances();
+
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -261,7 +269,14 @@ public:
 	/**
 	 * Set communication protocol for this mavlink instance
 	 */
-	void 		set_protocol(Protocol p) {_protocol = p;};
+	void 			set_protocol(Protocol p) { _protocol = p; }
+
+	/**
+	 * Set verbose mode
+	 */
+	void			set_verbose(bool v);
+
+	bool			get_verbose() const { return _verbose; }
 
 	/**
 	 * Get the manual input generation mode
@@ -288,7 +303,7 @@ public:
 	 *
 	 * @return the number of bytes sent or -1 in case of error
 	 */
-	int             send_packet();
+	int             	send_packet();
 
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3f073aa370..4b8ac66105 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2412,6 +2412,9 @@ MavlinkReceiver::receive_thread(void *arg)
 	ssize_t nread = 0;
 	hrt_abstime last_send_update = 0;
 
+	bool verbose = _mavlink->get_verbose();
+	_mission_manager.set_verbose(verbose);
+
 	while (!_mavlink->_task_should_exit) {
 		if (poll(&fds[0], 1, timeout) > 0) {
 			if (_mavlink->get_protocol() == SERIAL) {
@@ -2510,6 +2513,11 @@ MavlinkReceiver::receive_thread(void *arg)
 			_mavlink_ftp.send(t);
 			_mavlink_log_handler.send(t);
 			last_send_update = t;
+
+			if (verbose != _mavlink->get_verbose()) {
+				verbose = !verbose;
+				_mission_manager.set_verbose(verbose);
+			}
 		}
 
 	}