From 3f6848c4c68c137d8e3202af623d676a66b4bc53 Mon Sep 17 00:00:00 2001
From: jaseg <git@jaseg.net>
Date: Thu, 30 Jan 2020 14:32:55 +0100
Subject: Python receiver works now
---
gm_platform/fw/tw_test.c | 53 +++++++++++++-------
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()}')
--
cgit