From 11ed5169cc3ce40fdd3389e938617394f3867eed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 4 Jan 2016 23:53:34 +0100 Subject: [PATCH] don't use default source address for onboard udp link, wait on remote --- src/modules/mavlink/mavlink_main.cpp | 15 ++++++++++----- src/modules/mavlink/mavlink_main.h | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fc383fa996..a9c76322ba 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -930,7 +930,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID } #else if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + if (get_client_source_initialized()) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } struct telemetry_status_s &tstatus = get_rx_status(); @@ -1045,11 +1047,14 @@ Mavlink::init_udp() return; } - /* set default target address */ + /* set default target address, but not for onboard mode (will be set on first recieved packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); - _src_addr.sin_family = AF_INET; - inet_aton("127.0.0.1", &_src_addr.sin_addr); - _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + if (_mode != MAVLINK_MODE_ONBOARD) { + set_client_source_initialized(); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + } /* default broadcast address */ memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4f2967382c..1688398bb4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -339,6 +339,10 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + + void set_client_source_initialized() { _src_addr_initialized = true; }; + + bool get_client_source_initialized() { return _src_addr_initialized; }; #endif static bool boot_complete() { return _boot_complete; } @@ -423,6 +427,7 @@ private: struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; struct sockaddr_in _bcast_addr; + bool _src_addr_initialized; #endif int _socket_fd; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e73d706c19..231ea21c1d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,10 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg) struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); int localhost = (127 << 24) + 1; - if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) + || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // 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)); + _mavlink->set_client_source_initialized(); } #endif /* if read failed, this loop won't execute */