From 61a849bf6ba11b98b7332f89d0b32226601f4d63 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Feb 2014 19:13:57 +0100
Subject: Mavlink: don't allow multiple instances on the same device

---
 src/modules/mavlink/mavlink_main.cpp | 19 ++++++++++++++++++-
 src/modules/mavlink/mavlink_main.h   |  4 ++++
 2 files changed, 22 insertions(+), 1 deletion(-)

(limited to 'src/modules/mavlink')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 7e146604b..cbbe3c31f 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -155,6 +155,7 @@ namespace mavlink
 }
 
 Mavlink::Mavlink() :
+	device_name("/dev/ttyS1"),
 	_mavlink_fd(-1),
 	_next(nullptr),
 	_task_should_exit(false),
@@ -249,6 +250,19 @@ Mavlink* Mavlink::get_instance(unsigned instance)
 	return inst;
 }
 
+bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
+{
+	Mavlink* inst = ::_head;
+	while (inst != nullptr) {
+
+		/* don't compare with itself */
+		if (inst != self && !strcmp(device_name, inst->device_name))
+			return true;
+		inst = inst->_next;
+	}
+	return false;
+}
+
 int Mavlink::get_uart_fd(unsigned index)
 {
 	Mavlink* inst = get_instance(index);
@@ -1458,7 +1472,6 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_logbuffer_init(&lb, 5);
 
 	int ch;
-	const char *device_name = "/dev/ttyS1";
 	_baudrate = 57600;
 	_chan = MAVLINK_COMM_0;
 
@@ -1496,6 +1509,10 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 	}
 
+	if (Mavlink::instance_exists(device_name, this)) {
+		errx(1, "mavlink instance for %s already running", device_name);
+	}
+
 	struct termios uart_config_original;
 
 	bool usb_uart;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index b812af95a..bf7675267 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -168,10 +168,14 @@ public:
 
 	static Mavlink* get_instance(unsigned instance);
 
+	static bool	instance_exists(const char *device_name, Mavlink *self);
+
 	static int get_uart_fd(unsigned index);
 
 	int get_uart_fd() { return _uart; }
 
+	const char *device_name;
+
 	enum MAVLINK_MODE {
 		MODE_TX_HEARTBEAT_ONLY=0,
 		MODE_OFFBOARD,
-- 
cgit v1.2.3