forked from Archive/PX4-Autopilot
mavlink_{shell,ulog_streaming}.py: send heartbeat
This is helpful, so that a connected partner can switch from broadcasting to a 'connected' state.
This commit is contained in:
parent
25be7aa7cf
commit
fa461d018d
|
@ -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)
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue