diff options
Diffstat (limited to 'Tools/sdlog2/sdlog2_dump.py')
| -rw-r--r-- | Tools/sdlog2/sdlog2_dump.py | 334 |
1 files changed, 334 insertions, 0 deletions
diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py new file mode 100644 index 000000000..5b1e55e22 --- /dev/null +++ b/Tools/sdlog2/sdlog2_dump.py @@ -0,0 +1,334 @@ +#!/usr/bin/env python + +from __future__ import print_function + +"""Dump binary log generated by PX4's sdlog2 or APM as CSV + +Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] + + -v Use plain debug output instead of CSV. + + -e Recover from errors. + + -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.2" + +import struct, sys + +if sys.hexversion >= 0x030000F0: + runningPython3 = True + def _parseCString(cstr): + return str(cstr, 'ascii').split('\0')[0] +else: + runningPython3 = False + def _parseCString(cstr): + return str(cstr).split('\0')[0] + +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 = [] + __time_msg = None + __debug_out = False + __correct_errors = False + __file_name = None + __file = None + + def __init__(self): + return + + def reset(self): + self.__msg_descrs = {} # message descriptions by message type map + self.__msg_labels = {} # message labels by message name map + self.__msg_names = [] # message names in the same order as FORMAT messages + self.__buffer = bytearray() # buffer for input binary data + self.__ptr = 0 # read pointer in buffer + self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label" + self.__csv_data = {} # current values for all columns + self.__csv_updated = False + self.__msg_filter_map = {} # filter in form of map, with '*" expanded to full list of fields + + 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 setTimeMsg(self, time_msg): + self.__time_msg = time_msg + + def setDebugOut(self, debug_out): + self.__debug_out = debug_out + + def setCorrectErrors(self, correct_errors): + self.__correct_errors = correct_errors + + def setFileName(self, file_name): + self.__file_name = file_name + if file_name != None: + self.__file = open(file_name, 'w+') + else: + self.__file = None + + + def process(self, fn): + self.reset() + if self.__debug_out: + # init __msg_filter_map + for msg_name, show_fields in self.__msg_filter: + self.__msg_filter_map[msg_name] = show_fields + first_data_msg = True + f = open(fn, "rb") + bytes_read = 0 + 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 = self.__buffer[self.__ptr] + head2 = self.__buffer[self.__ptr+1] + if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2): + if self.__correct_errors: + self.__ptr += 1 + continue + else: + raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) + msg_type = self.__buffer[self.__ptr+2] + if msg_type == self.MSG_TYPE_FORMAT: + # parse FORMAT message + if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: + break + self.__parseMsgDescr() + else: + # parse data message + 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: + # build CSV columns and init data map + self.__initCSV() + first_data_msg = False + self.__parseMsg(msg_descr) + bytes_read += self.__ptr + if not self.__debug_out and self.__time_msg != None and self.__csv_updated: + self.__printCSVRow() + f.close() + + def __bytesLeft(self): + return len(self.__buffer) - self.__ptr + + def __filterMsg(self, msg_name): + show_fields = "*" + if len(self.__msg_filter_map) > 0: + show_fields = self.__msg_filter_map.get(msg_name) + return show_fields + + def __initCSV(self): + if len(self.__msg_filter) == 0: + for msg_name in self.__msg_names: + self.__msg_filter.append((msg_name, "*")) + for msg_name, show_fields in self.__msg_filter: + if show_fields == "*": + show_fields = self.__msg_labels.get(msg_name, []) + self.__msg_filter_map[msg_name] = show_fields + for field in show_fields: + full_label = msg_name + "_" + field + self.__csv_columns.append(full_label) + self.__csv_data[full_label] = None + if self.__file != None: + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + else: + print(self.__csv_delim.join(self.__csv_columns)) + + def __printCSVRow(self): + s = [] + for full_label in self.__csv_columns: + v = self.__csv_data[full_label] + if v == None: + v = self.__csv_null + else: + v = str(v) + s.append(v) + + if self.__file != None: + print(self.__csv_delim.join(s), file=self.__file) + else: + print(self.__csv_delim.join(s)) + + def __parseMsgDescr(self): + if runningPython3: + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + else: + data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])) + msg_type = data[0] + if msg_type != self.MSG_TYPE_FORMAT: + msg_length = data[1] + msg_name = _parseCString(data[2]) + msg_format = _parseCString(data[3]) + msg_labels = _parseCString(data[4]).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) + self.__msg_labels[msg_name] = msg_labels + self.__msg_names.append(msg_name) + if self.__debug_out: + if self.__filterMsg(msg_name) != None: + 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)) + 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 + if not self.__debug_out and self.__time_msg != None and msg_name == self.__time_msg and self.__csv_updated: + self.__printCSVRow() + self.__csv_updated = False + show_fields = self.__filterMsg(msg_name) + if (show_fields != None): + if runningPython3: + data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) + else: + data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) + for i in range(len(data)): + if type(data[i]) is str: + data[i] = _parseCString(data[i]) + m = msg_mults[i] + if m != None: + data[i] = data[i] * m + if self.__debug_out: + s = [] + for i in range(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 range(len(data)): + label = msg_labels[i] + if label in show_fields: + self.__csv_data[msg_name + "_" + label] = data[i] + if self.__time_msg != None and msg_name != self.__time_msg: + self.__csv_updated = True + if self.__time_msg == None: + self.__printCSVRow() + self.__ptr += msg_length + +def _main(): + if len(sys.argv) < 2: + print("Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n") + print("\t-v\tUse plain debug output instead of CSV.\n") + print("\t-e\tRecover from errors.\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.") + print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n") + print("\t-fPrint to file instead of stdout") + return + fn = sys.argv[1] + debug_out = False + correct_errors = False + msg_filter = [] + csv_null = "" + csv_delim = "," + time_msg = "TIME" + file_name = None + opt = None + for arg in sys.argv[2:]: + if opt != None: + if opt == "d": + csv_delim = arg + elif opt == "n": + csv_null = arg + elif opt == "t": + time_msg = arg + elif opt == "f": + file_name = arg + elif opt == "m": + show_fields = "*" + a = arg.split("_") + if len(a) > 1: + show_fields = a[1].split(",") + msg_filter.append((a[0], show_fields)) + opt = None + else: + if arg == "-v": + debug_out = True + elif arg == "-e": + correct_errors = True + elif arg == "-d": + opt = "d" + elif arg == "-n": + opt = "n" + elif arg == "-m": + opt = "m" + elif arg == "-t": + opt = "t" + elif arg == "-f": + opt = "f" + + if csv_delim == "\\t": + csv_delim = "\t" + parser = SDLog2Parser() + parser.setCSVDelimiter(csv_delim) + parser.setCSVNull(csv_null) + parser.setMsgFilter(msg_filter) + parser.setTimeMsg(time_msg) + parser.setFileName(file_name) + parser.setDebugOut(debug_out) + parser.setCorrectErrors(correct_errors) + parser.process(fn) + +if __name__ == "__main__": + _main() |