From cc5756f61ffa9b84abcbebfcf89d213d99d100d9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Feb 2014 18:35:45 +0100
Subject: [PATCH] Mavlink: enable log messages to multiple UARTs

---
 src/modules/mavlink/mavlink_main.cpp | 20 ++++++++++++--------
 src/modules/mavlink/mavlink_main.h   |  3 +++
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 2585593eaa..7e146604b7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -93,9 +93,6 @@ static Mavlink* _head = nullptr;
 /* TODO: if this is a class member it crashes */
 static struct file_operations fops;
 
-static struct mavlink_logbuffer lb;
-static unsigned int total_counter;
-
 /**
  * mavlink app start / stop handling function
  *
@@ -227,11 +224,11 @@ Mavlink* Mavlink::new_instance()
 		::_head = inst;
 	/* afterwards follow the next and append the instance */
 	} else {
-		while (next != nullptr) {
+		while (next->_next != nullptr) {
 			next = next->_next;
 		}
 		/* now parent has a null pointer, fill it */
-		next = inst;
+		next->_next = inst;
 	}
 	return inst;
 }
@@ -288,8 +285,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 //		printf("logmsg: %s\n", txt);
 		struct mavlink_logmessage msg;
 		strncpy(msg.text, txt, sizeof(msg.text));
-		mavlink_logbuffer_write(&lb, &msg);
-		total_counter++;
+
+		Mavlink* inst = ::_head;
+		while (inst != nullptr) {
+
+			mavlink_logbuffer_write(&inst->lb, &msg);
+			inst->total_counter++;
+			inst = inst->_next;
+
+		}
 		return OK;
 	}
 
@@ -1451,7 +1455,7 @@ Mavlink::task_main(int argc, char *argv[])
 	fflush(stdout);
 
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
+	mavlink_logbuffer_init(&lb, 5);
 
 	int ch;
 	const char *device_name = "/dev/ttyS1";
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index ce2d2b34a2..b812af95a8 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -293,6 +293,9 @@ private:
 //  XXX probably should be in a header... 
 // extern pthread_t receive_start(int uart);
 
+	struct mavlink_logbuffer lb;
+	unsigned int total_counter;
+
 	pthread_t receive_thread;
 	pthread_t uorb_receive_thread;