fw: dump accelerometer measurements over uart

3 files changed, 160 insertions, 51 deletions

diff --git a/prototype/fw/include/global.h b/prototype/fw/include/global.h
index d0c56d7..0af7e49 100644
--- a/prototype/fw/include/global.h
+++ b/prototype/fw/include/global.h
@@ -12,6 +12,8 @@
 
 #include <generated/stm32_device.h>
 
+#define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x])))))
+
 #define APB1_PRESC (1<<(APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE1_Msk) >> RCC_CFGR_PPRE1_Pos]))
 #define APB2_PRESC (1<<(APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE2_Msk) >> RCC_CFGR_PPRE2_Pos]))
 #define AHB_PRESC  (1<<(AHBPrescTable[(RCC->CFGR & RCC_CFGR_HPRE_Msk) >> RCC_CFGR_HPRE_Pos]))
diff --git a/prototype/fw/src/main.c b/prototype/fw/src/main.c
index cf0c9f4..377180b 100644
--- a/prototype/fw/src/main.c
+++ b/prototype/fw/src/main.c
@@ -4,6 +4,16 @@
 #include "microcobs.h"
 #include "crc32.h"
 
+static uint8_t crc8_calc(uint8_t *data, size_t len);
+static bool parity_calc(uint8_t *data, size_t len);
+uint32_t mems_trx_word(uint32_t data);
+uint32_t mems_trx_cmd(uint32_t cmd);
+void mems_write_reg(int addr, int val);
+uint32_t mems_read_reg(int addr);
+int16_t mems_read_meas(int ch);
+void mems_spi_init(void);
+void mems_init(void);
+
 struct __attribute__((packed)) ll_pkt_trailer {
     uint32_t crc32;
 };
@@ -14,8 +24,8 @@ struct __attribute__((packed)) req_pkt {
 };
 
 struct __attribute__((packed)) res_pkt {
-    uint32_t req_seq;
     uint32_t res_seq;
+    uint16_t meas_data[16];
     struct ll_pkt_trailer trailer;
 };
 
@@ -156,7 +166,7 @@ uint32_t mems_read_reg(int addr) {
 int16_t mems_read_meas(int ch) {
     ch &= 3;
     mems_trx_cmd((ch<<MEMS_OPCODE_Pos) | (1<<MEMS_SEN_Pos));
-    for (int i=0; i<2000; i++)
+    for (int i=0; i<10; i++)
         asm volatile ("nop");
     uint32_t rv = mems_trx_cmd(3<<MEMS_OPCODE_Pos);
     /* shift 14-bit data left to align the MSB with the int16_t's sign bit */
@@ -166,7 +176,7 @@ int16_t mems_read_meas(int ch) {
 }
 
 void mems_spi_init(void) {
-    SPI1->CR1 = (6<<SPI_CR1_BR_Pos) | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI;
+    SPI1->CR1 = (1<<SPI_CR1_BR_Pos) | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI;
     SPI1->CR2 = (15<<SPI_CR2_DS_Pos);
     SPI1->CR1 |= SPI_CR1_SPE;
 }
@@ -182,10 +192,40 @@ void mems_init(void) {
     mems_write_reg(MEMS_REG_CTRL0, 0x01);
 }
 
+#define WIN_LEN 8
+int16_t meas_buf[WIN_LEN * 3] = {0};
+size_t meas_buf_wptr = 0;
+size_t meas_buf_rptr = 0;
+int res_seq = 0;
+
+void TIM1_BRK_TIM15_IRQHandler (void) {
+    TIM15->SR = 0;
+    int16_t data = mems_read_meas(0);
+
+    /* write into meas_buf as circular buffer */
+    meas_buf[meas_buf_wptr] = data;
+    meas_buf_wptr += 1;
+    if (meas_buf_wptr >= COUNT_OF(meas_buf)) {
+        meas_buf_wptr = 0;
+    }
+
+    /* set read pointer to oldest 8-measurement block by rounding down meas_buf_wptr by 8, then adding 8 and wrapping */
+    size_t tmp = 8 * (meas_buf_wptr / 8 + 1);
+    if (tmp >= COUNT_OF(meas_buf)) {
+        tmp = 0;
+    }
+
+    /* Update sequence pointer when the transmission window changes. */
+    if (tmp != meas_buf_rptr) {
+        res_seq += 1;
+        meas_buf_rptr = tmp;
+    }
+}
+
 int main(void) {
 
     RCC->AHBENR |= RCC_AHBENR_GPIOAEN;
-    RCC->APB2ENR |= RCC_APB2ENR_USART1EN | RCC_APB2ENR_SPI1EN;
+    RCC->APB2ENR |= RCC_APB2ENR_USART1EN | RCC_APB2ENR_SPI1EN | RCC_APB2ENR_TIM15EN;
 
 #define AFRL(pin, val) ((val) << ((pin)*4))
 #define AFRH(pin, val) ((val) << (((pin)-8)*4))
@@ -210,6 +250,12 @@ int main(void) {
     SystemCoreClockUpdate();
     int apb2_clock = SystemCoreClock / APB2_PRESC;
     
+    TIM15->PSC = apb2_clock / 1000000 * 100 - 1; /* 100us ticks */
+    TIM15->ARR = 1000 - 1; /* 100ms overflow interrupt interval */
+    TIM15->DIER = TIM_DIER_UIE;
+    TIM15->CR1 = TIM_CR1_CEN;
+    NVIC_EnableIRQ(TIM1_BRK_TIM15_IRQn);
+
     int baudrate = 115200;
 
     USART1->CR1 = USART_CR1_TE | USART_CR1_RE;
@@ -217,56 +263,31 @@ int main(void) {
     USART1->CR2 |= USART_CR2_RXINV; //| USART_CR2_TXINV;
     USART1->CR1 |= USART_CR1_UE;
 
-    /* FIXME DEUBG */
-    while (1) {
-        mems_init();
-        for (int i=0; i<100000; i++)
-            asm volatile("nop");
-        for (int i=0; i<300; i++) {
-            mems_read_meas(0);
-            //mems_read_reg(MEMS_REG_ID_SENSOR_TYPE);
-            for (int i=0; i<10000; i++)
-                asm volatile("nop");
-        }
-        for (int i=0; i<100000; i++)
-            asm volatile("nop");
-    }
+    mems_init();
 
-    int req_seq = 0;
-    int res_seq = 0;
-    struct req_pkt req_buf = { 0 };
     struct tx_state tx_st = { 0 };
     struct res_pkt res_buf = { 0 };
-    uint8_t rx_buf[512];
     uint8_t tx_buf[512];
+    /*
+    int req_seq = 0;
+    struct req_pkt req_buf = { 0 };
+    uint8_t rx_buf[512];
     size_t rx_char = 0;
     unsigned int rx_overrun = 0;
     unsigned int rx_cobs_error = 0;
     unsigned int rx_framing_error = 0;
     unsigned int rx_crc_error = 0;
+    */
 
     USART1->TDR = 0; /* Kick off transmission */
-    int i = 0;
     while (23) {
         if (tx_st.remaining_bytes == 0) {
-            if (i > 100) {
-                res_buf.req_seq = req_seq;
-                res_buf.res_seq = res_seq;
-                res_seq += 1;
-                packetize(&res_buf, &res_buf.trailer);
-                tx_st.tx_char = tx_buf;
-                tx_st.remaining_bytes = cobs_encode((uint8_t *)&res_buf, sizeof(res_buf), tx_buf, sizeof(tx_buf));;
-                /*
-                for (int k=0; k < 8; k++)
-                    tx_buf[k] = 255-k;
-                for (int k=0; k < 8; k++)
-                    tx_buf[8+k] = (k + 1) % 8;
-                tx_st.remaining_bytes = 16;
-                */
-                i = 0;
-            } else {
-                i++;
-            }
+            res_buf.res_seq = res_seq;
+            memcpy(res_buf.meas_data, meas_buf + meas_buf_rptr, 8 * sizeof(meas_buf[0]));
+            memcpy(res_buf.meas_data + 8, meas_buf + ((meas_buf_rptr + 8) % COUNT_OF(meas_buf)) , 8 * sizeof(meas_buf[0]));
+            packetize(&res_buf, &res_buf.trailer);
+            tx_st.tx_char = tx_buf;
+            tx_st.remaining_bytes = cobs_encode((uint8_t *)&res_buf, sizeof(res_buf), tx_buf, sizeof(tx_buf));;
         }
 
         if (USART1->ISR & USART_ISR_TXE && tx_st.remaining_bytes > 0) {
@@ -286,6 +307,8 @@ int main(void) {
 
         if (USART1->ISR & USART_ISR_RXNE) {
             uint8_t c = USART1->RDR;
+            (void) c;
+            /*
             if (!c) {
                 if (rx_char < sizeof(rx_buf)) {
                     int rc = cobs_decode(rx_buf, rx_char, (uint8_t *)&req_buf, sizeof(req_buf));
@@ -313,10 +336,57 @@ int main(void) {
                     rx_overrun += 1;
                 }
             }
+            */
         }
     }
 }
 
+void *memcpy(void *restrict dest, const void *restrict src, size_t n)
+{
+	unsigned char *d = dest;
+	const unsigned char *s = src;
+
+	for (; n; n--) *d++ = *s++;
+	return dest;
+}
+
+void *memset(void *dest, int c, size_t n)
+{
+	unsigned char *s = dest;
+	size_t k;
+
+	/* Fill head and tail with minimal branching. Each
+	 * conditional ensures that all the subsequently used
+	 * offsets are well-defined and in the dest region. */
+
+	if (!n) return dest;
+	s[0] = c;
+	s[n-1] = c;
+	if (n <= 2) return dest;
+	s[1] = c;
+	s[2] = c;
+	s[n-2] = c;
+	s[n-3] = c;
+	if (n <= 6) return dest;
+	s[3] = c;
+	s[n-4] = c;
+	if (n <= 8) return dest;
+
+	/* Advance pointer to align it at a 4-byte boundary,
+	 * and truncate n to a multiple of 4. The previous code
+	 * already took care of any head/tail that get cut off
+	 * by the alignment. */
+
+	k = -(uintptr_t)s & 3;
+	s += k;
+	n -= k;
+	n &= -4;
+
+	for (; n; n--, s++) *s = c;
+
+	return dest;
+}
+
 void __libc_init_array (void) __attribute__((weak));
 void __libc_init_array ()
 {
diff --git a/prototype/fw/tools/ser_test.py b/prototype/fw/tools/ser_test.py
index ebd95ed..34ef8f2 100644
--- a/prototype/fw/tools/ser_test.py
+++ b/prototype/fw/tools/ser_test.py
@@ -7,6 +7,7 @@ import string
 import serial
 import time
 import zlib
+import sqlite3
 
 from cobs import cobs
 
@@ -15,8 +16,21 @@ if __name__ == '__main__':
     parser = argparse.ArgumentParser()
     parser.add_argument('port')
     parser.add_argument('-b', '--baudrate', type=int, default=115200)
+    parser.add_argument('-p', '--print-hex', action='store_true', default=False)
+    parser.add_argument('-d', '--database', default=':memory:')
+    parser.add_argument('-q', '--quiet', action='store_true', default=False)
     args = parser.parse_args()
 
+    db = sqlite3.connect(args.database)
+    db.execute('''CREATE TABLE IF NOT EXISTS packets (
+    run_id INTEGER,
+    timestamp_us INTEGER,
+    data BLOB)''')
+
+    run_id, = db.execute('SELECT IFNULL(MAX(run_id), 0) FROM packets').fetchone()
+    run_id += 1
+    print(f'Run ID #{run_id}')
+
     ser = serial.Serial(args.port, args.baudrate, timeout=0)
 
     byte_count = 0
@@ -29,18 +43,22 @@ if __name__ == '__main__':
     field_colors = [ make_color(x) for x in [ 48, 48, 48, 48, 220, 220, 220, 220, 207, 207, 207, 207 ] ]
     last_tx = time.time()
     lastc = -10000
+    last_seq = None
     while True:
         data = ser.read()
         for c in data:
-            #if byte_count == 0:
-                #print(f'\033[38;5;244m{time.time() - start_time: 8.3f} \033[0m', end='')
+            if args.print_hex:
+                if byte_count == 0:
+                    print(f'\033[38;5;244m{time.time() - start_time: 8.3f} \033[0m', end='')
 
             col = '\033[91m' if c == 0 else '\033[0m' # if c == (lastc - 1) % 256 else '\033[92m')
-            #print(f'{col}{c:02x}', end=' ')
+            if args.print_hex:
+                print(f'{col}{c:02x}', end=' ')
             line += bytes([c])
             byte_count += 1
             if c == 0:
-                #print('   ' * (16 - byte_count), end='\033[0m\n')
+                if args.print_hex:
+                    print('   ' * (16 - byte_count), end='\033[0m\n')
                 byte_count =  16
                 try:
                     payload = cobs.decode(packet)
@@ -51,18 +69,37 @@ if __name__ == '__main__':
                     else:
                         crc_ok = False
                         ref_crc = ''
-                    if len(payload) == 12:
-                        print(' '.join(f'{col if col else ""}{c if c else 0:02x}' for col, c in itertools.zip_longest(field_colors,
-                        payload)), 'OK' if crc_ok else f'WRONG')
+                    #if len(payload) == 40:
+                    #    print(' '.join(f'{col if col else ""}{c if c else 0:02x}' for col, c in itertools.zip_longest(field_colors,
+                    #    payload)), 'OK' if crc_ok else f'WRONG')
+                    if crc_ok:
+                        seq, *data = struct.unpack('<I16H', payload[:-4])
+                        db.execute('INSERT INTO packets(run_id, timestamp_us, data) VALUES (?,?,?)',
+                                (run_id, int(time.time()*1000000), payload))
+                        db.commit()
+                        if seq != last_seq:
+                            print()
+                            if last_seq is not None:
+                                if seq - last_seq > 2:
+                                    for i in range(last_seq+1, seq):
+                                        print(make_color(196), i, end=' ', flush=True)
+                                elif seq - last_seq == 2:
+                                    print(make_color(248), last_seq+1, end=' ', flush=True)
+                            print(make_color(48), seq, end=' ', flush=True)
+                            last_seq = seq
+                        else:
+                            print('.', end='', flush=True)
                 except cobs.DecodeError:
-                    print('COBS framing error')
+                    if not args.quiet:
+                        print('COBS framing error', len(packet))
                 packet = b''
             else:
                 packet += bytes([c])
             isprint = lambda c: c in b'0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ!"#$%&\'()*+,-./:;<=>?@[\\]^_`{|}~ '
             if byte_count == 16:
-                printable = ''.join( chr(c) if isprint(c) else '.' for c in line )
-                #print(f'\033[93m | {printable}\033[0m')
+                if args.print_hex:
+                    printable = ''.join( chr(c) if isprint(c) else '.' for c in line )
+                    print(f'\033[93m | {printable}\033[0m')
                 byte_count = 0
                 line = b''
             lastc = c