diff options
| -rw-r--r-- | prototype/fw/include/global.h | 2 | ||||
| -rw-r--r-- | prototype/fw/src/main.c | 152 | ||||
| -rw-r--r-- | prototype/fw/tools/ser_test.py | 57 |
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 |