From fa461d018d77a896cef965ab49a9e978f8716c96 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 4 Nov 2016 10:20:48 +0100
Subject: [PATCH] mavlink_{shell,ulog_streaming}.py: send heartbeat

This is helpful, so that a connected partner can switch from broadcasting
to a 'connected' state.
---
 Tools/mavlink_shell.py          | 10 ++++++++++
 Tools/mavlink_ulog_streaming.py | 11 +++++++++++
 2 files changed, 21 insertions(+)

diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py
index acde3e88ec..3a698762fe 100755
--- a/Tools/mavlink_shell.py
+++ b/Tools/mavlink_shell.py
@@ -10,6 +10,7 @@ Open a shell over MAVLink.
 from __future__ import print_function
 import sys, select
 import termios
+from timeit import default_timer as timer
 
 try:
     from pymavlink import mavutil
@@ -141,6 +142,8 @@ def main():
             ERASE_END_LINE = '\x1b[K'
             sys.stdout.write(CURSOR_BACK_N + ERASE_END_LINE)
 
+        next_heartbeat_time = timer()
+
         while True:
             while True:
                 i, o, e = select.select([sys.stdin], [], [], 0)
@@ -194,6 +197,13 @@ def main():
                 sys.stdout.write(data)
                 sys.stdout.flush()
 
+            # handle heartbeat sending
+            heartbeat_time = timer()
+            if heartbeat_time > next_heartbeat_time:
+                mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
+                        mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
+                next_heartbeat_time = heartbeat_time + 1
+
     except serial.serialutil.SerialException as e:
         print(e)
 
diff --git a/Tools/mavlink_ulog_streaming.py b/Tools/mavlink_ulog_streaming.py
index 369303c3f7..3da8e14dce 100755
--- a/Tools/mavlink_ulog_streaming.py
+++ b/Tools/mavlink_ulog_streaming.py
@@ -68,7 +68,18 @@ class MavlinkLogStreaming():
         ''' main loop reading messages '''
         measure_time_start = timer()
         measured_data = 0
+
+        next_heartbeat_time = timer()
         while True:
+
+            # handle heartbeat sending
+            heartbeat_time = timer()
+            if heartbeat_time > next_heartbeat_time:
+                self.debug('sending heartbeat')
+                self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
+                        mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
+                next_heartbeat_time = heartbeat_time + 1
+
             m, first_msg_start, num_drops = self.read_message()
             if m is not None:
                 self.process_streamed_ulog_data(m, first_msg_start, num_drops)