Python receiver works now

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()}')