sdlog2 strick packing fixed, length bug fixed, "sdlog2_dump.py" debug tool added

Tools/sdlog2_dump.py

@@ -0,0 +1,90 @@
+#!/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()

src/modules/sdlog2/sdlog2.c

@@ -492,6 +492,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	} subs;
 
 	/* log message buffer: header + body */
+	#pragma pack(push, 1)
 	struct {
 		LOG_PACKET_HEADER;
 		union {
@@ -502,6 +503,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
 	};
+	#pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
 	/* --- MANAGEMENT - LOGGING COMMAND --- */

src/modules/sdlog2/sdlog2_format.h

@@ -77,7 +77,7 @@ Format characters in the format string for binary log messages
 
 struct log_format_s {
 	uint8_t type;
-	uint8_t length;
+	uint8_t length;		// full packet length including header
 	char name[4];
 	char format[16];
 	char labels[64];
@@ -85,13 +85,13 @@ struct log_format_s {
 
 #define LOG_FORMAT(_name, _format, _labels) { \
 		.type = LOG_##_name##_MSG, \
-			.length = sizeof(struct log_##_name##_s) + 8, \
+		.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
-				  .name = #_name, \
+		.name = #_name, \
-					  .format = _format, \
+		.format = _format, \
-						    .labels = _labels \
+		.labels = _labels \
 	}
 
-#define LOG_FORMAT_MSG	  128
+#define LOG_FORMAT_MSG	  0x80
 
 #define LOG_PACKET_SIZE(_name)	LOG_PACKET_HEADER_LEN + sizeof(log_msg.body.log_##_name)