jaseg/master-thesis
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Python receiver works now

Browse source
This commit is contained in:
jaseg 2020-01-30 14:32:55 +01:00
parent b778151734
commit 3f6848c4c6
2 changed files with 131 additions and 41 deletions
Download patch file Download diff file Expand all files Collapse all files

53
gm_platform/fw/tw_test.c
View file

@ -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;
}
}

119
gm_platform/fw/tw_test.py
View file

@ -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)
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser()
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
parser.add_argument('-b', '--baudrate', type=int, default=250000)
parser.add_argument('port', nargs='?', default=None)
parser.add_argument('dbfile')
args = parser.parse_args()
crc32, payload = unpack_head('I', cobs.decode(data[:-1]))
pid, seq, data = unpack_head('xBH', payload)
ser.write(ctrl_ack(pid))
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()}')