From c5d03206965938db1777f7a7e146c53f6286f9e5 Mon Sep 17 00:00:00 2001 From: jaseg Date: Wed, 24 May 2023 20:05:05 +0200 Subject: [PATCH] WIP --- include/adc.h | 6 ++- include/global.h | 2 + include/proto.h | 21 ++++++-- src/adc.c | 9 +++- src/main.c | 14 +++++- src/proto.c | 126 +++++++++++++++++++++++++++++++++++------------ 6 files changed, 139 insertions(+), 39 deletions(-) diff --git a/include/adc.h b/include/adc.h index 7e25c3a..577d6ec 100644 --- a/include/adc.h +++ b/include/adc.h @@ -7,7 +7,11 @@ struct adc_state { uint8_t txbuf[12]; uint8_t rxbuf[12]; uint32_t dma_ccr3; - int32_t data[2]; + uint32_t sampling_interval_us; + uint32_t sample_count; + int32_t samples[16][2]; + size_t sample_wr_idx; + size_t sample_rd_idx; bool has_adc; bool overload[2]; ErrorCode error; diff --git a/include/global.h b/include/global.h index 1ba0609..04bbea7 100644 --- a/include/global.h +++ b/include/global.h @@ -84,8 +84,10 @@ enum board_addr { }; void delay_us(int duration_us); +uint64_t get_sync_time(void); extern enum board_config board_config; extern uint64_t sys_time_us; +extern uint64_t sync_time_us; #endif /* __GLOBAL_H__ */ diff --git a/include/proto.h b/include/proto.h index 35a6b99..2195dfa 100644 --- a/include/proto.h +++ b/include/proto.h @@ -40,6 +40,13 @@ struct proto_state { uint8_t rx_buttons; bool rx_lid_open; bool rx_estop; + + uint32_t adc_samples[2][2][512]; + size_t adc_sp_wr_idx[2]; + size_t adc_sp_rd_idx[2]; + uint64_t adc_sample_timestamp[2]; + uint32_t adc_sampling_interval[2]; + uint32_t adc_sample_count[2]; }; enum proto_led_format { @@ -82,11 +89,19 @@ struct __attribute__((__packed__)) pkt_time_sync { uint64_t timestamp; }; +struct __attribute__((__packed__)) pkt_adc_data { + struct ll_pkt ll_pkt; + uint64_t timestamp; + uint32_t sampling_interval_us; + uint32_t sample_count; + uint8_t samples[16][2][3]; +}; + + extern struct proto_state st_proto; -void proto_init_host(void); -void proto_loop_host(void); - +void proto_init(void); +void proto_loop(void); #endif /* __PROTO_H__ */ diff --git a/src/adc.c b/src/adc.c index 2492925..63b385f 100644 --- a/src/adc.c +++ b/src/adc.c @@ -113,6 +113,7 @@ void adc_init() { adc_sync_write(ADC_REG_CLOCK, ADC_CLOCK_CH1_EN | ADC_CLOCK_CH2_EN | ADC_CLOCK_OSR_8192 | ADC_CLOCK_PWR_HIRES); adc_sync_write(ADC_REG_GAIN, ADC_GAIN1_PGAGAIN1_4 | ADC_GAIN1_PGAGAIN0_4); adc_sync_write(ADC_REG_MODE, (1< ADC_FSR_POS || val < ADC_FSR_NEG) { st_adc.overload[0] = true; @@ -175,7 +177,10 @@ void adc_dma_interrupt(uint32_t channel, uint32_t flags) { if (val&0x00800000) { val |= 0xff000000; } - st_adc.data[1] = val; + st_adc.samples[wr_idx][1] = val; + st_adc.sample_rd_idx = wr_idx; + st_adc.sample_wr_idx = (wr_idx+1) % COUNT_OF(st_adc.data); + st_adc.sample_count++; if (val > ADC_FSR_POS || val < ADC_FSR_NEG) { st_adc.overload[1] = true; diff --git a/src/main.c b/src/main.c index baade19..090d4c9 100644 --- a/src/main.c +++ b/src/main.c @@ -34,6 +34,7 @@ static void loop_adc(void); enum board_config board_config; uint64_t sys_time_us; +uint64_t sync_time_us; int bt_inputs() { bool bt0 = !!(GPIOB->IDR & (1<<2)); @@ -354,9 +355,11 @@ int main(void) { uart_st.addr = BADDR_MEAS_BASE + ((bt>>1) & 3); } } + proto_init(); usb_init(); sys_time_us = 0; + sync_time_us = 0; SysTick->LOAD = SystemCoreClock / SYSTICK_INTERVAL_US; /* 1ms interrupt interval */ SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk; NVIC_EnableIRQ(SysTick_IRQn); @@ -382,11 +385,13 @@ int main(void) { update_leds(); } + proto_loop(); + if (board_config == BCFG_DISPLAY) { loop_display(); - if (board_config == BCFG_MOTOR) { + } else if (board_config == BCFG_MOTOR) { loop_motor(); - if (board_config == BCFG_MEAS) { + } else if (board_config == BCFG_MEAS) { loop_adc(); } } @@ -499,6 +504,11 @@ void loop_adc() { void SysTick_Handler() { sys_time_us += SYSTICK_INTERVAL_US; + sync_time_us += SYSTICK_INTERVAL_US; +} + +uint64_t get_sync_time() { + return sync_time_us + SysTick->VAL * SYSTICK_INTERVAL_US / SysTick->LOAD; } void handle_usb_packet(void *packet, size_t len) { diff --git a/src/proto.c b/src/proto.c index 7c1de38..a280e70 100644 --- a/src/proto.c +++ b/src/proto.c @@ -1,26 +1,32 @@ #include +#include static void tx_time_sync(void); -static void tx_fetch_adc(int index); -static void proto_handle(bool timeout); +static ErrorCode tx_fetch_adc(int index); +static void proto_handle_host(bool timeout); static void proto_update_lcd_strings(void); -static void tx_update_display(void); +static ErrorCode tx_update_display(void); +static void tx_adc_data(void); struct proto_state st_proto; -void proto_init_host() { +void proto_init() { memset(&st_proto, 0, sizeof(st_proto)); } -void proto_loop_host() { - if (sys_time_us - st_proto.last_start > PROTO_UPDATE_INTERVAL_MS*1000 - st_proto.delta ) { - st_proto.fsm = ST_START; - } else if (st_proto.fsm > ST_START && st_proto.fsm < ST_WAITING) { - if (sys_time_us - uart_st.last_transmission > PROTO_RESPONSE_TIMEOUT_MS*1000) { - proto_handle(true); +void proto_loop() { + if (board_config == BCFG_MOTOR) { /* We are the protocol host */ + if (sys_time_us - st_proto.last_start > PROTO_UPDATE_INTERVAL_MS*1000 - st_proto.delta ) { + st_proto.fsm = ST_START; + proto_handle_host(false); + + } else if (st_proto.fsm > ST_START && st_proto.fsm < ST_WAITING) { + if (sys_time_us - uart_st.last_transmission > PROTO_RESPONSE_TIMEOUT_MS*1000) { + proto_handle_host(true); + } } } } @@ -32,12 +38,34 @@ void uart_handle_user_packet(struct ll_pkt *pkt, size_t len) { st_proto.rx_buttons = ((struct pkt_button_state *)pkt)->buttons; st_proto.rx_lid_open = st_proto.rx_buttons & PROTO_DSP_BT_LID; st_proto.rx_estop = st_proto.rx_buttons & PROTO_DSP_BT_ESTOP; + + } else if (pkt->type == PK_TIME_SYNC) { + sync_time_us = ((pkt_time_sync *)pkt)->timestamp; } - proto_handle(false); + if (board_config == BCFG_MOTOR) { /* We are the protocol host */ + if (pkt->type == PK_FETCH_RESPONSE) { + /* ... */ + + proto_handle_host(false); + + } else { + proto_handle_host(true); + } + + } else if (board_config == BCFG_DISPLAY) { + if (pkt->type == PK_DISPLAY_UPDATE) { + /* ... */ + } + } else if (board_config == BCFG_MEAS) { + if (pkt->type == PK_FETCH_ADC) { + tx_adc_data(); + } + } } -void proto_handle(bool timeout) { +void proto_handle_host(bool timeout) { + ErrorCode rc = ERR_SUCCESS; switch (st_proto.fsm) { case ST_START: st_proto.delta += (sys_time_us - st_proto.last_start) - UPDATE_INTERVAL_MS*1000; @@ -45,21 +73,21 @@ void proto_handle(bool timeout) { tx_time_sync(); st_proto.fsm = ST_TIME_SYNC; /* immediately enqueue next command, since time syncs are a broadcast and do not require a response. */ - proto_handle(false); + proto_handle_host(false); break; case ST_TIME_SYNC: - tx_fetch_adc(0); + rc = tx_fetch_adc(0); st_proto.fsm = ST_FETCH_0; break; case ST_FETCH_0: - tx_fetch_adc(1); + rc = tx_fetch_adc(1); st_proto.fsm = ST_FETCH_1; break; case ST_FETCH_1: - tx_update_display(); + rc = tx_update_display(); st_proto.fsm = ST_UPDATE_DISPLAY; break; @@ -71,6 +99,53 @@ void proto_handle(bool timeout) { st_proto.fsm = ST_WAITING; break; } + + if (rc) { + st_proto.error = rc; + st_proto.last_error = sys_time_us; + /* If there was a transmission error, immediately enqueue next transmission. */ + proto_handle_host(true); + } +} + +void tx_adc_data() { + struct pkt_adc_data pk = { + .ll_pkt = { + .src = uart_st.addr, + .dst = SER_ADDR_BROADCAST, + .pid = 0, + .type = PK_TIME_SYNC, + }, + .timestamp = sync_time_us, + .sampling_interval_us = st_adc.sampling_interval_us, + .sample_count = st_adc.sample_count, + }; + + /* Format samples stored in native 32 bit ints into 24 bit fields */ + ssize_t rd_idx = st_adc.sample_rd_idx; + uint8_t *p = samples; + for (size_t i=0; i>8)&0xff; + *p++ = (sp>>16)&0xff; + + sp = st_adc.samples[rd_idx][1]; + *p++ = sp&0xff; + *p++ = (sp>>8)&0xff; + *p++ = (sp>>16)&0xff; + + rd_idx -= 1; + if (rd_idx < 0) { + rd_idx = COUNT_OF(st_adc.samples); + } + } + + ErrorCode rc = uart_send_packet_nonblocking(&pk, sizeof(pk)); + if (rc) { + st_proto.error = rc; + st_proto.last_error = sys_time_us; + } } void tx_time_sync() { @@ -88,11 +163,10 @@ void tx_time_sync() { if (rc) { st_proto.error = rc; st_proto.last_error = sys_time_us; - proto_handle(true); } } -void tx_fetch_adc(int index) { +ErrorCode tx_fetch_adc(int index) { struct ll_pkt pk = { .src = uart_st.addr, .dst = BADDR_MEAS_BASE + index, @@ -100,12 +174,7 @@ void tx_fetch_adc(int index) { .type = PK_FETCH_ADC, }; - ErrorCode rc = uart_send_packet_nonblocking(&pk, sizeof(pk)); - if (rc) { - st_proto.error = rc; - st_proto.last_error = sys_time_us; - proto_handle(true); - } + return uart_send_packet_nonblocking(&pk, sizeof(pk)); } void proto_update_lcd_strings(void) { @@ -119,7 +188,7 @@ void proto_update_lcd_strings(void) { } } -void tx_update_display() { +ErrorCode tx_update_display() { struct pkt_update_display pk = { .ll_pkt = { .src = uart_st.addr, @@ -148,11 +217,6 @@ void tx_update_display() { memcpy(pk.lcd[0], st_proto.status_msg, strlen(st_proto.status_msg)); } - ErrorCode rc = uart_send_packet_nonblocking(&pk, sizeof(pk)); - if (rc) { - st_proto.error = rc; - st_proto.last_error = sys_time_us; - proto_handle(true); - } + return uart_send_packet_nonblocking(&pk, sizeof(pk)); }