diff options
Diffstat (limited to 'gm_platform/fw')
-rw-r--r-- | gm_platform/fw/tw_test.c | 53 | ||||
-rw-r--r-- | gm_platform/fw/tw_test.py | 123 |
2 files changed, 133 insertions, 43 deletions
diff --git a/gm_platform/fw/tw_test.c b/gm_platform/fw/tw_test.c index 60c6c67..eb41dbe 100644 --- a/gm_platform/fw/tw_test.c +++ b/gm_platform/fw/tw_test.c @@ -281,11 +281,22 @@ int main(int argc, char *argv[]) { if (epoll_ctl(epollfd, EPOLL_CTL_ADD, fd, &ev) < 0) goto epoll_err; - int current_seq = -1; wpacket.type = 1; wpacket.pid = 0; cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket)); write(fd, wbuf, sizeof(wbuf)); + + /* FIXME begin debug code */ + for (int i=0; i<32; i++) { + wpacket.type = 2; + wpacket.pid = packet.pid; + cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket)); + write(fd, wbuf, sizeof(wbuf)); + usleep(20); + } + /* FIXME end debug code */ + + int current_seq = -1; uint64_t local_seq = 0; while (23) { int nfds = epoll_wait(epollfd, events, MAX_EVENTS, -1); @@ -296,6 +307,8 @@ int main(int argc, char *argv[]) { continue; ssize_t n = read(fd, buf+wpos, sizeof(buf)-wpos); + printf("--- read wpos=%d n=%ld\n", wpos, n); + hexdump(buf+wpos, n); if (n<0) { if (errno == EAGAIN || errno == EINTR) continue; @@ -303,17 +316,17 @@ int main(int argc, char *argv[]) { fprintf(stderr, "Error reading from port: %s\n", strerror(errno)); goto loop_err; } - printf("--- debug: read n=%d bytes at wpos=%d\n", n, wpos); - fflush(stdout); + //printf("--- debug: read n=%d bytes at wpos=%d\n", n, wpos); + //fflush(stdout); wpos += n; while (23) { void *first_nul = memchr(buf, 0, wpos) ; + ssize_t first_nul_offx = first_nul - (void*)buf; + ssize_t remaining = wpos - first_nul_offx; if (!in_sync) { if (first_nul) { - ssize_t first_nul_offx = first_nul - (void*)buf; - ssize_t remaining = wpos - first_nul_offx; memmove(buf, first_nul+1, remaining-1); wpos = remaining-1; in_sync = 1; @@ -328,6 +341,9 @@ int main(int argc, char *argv[]) { if (!first_nul) break; + printf("--- debug: first_nul=%p (idx=%ld) wpos=%d remaining=%ld\n", first_nul, first_nul_offx, wpos, remaining); + hexdump(buf, 80); + int rc = cobs_decode((char *)&packet, sizeof(packet), buf, wpos); if (rc < 0) { printf("Framing error: rc=%d\n", rc); @@ -344,21 +360,26 @@ int main(int argc, char *argv[]) { } } + bool error = false; /* Check CRC */ if (our_crc != packet.crc) { printf("CRC mismatch: seq=%d packet=%08x computed=%08x\n", packet.pid, packet.crc, our_crc); - goto it_err; + error = true; } /* Check device sequence number */ int last_seq = current_seq; int predicted_seq = (last_seq+1) % 0xffff; - current_seq = packet.seq; + if (!error) + current_seq = packet.seq; if (last_seq >= 0 && packet.seq != predicted_seq) { printf("SEQ mismatch: packet=%d computed=%d\n", packet.seq, predicted_seq); - goto it_err; + error = true; } + if (error) + goto it_err; + /* Write to database */ struct timespec ts; if (clock_gettime(CLOCK_REALTIME, &ts)) { @@ -394,23 +415,21 @@ int main(int argc, char *argv[]) { printf("OK: seq=%d crc=%08x\n", current_seq, packet.crc); -it_err: - /* FIXME don't send acks in case of error */ /* send ACK reply */ wpacket.type = 2; wpacket.pid = packet.pid; cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket)); write(fd, wbuf, sizeof(wbuf)); +it_err: /* Fixup buffer for next iteration */ - ssize_t first_nul_offx = first_nul - (void*)buf; - ssize_t remaining = wpos - first_nul_offx; - printf("--- debug: first_nul=%p (idx=%d) wpos=%d remaining=%d\n", first_nul, first_nul_offx, wpos, remaining); - hexdump(buf, 80); - printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%d);-->\n", buf, first_nul+1, remaining-1); - memmove(buf, first_nul+1, remaining-1); - hexdump(buf, 80); + if (remaining-1 > 0) { + printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%ld);-->\n", buf, first_nul+1, remaining-1); + memmove(buf, first_nul+1, remaining-1); + } + //hexdump(buf, 80); fflush(stdout); + printf("--- continuing wpos=%d->%d\n", wpos, (int)(remaining-1)); wpos = remaining-1; } } diff --git a/gm_platform/fw/tw_test.py b/gm_platform/fw/tw_test.py index 0380f95..a05a44b 100644 --- a/gm_platform/fw/tw_test.py +++ b/gm_platform/fw/tw_test.py @@ -1,11 +1,13 @@ #!/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 @@ -18,7 +20,7 @@ class CtrlPacketTypes(enum.Enum): def unpack_head(fmt, data): split = struct.calcsize(fmt) - return *struct.unpack(fmt, data[:split]), data[split:] + 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' @@ -28,31 +30,100 @@ ctrl_ack = lambda pid: ctrl_packet(CtrlPacketTypes.ACK, pid) ctrl_retransmit = lambda pid: ctrl_packet(CtrlPacketTypes.RETRANSMIT, pid) -ser = serial.Serial('/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0', 250000, timeout=1.0) -ser.write(b'foobar'*32) -sys.exit(0) - -log = [] -ser.flushInput() -ser.write(ctrl_reset()) -ser.flushOutput() -for _ in range(100): - #ser.write(cobs.encode(b'\x01\xff') + b'\0') - data = ser.read_until(b'\0') - if not data or data[-1] != 0x00: - #print(f'{time():>7.3f} Timeout: resetting') - #ser.write(cobs.encode(b'\x01\xff') + b'\0') # reset - continue - - crc32, payload = unpack_head('I', cobs.decode(data[:-1])) - pid, seq, data = unpack_head('xBH', payload) - ser.write(ctrl_ack(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() - # Calculate byte-wise CRC32 - #our_crc = zlib.crc32(bytes(b for x in payload for b in (0, 0, 0, x))) - our_crc = 0 - #log.append((time(), seq, crc32, our_crc, pid, data)) + last_pid = None + lines_written = 0 + cur = db.cursor() + while True: + #ser.write(cobs.encode(b'\x01\xff') + b'\0') + data = ser.read_until(b'\0') + if not data or data[-1] != 0x00: + #print(f'{time():>7.3f} Timeout: resetting') + #ser.write(cobs.encode(b'\x01\xff') + b'\0') # reset + continue + + try: + if len(data) <= 1: # delimiting zero for retransmission + 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[:-1])) + pid, seq, data = unpack_head('xBH', 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)) + print(f'{ts:>7.3f} {seq:05d} {crc32:08x} {our_crc:08x} {pid} {hexlify(data).decode()}', end='') + + error = False + suppress_ack = False + if crc32 != our_crc: + print(' CRC ERROR', end='') + suppress_ack = True + error = True + + if last_pid is not None and pid != (last_pid+1)%8: + print(' PID ERROR', end='') + error = True + else: + last_pid = pid + + if not suppress_ack: + ser.write(ctrl_ack(pid)) + ser.flushOutput() + + if not error: + cur.execute('INSERT INTO measurements VALUES (?, ?, ?, ?)', (run_id, int(ts*1000), seq, data)) + else: + 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 == 80: + lines_written = 0 + print('\033[2J\033[H', end='') + db.commit() + + except Exception as e: + print(e, len(data)) + ser.write(ctrl_ack(0)) # FIXME delet this -for time, seq, crc32, our_crc, pid, data in log: - print(f'{time:>7.3f} {seq:05d} {crc32:08x} {our_crc:08x} {pid} {hexlify(data).decode()}') |