diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e31cf29411..06bee091cf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -942,11 +942,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID struct telemetry_status_s &tstatus = get_rx_status(); /* resend heartbeat via broadcast */ - if ((_mode != MAVLINK_MODE_ONBOARD) - && (_mavlink_start_time > 0 && (hrt_elapsed_time(&_mavlink_start_time) > 4 * 1000 * 1000)) - && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || - (tstatus.heartbeat_time == 0)) && - msgid == MAVLINK_MSG_ID_HEARTBEAT)) { + if ((_mode != MAVLINK_MODE_ONBOARD) && + (!get_client_source_initialized() + || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000)) + && (msgid == MAVLINK_MSG_ID_HEARTBEAT)) { int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5e5cc34649..e16c27491f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -345,6 +345,9 @@ public: bool get_client_source_initialized() { return _src_addr_initialized; }; #endif + + uint64_t get_start_time() { return _mavlink_start_time; } + static bool boot_complete() { return _boot_complete; } bool is_usb_uart() { return _is_usb_uart; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06f49cc3ec..5c2bed9bb5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,32 +1814,38 @@ 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)) - || (_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 -// This is causing issues on Linux, so use default port for now -// this will kill tablet testing on Linux and VMs -#ifndef __PX4_LINUX - srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; - srcaddr_last->sin_port = srcaddr.sin_port; -#endif - _mavlink->set_client_source_initialized(); + if (!_mavlink->get_client_source_initialized()) { + + // set the address either if localhost or if 3 seconds have passed + // this ensures that a GCS running on localhost can get a hold of + // the system within the first N seconds + hrt_abstime stime = _mavlink->get_start_time(); + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000)) + || (srcaddr_last->sin_addr.s_addr == htonl(localhost))) { + srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; + srcaddr_last->sin_port = srcaddr.sin_port; + _mavlink->set_client_source_initialized(); + } } #endif - /* if read failed, this loop won't execute */ - for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { - /* handle generic messages and commands */ - handle_message(&msg); - - /* handle packet with parent object */ - _mavlink->handle_message(&msg); + // only start accepting messages once we're sure who we talk to + + if (_mavlink->get_client_source_initialized()) { + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { + /* handle generic messages and commands */ + handle_message(&msg); + + /* handle packet with parent object */ + _mavlink->handle_message(&msg); + } } - } - /* count received bytes (nread will be -1 on read error) */ - if (nread > 0) { - _mavlink->count_rxbytes(nread); + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + } } } }