fw: dump accelerometer measurements over uart
This commit is contained in:
jaseg
parent
df51e1c508
commit
c67f7d626b
3 changed files with 160 additions and 51 deletions
|
|
@ -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]))
|
||||
|
|
|
|||
|
|
@ -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 ()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue