forked from Archive/PX4-Autopilot
91 lines
3.1 KiB
Python
91 lines
3.1 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
"""Parse and dump binary log generated by sdlog2
|
||
|
|
|
||
|
|
Usage: python sdlog2_dump.py <log.bin>"""
|
||
|
|
|
||
|
|
__author__ = "Anton Babushkin"
|
||
|
|
__version__ = "0.1"
|
||
|
|
|
||
|
|
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_TYPE_FORMAT = 0x80
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
return
|
||
|
|
|
||
|
|
def reset(self):
|
||
|
|
self.msg_formats = {}
|
||
|
|
self.buffer = ""
|
||
|
|
self.ptr = 0
|
||
|
|
|
||
|
|
def process(self, fn):
|
||
|
|
self.reset()
|
||
|
|
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._bytes_left() >= 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._parse_msg_format()
|
||
|
|
else:
|
||
|
|
msg_format = self.msg_formats[msg_type]
|
||
|
|
if msg_format == None:
|
||
|
|
raise Exception("Unknown msg type: %i" % msg_type)
|
||
|
|
msg_length = msg_format[0]
|
||
|
|
if self._bytes_left() < msg_length:
|
||
|
|
break
|
||
|
|
self._parse_msg(msg_format)
|
||
|
|
f.close()
|
||
|
|
|
||
|
|
def _bytes_left(self):
|
||
|
|
return len(self.buffer) - self.ptr
|
||
|
|
|
||
|
|
def _parse_msg_format(self):
|
||
|
|
if self._bytes_left() < self.MSG_FORMAT_PACKET_LEN:
|
||
|
|
raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self._bytes_left(), self.MSG_FORMAT_PACKET_LEN))
|
||
|
|
msg_type = ord(self.buffer[self.ptr+3])
|
||
|
|
msg_length = ord(self.buffer[self.ptr+4])
|
||
|
|
msg_name = self.buffer[self.ptr+5:self.ptr+9].strip('\0')
|
||
|
|
msg_struct = self.buffer[self.ptr+9:self.ptr+25].strip('\0')
|
||
|
|
msg_labels = self.buffer[self.ptr+25:self.ptr+89].strip('\0').split(",")
|
||
|
|
print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s" % (msg_type, msg_length, msg_name, msg_struct, str(msg_labels))
|
||
|
|
self.msg_formats[msg_type] = (msg_length, msg_name, msg_struct, msg_labels)
|
||
|
|
self.ptr += self.MSG_FORMAT_PACKET_LEN
|
||
|
|
|
||
|
|
def _parse_msg(self, msg_format):
|
||
|
|
msg_length = msg_format[0]
|
||
|
|
msg_name = msg_format[1]
|
||
|
|
msg_struct = "<" + msg_format[2]
|
||
|
|
data = struct.unpack(msg_struct, self.buffer[self.ptr+self.MSG_HEADER_LEN:self.ptr+msg_length])
|
||
|
|
print "MSG %s: %s" % (msg_name, str(data))
|
||
|
|
self.ptr += msg_format[0]
|
||
|
|
|
||
|
|
def _main():
|
||
|
|
if len(sys.argv) < 2:
|
||
|
|
print "Usage:\npython sdlog2_dump.py <log.bin>"
|
||
|
|
return
|
||
|
|
fn = sys.argv[1]
|
||
|
|
parser = SDLog2Parser()
|
||
|
|
parser.process(fn)
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
_main()
|