1 files changed, 111 insertions, 41 deletions

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 ()
 {