From 22f5e7afc03785ea53afec8a9957f57ed511fc91 Mon Sep 17 00:00:00 2001
From: dgburr <dburr@dburr.net>
Date: Wed, 10 Jan 2024 15:50:16 +0100
Subject: [PATCH] Add --unbuffered option to sbp2json.py (#1400)

Add a new `--unbuffered` command line option to sbp2json.py which enables a mode where it reads and parses messages one-at-a-time. The existing behaviour (which remains the default) waits for 4096 bytes before parsing, which means that when decoding real-time streams which only output messages intermittently, there are often large pauses where no output is produced, followed by a large batch of decoded messages at once.
---
 python/sbp/__init__.py            |  0
 python/sbp/acquisition.py         |  0
 python/sbp/bootload.py            |  0
 python/sbp/logging.py             |  0
 python/sbp/msg.py                 |  0
 python/sbp/navigation.py          |  0
 python/sbp/piksi.py               |  0
 python/sbp/sbp2json.py            | 65 ++++++++++++++++++++++++++++---
 python/sbp/table.py               |  0
 python/sbp/utils.py               |  0
 python/tests/sbp/test_sbp2json.py | 20 ++++++++--
 11 files changed, 77 insertions(+), 8 deletions(-)
 mode change 100755 => 100644 python/sbp/__init__.py
 mode change 100755 => 100644 python/sbp/acquisition.py
 mode change 100755 => 100644 python/sbp/bootload.py
 mode change 100755 => 100644 python/sbp/logging.py
 mode change 100755 => 100644 python/sbp/msg.py
 mode change 100755 => 100644 python/sbp/navigation.py
 mode change 100755 => 100644 python/sbp/piksi.py
 mode change 100755 => 100644 python/sbp/table.py
 mode change 100755 => 100644 python/sbp/utils.py

diff --git a/python/sbp/__init__.py b/python/sbp/__init__.py
old mode 100755
new mode 100644
diff --git a/python/sbp/acquisition.py b/python/sbp/acquisition.py
old mode 100755
new mode 100644
diff --git a/python/sbp/bootload.py b/python/sbp/bootload.py
old mode 100755
new mode 100644
diff --git a/python/sbp/logging.py b/python/sbp/logging.py
old mode 100755
new mode 100644
diff --git a/python/sbp/msg.py b/python/sbp/msg.py
old mode 100755
new mode 100644
diff --git a/python/sbp/navigation.py b/python/sbp/navigation.py
old mode 100755
new mode 100644
diff --git a/python/sbp/piksi.py b/python/sbp/piksi.py
old mode 100755
new mode 100644
diff --git a/python/sbp/sbp2json.py b/python/sbp/sbp2json.py
index 5d4b0b9abe..d564fa857e 100644
--- a/python/sbp/sbp2json.py
+++ b/python/sbp/sbp2json.py
@@ -39,6 +39,8 @@ def base_cl_options():
                         help="the JSON serialization library to use, default: {}".format(DEFAULT_JSON))
     parser.add_argument('--include', nargs="+", type=int, default=[],
                         help="list of SBP message IDs to include, empty means all")
+    parser.add_argument("--unbuffered", action="store_true",
+                        help="disable buffering when reading data from input (slower)")
     parser.add_argument('file', nargs='?', metavar='FILE', type=argparse.FileType('rb'),
                         default=sys.stdin, help="the input file, stdin by default")
 
@@ -61,6 +63,13 @@ def get_args():
     return args
 
 
+# return the read and expected CRCs from 'buf'
+def get_crcs(buf, payload_len):
+    crc_read, = struct.unpack("<H", buf[SBP_HEADER_LEN + payload_len:SBP_HEADER_LEN + payload_len + 2])
+    crc_expected = binascii.crc_hqx(buf[1:SBP_HEADER_LEN + payload_len], 0)
+    return crc_read, crc_expected
+
+
 def dump(args, res):
     if 'json' == args.mode:
         sys.stdout.write(json.dumps(res.to_json_dict(),
@@ -72,8 +81,54 @@ def dump(args, res):
     sys.stdout.write("\n")
 
 
-# generator to produce SBP messages from a file object
-def iter_messages(fp):
+# Generator which parses SBP messages from a file object.
+# Messages are read one at a time from the stream.
+def iter_messages_unbuffered(fp):
+    buf = b''
+
+    def read_bytes_until(target_len):
+        nonlocal buf
+        while len(buf) < target_len:
+            read_bytes = fp.read(target_len - len(buf))
+            if len(read_bytes) == 0:
+                raise IOError
+            buf += read_bytes
+
+    while True:
+        # read header
+        try:
+            read_bytes_until(SBP_HEADER_LEN)
+        except IOError:
+            return
+
+        # buf now contains at least SBP_HEADER_LEN bytes
+
+        preamble, msg_type, sender, payload_len = struct.unpack("<BHHB", buf[:SBP_HEADER_LEN])
+
+        # check preamble
+        if preamble != SBP_PREAMBLE:
+            buf = buf[1:] # drop first byte
+            continue
+
+        # read payload and CRC
+        try:
+            read_bytes_until(SBP_HEADER_LEN + payload_len + 2)
+        except IOError:
+            return
+
+        # check CRC
+        crc_read, crc_expected = get_crcs(buf, payload_len)
+        if crc_read == crc_expected:
+            yield msg_type, sender, payload_len, buf[:SBP_HEADER_LEN + payload_len + 2], crc_read
+            buf = buf[SBP_HEADER_LEN + payload_len + 3:] # drop message
+        else:
+            sys.stderr.write("CRC error: {} vs {} for msg type {}\n".format(crc_read, crc_expected, msg_type))
+            buf = buf[1:] # drop first byte
+
+
+# Generator which parses SBP messages from a file object.
+# Data is read from the stream in 4096 byte chunks.
+def iter_messages_buffered(fp):
     buf = memoryview(bytearray(4096))
     unconsumed_offset = 0
     read_offset = 0
@@ -114,8 +169,7 @@ def iter_messages(fp):
                     else:
                         # check CRC
                         b = b[:SBP_HEADER_LEN + payload_len + 2]
-                        crc_read, = struct.unpack("<H", b[SBP_HEADER_LEN + payload_len:SBP_HEADER_LEN + payload_len + 2])
-                        crc_expected = binascii.crc_hqx(b[1:SBP_HEADER_LEN + payload_len], 0)
+                        crc_read, crc_expected = get_crcs(b, payload_len)
                         if crc_read == crc_expected:
                             yield msg_type, sender, payload_len, b, crc_read
                             consumed = SBP_HEADER_LEN + payload_len + 2
@@ -132,8 +186,9 @@ def iter_messages(fp):
 def sbp_main(args):
     reader = io.open(args.file.fileno(), 'rb')
     include = set(args.include)
+    iter_fn = iter_messages_unbuffered if args.unbuffered else iter_messages_buffered
 
-    for msg_type, sender, payload_len, buf, crc_read in iter_messages(reader):
+    for msg_type, sender, payload_len, buf, crc_read in iter_fn(reader):
         msg_buf = buf[SBP_HEADER_LEN:SBP_HEADER_LEN + payload_len]
         if not include or msg_type in include:
             try:
diff --git a/python/sbp/table.py b/python/sbp/table.py
old mode 100755
new mode 100644
diff --git a/python/sbp/utils.py b/python/sbp/utils.py
old mode 100755
new mode 100644
diff --git a/python/tests/sbp/test_sbp2json.py b/python/tests/sbp/test_sbp2json.py
index df2f4712ab..436675ccef 100644
--- a/python/tests/sbp/test_sbp2json.py
+++ b/python/tests/sbp/test_sbp2json.py
@@ -15,8 +15,7 @@
 TEST_DATA = os.path.join(PYTHON_ROOT, "..", "test_data", "benchmark.sbp")
 
 
-# make sure that we parse exactly 150000 SBP messages from TEST_DATA
-def test_sbp2json():
+def get_message_count(unbuffered):
     msg_count = 0
     def counter(args, res):
         nonlocal msg_count
@@ -27,9 +26,24 @@ def counter(args, res):
     # anonymous object to emulate parsed arguments
     args = type('',(object,),{
         'file': open(TEST_DATA, "rb"),
-        'include': []
+        'include': [],
+        'unbuffered': unbuffered
     })()
 
     sbp.sbp2json.sbp_main(args)
 
+    return msg_count
+
+
+# make sure that we parse exactly 150000 SBP messages from TEST_DATA
+# when running sbp2json without the --unbuffered flag
+def test_sbp2json_buffered():
+    msg_count = get_message_count(False)
+    assert msg_count == 150000
+
+
+# make sure that we parse exactly 150000 SBP messages from TEST_DATA
+# when running sbp2json with the --unbuffered flag
+def test_sbp2json_unbuffered():
+    msg_count = get_message_count(True)
     assert msg_count == 150000