diff --git a/src/parser.c b/src/parser.c index 5dc8c43..5696d4c 100644 --- a/src/parser.c +++ b/src/parser.c @@ -46,6 +46,7 @@ #define PIEZO_CFG_DELAY_MAX_US 100 #define PIEZO_POST_SELECT_SETTLE_US 500 #define PIEZO_AVG_INTER_BURST_GAP_US 500 +#define PIEZO_DUMMY_CAPTURE_COUNT 5 static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][ECHO_ADC_MAX_SAMPLES]; static uint16_t echo_capture[ECHO_ADC_MAX_SAMPLES]; @@ -424,6 +425,22 @@ static int perform_piezo_sweep(void) return ECHO_STATUS_CAPTURE; } + for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++) + { + k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US); + unsigned int key = irq_lock(); + piezo_burst_sw((uint8_t)cfg->cycles); + k_busy_wait(capture_delay_us); + + err = echo_adc_capture(echo_capture, cfg->samples); + irq_unlock(key); + if (err) + { + DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err); + return ECHO_STATUS_CAPTURE; + } + } + memset(echo_accum, 0, sizeof(echo_accum)); for (uint16_t avg = 0; avg < cfg->avg; avg++) @@ -496,6 +513,21 @@ static int perform_single_piezo_capture(uint8_t cycles, return ECHO_STATUS_CAPTURE; } + for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++) + { + k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US); + unsigned int key = irq_lock(); + piezo_burst_sw(cycles); + k_busy_wait(delay_us); + + err = echo_adc_capture(echo_capture, num_samples); + irq_unlock(key); + if (err) + { + return ECHO_STATUS_CAPTURE; + } + } + memset(echo_accum, 0, sizeof(echo_accum)); for (uint16_t avg = 0; avg < averaging; avg++)