jh.chun/VesiScan-Basic_Zephyr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

기타등등

Browse Source
This commit is contained in:
jhchun
2026-06-11 17:48:35 +09:00
parent d7c5b34da3
commit 1f7436e27d
14 changed files with 714 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/parser.c
+242 -48
View File
@@ -8,6 +8,11 @@
* 현재 구현된 커맨드: msn? (배터리 측정), mls? (LED 상태 설정)
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
#if IS_ENABLED(CONFIG_SETTINGS)
#include <zephyr/settings/settings.h>
#endif
#include <errno.h>
#include <string.h>
#include <limits.h>
@@ -40,9 +45,15 @@
#define PIEZO_CFG_DELAY_DEFAULT 10
#define PIEZO_CFG_SAMPLES_DEFAULT 100
#define PIEZO_CFG_AVG_DEFAULT 10
#define PIEZO_CFG_AVG_MAX 100
#define PIEZO_CFG_DELAY_MAX_US 1000
#define PIEZO_POST_SELECT_SETTLE_US 500
#define PIEZO_AVG_INTER_BURST_GAP_US 500
#define PIEZO_SETTINGS_NAME "vesiscan"
#define PIEZO_SETTINGS_CFG_KEY "piezo"
#define PIEZO_SETTINGS_FULL_KEY "vesiscan/piezo"
static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][ECHO_ADC_MAX_SAMPLES];
static uint16_t echo_capture[ECHO_ADC_MAX_SAMPLES];
static uint32_t echo_accum[ECHO_ADC_MAX_SAMPLES];
@@ -52,6 +63,140 @@ static uint8_t tx_echo_buf[4 + 2 + (ECHO_ADC_MAX_SAMPLES * 2) + 2];
static uint8_t tx_bundle_buf[22];
static uint8_t tx_cfg_buf[16];
static uint8_t tx_ascii_buf[4 + HW_NO_LENGTH + 2];
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,
};
static bool piezo_config_validate(const piezo_config_t *cfg)
{
if (cfg == NULL) {
return false;
}
if (cfg->freq != PIEZO_CFG_FREQ_DEFAULT) {
return false;
}
if ((cfg->cycles < 3U) || (cfg->cycles > 7U)) {
return false;
}
if ((cfg->avg == 0U) || (cfg->avg > PIEZO_CFG_AVG_MAX)) {
return false;
}
if (cfg->delay_us > PIEZO_CFG_DELAY_MAX_US) {
return false;
}
if ((cfg->samples == 0U) || (cfg->samples > ECHO_ADC_MAX_SAMPLES)) {
return false;
}
return true;
}
static void piezo_config_log(const char *prefix, const piezo_config_t *cfg)
{
DBG_PRINTF("%s freq=%u cycles=%u avg=%u delay=%u samples=%u\r\n",
prefix,
cfg->freq,
cfg->cycles,
cfg->avg,
cfg->delay_us,
cfg->samples);
}
const piezo_config_t *piezo_config_get(void)
{
return &g_piezo_config;
}
#if IS_ENABLED(CONFIG_SETTINGS)
static int piezo_settings_set(const char *name, size_t len,
settings_read_cb read_cb, void *cb_arg)
{
const char *next;
int rc;
if (settings_name_steq(name, PIEZO_SETTINGS_CFG_KEY, &next) && (next == NULL))
{
piezo_config_t loaded;
if (len != sizeof(loaded)) {
DBG_ERR("[CFG] piezo settings size mismatch (%u)\r\n", (uint32_t)len);
return -EINVAL;
}
rc = read_cb(cb_arg, &loaded, sizeof(loaded));
if (rc < 0) {
DBG_ERR("[CFG] piezo settings read failed err=%d\r\n", rc);
return rc;
}
if (rc != sizeof(loaded)) {
DBG_ERR("[CFG] piezo settings short read (%d)\r\n", rc);
return -EINVAL;
}
if (!piezo_config_validate(&loaded)) {
DBG_ERR("[CFG] piezo settings invalid, keep defaults\r\n");
return 0;
}
g_piezo_config = loaded;
piezo_config_log("[CFG] piezo restored", &g_piezo_config);
return 0;
}
return -ENOENT;
}
SETTINGS_STATIC_HANDLER_DEFINE(piezo_settings,
PIEZO_SETTINGS_NAME,
NULL,
piezo_settings_set,
NULL,
NULL);
#endif
int piezo_config_init(void)
{
#if IS_ENABLED(CONFIG_SETTINGS)
int err = settings_subsys_init();
if (err && (err != -EALREADY)) {
DBG_ERR("[CFG] settings init failed err=%d\r\n", err);
return err;
}
err = settings_load_subtree(PIEZO_SETTINGS_NAME);
if (err) {
DBG_ERR("[CFG] piezo settings load failed err=%d\r\n", err);
return err;
}
#endif
piezo_config_log("[CFG] piezo active", &g_piezo_config);
return 0;
}
static int piezo_config_save(void)
{
#if IS_ENABLED(CONFIG_SETTINGS)
int err = settings_save_one(PIEZO_SETTINGS_FULL_KEY,
&g_piezo_config,
sizeof(g_piezo_config));
if (err) {
DBG_ERR("[CFG] piezo settings save failed err=%d\r\n", err);
}
return err;
#else
return 0;
#endif
}
/*==============================================================================
* CRC16 (CRC-CCITT, Nordic SDK 호환)
@@ -312,6 +457,13 @@ static int start_piezo_session(void)
static int perform_piezo_sweep(void)
{
const piezo_config_t *cfg = piezo_config_get();
uint16_t capture_delay_us = cfg->delay_us;
if (capture_delay_us < PIEZO_BURST_TO_ADC_DELAY_US) {
capture_delay_us = PIEZO_BURST_TO_ADC_DELAY_US;
}
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
DBG_PRINTF("[MBB] sweep ch=%d start\r\n", ch);
@@ -334,32 +486,32 @@ static int perform_piezo_sweep(void)
memset(echo_accum, 0, sizeof(echo_accum));
for (uint8_t avg = 0; avg < PIEZO_AVERAGE_COUNT; avg++)
for (uint16_t avg = 0; avg < cfg->avg; avg++)
{
if (avg > 0U)
{
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
}
piezo_burst_sw(PIEZO_SW_BURST_CYCLES);
k_busy_wait(PIEZO_BURST_TO_ADC_DELAY_US);
piezo_burst_sw((uint8_t)cfg->cycles);
k_busy_wait(capture_delay_us);
err = echo_adc_capture(echo_capture, ECHO_ADC_MAX_SAMPLES);
err = echo_adc_capture(echo_capture, cfg->samples);
if (err)
{
DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, avg, err);
return ECHO_STATUS_CAPTURE;
}
for (uint16_t i = 0; i < ECHO_ADC_MAX_SAMPLES; i++)
for (uint16_t i = 0; i < cfg->samples; i++)
{
echo_accum[i] += echo_capture[i];
}
}
for (uint16_t i = 0; i < ECHO_ADC_MAX_SAMPLES; i++)
for (uint16_t i = 0; i < cfg->samples; i++)
{
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / PIEZO_AVERAGE_COUNT);
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / cfg->avg);
}
DBG_PRINTF("[MBB] sweep ch=%d done\r\n", ch);
@@ -521,12 +673,15 @@ static int cmd_mst(const uint8_t *data, uint8_t data_len)
return 1;
}
/**
* 테스트용
* mpa?: piezo TX/RX 전원 레일만 켬
*/
static int cmd_mpa(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
/* mpa?: piezo TX/RX 전원 레일만 켠다. 측정은 하지 않는다. */
DBG_CORE("[MPA] enter\r\n");
if (piezo_init() != 0) {
DBG_ERR("[MPA] piezo_init failed\r\n");
@@ -563,10 +718,16 @@ static int cmd_mpb(const uint8_t *data, uint8_t data_len)
return 1;
}
/**
* 테스트용
* mpc?: burst만 한 번 발생시키는 테스트 명령
* echo를 읽지 않고 초음파가 나가는지만 볼 때 사용
*/
static int cmd_mpc(const uint8_t *data, uint8_t data_len)
{
uint16_t cycles = 5;
uint16_t freq_option = 1;
const piezo_config_t *cfg = piezo_config_get();
uint16_t cycles = cfg->cycles;
uint16_t freq_option = cfg->freq;
uint16_t piezo_ch = 0;
get_data_u16_be(data, data_len, 0, &cycles);
@@ -575,10 +736,6 @@ static int cmd_mpc(const uint8_t *data, uint8_t data_len)
ARG_UNUSED(freq_option);
/*
* mpc?: burst만 한 번 발생시키는 테스트 명령.
* echo를 읽지 않으므로 "초음파가 나가는지"만 빠르게 볼 때 쓴다.
*/
if ((cycles < 3U) || (cycles > 7U))
{
send_response_u16("rpc:", 2);
@@ -603,8 +760,8 @@ static int cmd_mpc(const uint8_t *data, uint8_t data_len)
}
/*
* 현재 Zephyr 포팅본은 2.1MHz SW burst 하나만 구현되어 있다.
* 레거시의 freq_option 값은 받아두되, 아직은 같은 burst 함수로 처리한다.
* 현재 Zephyr 포팅본은 2.1MHz SW burst 하나만 구현되어 있음
* 레거시의 freq_option 값은 받아두되, 아직은 같은 burst 함수로 처리
*/
piezo_burst_sw((uint8_t)cycles);
piezo_power_off();
@@ -616,11 +773,12 @@ static int cmd_mpc(const uint8_t *data, uint8_t data_len)
static int cmd_mec(const uint8_t *data, uint8_t data_len)
{
uint16_t freq_option = 1;
uint16_t delay_us = PIEZO_BURST_TO_ADC_DELAY_US;
uint16_t num_samples = ECHO_ADC_MAX_SAMPLES;
uint16_t cycles = PIEZO_SW_BURST_CYCLES;
uint16_t averaging = 1;
const piezo_config_t *cfg = piezo_config_get();
uint16_t freq_option = cfg->freq;
uint16_t delay_us = cfg->delay_us;
uint16_t num_samples = cfg->samples;
uint16_t cycles = cfg->cycles;
uint16_t averaging = cfg->avg;
uint16_t piezo_ch = 0;
get_data_u16_be(data, data_len, 0, &freq_option);
@@ -633,8 +791,8 @@ static int cmd_mec(const uint8_t *data, uint8_t data_len)
ARG_UNUSED(freq_option);
/*
* mec?: 단일 채널 burst + echo capture.
* maa?/mbb?보다 가벼워서 "한 채널만 먼저 살아 있는지" 확인하기 좋다.
* 테스트용
* mec?: 단일 채널 burst + echo capture
*/
if (num_samples > ECHO_ADC_MAX_SAMPLES) {
num_samples = ECHO_ADC_MAX_SAMPLES;
@@ -671,19 +829,19 @@ static int cmd_mec(const uint8_t *data, uint8_t data_len)
return 1;
}
/**
* 테스트용(이전 정렬모드)
* maa?:
* - piezo 6채널 burst 순서대로 쏘고
* - echo 샘플을 채널별로 모은 뒤
* - reb: 패킷 6개 전송
* - raa: 상태값은 전체 작업 성공/실패 요약
*/
static int cmd_maa(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
/*
* maa?:
* - piezo 6채널을 순서대로 쏘고
* - echo 샘플을 채널별로 모은 뒤
* - reb: 패킷 6개를 보낸다.
*
* 마지막 raa: 상태값은 "전체 작업 성공/실패 요약"이다.
*/
processing = true;
power_button_suspend(true);
@@ -697,7 +855,7 @@ static int cmd_maa(const uint8_t *data, uint8_t data_len)
{
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
send_response_echo(piezo_channels[ch], ECHO_ADC_MAX_SAMPLES);
send_response_echo(piezo_channels[ch], piezo_config_get()->samples);
}
}
@@ -710,6 +868,9 @@ static int cmd_maa(const uint8_t *data, uint8_t data_len)
return 1;
}
/**
* 전체 측정 시 사용
*/
static int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
@@ -747,7 +908,7 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len)
if (status == ECHO_STATUS_OK)
{
/* info 성격 데이터는 echo sweep이 정상 끝났을 때만 읽는다. */
/* info 성격 데이터는 echo sweep이 정상 끝났을 때만 읽음 */
DBG_PRINTF("[MBB] battery read\r\n");
batt_mv = battery_read_mv();
if (batt_mv < 0)
@@ -777,7 +938,7 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len)
if (status == ECHO_STATUS_OK)
{
/* rbb: 한 번에 보내는 "요약 정보 묶음" 패킷 */
/* rbb: 센서(배터리+IMU+온도) 정보 패킷 */
DBG_PRINTF("[MBB] response tx start\r\n");
send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);
@@ -785,7 +946,7 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
/* reb: 채널별 raw echo 파형 */
DBG_PRINTF("[MBB] tx reb ch=%d\r\n", ch);
send_response_echo(piezo_channels[ch], ECHO_ADC_MAX_SAMPLES);
send_response_echo(piezo_channels[ch], piezo_config_get()->samples);
}
}
@@ -799,22 +960,52 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len)
return 1;
}
/**
* mcf?: piezo 측정 파라미터 읽기
* 응답 rcf: + 설정값 echo back
*/
static int cmd_mcf(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
send_response_piezo_config(PIEZO_CFG_FREQ_DEFAULT,
PIEZO_CFG_CYCLES_DEFAULT,
PIEZO_CFG_AVG_DEFAULT,
PIEZO_CFG_DELAY_DEFAULT,
PIEZO_CFG_SAMPLES_DEFAULT);
DBG_PRINTF("[CMD] mcf -> freq=%d cycles=%d avg=%d delay=%d samples=%d\r\n",
PIEZO_CFG_FREQ_DEFAULT,
PIEZO_CFG_CYCLES_DEFAULT,
PIEZO_CFG_AVG_DEFAULT,
PIEZO_CFG_DELAY_DEFAULT,
PIEZO_CFG_SAMPLES_DEFAULT);
const piezo_config_t *cfg = piezo_config_get();
send_response_piezo_config(cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
piezo_config_log("[CMD] mcf ->", cfg);
return 1;
}
/**
* mcs?: piezo 측정 파라미터 쓰기
*/
static int cmd_mcs(const uint8_t *data, uint8_t data_len)
{
piezo_config_t cfg;
if (data_len < 10U)
{
send_response_u16("rcs:", 0xFFFF);
DBG_PRINTF("[CMD] mcs: insufficient data len=%u\r\n", data_len);
return 1;
}
get_data_u16_be(data, data_len, 0, &cfg.freq);
get_data_u16_be(data, data_len, 1, &cfg.cycles);
get_data_u16_be(data, data_len, 2, &cfg.avg);
get_data_u16_be(data, data_len, 3, &cfg.delay_us);
get_data_u16_be(data, data_len, 4, &cfg.samples);
if (!piezo_config_validate(&cfg))
{
send_response_u16("rcs:", 0xFFFE);
piezo_config_log("[CMD] mcs invalid", &cfg);
return 1;
}
g_piezo_config = cfg;
int err = piezo_config_save();
send_response_u16("rcs:", (err == 0) ? 0U : 0xFFFD);
piezo_config_log("[CMD] mcs saved", &g_piezo_config);
return 1;
}
@@ -977,6 +1168,7 @@ static const cmd_entry_t cmd_table[] =
{ "maa?", cmd_maa },
{ "mbb?", cmd_mbb },
{ "mcf?", cmd_mcf },
{ "mcs?", cmd_mcs },
{ "mfv?", cmd_mfv },
{ "mwh?", cmd_mwh },
{ "mrh?", cmd_mrh },
@@ -1026,7 +1218,8 @@ int dr_parser(const uint8_t *buf, uint16_t len)
}
/* TAG 추출 (4바이트) */
char tag[5] = {
char tag[5] =
{
ascii_to_lower((char)buf[0]),
ascii_to_lower((char)buf[1]),
ascii_to_lower((char)buf[2]),
@@ -1050,7 +1243,8 @@ int dr_parser(const uint8_t *buf, uint16_t len)
}
DBG_ERR("[CMD] Unknown: raw=%s normalized=%s\r\n", raw_tag, tag);
if (send_response_tag_echo("rxx:", raw_tag) != 0) {
if (send_response_tag_echo("rxx:", raw_tag) != 0)
{
DBG_ERR("[CMD] rxx tx failed\r\n");
}
return 0;