From d39e3137688c0f236bc9a482f166661f072750e9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 31 Dec 2015 11:50:52 +0100 Subject: [PATCH] use the proper check to prevent multiple mavlink instances on the same udp port, added warning when different remote than localhost connects to udp --- src/modules/mavlink/mavlink_main.cpp | 18 ++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 1 + 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c3004add29..10e0a3ac22 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1570,14 +1570,20 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); - return ERROR; - } - if (get_protocol() == SERIAL) { - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + } else if (get_protocol() == UDP) { + if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { + warnx("port %d already occupied", _network_port); + return ERROR; + } + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); } /* flush stdout in case MAVLink is about to take it over */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 51f08bb01c..e73d706c19 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1817,6 +1817,7 @@ MavlinkReceiver::receive_thread(void *arg) if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); } #endif /* if read failed, this loop won't execute */