From eff94677a47382146a3437cef49539eaee0bebb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:26:09 +0100 Subject: [PATCH] MAVLink: Only broadcast heartbeat on local network if not in onboard mode --- src/modules/mavlink/mavlink_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8e575e1b8f..c3004add29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,9 +896,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 (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || + if ((_mode != MAVLINK_MODE_ONBOARD) + && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && - msgid == MAVLINK_MSG_ID_HEARTBEAT) { + msgid == MAVLINK_MSG_ID_HEARTBEAT)) { int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));