demo fw WIP

1 files changed, 73 insertions, 0 deletions

diff --git a/prototype/fw/tools/ser_test.py b/prototype/fw/tools/ser_test.py
new file mode 100644
index 0000000..191124b
--- /dev/null
+++ b/prototype/fw/tools/ser_test.py
@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+
+import struct
+import itertools
+import binascii
+import string
+import serial
+import time
+import zlib
+
+from cobs import cobs
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('port')
+    parser.add_argument('-b', '--baudrate', type=int, default=115200)
+    args = parser.parse_args()
+
+    ser = serial.Serial(args.port, args.baudrate, timeout=0)
+
+    byte_count = 0
+    line = b''
+    packet = b''
+    start_time = time.time()
+    seq = 0
+
+    make_color = lambda x: f'\033[38;5;{x}m'
+    field_colors = [ make_color(x) for x in [ 48, 48, 48, 48, 220, 220, 220, 220, 207, 207, 207, 207 ] ]
+    last_tx = time.time()
+    while True:
+        data = ser.read()
+        for c in data:
+            if byte_count == 0:
+                print(f'\033[38;5;244m{time.time() - start_time: 8.3f} \033[0m', end='')
+
+            col = '\033[91m' if c == 0 else '\033[0m'
+            print(f'{col}{c:02x}', end=' ')
+            line += bytes([c])
+            byte_count += 1
+            if c == 0:
+                print('   ' * (16 - byte_count), end='\033[0m\n')
+                byte_count =  16
+                try:
+                    payload = cobs.decode(packet)
+                    if len(payload) > 4:
+                        crc = zlib.crc32(payload[:-4])
+                        ref_crc = struct.pack('<I', crc)
+                        crc_ok = (crc,) == struct.unpack('<I', payload[-4:])
+                    else:
+                        crc_ok = False
+                        ref_crc = ''
+                    print(' '.join(f'{col if col else ""}{c if c else 0:02x}' for col, c in itertools.zip_longest(field_colors,
+                        payload)), 'OK' if crc_ok else f'WRONG')
+                except cobs.DecodeError:
+                    print('COBS framing error')
+                packet = b''
+            else:
+                packet += bytes([c])
+            isprint = lambda c: c in b'0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ!"#$%&\'()*+,-./:;<=>?@[\\]^_`{|}~ '
+            if byte_count == 16:
+                printable = ''.join( chr(c) if isprint(c) else '.' for c in line )
+                print(f'\033[93m | {printable}\033[0m')
+                byte_count = 0
+                line = b''
+
+
+        if time.time() - last_tx > 0.01:
+            data = struct.pack('<I', seq)
+            seq += 1
+            ser.write(cobs.encode(data + struct.pack('<I', zlib.crc32(data))) + b'\0')
+            last_tx = time.time()
+        #time.sleep(0.01)