path: root/Tools/sdlog2_dump.py

#!/usr/bin/env python

"""Dump binary log generated by sdlog2 or APM as CSV
    
Usage: python sdlog2_dump.py <log.bin> [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
    
    -v  Use plain debug output instead of CSV.
    
    -d  Use "delimiter" in CSV. Default is ",".
    
    -n  Use "null" as placeholder for empty values in CSV. Default is empty.
    
    -m MSG[.field1,field2,...]
        Dump only messages of specified type, and only specified fields.
        Multiple -m options allowed."""

__author__  = "Anton Babushkin"
__version__ = "1.0"

import struct, sys

class BufferUnderflow(Exception):
    pass

class SDLog2Parser:
    BLOCK_SIZE = 8192
    MSG_HEADER_LEN = 3
    MSG_HEAD1 = 0xA3
    MSG_HEAD2 = 0x95
    MSG_FORMAT_PACKET_LEN = 89
    MSG_FORMAT_STRUCT = "BB4s16s64s"
    MSG_TYPE_FORMAT = 0x80
    FORMAT_TO_STRUCT = {
        "b": ("b", None),
        "B": ("B", None),
        "h": ("h", None),
        "H": ("H", None),
        "i": ("i", None),
        "I": ("I", None),
        "f": ("f", None),
        "n": ("4s", None),
        "N": ("16s", None),
        "Z": ("64s", None),
        "c": ("h", 0.01),
        "C": ("H", 0.01),
        "e": ("i", 0.01),
        "E": ("I", 0.01),
        "L": ("i", 0.0000001),
        "M": ("b", None),
        "q": ("q", None),
        "Q": ("Q", None),
    }
    __csv_delim = ","
    __csv_null = ""
    __msg_filter = {}
    __debug_out = False
    
    def __init__(self):
        return

    def reset(self):
        self.__msg_descrs = {}
        self.__buffer = ""
        self.__ptr = 0
        self.__csv_columns = []
        self.__csv_data = {}
    
    def setCSVDelimiter(self, csv_delim):
        self.__csv_delim = csv_delim
    
    def setCSVNull(self, csv_null):
        self.__csv_null = csv_null
    
    def setMsgFilter(self, msg_filter):
        self.__msg_filter = msg_filter
    
    def setDebugOut(self, debug_out):
        self.__debug_out = debug_out
    
    def process(self, fn):
        self.reset()
        first_data_msg = True
        f = open(fn, "r")
        while True:
            chunk = f.read(self.BLOCK_SIZE)
            if len(chunk) == 0:
                break
            self.__buffer = self.__buffer[self.__ptr:] + chunk
            self.__ptr = 0
            while self.__bytesLeft() >= self.MSG_HEADER_LEN:
                head1 = ord(self.__buffer[self.__ptr])
                head2 = ord(self.__buffer[self.__ptr+1])
                if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
                    raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
                msg_type = ord(self.__buffer[self.__ptr+2])
                if msg_type == self.MSG_TYPE_FORMAT:
                    self.__parseMsgDescr()
                else:
                    msg_descr = self.__msg_descrs[msg_type]
                    if msg_descr == None:
                        raise Exception("Unknown msg type: %i" % msg_type)
                    msg_length = msg_descr[0]
                    if self.__bytesLeft() < msg_length:
                        break
                    if first_data_msg:
                        print self.__csv_delim.join(self.__csv_columns)
                        first_data_msg = False
                    self.__parseMsg(msg_descr)
    
        f.close()

    def __bytesLeft(self):
        return len(self.__buffer) - self.__ptr

    def __filterMsg(self, msg_name):
        show_fields = "*"
        if len(self.__msg_filter) > 0:
            show_fields = self.__msg_filter.get(msg_name)
        return show_fields

    def __parseMsgDescr(self):
        if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
            raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self.__bytesLeft(), self.MSG_FORMAT_PACKET_LEN))
        data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
        msg_type = data[0]
        msg_length = data[1]
        msg_name = data[2].strip("\0")
        msg_format = data[3].strip("\0")
        msg_labels = data[4].strip("\0").split(",")
        # Convert msg_format to struct.unpack format string
        msg_struct = ""
        msg_mults = []
        for c in msg_format:
            try:
                f = self.FORMAT_TO_STRUCT[c]
                msg_struct += f[0]
                msg_mults.append(f[1])
            except KeyError as e:
                raise Exception("Unsupported format char: %s in message %s (%i)" % (c, msg_name, msg_type))
        msg_struct = "<" + msg_struct   # force little-endian
        self.__msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults)
        show_fields = self.__filterMsg(msg_name)
        if show_fields != None:
            if self.__debug_out:
                print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
                            msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)
            else:
                if show_fields == "*":
                    fields = msg_labels
                else:
                    fields = []
                    for field in show_fields:
                        if field in msg_labels:
                            fields.append(field)
                for field in fields:
                    msg_field = msg_name + "." + field
                    self.__csv_columns.append(msg_field)
                    self.__csv_data[msg_field] = None
        self.__ptr += self.MSG_FORMAT_PACKET_LEN

    def __parseMsg(self, msg_descr):
        msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr
        show_fields = self.__filterMsg(msg_name)
        if (show_fields != None):
            data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
            for i in xrange(len(data)):
                if type(data[i]) is str:
                    data[i] = data[i].strip("\0")
                m = msg_mults[i]
                if m != None:
                    data[i] = data[i] * m
            if self.__debug_out:
                s = []
                for i in xrange(len(data)):
                    label = msg_labels[i]
                    if show_fields == "*" or label in show_fields:
                        s.append(label + "=" + str(data[i]))
                print "MSG %s: %s" % (msg_name, ", ".join(s))
            else:
                # update CSV data buffer
                for i in xrange(len(data)):
                    label = msg_labels[i]
                    if show_fields == "*" or label in show_fields:
                        self.__csv_data[msg_name + "." + label] = data[i]
                # format and print CSV row
                s = []
                for field in self.__csv_columns:
                    v = self.__csv_data[field]
                    if v == None:
                        v = self.__csv_null
                    else:
                        v = str(v)
                    s.append(v)
                print self.__csv_delim.join(s)
        self.__ptr += msg_length

def _main():
    if len(sys.argv) < 2:
        print "Usage: python sdlog2_dump.py <log.bin> [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]\n"
        print "\t-v\tUse plain debug output instead of CSV.\n"
        print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n"
        print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n"
        print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed."
        return
    fn = sys.argv[1]
    debug_out = False
    msg_filter = {}
    csv_null = ""
    csv_delim = ","
    opt = None
    for arg in sys.argv[2:]:
        if opt != None:
            if opt == "d":
                csv_delim = arg
            elif opt == "n":
                csv_null = arg
            if opt == "m":
                show_fields = "*"
                a = arg.split(".")
                if len(a) > 1:
                    show_fields = set(a[1].split(","))
                msg_filter[a[0]] = show_fields
            opt = None
        else:
            if arg == "-v":
                debug_out = True
            elif arg == "-d":
                opt = "d"
            elif arg == "-n":
                opt = "n"
            elif arg == "-m":
                opt = "m"

    if csv_delim == "\\t":
        csv_delim = "\t"
    parser = SDLog2Parser()
    parser.setCSVDelimiter(csv_delim)
    parser.setCSVNull(csv_null)
    parser.setMsgFilter(msg_filter)
    parser.setDebugOut(debug_out)
    parser.process(fn)

if __name__ == "__main__":
    _main()