478 lines
13 KiB
C
478 lines
13 KiB
C
/*******************************************************************************
|
|
* @file piezo_measure.c
|
|
* @brief Piezo echo measurement sequencing
|
|
******************************************************************************/
|
|
#include <zephyr/kernel.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
|
|
#include "piezo_measure.h"
|
|
#include "app_nvs.h"
|
|
#include "debug_print.h"
|
|
|
|
/* burst 주파수 */
|
|
#define PIEZO_CFG_FREQ_1_8MHZ 0
|
|
#define PIEZO_CFG_FREQ_1_9MHZ 1
|
|
#define PIEZO_CFG_FREQ_2_0MHZ 2
|
|
#define PIEZO_CFG_FREQ_2_1MHZ 3
|
|
#define PIEZO_CFG_FREQ_2_2MHZ 4
|
|
#define PIEZO_CFG_FREQ_2_3MHZ 5
|
|
|
|
/* 앱에서 저장/변경 가능한 piezo 측정 기본값 */
|
|
#define PIEZO_CFG_FREQ_DEFAULT PIEZO_CFG_FREQ_2_1MHZ
|
|
#define PIEZO_CFG_CYCLES_DEFAULT 7
|
|
#define PIEZO_CFG_DELAY_DEFAULT 10
|
|
#define PIEZO_CFG_SAMPLES_DEFAULT 100
|
|
#define PIEZO_CFG_AVG_DEFAULT 3
|
|
|
|
/* 설정값 검증 범위 */
|
|
#define PIEZO_CFG_FREQ_MAX PIEZO_CFG_FREQ_2_3MHZ
|
|
#define PIEZO_CFG_AVG_MAX 10
|
|
#define PIEZO_CFG_SAMPLES_MIN 80
|
|
#define PIEZO_CFG_SAMPLES_MAX 117
|
|
#define PIEZO_CFG_DELAY_MAX_US 50
|
|
|
|
/* piezo_select_channel() 안에서 MUX settle 1.3 ms를 이미 기다린 뒤 analog path/ADC 입력 안정화를 위해 0.5 ms를 한 번 더 기다림 */
|
|
#define PIEZO_POST_SELECT_SETTLE_US 500
|
|
|
|
/* burst 사이의 회복 시간 */
|
|
#define PIEZO_AVG_INTER_BURST_GAP_US 650
|
|
|
|
/* real capture 전에 같은 채널에서 버스트+ADC capture를 버리는 횟수(dummy) */
|
|
#define PIEZO_DUMMY_CAPTURE_COUNT 1
|
|
|
|
/* 채널 0 시작 전 안정을 위한 추가 딜레이 */
|
|
#define PIEZO_CH0_WARMUP_TO_REAL_SETTLE_US 3000
|
|
|
|
/* 6채널 sweep 결과 (각 채널의 평균 완료 후 최종만 저장) */
|
|
static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][PIEZO_MEASURE_MAX_SAMPLES];
|
|
|
|
/* 단일 capture 임시 버퍼 (dummy capture와 real capture 모두 이 버퍼 재사용) */
|
|
static uint16_t echo_capture[PIEZO_MEASURE_MAX_SAMPLES];
|
|
|
|
/* 평균 누적 버퍼 (uint16 샘플을 avg 횟수만큼 더하므로 overflow 여유를 위해 uint32) */
|
|
static uint32_t echo_accum[PIEZO_MEASURE_MAX_SAMPLES];
|
|
|
|
/* NVS에서 읽어 오거나 mcs? 명령으로 갱신되는 현재 측정 설정 */
|
|
static piezo_config_t g_piezo_config = {
|
|
.freq = PIEZO_CFG_FREQ_DEFAULT,
|
|
.cycles = PIEZO_CFG_CYCLES_DEFAULT,
|
|
.avg = PIEZO_CFG_AVG_DEFAULT,
|
|
.delay_us = PIEZO_CFG_DELAY_DEFAULT,
|
|
.samples = PIEZO_CFG_SAMPLES_DEFAULT,
|
|
};
|
|
|
|
/*
|
|
* 저장/측정에 사용할 piezo 설정값 검증
|
|
*/
|
|
bool piezo_config_validate(const piezo_config_t *cfg)
|
|
{
|
|
if (cfg == NULL)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (cfg->freq > PIEZO_CFG_FREQ_MAX) // 주파수 : 1.8 MHz (00 00) ~ 2.3 MHz (00 05) 범위에서만 동작하도록 제한
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if ((cfg->cycles < 3U) || (cfg->cycles > 7U)) // burst cycles : 3 ~ 7 범위만 허용
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if ((cfg->avg == 0U) || (cfg->avg > PIEZO_CFG_AVG_MAX)) // averaging : 1 ~ 10 범위만 허용
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (cfg->delay_us > PIEZO_CFG_DELAY_MAX_US) // burst-to-capture delay : 0 ~ 50 us 범위만 허용
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if ((cfg->samples < PIEZO_CFG_SAMPLES_MIN) || (cfg->samples > PIEZO_CFG_SAMPLES_MAX)) // sample count : 80 ~ 117 범위만 허용
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
const piezo_config_t *piezo_config_get(void)
|
|
{
|
|
return &g_piezo_config;
|
|
}
|
|
|
|
/*
|
|
* 부팅 시 NVS 설정 읽기 (없거나 유효하지 않으면 기본값 유지)
|
|
*/
|
|
int piezo_config_init(void)
|
|
{
|
|
int err = app_nvs_init(&g_piezo_config, piezo_config_validate, NULL);
|
|
if (err)
|
|
{
|
|
return err;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
* 런타임 설정 변경: RAM 설정을 갱신한 뒤 NVS에도 저장
|
|
*/
|
|
int piezo_config_set(const piezo_config_t *cfg)
|
|
{
|
|
if (!piezo_config_validate(cfg))
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
g_piezo_config = *cfg;
|
|
return app_nvs_save_piezo(&g_piezo_config);
|
|
}
|
|
|
|
/*
|
|
* sweep 명령이 끝난 뒤 BLE 응답에서 채널별 결과 버퍼 참조 시 사용
|
|
*/
|
|
const uint16_t *piezo_measure_channel_buffer(uint8_t channel)
|
|
{
|
|
if (channel >= PIEZO_NUM_CHANNELS)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
return piezo_channels[channel];
|
|
}
|
|
|
|
/*
|
|
* mec?/mad? 같은 단일 채널 명령의 최종 결과 버퍼 반환
|
|
*/
|
|
const uint16_t *piezo_measure_single_buffer(void)
|
|
{
|
|
return echo_capture;
|
|
}
|
|
|
|
/*
|
|
* piezo 측정 공통 준비 단계
|
|
* 전원 인가, piezo GPIO 초기화, echo ADC 초기화/wake를 한 번에 수행
|
|
*/
|
|
int piezo_measure_start_session(void)
|
|
{
|
|
int err = piezo_init();
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[PIEZO] init fail err=%d\r\n", err);
|
|
return ECHO_STATUS_PIEZO;
|
|
}
|
|
|
|
piezo_power_on();
|
|
k_msleep(PIEZO_POWER_STABILIZE_MS);
|
|
|
|
err = echo_adc_init();
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
|
|
return ECHO_STATUS_ADC_INIT;
|
|
}
|
|
|
|
err = echo_adc_wake();
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[ECHO] wake fail err=%d\r\n", err);
|
|
return ECHO_STATUS_ADC_INIT;
|
|
}
|
|
|
|
DBG_PRINTF("[SWEEP] piezo session ready\r\n");
|
|
return ECHO_STATUS_OK;
|
|
}
|
|
|
|
/*
|
|
* 6채널 전체 sweep
|
|
* 각 채널마다 dummy capture를 먼저 버린 뒤, cfg->avg회 real capture를 sample index별로 평균
|
|
*/
|
|
int piezo_measure_sweep(void)
|
|
{
|
|
const piezo_config_t *cfg = piezo_config_get();
|
|
|
|
uint8_t freq = cfg->freq;
|
|
uint8_t cycles = cfg->cycles;
|
|
uint16_t capture_delay_us = cfg->delay_us;
|
|
uint16_t samples = cfg->samples;
|
|
uint8_t avg = cfg->avg;
|
|
|
|
if (capture_delay_us < 0)
|
|
{
|
|
capture_delay_us = 0;
|
|
}
|
|
|
|
DBG_PRINTF("[SWEEP] freq=0x%04X cycles=%u avg=%u delay_us=%u samples=%u\r\n", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
|
|
|
|
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
|
|
{
|
|
//DBG_PRINTF("[SWEEP] ch=%d start\r\n", ch);
|
|
|
|
// MUX 채널 선택
|
|
int err = piezo_select_channel(ch);
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[PIEZO] mux fail ch=%d err=%d\r\n", ch, err);
|
|
return ECHO_STATUS_MUX;
|
|
}
|
|
|
|
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
|
|
|
// ADC dummy read/wake 2byte (채널 전환 직후 첫 ADC 접근을 실제 데이터에 섞지 않기 위한 준비 단계)
|
|
err = echo_adc_wake();
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[ECHO] dummy read fail ch=%d err=%d\r\n", ch, err);
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
if (ch == 0U) // 채널 0 시작 전에만 dummy burst + ADC capture 수행
|
|
{
|
|
// Dummy burst + ADC caputre
|
|
/*
|
|
for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
|
|
{
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
|
|
// 실제 측정 신호 흔들림 방지를 위해 dummy 측정에도 IRQ 잠금 적용
|
|
unsigned int key = irq_lock();
|
|
|
|
// burst + delay + capture
|
|
piezo_burst_sw_freq(freq, cycles); // 주파수 옵션 있는 burst 함수
|
|
k_busy_wait(capture_delay_us);
|
|
err = echo_adc_capture(echo_capture, samples);
|
|
|
|
// IRQ 잠금 해제
|
|
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;
|
|
}
|
|
}*/
|
|
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
piezo_burst_sw_freq(freq, cycles);
|
|
k_busy_wait(capture_delay_us);
|
|
err = echo_adc_capture(echo_capture, samples);
|
|
if (err)
|
|
{
|
|
//DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err);
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
// 마지막 dummy capture와 첫 real capture 사이에도 같은 회복 시간 + 추가 안정화
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US + PIEZO_CH0_WARMUP_TO_REAL_SETTLE_US);
|
|
}
|
|
|
|
// 여기부터 실제 평균에 들어갈 capture
|
|
memset(echo_accum, 0, sizeof(echo_accum));
|
|
|
|
for (uint16_t a = 0; a < avg; a++)
|
|
{
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
|
|
// burst-to-capture 사이 타이밍 흔들림을 줄이기 위해 real capture 구간만 IRQ를 잠금
|
|
unsigned int key = irq_lock();
|
|
|
|
// burst + delay + capture
|
|
piezo_burst_sw_freq(freq, cycles); // 주파수 옵션 있는 burst 함수
|
|
k_busy_wait(capture_delay_us);
|
|
err = echo_adc_capture(echo_capture, samples);
|
|
|
|
// IRQ 잠금 해제
|
|
irq_unlock(key);
|
|
|
|
if (err)
|
|
{
|
|
DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, a, err);
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
// 같은 sample index끼리 누적
|
|
for (uint16_t i = 0; i < samples; i++)
|
|
{
|
|
echo_accum[i] += echo_capture[i];
|
|
}
|
|
}
|
|
|
|
// 채널별 최종 평균 파형 저장 - 이후 cmd handler가 reb: 패킷으로 전송
|
|
for (uint16_t i = 0; i < samples; i++)
|
|
{
|
|
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / avg);
|
|
}
|
|
|
|
//DBG_PRINTF("[SWEEP] ch=%d done\r\n", ch);
|
|
}
|
|
|
|
//DBG_PRINTF("[SWEEP] done\r\n");
|
|
return ECHO_STATUS_OK;
|
|
}
|
|
|
|
/*
|
|
* 테스트용
|
|
* 단일 채널 burst + echo capture
|
|
* mec? 디버그/테스트 명령에서 사용하며, 결과는 echo_capture에 평균 저장
|
|
*/
|
|
int piezo_measure_single_capture(uint8_t freq, uint8_t cycles, uint16_t delay_us, uint16_t num_samples, uint16_t averaging, uint8_t channel)
|
|
{
|
|
if (channel >= PIEZO_NUM_CHANNELS)
|
|
{
|
|
return ECHO_STATUS_MUX;
|
|
}
|
|
|
|
if ((num_samples == 0U) || (num_samples > PIEZO_MEASURE_MAX_SAMPLES))
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
/* averaging 0은 명령 파라미터 누락/미지정으로 보고 1회 capture로 보정 */
|
|
if (averaging == 0U)
|
|
{
|
|
averaging = 1U;
|
|
}
|
|
|
|
int err = piezo_select_channel(channel);
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_MUX;
|
|
}
|
|
|
|
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
|
|
|
err = echo_adc_wake();
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
// real capture 전에 같은 채널에서 dummy burst/capture를 수행하고 버림
|
|
for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
|
|
{
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
piezo_burst_sw_freq(freq, cycles);
|
|
k_busy_wait(delay_us);
|
|
|
|
err = echo_adc_capture(echo_capture, num_samples);
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
}
|
|
|
|
// 마지막 dummy capture와 첫 real capture 사이에도 같은 회복 시간
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
|
|
memset(echo_accum, 0, sizeof(echo_accum));
|
|
|
|
for (uint16_t avg = 0; avg < averaging; avg++)
|
|
{
|
|
if (avg > 0U)
|
|
{
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
}
|
|
|
|
unsigned int key = irq_lock();
|
|
|
|
piezo_burst_sw_freq(freq, cycles);
|
|
k_busy_wait(delay_us);
|
|
err = echo_adc_capture(echo_capture, num_samples);
|
|
|
|
irq_unlock(key);
|
|
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
for (uint16_t i = 0; i < num_samples; i++)
|
|
{
|
|
echo_accum[i] += echo_capture[i];
|
|
}
|
|
}
|
|
|
|
for (uint16_t i = 0; i < num_samples; i++)
|
|
{
|
|
echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
|
|
}
|
|
|
|
return ECHO_STATUS_OK;
|
|
}
|
|
|
|
/*
|
|
* 테스트용
|
|
* 단일 채널 ADC-only capture
|
|
* piezo burst 없이 현재 echo ADC 입력만 평균해서 확인할 때 사용
|
|
*/
|
|
int piezo_measure_adc_only_capture(uint16_t num_samples, uint16_t averaging, uint8_t channel)
|
|
{
|
|
if (channel >= PIEZO_NUM_CHANNELS)
|
|
{
|
|
return ECHO_STATUS_MUX;
|
|
}
|
|
|
|
if ((num_samples == 0U) || (num_samples > PIEZO_MEASURE_MAX_SAMPLES))
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
if (averaging == 0U)
|
|
{
|
|
averaging = 1U;
|
|
}
|
|
|
|
int err = piezo_select_channel(channel);
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_MUX;
|
|
}
|
|
|
|
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
|
|
|
err = echo_adc_wake();
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
memset(echo_accum, 0, sizeof(echo_accum));
|
|
|
|
for (uint16_t avg = 0; avg < averaging; avg++)
|
|
{
|
|
if (avg > 0U)
|
|
{
|
|
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
|
}
|
|
|
|
// burst가 없으므로 ADC capture 자체의 간섭만 줄이기 위해 짧게 IRQ를 잠금
|
|
unsigned int key = irq_lock();
|
|
|
|
err = echo_adc_capture(echo_capture, num_samples);
|
|
|
|
irq_unlock(key);
|
|
|
|
if (err)
|
|
{
|
|
return ECHO_STATUS_CAPTURE;
|
|
}
|
|
|
|
for (uint16_t i = 0; i < num_samples; i++)
|
|
{
|
|
echo_accum[i] += echo_capture[i];
|
|
}
|
|
}
|
|
|
|
for (uint16_t i = 0; i < num_samples; i++)
|
|
{
|
|
echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
|
|
}
|
|
|
|
return ECHO_STATUS_OK;
|
|
}
|