diff options
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 19 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 4 |
2 files changed, 22 insertions, 1 deletions
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, |