Framing works now

2 files changed, 66 insertions, 50 deletions

diff --git a/fw/main.c b/fw/main.c
index 663f7a4..dfbb79a 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -348,14 +348,46 @@ void uart_config(void) {
     NVIC_SetPriority(USART1_IRQn, 3);
 }
 
+/* Error counters */
 static unsigned int overruns = 0;
 static unsigned int frame_overruns = 0;
+static unsigned int invalid = 0;
+
+volatile uint8_t *packet_received(int len) {
+    static int packet_state = 0; 
+    if (len == 0) {
+        packet_state = 0;
+    } else if (len == sizeof(rx_buf.set_fb_rq)/2) {
+        if (packet_state == 0) {
+            packet_state = 1;
+            return rx_buf.byte_data + (sizeof(rx_buf.set_fb_rq)/2);
+        } else if (packet_state == 1) {
+            if (fb_op == FB_WRITE) {
+                fb_op = FB_FORMAT;
+            } else {
+                /* FIXME An overrun happend. What should we do? */
+                frame_overruns++;
+            }
+            packet_state = 2;
+        }
+    } else {
+        /* FIXME An invalid packet has been received. What should we do? */
+        invalid++;
+        packet_state = 2;
+    }
+    return rx_buf.byte_data;
+}
+
 #define SYNC_LENGTH 32 /* Must be a power of two */
 void USART1_IRQHandler(void) {
-    static uint8_t expect_framing = 1;
+    static volatile uint8_t *writep = rx_buf.byte_data;
     static int rxpos = 0;
-    static int resync = SYNC_LENGTH+1;
-    static int sync_chars = 0;
+    static enum {
+        COBS_WAIT_SYNC = 0,
+        COBS_WAIT_START = 1,
+        COBS_RUNNING = 2
+    } cobs_state = 0;
+    static int cobs_count = 0;
 
     GPIOA->BSRR = GPIO_BSRR_BS_0; // Debug
 
@@ -363,42 +395,28 @@ void USART1_IRQHandler(void) {
         /* FIXME An overrun happend. What should we do? */
         overruns++;
         rxpos = 0;
-        expect_framing = 1;
+        cobs_state = COBS_WAIT_SYNC;
         USART1->ICR = USART_ICR_ORECF;
     } else { /* RXNE */
         uint8_t data = USART1->RDR;
 
-        if (data == 0x23)
-            sync_chars++;
-        else
-            sync_chars = 0;
-
-        if (resync) {
-            if (sync_chars == SYNC_LENGTH+1) {
-                resync = 0;
-            }
-        } else if (expect_framing) {
-            if (data == 0x42) {
-                expect_framing = 0;
-            } else {
-                rxpos = 0;
-                resync = 1;
-            }
+        if (data == 0x00) { /* End-of-packet */
+            writep = packet_received(rxpos);
+            cobs_state = COBS_WAIT_START;
+            rxpos = 0;
         } else {
-            rx_buf.byte_data[rxpos] = data;
-            rxpos++;
-            if ((rxpos&(SYNC_LENGTH-1)) == 0) {
-                expect_framing = 1;
-            }
-            if (rxpos >= sizeof(rx_buf.set_fb_rq)) {
-                rxpos = 0;
-                expect_framing = 1;
-                if (fb_op == FB_WRITE) {
-                    fb_op = FB_FORMAT;
-                } else {
-                    /* FIXME An overrun happend. What should we do? */
-                    frame_overruns++;
+            if (cobs_state == COBS_WAIT_SYNC) {
+                /* ignore data */
+            } else if (cobs_state == COBS_WAIT_START) {
+                cobs_count = data;
+                cobs_state = COBS_RUNNING;
+            } else {
+                if (--cobs_count == 0) {
+                    cobs_count = data;
+                    data = 0;
                 }
+
+                writep[rxpos++] = data;
             }
         }
     }
@@ -538,10 +556,6 @@ int main(void) {
             led_state = (led_state+1)&7;
         }
         if (fb_op == FB_FORMAT) {
-            for (int i=0; i<sizeof(rx_buf.set_fb_rq); i++) {
-                if (rx_buf.byte_data[i] == 0x42)
-                    asm("bkpt");
-            }
             transpose_data(rx_buf.byte_data, write_fb);
             fb_op = FB_UPDATE;
         }
diff --git a/fw/test.py b/fw/test.py
index 2f0da3f..97cf7aa 100755
--- a/fw/test.py
+++ b/fw/test.py
@@ -5,11 +5,15 @@ def chunked(data, chunk_size):
     for i in range(0, len(data), chunk_size):
         yield data[i:i+chunk_size]
 
-def frame_packet(data, chunk_size=32, frame_char=b'\x42'):
-    return frame_char + frame_char.join(chunked(data, chunk_size))
-
-def sync_frame(sync_char=b'\x23', chunk_size=32):
-    return sync_char*(chunk_size+1)
+def frame_packet(data):
+    if len(data) > 254:
+        raise ValueError('Input too long')
+    out = b''
+    for run in data.split(b'\0'):
+        out += bytes([len(run)+1])
+        out += run
+    out += b'\0'
+    return out
 
 def format_packet(data):
     out = b''
@@ -18,6 +22,7 @@ def format_packet(data):
         al, bl, cl, dl = a&0xff, b&0xff, c&0xff, d&0xff
         # FIXME check order of high bits
         out += bytes([al, bl, cl, dl, (ah<<6 | bh<<4 | ch<<2 | dh<<0)&0xff])
+    out += bytes([1, 0, 0, 0]) # global intensity
     return out
 
 if __name__ == '__main__':
@@ -35,17 +40,14 @@ if __name__ == '__main__':
     frames = \
             [black]*10 +\
             [red]*10 +\
-            [[i]*frame_len for i in range(0, 256, 4)] +\
-            [[(i + (d//8)*8) % 256*8 for d in range(frame_len)] for i in range(0, 256, 16)]
+            [[i]*frame_len for i in range(256)] +\
+            [[(i + (d//8)*8) % 256*8 for d in range(frame_len)] for i in range(256)]
 
-    frames = [red, black]*5
+    #frames = [red, black]*5
     while True:
-        print('Sending sync structure')
-        ser.write(sync_frame())
         for i, frame in enumerate(frames):
             formatted = format_packet(frame)
-            #formatted = format_packet(list(range(256)))
-            framed = frame_packet(formatted)
+            framed = b'\0' + frame_packet(formatted[:162]) + frame_packet(formatted[162:])
             print('sending', i, len(frame), len(formatted), len(framed))
             ser.write(framed)
-            time.sleep(0.1)
+            time.sleep(0.02)