Tools: add mavlink_ulog_streaming.py script to stream ULog via MAVLink to a file

Tools/mavlink_ulog_streaming.py

+#!/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()
+