forked from Archive/PX4-Autopilot
Tools: add mavlink_ulog_streaming.py script to stream ULog via MAVLink to a file
This commit is contained in:
Beat Küng
Lorenz Meier
parent
7d72f31a29
commit
6999bb3e9a
|
|
@ -0,0 +1,248 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
"""
|
||||
Stream ULog data over MAVLink.
|
||||
|
||||
@author: Beat Kueng (beat-kueng@gmx.net)
|
||||
"""
|
||||
|
||||
|
||||
from __future__ import print_function
|
||||
import sys, select, os
|
||||
import datetime
|
||||
from timeit import default_timer as timer
|
||||
os.environ['MAVLINK20'] = '1' # The commands require mavlink 2
|
||||
|
||||
try:
|
||||
from pymavlink import mavutil
|
||||
except:
|
||||
print("Failed to import pymavlink.")
|
||||
print("You may need to install it with 'pip install pymavlink'")
|
||||
exit(-1)
|
||||
from argparse import ArgumentParser
|
||||
|
||||
|
||||
class MavlinkLogStreaming():
|
||||
'''Streams log data via MAVLink'''
|
||||
def __init__(self, portname, baudrate, output_filename, debug=0):
|
||||
self.baudrate = 0
|
||||
self._debug = debug
|
||||
self.buf = ''
|
||||
self.debug("Connecting with MAVLink to %s ..." % portname)
|
||||
self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
|
||||
self.mav.wait_heartbeat()
|
||||
self.debug("HEARTBEAT OK\n")
|
||||
self.debug("Locked serial device\n")
|
||||
|
||||
self.got_ulog_header = False
|
||||
self.got_header_section = False
|
||||
self.ulog_message = []
|
||||
self.file = open(output_filename,'wb')
|
||||
self.start_time = timer()
|
||||
self.last_sequence = -1
|
||||
self.logging_started = False
|
||||
self.num_dropouts = 0
|
||||
|
||||
def debug(self, s, level=1):
|
||||
'''write some debug text'''
|
||||
if self._debug >= level:
|
||||
print(s)
|
||||
|
||||
def start_log(self):
|
||||
self.mav.mav.command_long_send(self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_LOGGING_START, 0,
|
||||
0, 0, 0, 0, 0, 0, 0)
|
||||
|
||||
def stop_log(self):
|
||||
self.mav.mav.command_long_send(self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0,
|
||||
0, 0, 0, 0, 0, 0, 0)
|
||||
|
||||
def read_messages(self):
|
||||
''' main loop reading messages '''
|
||||
measure_time_start = timer()
|
||||
measured_data = 0
|
||||
while True:
|
||||
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)
|
||||
|
||||
# status output
|
||||
if self.logging_started:
|
||||
measured_data += len(m)
|
||||
measure_time_cur = timer()
|
||||
dt = measure_time_cur - measure_time_start
|
||||
if dt > 1:
|
||||
sys.stdout.write('\rData Rate: {:0.1f} KB/s Drops: {:} \033[K'.format(
|
||||
measured_data / dt / 1024, self.num_dropouts))
|
||||
sys.stdout.flush()
|
||||
measure_time_start = measure_time_cur
|
||||
measured_data = 0
|
||||
|
||||
if not self.logging_started and timer()-self.start_time > 4:
|
||||
raise Exception('Start timed out. Is the logger running in MAVLink mode?')
|
||||
|
||||
|
||||
def read_message(self):
|
||||
''' read a single mavlink message, handle ACK & return a tuple of (data, first
|
||||
message start, num dropouts) '''
|
||||
m = self.mav.recv_match(type=['LOGGING_DATA_ACKED',
|
||||
'LOGGING_DATA', 'COMMAND_ACK'], blocking=True,
|
||||
timeout=0.05)
|
||||
if m is not None:
|
||||
self.debug(m, 3)
|
||||
|
||||
if m.get_type() == 'COMMAND_ACK':
|
||||
if m.command == mavutil.mavlink.MAV_CMD_LOGGING_START and \
|
||||
not self.got_header_section:
|
||||
if m.result == 0:
|
||||
self.logging_started = True
|
||||
print('Logging started. Waiting for Header...')
|
||||
else:
|
||||
raise Exception('Logging start failed', m.result)
|
||||
return None, 0, 0
|
||||
|
||||
# m is either 'LOGGING_DATA_ACKED' or 'LOGGING_DATA':
|
||||
is_newer, num_drops = self.check_sequence(m.sequence)
|
||||
|
||||
if is_newer:
|
||||
if num_drops > 0:
|
||||
self.num_dropouts += num_drops
|
||||
|
||||
if m.get_type() == 'LOGGING_DATA_ACKED':
|
||||
self.mav.mav.logging_ack_send(m.sequence)
|
||||
else:
|
||||
if not self.got_header_section:
|
||||
print('Header received in {:0.2f}s'.format(timer()-self.start_time))
|
||||
self.logging_started = True
|
||||
self.got_header_section = True
|
||||
self.last_sequence = m.sequence
|
||||
return m.data[:m.length], m.first_message_offset, num_drops
|
||||
|
||||
else:
|
||||
self.debug('dup/reordered message '+str(m.sequence))
|
||||
|
||||
return None, 0, 0
|
||||
|
||||
|
||||
def check_sequence(self, seq):
|
||||
''' check if a sequence is newer than the previously received one & if
|
||||
there were dropped messages between the last and this '''
|
||||
if self.last_sequence == -1:
|
||||
return True, 0
|
||||
if seq == self.last_sequence: # duplicate
|
||||
return False, 0
|
||||
if seq > self.last_sequence:
|
||||
# account for wrap-arounds, sequence is 2 bytes
|
||||
if seq - self.last_sequence > (1<<15): # assume reordered
|
||||
return False, 0
|
||||
return True, seq - self.last_sequence - 1
|
||||
else:
|
||||
if self.last_sequence - seq > (1<<15):
|
||||
return True, (1<<16) - self.last_sequence - 1 + seq
|
||||
return False, 0
|
||||
|
||||
|
||||
def process_streamed_ulog_data(self, data, first_msg_start, num_drops):
|
||||
''' write streamed data to a file '''
|
||||
if not self.got_ulog_header: # the first 16 bytes need special treatment
|
||||
if len(data) < 16: # that's never the case anyway
|
||||
raise Exception('first received message too short')
|
||||
self.file.write(bytearray(data[0:16]))
|
||||
data = data[16:]
|
||||
self.got_ulog_header = True
|
||||
|
||||
if self.got_header_section and num_drops > 0:
|
||||
if num_drops > 25: num_drops = 25
|
||||
# write a dropout message. We don't really know the actual duration,
|
||||
# so just use the number of drops * 10 ms
|
||||
self.file.write(bytearray([ 2, 0, 79, num_drops*10, 0 ]))
|
||||
|
||||
if num_drops > 0:
|
||||
self.write_ulog_messages(self.ulog_message)
|
||||
self.ulog_message = []
|
||||
if first_msg_start == 255:
|
||||
return # no useful information in this message: drop it
|
||||
data = data[first_msg_start:]
|
||||
first_msg_start = 0
|
||||
|
||||
if first_msg_start == 255 and len(self.ulog_message) > 0:
|
||||
self.ulog_message.extend(data)
|
||||
return
|
||||
|
||||
if len(self.ulog_message) > 0:
|
||||
self.file.write(bytearray(self.ulog_message + data[:first_msg_start]))
|
||||
self.ulog_message = []
|
||||
|
||||
data = self.write_ulog_messages(data[first_msg_start:])
|
||||
self.ulog_message = data # store the rest for the next message
|
||||
|
||||
|
||||
def write_ulog_messages(self, data):
|
||||
''' write ulog data w/o integrity checking, assuming data starts with a
|
||||
valid ulog message. returns the remaining data at the end. '''
|
||||
while len(data) > 2:
|
||||
message_length = data[0] + data[1] * 256 + 3 # 3=ULog msg header
|
||||
if message_length > len(data):
|
||||
break
|
||||
self.file.write(bytearray(data[:message_length]))
|
||||
data = data[message_length:]
|
||||
return data
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
parser = ArgumentParser(description=__doc__)
|
||||
parser.add_argument('port', metavar='PORT', nargs='?', default = None,
|
||||
help='Mavlink port name: serial: DEVICE[,BAUD], udp: IP:PORT, tcp: tcp:IP:PORT. Eg: \
|
||||
/dev/ttyUSB0 or 0.0.0.0:14550. Auto-detect serial if not given.')
|
||||
parser.add_argument("--baudrate", "-b", dest="baudrate", type=int,
|
||||
help="Mavlink port baud rate (default=115200)", default=115200)
|
||||
parser.add_argument("--output", "-o", dest="output", default = '.',
|
||||
help="output file or directory (default=CWD)")
|
||||
args = parser.parse_args()
|
||||
|
||||
if os.path.isdir(args.output):
|
||||
filename = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S.ulg")
|
||||
filename = os.path.join(args.output, filename)
|
||||
else:
|
||||
filename = args.output
|
||||
print('Output file name: {:}'.format(filename))
|
||||
|
||||
if args.port == None:
|
||||
serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',
|
||||
"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*'])
|
||||
|
||||
if len(serial_list) == 0:
|
||||
print("Error: no serial connection found")
|
||||
return
|
||||
|
||||
if len(serial_list) > 1:
|
||||
print('Auto-detected serial ports are:')
|
||||
for port in serial_list:
|
||||
print(" {:}".format(port))
|
||||
print('Using port {:}'.format(serial_list[0]))
|
||||
args.port = serial_list[0].device
|
||||
|
||||
|
||||
print("Connecting to MAVLINK...")
|
||||
mav_log_streaming = MavlinkLogStreaming(args.port, args.baudrate, filename)
|
||||
|
||||
try:
|
||||
print('Starting log...')
|
||||
mav_log_streaming.start_log()
|
||||
mav_log_streaming.read_messages()
|
||||
|
||||
print('Stopping log')
|
||||
mav_log_streaming.stop_log()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print('Stopping log')
|
||||
mav_log_streaming.stop_log()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
Loading…
Reference in New Issue