1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
|
#!/usr/bin/env python3
import os
from time import time
from binascii import hexlify
import enum
import struct
import zlib
import sys
import sqlite3
import serial
from cobs import cobs
class CtrlPacketTypes(enum.Enum):
RESET = 1
ACK = 2
RETRANSMIT = 3
def unpack_head(fmt, data):
split = struct.calcsize(fmt)
return [ *struct.unpack(fmt, data[:split]), data[split:] ]
def ctrl_packet(ptype, pid=0):
return cobs.encode(struct.pack('BB', ptype.value, pid)) + b'\0'
ctrl_reset = lambda: ctrl_packet(CtrlPacketTypes.RESET)
ctrl_ack = lambda pid: ctrl_packet(CtrlPacketTypes.ACK, pid)
ctrl_retransmit = lambda pid: ctrl_packet(CtrlPacketTypes.RETRANSMIT, pid)
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('-b', '--baudrate', type=int, default=250000)
parser.add_argument('port', nargs='?', default=None)
parser.add_argument('dbfile')
args = parser.parse_args()
if args.port is None:
try:
candidate, = os.listdir('/dev/serial/by-id')
args.port = os.path.join('/dev/serial/by-id', candidate)
print(f'No port given, guessing {args.port}')
except:
print('No port given and could not guess port. Exiting.')
sys.exit(1)
ser = serial.Serial(args.port, args.baudrate, timeout=1.0)
db = sqlite3.connect(args.dbfile)
db.execute('CREATE TABLE IF NOT EXISTS measurements (run_id INTEGER, rx_ts INTEGER, seq INTEGER, data BLOB)')
db.execute('''CREATE TABLE IF NOT EXISTS errors (
run_id INTEGER,
rx_ts INTEGER,
type TEXT,
seq INTEGER,
pid INTEGER,
pid_expected INTEGER,
crc32 INTEGER,
crc32_expected INTEGER,
data BLOB)''')
run_id, = db.execute('SELECT IFNULL(MAX(run_id), -1) + 1 FROM measurements').fetchone()
ser.flushInput()
ser.write(ctrl_reset())
ser.flushOutput()
last_pid = None
lines_written = 0
cur = db.cursor()
capture_start = time()
while True:
#ser.write(cobs.encode(b'\x01\xff') + b'\0')
data = ser.read_until(b'\0')
for data in data.split(b'\0')[:-1]: # data always ends on \0 due to read_until, so split off the trailing empty bytes()
try:
if not data:
#print(f'{time():>7.3f} Timeout: resetting')
#ser.write(cobs.encode(b'\x01\xff') + b'\0') # reset
ser.write(ctrl_ack(0)) # FIXME delet this
cur.execute('INSERT INTO errors(run_id, rx_ts, type) VALUES (?, ?, "retransmission")',
(run_id, int(time()*1000)))
continue
crc32, payload = unpack_head('I', cobs.decode(data))
pid, seq, gps_1pps, data = unpack_head('BxHI', payload)
ts = time()
# Calculate byte-wise CRC32
our_crc = zlib.crc32(bytes(b for x in payload for b in (0, 0, 0, x)))
#log.append((time(), seq, crc32, our_crc, pid, data))
bars = '\u2581\u2582\u2583\u2584\u2585\u2586\u2587\u2588'
sparkline = ''.join(bars[int(x/4096*8)] for x in struct.unpack('<32H', data))
print(f'\033[38;5;249m{ts-capture_start:>10.3f}',
f'\033[94m{seq:05d}',
f'\033[38;5;243m{crc32:08x}',
f'\033[38;5;243m{our_crc:08x}',
f'\033[38;5;243m{pid}',
f'\033[0m{hexlify(data).decode()}',
f'\033[94m{sparkline}\033[0m', end='')
error = False
suppress_ack = False
if crc32 != our_crc:
print(' \033[1;91mCRC ERROR\033[0m', end='')
suppress_ack = True
error = True
if last_pid is not None and pid != (last_pid+1)%8:
print(' \033[1;93mPID ERROR\033[0m', end='')
error = True
else:
last_pid = pid
if not suppress_ack:
ser.write(ctrl_ack(pid))
ser.flushOutput()
if not suppress_ack:
cur.execute('INSERT INTO measurements VALUES (?, ?, ?, ?)', (run_id, int(ts*1000), seq, data))
if error:
cur.execute('INSERT INTO errors VALUES (?, ?, "pid", ?, ?, ?, ?, ?, ?)',
(run_id, int(ts*1000), seq, pid, (last_pid+1)%8, crc32, our_crc, data))
print()
lines_written += 1
if lines_written == 50:
lines_written = 0
print('\033[2J\033[H', end='')
delta = ts-capture_start
print(f'\033[7mRun {run_id}, capturing for {delta//3600//24:> 3.0f}:{delta//3600%24:02.0f}:{delta//60%60:02.0f}:{delta%60:06.3f}\033[0m')
db.commit()
except Exception as e:
print(e, len(data))
ser.write(ctrl_ack(0)) # FIXME delet this
|
