From c67f7d626b76238e561304b53f31afdad7e2f671 Mon Sep 17 00:00:00 2001 From: jaseg Date: Thu, 18 Mar 2021 10:11:41 +0100 Subject: fw: dump accelerometer measurements over uart --- prototype/fw/src/main.c | 152 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 111 insertions(+), 41 deletions(-) (limited to 'prototype/fw/src') 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<CR1 = (6<CR1 = (1<CR2 = (15<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 () { -- cgit