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

ADC까지, 신호 확인 필요(반사 신호 약함 or 뒤쪽을 찍고 있음)

Browse Source
This commit is contained in:
jhchun
2026-05-08 18:09:04 +09:00
parent 17a78d4418
commit d7c5b34da3
23 changed files with 1992 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/ble/ble_service.c
+156 -15
View File
@@ -30,8 +30,92 @@ LOG_MODULE_REGISTER(ble_svc, LOG_LEVEL_INF);
static struct bt_conn *current_conn;
static ble_data_rx_cb_t rx_callback;
/*
* NUS 전송은 "보냈다" 호출만 한다고 바로 끝나는 게 아니다.
* 내부 TX 버퍼에 들어간 뒤 실제로 무선으로 나가고,
* Zephyr가 sent 콜백을 줄 때 비로소 한 건이 끝났다고 볼 수 있다.
*
* 그래서 아래 2개를 같이 쓴다.
* - mutex: 여러 곳에서 동시에 NUS 전송을 시작하지 못하게 막음
* - sem : "직전 전송이 정말 끝났는지" 기다리는 신호
*
* mbb?처럼 큰 패킷을 여러 개 연속으로 보내는 경우 이 보호가 없으면
* - 중간에 -ENOMEM이 뜨거나
* - 몇 개는 나가고 몇 개는 실패하거나
* - 앱 쪽에서 응답이 꼬여 보일 수 있다.
*/
static struct k_mutex nus_tx_lock;
static struct k_sem nus_tx_done_sem;
/* BLE API는 ISR/콜백에서 직접 호출하면 안전하지 않을 수 있으므로 k_work 사용 */
static struct k_work adv_restart_work;
static const bt_addr_le_t fixed_identity_addr = {
.type = BT_ADDR_LE_RANDOM,
.a = {
.val = { 0xF1, 0x8E, 0x81, 0xFA, 0x87, 0xC5 },
},
};
static const char *ble_addr_type_str(uint8_t type)
{
switch (type)
{
case BT_ADDR_LE_PUBLIC:
return "public";
case BT_ADDR_LE_RANDOM:
return "random";
case BT_ADDR_LE_PUBLIC_ID:
return "public-id";
case BT_ADDR_LE_RANDOM_ID:
return "random-id";
default:
return "unknown";
}
}
static void ble_log_local_identity(void)
{
bt_addr_le_t addr = {0};
size_t count = 1;
char addr_str[BT_ADDR_LE_STR_LEN];
bt_id_get(&addr, &count);
if (count == 0U)
{
DBG_ERR("[BLE] No local identity address\r\n");
return;
}
bt_addr_le_to_str(&addr, addr_str, sizeof(addr_str));
DBG_CORE("[BLE] Local identity: %s (%s)\r\n",
addr_str,
ble_addr_type_str(addr.type));
}
static int ble_configure_fixed_identity(void)
{
bt_addr_le_t addr = fixed_identity_addr;
char addr_str[BT_ADDR_LE_STR_LEN];
int id;
bt_addr_le_to_str(&addr, addr_str, sizeof(addr_str));
DBG_CORE("[BLE] Request fixed identity: %s\r\n", addr_str);
id = bt_id_create(&addr, NULL);
if (id < 0)
{
DBG_ERR("[BLE] bt_id_create failed (err %d)\r\n", id);
return id;
}
if (id != BT_ID_DEFAULT)
{
DBG_ERR("[BLE] Unexpected identity slot %d\r\n", id);
return -EINVAL;
}
return 0;
}
static void adv_restart_handler(struct k_work *work)
{
@@ -61,7 +145,7 @@ static void connected(struct bt_conn *conn, uint8_t err)
{
if (err)
{
DBG_PRINTF("[BLE] Connect failed (err %d) -> re-adv\r\n", err);
DBG_ERR("[BLE] Connect failed (err %d) -> re-adv\r\n", err);
k_work_submit(&adv_restart_work);
return;
}
@@ -72,7 +156,7 @@ static void connected(struct bt_conn *conn, uint8_t err)
/* 연결 정보 출력 */
char addr_str[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));
DBG_PRINTF("[BLE] Peer: %s\r\n", addr_str);
DBG_CORE("[BLE] Peer: %s\r\n", addr_str);
/* Connection parameters (30ms interval) */
struct bt_le_conn_param conn_param =
@@ -85,7 +169,7 @@ static void connected(struct bt_conn *conn, uint8_t err)
bt_conn_le_param_update(conn, &conn_param);
led_set_state(LED_STATE_OFF);
DBG_PRINTF("[BLE] Connected\r\n");
DBG_CORE("[BLE] Connected\r\n");
}
static void disconnected(struct bt_conn *conn, uint8_t reason)
@@ -97,7 +181,7 @@ static void disconnected(struct bt_conn *conn, uint8_t reason)
}
ble_connection_st = false;
DBG_PRINTF("[BLE] Disconnected (reason 0x%02x)\r\n", reason);
DBG_CORE("[BLE] Disconnected (reason 0x%02x)\r\n", reason);
/* 워크큐에서 advertising 재시작 */
k_work_submit(&adv_restart_work);
@@ -122,6 +206,9 @@ BT_CONN_CB_DEFINE(conn_callbacks) =
*============================================================================*/
static void nus_received(struct bt_conn *conn, const uint8_t *data, uint16_t len)
{
ARG_UNUSED(conn);
DBG_CORE("[NUS RX] len=%u\r\n", len);
if (rx_callback)
{
rx_callback(data, len);
@@ -130,7 +217,10 @@ static void nus_received(struct bt_conn *conn, const uint8_t *data, uint16_t len
static void nus_sent(struct bt_conn *conn)
{
DBG_PRINTF("[NUS TX] complete\r\n");
ARG_UNUSED(conn);
/* 이 시점이 "직전 NUS 패킷 전송이 끝났다"는 완료 신호다. */
k_sem_give(&nus_tx_done_sem);
DBG_CORE("[NUS TX] complete\r\n");
}
static struct bt_nus_cb nus_cb =
@@ -147,26 +237,36 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
int err;
rx_callback = rx_cb;
k_mutex_init(&nus_tx_lock);
k_sem_init(&nus_tx_done_sem, 0, 1);
k_work_init(&adv_restart_work, adv_restart_handler);
err = ble_configure_fixed_identity();
if (err)
{
return err;
}
/* Enable BLE stack */
err = bt_enable(NULL);
if (err)
{
DBG_PRINTF("[BLE] bt_enable failed (err %d)\r\n", err);
DBG_ERR("[BLE] bt_enable failed (err %d)\r\n", err);
return err;
}
DBG_PRINTF("[BLE] Stack enabled\r\n");
DBG_CORE("[BLE] Stack enabled\r\n");
ble_log_local_identity();
/* Initialize NUS */
err = bt_nus_init(&nus_cb);
if (err)
{
DBG_PRINTF("[BLE] NUS init failed (err %d)\r\n", err);
DBG_ERR("[BLE] NUS init failed (err %d)\r\n", err);
return err;
}
DBG_PRINTF("[BLE] NUS initialized\r\n");
DBG_CORE("[BLE] NUS initialized\r\n");
return 0;
}
@@ -185,7 +285,7 @@ int ble_advertising_start(void)
err = bt_le_adv_start(&adv_param, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
if (err)
{
DBG_PRINTF("[BLE] Adv start failed (err %d)\r\n", err);
DBG_ERR("[BLE] Adv start failed (err %d)\r\n", err);
return err;
}
@@ -198,7 +298,7 @@ int ble_advertising_stop(void)
int err = bt_le_adv_stop();
if (err)
{
DBG_PRINTF("[BLE] Adv stop failed (err %d)\r\n", err);
DBG_ERR("[BLE] Adv stop failed (err %d)\r\n", err);
return err;
}
@@ -208,18 +308,59 @@ int ble_advertising_stop(void)
int ble_data_send(const uint8_t *data, uint16_t len)
{
int err;
if (!current_conn)
{
DBG_PRINTF("[NUS TX] not connected\r\n");
DBG_ERR("[NUS TX] not connected\r\n");
return -ENOTCONN;
}
DBG_PRINTF("[NUS TX] %d bytes\r\n", len);
int err = bt_nus_send(current_conn, data, len);
/*
* NUS는 한 번에 한 패킷씩 질서 있게 보내는 편이 안전하다.
* 특히 mbb?는 rbb + reb x6 + raa처럼 여러 응답을 연속 전송하므로
* 먼저 들어온 전송이 끝나기 전 다음 전송이 겹치지 않게 잠근다.
*/
k_mutex_lock(&nus_tx_lock, K_FOREVER);
/* 혹시 남아 있는 완료 신호가 있으면 비워서 "이번 전송 전용" 상태로 만든다. */
while (k_sem_take(&nus_tx_done_sem, K_NO_WAIT) == 0) {
}
DBG_CORE("[NUS TX] send len=%u\r\n", len);
for (int retry = 0; ; retry++)
{
err = bt_nus_send(current_conn, data, len);
if (err != -ENOMEM || retry >= 20) {
break;
}
/*
* -ENOMEM은 지금 당장 보낼 자리(TX 버퍼)가 없다는 뜻이다.
* 바로 포기하지 않고 짧게 쉬었다가 다시 시도한다.
*/
DBG_ERR("[NUS TX] busy, retry=%d\r\n", retry + 1);
k_msleep(5);
}
if (err)
{
DBG_PRINTF("[NUS TX] failed (err %d)\r\n", err);
DBG_ERR("[NUS TX] failed (err %d)\r\n", err);
k_mutex_unlock(&nus_tx_lock);
return err;
}
/*
* bt_nus_send()가 성공했다고 해서 공중으로 다 나간 것은 아니다.
* sent 콜백이 올 때까지 조금 기다려서 "다음 패킷을 보내도 되는 시점"을 맞춘다.
*/
err = k_sem_take(&nus_tx_done_sem, K_MSEC(1000));
if (err)
{
DBG_ERR("[NUS TX] completion timeout (err %d)\r\n", err);
}
k_mutex_unlock(&nus_tx_lock);
return err;
}
src/debug_print.h
+16 -9
View File
@@ -8,16 +8,23 @@
#include <zephyr/logging/log.h>
/*
* DBG_PRINTF maps to printk for RTT/UART console output.
* For module-level logging use LOG_INF/LOG_DBG etc.
* 로그를 3단계로 나눠 쓴다.
*
* - DBG_PRINTF: 상세 흐름용
* 평소에는 가장 많이 쏟아지는 로그라서, RTT가 잘리거나 너무 시끄러울 때
* 이 매크로만 잠깐 꺼두면 된다.
*
* - DBG_CORE: 부팅/상태 전환처럼 "항상 보고 싶은 핵심 로그"
*
* - DBG_ERR: 실패/이상 상황 로그
*
* 지금은 RTT 로그가 중간에 끊기는 문제를 줄이기 위해
* DBG_PRINTF만 잠깐 비활성화한 상태다.
*/
#define ENABLE_PRINTF 1
#include <zephyr/sys/printk.h>
#if ENABLE_PRINTF
#include <zephyr/sys/printk.h>
#define DBG_PRINTF(...) printk(__VA_ARGS__)
#else
#define DBG_PRINTF(...)
#endif
#define DBG_PRINTF(...) printk(__VA_ARGS__)
#define DBG_CORE(...) printk(__VA_ARGS__)
#define DBG_ERR(...) printk(__VA_ARGS__)
#endif /* DEBUG_PRINT_H */
src/drivers/echo_adc/echo_adc.c
+145
View File
@@ -0,0 +1,145 @@
/*******************************************************************************
* @file echo_adc.c
* @brief ADC121S051 echo capture driver using nrfx SPIM3
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/devicetree.h>
#include <zephyr/sys/util.h>
#include <nrfx_spim.h>
#include <errno.h>
#include "echo_adc.h"
#include "debug_print.h"
#define ECHO_SCLK_NODE DT_NODELABEL(echo_sclk)
#define ECHO_MISO_NODE DT_NODELABEL(echo_miso)
#define ECHO_CS_NODE DT_NODELABEL(echo_cs)
static const struct gpio_dt_spec echo_sclk = GPIO_DT_SPEC_GET(ECHO_SCLK_NODE, gpios);
static const struct gpio_dt_spec echo_miso = GPIO_DT_SPEC_GET(ECHO_MISO_NODE, gpios);
static const struct gpio_dt_spec echo_cs = GPIO_DT_SPEC_GET(ECHO_CS_NODE, gpios);
/*
* NRFX_SPIM_INSTANCE() takes the peripheral register symbol (for example
* NRF_SPIM3), not a numeric instance index. Passing 3 makes p_reg invalid
* and causes a fault inside nrfx_spim_init().
*/
static nrfx_spim_t echo_spim = NRFX_SPIM_INSTANCE(NRF_SPIM3);
static bool echo_adc_initialized;
static uint8_t raw_capture[ECHO_ADC_MAX_SAMPLES * 2];
static uint32_t gpio_abs_pin(const struct gpio_dt_spec *spec)
{
uint32_t port = (spec->port == DEVICE_DT_GET(DT_NODELABEL(gpio1))) ? 1U : 0U;
return NRF_GPIO_PIN_MAP(port, spec->pin);
}
static int echo_adc_transfer_rx(uint8_t *buf, size_t len)
{
nrfx_spim_xfer_desc_t xfer = NRFX_SPIM_XFER_RX(buf, len);
int err = nrfx_spim_xfer(&echo_spim, &xfer, 0);
if (err != 0)
{
DBG_PRINTF("[ECHO] xfer fail err=%d\r\n", err);
return -EIO;
}
return 0;
}
int echo_adc_init(void)
{
if (echo_adc_initialized || nrfx_spim_init_check(&echo_spim))
{
echo_adc_initialized = true;
return 0;
}
const struct gpio_dt_spec *pins[] = { &echo_sclk, &echo_miso, &echo_cs };
for (size_t i = 0; i < ARRAY_SIZE(pins); i++)
{
if (!device_is_ready(pins[i]->port))
{
DBG_PRINTF("[ECHO] GPIO device not ready (%d)\r\n", (int)i);
return -ENODEV;
}
}
nrfx_spim_config_t config = NRFX_SPIM_DEFAULT_CONFIG(
gpio_abs_pin(&echo_sclk),
NRF_SPIM_PIN_NOT_CONNECTED,
gpio_abs_pin(&echo_miso),
gpio_abs_pin(&echo_cs));
config.frequency = NRFX_MHZ_TO_HZ(16);
config.mode = NRF_SPIM_MODE_3;
config.bit_order = NRF_SPIM_BIT_ORDER_MSB_FIRST;
config.orc = 0x00;
config.miso_pull = NRF_GPIO_PIN_NOPULL;
config.ss_active_high = false;
nrfx_err_t err = nrfx_spim_init(&echo_spim, &config, NULL, NULL);
if (err != NRFX_SUCCESS)
{
DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
return -EIO;
}
echo_adc_initialized = true;
DBG_PRINTF("[ECHO] SPIM3 init OK\r\n");
return 0;
}
int echo_adc_wake(void)
{
int err = echo_adc_init();
if (err)
{
return err;
}
uint8_t discard[2];
return echo_adc_transfer_rx(discard, sizeof(discard));
}
int echo_adc_capture(uint16_t *samples, uint16_t num_samples)
{
if (samples == NULL)
{
return -EINVAL;
}
if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
{
return -EINVAL;
}
int err = echo_adc_init();
if (err)
{
return err;
}
for (uint16_t i = 0; i < num_samples; i++)
{
/*
* Keep the framing that matched the old firmware behavior:
* one 16-clock / 2-byte SPI frame per sample.
*/
err = echo_adc_transfer_rx(&raw_capture[i * 2U], 2U);
if (err)
{
return err;
}
uint16_t raw16 = ((uint16_t)raw_capture[i * 2U] << 8) |
(uint16_t)raw_capture[i * 2U + 1U];
samples[i] = (raw16 >> 1) & 0x0FFF;
}
return 0;
}
src/drivers/echo_adc/echo_adc.h
+17
View File
@@ -0,0 +1,17 @@
/*******************************************************************************
* @file echo_adc.h
* @brief ADC121S051 echo capture driver
******************************************************************************/
#ifndef ECHO_ADC_H__
#define ECHO_ADC_H__
#include <stdint.h>
#define ECHO_ADC_MAX_SAMPLES 100
int echo_adc_init(void);
int echo_adc_wake(void);
int echo_adc_capture(uint16_t *samples, uint16_t num_samples);
#endif /* ECHO_ADC_H__ */
src/drivers/piezo/piezo.c
+219
View File
@@ -0,0 +1,219 @@
/*******************************************************************************
* @file piezo.c
* @brief Piezo TX burst, mux selection, and power control
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/devicetree.h>
#include <zephyr/sys/util.h>
#include <hal/nrf_gpio.h>
#include <errno.h>
#include "piezo.h"
#include "debug_print.h"
#define PIEZO_PWR_NODE DT_NODELABEL(piezo_pwr)
#define PIEZO_PE_NODE DT_NODELABEL(piezo_pe)
#define PIEZO_P_OUT_NODE DT_NODELABEL(piezo_p_out)
#define PIEZO_N_OUT_NODE DT_NODELABEL(piezo_n_out)
#define PIEZO_DMP_NODE DT_NODELABEL(piezo_dmp)
#define MUX_EN_A_NODE DT_NODELABEL(mux_en_a)
#define MUX_EN_B_NODE DT_NODELABEL(mux_en_b)
#define MUX_SEL0_NODE DT_NODELABEL(mux_sel0)
#define MUX_SEL1_NODE DT_NODELABEL(mux_sel1)
static const struct gpio_dt_spec piezo_pwr = GPIO_DT_SPEC_GET(PIEZO_PWR_NODE, gpios);
static const struct gpio_dt_spec piezo_pe = GPIO_DT_SPEC_GET(PIEZO_PE_NODE, gpios);
static const struct gpio_dt_spec piezo_p_out = GPIO_DT_SPEC_GET(PIEZO_P_OUT_NODE, gpios);
static const struct gpio_dt_spec piezo_n_out = GPIO_DT_SPEC_GET(PIEZO_N_OUT_NODE, gpios);
static const struct gpio_dt_spec piezo_dmp = GPIO_DT_SPEC_GET(PIEZO_DMP_NODE, gpios);
static const struct gpio_dt_spec mux_en_a = GPIO_DT_SPEC_GET(MUX_EN_A_NODE, gpios);
static const struct gpio_dt_spec mux_en_b = GPIO_DT_SPEC_GET(MUX_EN_B_NODE, gpios);
static const struct gpio_dt_spec mux_sel0 = GPIO_DT_SPEC_GET(MUX_SEL0_NODE, gpios);
static const struct gpio_dt_spec mux_sel1 = GPIO_DT_SPEC_GET(MUX_SEL1_NODE, gpios);
struct mux_state {
uint8_t en_a;
uint8_t en_b;
uint8_t sel0;
uint8_t sel1;
};
static const struct mux_state mux_map[PIEZO_NUM_CHANNELS] = {
{1, 0, 0, 0},
{1, 0, 1, 0},
{1, 0, 0, 1},
{1, 0, 1, 1},
{0, 1, 0, 0},
{0, 1, 1, 0},
};
static bool piezo_initialized;
static uint32_t piezo_pe_abs;
static uint32_t piezo_p_out_abs;
static uint32_t piezo_n_out_abs;
static uint32_t piezo_dmp_abs;
static uint32_t piezo_pe_mask;
static uint32_t piezo_p_out_mask;
static uint32_t piezo_n_out_mask;
static uint32_t piezo_dmp_mask;
static uint32_t gpio_abs_pin(const struct gpio_dt_spec *spec)
{
uint32_t port = (spec->port == DEVICE_DT_GET(DT_NODELABEL(gpio1))) ? 1U : 0U;
return NRF_GPIO_PIN_MAP(port, spec->pin);
}
static inline void burst_nop(void)
{
__asm__ volatile("nop");
}
static inline void nop_delay_9(void)
{
burst_nop(); burst_nop(); burst_nop();
burst_nop(); burst_nop(); burst_nop();
burst_nop(); burst_nop(); burst_nop();
}
static inline void nop_delay_14(void)
{
burst_nop(); burst_nop(); burst_nop(); burst_nop();
burst_nop(); burst_nop(); burst_nop(); burst_nop();
burst_nop(); burst_nop(); burst_nop(); burst_nop();
burst_nop(); burst_nop();
}
static inline void nop_delay_32(void)
{
for (int i = 0; i < 32; i++) {
burst_nop();
}
}
static void piezo_drive_idle(void)
{
nrf_gpio_pin_clear(piezo_pe_abs);
nrf_gpio_pin_clear(piezo_p_out_abs);
nrf_gpio_pin_clear(piezo_n_out_abs);
nrf_gpio_pin_clear(piezo_dmp_abs);
gpio_pin_set_dt(&mux_en_a, 0);
gpio_pin_set_dt(&mux_en_b, 0);
gpio_pin_set_dt(&mux_sel0, 0);
gpio_pin_set_dt(&mux_sel1, 0);
}
int piezo_init(void)
{
if (piezo_initialized) {
return 0;
}
const struct gpio_dt_spec *pins[] = {
&piezo_pwr, &piezo_pe, &piezo_p_out, &piezo_n_out,
&piezo_dmp, &mux_en_a, &mux_en_b, &mux_sel0, &mux_sel1
};
for (size_t i = 0; i < ARRAY_SIZE(pins); i++) {
if (!device_is_ready(pins[i]->port)) {
DBG_PRINTF("[PIEZO] GPIO device not ready (%d)\r\n", (int)i);
return -ENODEV;
}
int err = gpio_pin_configure_dt(pins[i], GPIO_OUTPUT_INACTIVE);
if (err) {
DBG_PRINTF("[PIEZO] GPIO init fail idx=%d err=%d\r\n", (int)i, err);
return err;
}
}
piezo_pe_abs = gpio_abs_pin(&piezo_pe);
piezo_p_out_abs = gpio_abs_pin(&piezo_p_out);
piezo_n_out_abs = gpio_abs_pin(&piezo_n_out);
piezo_dmp_abs = gpio_abs_pin(&piezo_dmp);
piezo_pe_mask = BIT(piezo_pe.pin);
piezo_p_out_mask = BIT(piezo_p_out.pin);
piezo_n_out_mask = BIT(piezo_n_out.pin);
piezo_dmp_mask = BIT(piezo_dmp.pin);
piezo_drive_idle();
piezo_initialized = true;
DBG_PRINTF("[PIEZO] Init OK\r\n");
return 0;
}
void piezo_power_on(void)
{
if (!piezo_initialized && piezo_init() != 0) {
return;
}
gpio_pin_set_dt(&piezo_pwr, 1);
}
void piezo_power_off(void)
{
if (!piezo_initialized && piezo_init() != 0) {
return;
}
piezo_drive_idle();
gpio_pin_set_dt(&piezo_pwr, 0);
}
int piezo_select_channel(uint8_t channel)
{
if (!piezo_initialized) {
int err = piezo_init();
if (err) {
return err;
}
}
if (channel >= PIEZO_NUM_CHANNELS) {
return -EINVAL;
}
gpio_pin_set_dt(&mux_en_a, mux_map[channel].en_a);
gpio_pin_set_dt(&mux_en_b, mux_map[channel].en_b);
gpio_pin_set_dt(&mux_sel0, mux_map[channel].sel0);
gpio_pin_set_dt(&mux_sel1, mux_map[channel].sel1);
k_busy_wait(PIEZO_MUX_SETTLE_US);
return 0;
}
void piezo_burst_sw(uint8_t cycles)
{
if (!piezo_initialized || cycles == 0U) {
return;
}
unsigned int key = irq_lock();
uint32_t saved_p1_out = NRF_P1->OUT;
uint32_t p1_ctrl_mask = piezo_p_out_mask | piezo_n_out_mask | piezo_dmp_mask;
uint32_t p1_all_low = saved_p1_out & ~p1_ctrl_mask;
uint32_t p1_p_high_n_low = p1_all_low | piezo_p_out_mask;
uint32_t p1_p_low_n_high = p1_all_low | piezo_n_out_mask;
uint32_t p1_dmp_high = p1_all_low | piezo_dmp_mask;
NRF_P0->OUTCLR = piezo_pe_mask;
NRF_P1->OUT = p1_all_low;
NRF_P0->OUTSET = piezo_pe_mask;
for (uint8_t i = 0; i < cycles; i++) {
NRF_P1->OUT = p1_p_high_n_low;
nop_delay_14();
NRF_P1->OUT = p1_p_low_n_high;
nop_delay_9();
}
NRF_P1->OUT = p1_dmp_high;
nop_delay_32();
NRF_P1->OUT = p1_all_low;
NRF_P0->OUTCLR = piezo_pe_mask;
irq_unlock(key);
}
src/drivers/piezo/piezo.h
+23
View File
@@ -0,0 +1,23 @@
/*******************************************************************************
* @file piezo.h
* @brief Piezo transmit and mux control driver
******************************************************************************/
#ifndef PIEZO_H__
#define PIEZO_H__
#include <stdint.h>
#define PIEZO_NUM_CHANNELS 6
#define PIEZO_SW_BURST_CYCLES 7
#define PIEZO_MUX_SETTLE_US 1300
#define PIEZO_POWER_STABILIZE_MS 3
#define PIEZO_BURST_TO_ADC_DELAY_US 10
int piezo_init(void);
void piezo_power_on(void);
void piezo_power_off(void);
int piezo_select_channel(uint8_t channel);
void piezo_burst_sw(uint8_t cycles);
#endif /* PIEZO_H__ */
src/drivers/temperature/tmp235.c
+5 -5
View File
@@ -57,13 +57,13 @@ static int16_t raw_to_cdeg(int16_t raw)
int temp_init(void)
{
if (!adc_is_ready_dt(&temp_adc)) {
DBG_PRINTF("[TEMP] FAIL — ADC device not ready\r\n");
DBG_ERR("[TEMP] FAIL — ADC device not ready\r\n");
return -1;
}
int err = adc_channel_setup_dt(&temp_adc);
if (err) {
DBG_PRINTF("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
DBG_ERR("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
return -2;
}
@@ -79,7 +79,7 @@ int16_t temp_read_cdeg(void)
/* 매번 채널 재설정 (배터리 ADC와 SAADC 공유) */
int err = adc_channel_setup_dt(&temp_adc);
if (err) {
DBG_PRINTF("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
DBG_ERR("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
return INT16_MIN;
}
@@ -87,7 +87,7 @@ int16_t temp_read_cdeg(void)
err = adc_sequence_init_dt(&temp_adc, &seq);
if (err) {
DBG_PRINTF("[TEMP] FAIL — sequence init (err=%d)\r\n", err);
DBG_ERR("[TEMP] FAIL — sequence init (err=%d)\r\n", err);
return INT16_MIN;
}
@@ -96,7 +96,7 @@ int16_t temp_read_cdeg(void)
err = adc_read_dt(&temp_adc, &seq);
if (err) {
DBG_PRINTF("[TEMP] FAIL — read (err=%d)\r\n", err);
DBG_ERR("[TEMP] FAIL — read (err=%d)\r\n", err);
return INT16_MIN;
}
src/main.c
+170 -26
View File
@@ -28,6 +28,15 @@
LOG_MODULE_REGISTER(vesiscan, LOG_LEVEL_INF);
#define BLE_CMD_MAX_LEN 256
#define BLE_CMD_WORKQ_STACK_SZ 8192
/*
* mbb?는 piezo sweep, ADC capture, BLE 응답 패킷 조립까지 한 스레드에서 처리한다.
* 초기 4096 바이트로는 "어떤 날은 되고 어떤 날은 리셋되는" 경계 상태가 날 수 있어서
* 디버깅 중에는 여유 있게 8192 바이트로 올려 둔다.
*/
#define BLE_CMD_WORKQ_PRIORITY 10
/*==============================================================================
* Devicetree GPOP Settings
*============================================================================*/
@@ -51,13 +60,14 @@ volatile bool processing = false; /* 센서 데이터 처리 중 플
bool bond_data_delete = true; /* 본딩 데이터 삭제 요청 */
uint32_t m_life_cycle = 0; /* 디바이스 수명 카운터 */
uint8_t m_reset_status = 1; /* 리셋 상태 코드 */
char SERIAL_NO[SERIAL_NO_LENGTH]; /* 시리얼 번호 */
char HW_NO[HW_NO_LENGTH]; /* 하드웨어 번호 */
char m_static_passkey[PASSKEY_LENGTH]; /* BLE 정적 패스키 */
char SERIAL_NO[SERIAL_NO_BUF_SIZE]; /* 시리얼 번호 */
char HW_NO[HW_NO_BUF_SIZE]; /* 하드웨어 번호 */
char m_static_passkey[PASSKEY_BUF_SIZE]; /* BLE 정적 패스키 */
static uint16_t cnt_s; /* 전원 버튼 폴링 카운터 (5ms 단위) */
static bool device_on = false; /* 디바이스 전원 상태 (래치 완료 여부) */
static bool boot_btn_released = false; /* 부팅 후 버튼 놓았는지 여부 */
static bool power_btn_suspended; /* 측정 중 전원 버튼 상태머신 일시 정지 */
/*
* BLE advertising 제어용 워크 아이템
@@ -72,6 +82,56 @@ static bool boot_btn_released = false; /* 부팅 후 버튼 놓았는지 여
static struct k_work adv_start_work;
static struct k_work adv_stop_work;
/*
* BLE RX 콜백 안에서 무거운 일을 바로 처리하지 않기 위해 별도 work queue를 둔다.
*
* 이유:
* - BLE 콜백은 "일단 빨리 빠져나오는 것"이 중요하다.
* - 그 안에서 mbb? 같은 긴 측정을 돌리면 BLE 스택 타이밍을 망칠 수 있다.
* - 그래서 수신 데이터는 일단 복사만 하고,
* 실제 파싱/측정은 아래 work queue 스레드에서 천천히 처리한다.
*
* 현재 구조는 "동시에 여러 명령 처리"가 아니라 "한 번에 하나만 처리" 모델이다.
* 즉, 긴 명령 하나가 끝날 때까지 다음 명령은 drop된다.
*/
static struct k_work_q ble_cmd_work_q;
K_THREAD_STACK_DEFINE(ble_cmd_workq_stack, BLE_CMD_WORKQ_STACK_SZ);
static struct k_work ble_cmd_work;
static struct k_spinlock ble_cmd_lock;
static uint8_t ble_cmd_buf[BLE_CMD_MAX_LEN];
static uint16_t ble_cmd_len;
static bool ble_cmd_pending;
static void ble_cmd_work_handler(struct k_work *work)
{
ARG_UNUSED(work);
/* work queue 스레드에서 쓸 로컬 복사본. 원본 공유 버퍼를 오래 잡고 있지 않기 위해 만든다. */
uint8_t local_buf[BLE_CMD_MAX_LEN];
uint16_t local_len = 0U;
k_spinlock_key_t key = k_spin_lock(&ble_cmd_lock);
if (ble_cmd_pending) {
local_len = ble_cmd_len;
memcpy(local_buf, ble_cmd_buf, local_len);
}
k_spin_unlock(&ble_cmd_lock, key);
if (local_len == 0U) {
DBG_ERR("[BLE RX] worker: empty\r\n");
return;
}
/* 여기서부터는 BLE 콜백 문맥이 아니라 일반 스레드 문맥이라 긴 작업을 해도 된다. */
DBG_CORE("[BLE RX] worker dispatch len=%u\r\n", local_len);
dr_parser(local_buf, local_len);
key = k_spin_lock(&ble_cmd_lock);
ble_cmd_pending = false;
ble_cmd_len = 0U;
k_spin_unlock(&ble_cmd_lock, key);
}
/* 시스템 워크큐에서 BLE advertising 시작 */
static void adv_start_work_handler(struct k_work *work)
{
@@ -91,17 +151,50 @@ static void adv_stop_work_handler(struct k_work *work)
*============================================================================*/
static void ble_rx_handler(const uint8_t *data, uint16_t len)
{
DBG_PRINTF("[BLE RX] %d bytes: ", len);
DBG_CORE("[BLE RX] %u bytes:", len);
for (uint16_t i = 0; i < len && i < 32; i++)
{
DBG_PRINTF("%02X ", data[i]);
DBG_CORE(" %02X", data[i]);
}
if (len > 32)
{
DBG_PRINTF("...");
DBG_CORE(" ...");
}
DBG_CORE("\r\n");
if (len > BLE_CMD_MAX_LEN)
{
DBG_ERR("[BLE RX] drop: len=%u exceeds %d\r\n", len, BLE_CMD_MAX_LEN);
return;
}
/*
* 이미 긴 명령 하나를 처리 중이면 새 명령은 받지 않는다.
* 지금은 안정성 우선이라 "큐에 여러 개 쌓기"보다 "한 번에 하나"가 더 안전하다.
*/
k_spinlock_key_t key = k_spin_lock(&ble_cmd_lock);
if (ble_cmd_pending)
{
k_spin_unlock(&ble_cmd_lock, key);
DBG_ERR("[BLE RX] drop: command busy\r\n");
return;
}
memcpy(ble_cmd_buf, data, len);
ble_cmd_len = len;
ble_cmd_pending = true;
k_spin_unlock(&ble_cmd_lock, key);
DBG_CORE("[BLE RX] queued len=%u\r\n", len);
int err = k_work_submit_to_queue(&ble_cmd_work_q, &ble_cmd_work);
if (err < 0)
{
key = k_spin_lock(&ble_cmd_lock);
ble_cmd_pending = false;
ble_cmd_len = 0U;
k_spin_unlock(&ble_cmd_lock, key);
DBG_ERR("[BLE RX] queue submit fail err=%d\r\n", err);
}
DBG_PRINTF("\r\n");
dr_parser(data, len);
}
/*==============================================================================
@@ -121,7 +214,6 @@ static void power_control_handler(on_off_cont_t device_power_st)
{
if (device_power_st == OFF) {
gpio_pin_set_dt(&power_hold, 0); /* P0.08 LOW → 전원 래치 해제 → 전원 차단 */
DBG_PRINTF("[PWR] OFF\r\n");
} else {
gpio_pin_set_dt(&power_hold, 1); /* P0.08 HIGH → 전원 유지 */
DBG_PRINTF("[PWR] ON\r\n");
@@ -143,16 +235,33 @@ static void gpio_init(void)
*============================================================================*/
static void load_default_config(void)
{
memset(SERIAL_NO, 0, SERIAL_NO_LENGTH);
memcpy(SERIAL_NO, SERIAL_NUMBER, strlen(SERIAL_NUMBER));
size_t serial_len = strlen(SERIAL_NUMBER);
size_t hw_len = strlen(HARDWARE_VERSION);
size_t passkey_len = strlen(DEFAULT_PASSKEY);
memset(m_static_passkey, 0, PASSKEY_LENGTH);
memcpy(m_static_passkey, DEFAULT_PASSKEY, PASSKEY_LENGTH);
if (serial_len > SERIAL_NO_LENGTH) {
serial_len = SERIAL_NO_LENGTH;
}
if (hw_len > HW_NO_LENGTH) {
hw_len = HW_NO_LENGTH;
}
if (passkey_len > PASSKEY_LENGTH) {
passkey_len = PASSKEY_LENGTH;
}
memset(SERIAL_NO, 0, sizeof(SERIAL_NO));
memcpy(SERIAL_NO, SERIAL_NUMBER, serial_len);
memset(HW_NO, 0, sizeof(HW_NO));
memcpy(HW_NO, HARDWARE_VERSION, hw_len);
memset(m_static_passkey, 0, sizeof(m_static_passkey));
memcpy(m_static_passkey, DEFAULT_PASSKEY, passkey_len);
m_reset_status = 1;
bond_data_delete = true;
DBG_PRINTF("[CFG] Default (S/N=%s)\r\n", SERIAL_NO);
DBG_CORE("[CFG] Default (S/N=%s)\r\n", SERIAL_NO);
}
/*==============================================================================
@@ -184,6 +293,18 @@ void device_power_off(void)
k_timer_start(&m_power_off_delay_timer, K_MSEC(POWER_OFF_DELAY), K_NO_WAIT);
}
void power_button_suspend(bool suspend)
{
/*
* maa?/mbb?처럼 측정 시간이 길 때 전원 버튼 상태머신이 끼어들지 않게 잠깐 멈춘다.
*
* 이 함수는 "부팅을 막는 함수"가 아니다.
* 이미 켜진 뒤, 특정 커맨드 처리 중에만 버튼 폴링 판단을 잠깐 쉬게 하는 역할이다.
*/
power_btn_suspended = suspend;
cnt_s = 0;
}
/*==============================================================================
* 전원 버튼 상태머신 (5ms 폴링)
*
@@ -209,7 +330,7 @@ static void main_s(struct k_timer *timer)
{
if (!button_pressed) /* 버튼 놓음 → 2초 미만이면 전원 OFF */
{
DBG_PRINTF("[BTN] Short press (%d) -> OFF\r\n", cnt_s);
//DBG_PRINTF("[BTN] Short press (%d) -> OFF\r\n", cnt_s);
power_control_handler(OFF);
cnt_s = 0;
}
@@ -281,6 +402,12 @@ static void timers_init(void)
k_timer_init(&m_power_off_delay_timer, t_power_off_timeout_handler, NULL);
k_work_init(&adv_start_work, adv_start_work_handler);
k_work_init(&adv_stop_work, adv_stop_work_handler);
k_work_init(&ble_cmd_work, ble_cmd_work_handler);
k_work_queue_start(&ble_cmd_work_q,
ble_cmd_workq_stack,
K_THREAD_STACK_SIZEOF(ble_cmd_workq_stack),
BLE_CMD_WORKQ_PRIORITY,
NULL);
power_timer_init();
}
@@ -299,11 +426,28 @@ int main(void)
power_hold_init();
cnt_s = 0;
DBG_PRINTF("\r\n========================================\r\n");
DBG_PRINTF(" VesiScan BASIC %s (Zephyr)\r\n", FIRMWARE_VERSION);
DBG_PRINTF("========================================\r\n");
/*
* 현재는 전원 버튼/mbb 문제를 같이 디버깅 중이라
* "버튼을 2초 이상 눌러야 래치 ON" 규칙이 오히려 부팅 자체를 막고 있다.
*
* 그래서 디버깅 동안만, MCU가 main()까지 올라오면 바로 P0.08 래치를 ON으로 잡는다.
*
* 의미:
* - 사용자는 전원 버튼을 "부팅이 시작될 만큼만" 누르면 된다.
* - 예전처럼 2초를 정확히 맞춰 길게 누를 필요가 없다.
* - 일단 켜진 뒤의 BLE/측정/로그 문제를 보기 쉬워진다.
*
* 나중에 전원 시퀀스 디버깅이 끝나면 이 블록은 다시 제거하면 된다.
*/
device_on = false;
boot_btn_released = false;
DBG_PRINTF("[1] HW Init\r\n");
DBG_CORE("\r\n========================================\r\n");
DBG_CORE(" TEST BUILD %s (Zephyr)\r\n", FIRMWARE_VERSION);
DBG_CORE(" BUILD TAG: TEST-ADV-UNIT-042\r\n");
DBG_CORE("========================================\r\n");
DBG_CORE("[1] HW Init\r\n");
gpio_init();
timers_init();
load_default_config();
@@ -312,25 +456,25 @@ int main(void)
battery_timer_init();
imu_init();
temp_init();
DBG_PRINTF(" gpio/timer/config/led/batt/imu/temp OK\r\n");
DBG_CORE(" gpio/timer/config/led/batt/imu/temp OK\r\n");
/*── Phase 2: BLE 스택 + NUS ──*/
DBG_PRINTF("[2] BLE Init\r\n");
DBG_CORE("[2] BLE Init\r\n");
if (ble_service_init(ble_rx_handler) == 0)
{
DBG_PRINTF(" ble/nus OK\r\n");
DBG_CORE(" ble/nus OK\r\n");
}
else
{
DBG_PRINTF(" ble FAIL\r\n");
DBG_ERR(" ble FAIL\r\n");
}
/*── Phase 3: FDS/NVS (TODO) ──*/
/*── Phase 4: 애플리케이션 (TODO) ──*/
DBG_PRINTF("\r\n========================================\r\n");
DBG_PRINTF(" READY [%s]\r\n", SERIAL_NO);
DBG_PRINTF("========================================\r\n\r\n");
DBG_CORE("\r\n========================================\r\n");
DBG_CORE(" READY [%s]\r\n", SERIAL_NO);
DBG_CORE("========================================\r\n\r\n");
/* 전원 버튼 상태머신 시작 (부팅 시 버튼이 눌려있는 상태) */
timers_start();
src/main.h
+9 -5
View File
@@ -15,9 +15,9 @@
/*==============================================================================
* Firmware identification
*============================================================================*/
#define FIRMWARE_VERSION "VBTFW0102"
#define FIRMWARE_VERSION "TSTFW042"
#define HARDWARE_VERSION "VB0HW0000"
#define SERIAL_NUMBER "VB026030000"
#define SERIAL_NUMBER "VB0260300ZZ" // 앱이 실제로 쓰는 시리얼/광고 이름
#define DEFAULT_PASSKEY "123456"
/*==============================================================================
@@ -26,6 +26,9 @@
#define SERIAL_NO_LENGTH 12
#define HW_NO_LENGTH 12
#define PASSKEY_LENGTH 6
#define SERIAL_NO_BUF_SIZE (SERIAL_NO_LENGTH + 1)
#define HW_NO_BUF_SIZE (HW_NO_LENGTH + 1)
#define PASSKEY_BUF_SIZE (PASSKEY_LENGTH + 1)
/*==============================================================================
* Enumerations
@@ -61,15 +64,16 @@ typedef enum
void sleep_mode_enter(void);
void device_power_off(void);
void power_button_suspend(bool suspend);
/*==============================================================================
* Global variables (extern)
*============================================================================*/
extern volatile bool ble_connection_st;
extern volatile bool processing;
extern char SERIAL_NO[SERIAL_NO_LENGTH];
extern char HW_NO[HW_NO_LENGTH];
extern char m_static_passkey[PASSKEY_LENGTH];
extern char SERIAL_NO[SERIAL_NO_BUF_SIZE];
extern char HW_NO[HW_NO_BUF_SIZE];
extern char m_static_passkey[PASSKEY_BUF_SIZE];
extern bool bond_data_delete;
extern uint32_t m_life_cycle;
extern uint8_t m_reset_status;
src/parser.c
+850 -10
View File
@@ -9,6 +9,7 @@
******************************************************************************/
#include <zephyr/kernel.h>
#include <string.h>
#include <limits.h>
#include "parser.h"
#include "main.h"
@@ -16,7 +17,41 @@
#include "ble_service.h"
#include "battery_adc.h"
#include "led_control.h"
#include "echo_adc.h"
#include "imu_i2c.h"
#include "piezo.h"
#include "tmp235.h"
/*==============================================================================
* Piezo / echo measurement constants
*============================================================================*/
#define PIEZO_AVERAGE_COUNT 10
#define ECHO_STATUS_OK 0x0000
#define ECHO_STATUS_PIEZO 0x0001
#define ECHO_STATUS_ADC_INIT 0x0002
#define ECHO_STATUS_MUX 0x0003
#define ECHO_STATUS_CAPTURE 0x0004
#define ECHO_STATUS_BATT 0x0005
#define ECHO_STATUS_IMU 0x0006
#define ECHO_STATUS_TEMP 0x0007
#define PIEZO_CFG_FREQ_DEFAULT 1
#define PIEZO_CFG_CYCLES_DEFAULT 7
#define PIEZO_CFG_DELAY_DEFAULT 10
#define PIEZO_CFG_SAMPLES_DEFAULT 100
#define PIEZO_CFG_AVG_DEFAULT 10
#define PIEZO_POST_SELECT_SETTLE_US 500
#define PIEZO_AVG_INTER_BURST_GAP_US 500
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];
static uint8_t tx_u16_buf[8];
static uint8_t tx_imu_buf[18];
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];
/*==============================================================================
* CRC16 (CRC-CCITT, Nordic SDK 호환)
@@ -36,14 +71,47 @@ static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size)
return crc;
}
static bool get_data_u16_be(const uint8_t *data, uint8_t data_len,
uint8_t word_index, uint16_t *out)
{
uint8_t offset = (uint8_t)(word_index * 2U);
if ((offset + 1U) >= data_len) {
return false;
}
*out = ((uint16_t)data[offset] << 8) | (uint16_t)data[offset + 1U];
return true;
}
static void copy_fixed_ascii(char *dst, size_t dst_len,
const char *src, size_t src_len)
{
if (src_len > dst_len) {
src_len = dst_len;
}
memset(dst, 0, dst_len);
memcpy(dst, src, src_len);
}
static char ascii_to_lower(char ch)
{
if ((ch >= 'A') && (ch <= 'Z')) {
return (char)(ch - 'A' + 'a');
}
return ch;
}
/*==============================================================================
* 응답 패킷 전송
*============================================================================*/
/* TAG(4B) + uint16 값(2B) + CRC16(2B) = 8바이트 전송 */
static void send_response_u16(const char *tag, uint16_t value)
static int send_response_u16(const char *tag, uint16_t value)
{
uint8_t buf[8];
uint8_t *buf = tx_u16_buf;
buf[0] = tag[0];
buf[1] = tag[1];
@@ -56,7 +124,44 @@ static void send_response_u16(const char *tag, uint16_t value)
buf[6] = (uint8_t)(crc & 0xFF);
buf[7] = (uint8_t)(crc >> 8);
ble_data_send(buf, 8);
return ble_data_send(buf, 8);
}
static int send_response_ascii(const char *tag, const char *value, uint8_t value_len)
{
uint8_t *buf = tx_ascii_buf;
buf[0] = tag[0];
buf[1] = tag[1];
buf[2] = tag[2];
buf[3] = tag[3];
memcpy(&buf[4], value, value_len);
uint16_t crc = dr_crc16_compute(buf, (uint32_t)(4U + value_len));
buf[4 + value_len] = (uint8_t)(crc & 0xFF);
buf[5 + value_len] = (uint8_t)(crc >> 8);
return ble_data_send(buf, (uint16_t)(6U + value_len));
}
static int send_response_tag_echo(const char *tag, const char *echo_tag)
{
uint8_t *buf = tx_ascii_buf;
buf[0] = tag[0];
buf[1] = tag[1];
buf[2] = tag[2];
buf[3] = tag[3];
buf[4] = echo_tag[0];
buf[5] = echo_tag[1];
buf[6] = echo_tag[2];
buf[7] = echo_tag[3];
uint16_t crc = dr_crc16_compute(buf, 8);
buf[8] = (uint8_t)(crc & 0xFF);
buf[9] = (uint8_t)(crc >> 8);
return ble_data_send(buf, 10);
}
/*==============================================================================
@@ -65,9 +170,9 @@ static void send_response_u16(const char *tag, uint16_t value)
/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
* 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
static int send_response_imu(const int16_t accel[3], const int16_t gyro[3])
{
uint8_t buf[18];
uint8_t *buf = tx_imu_buf;
buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':';
@@ -84,7 +189,244 @@ static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
buf[16] = (uint8_t)(crc & 0xFF);
buf[17] = (uint8_t)(crc >> 8);
ble_data_send(buf, 18);
return ble_data_send(buf, 18);
}
static void send_response_echo(const uint16_t *samples, uint16_t num_samples)
{
/*
* reb: 패킷은 최대 208바이트라서 스택 지역변수로 두면
* mbb?처럼 반복 호출할 때 워커 스택을 꽤 먹는다.
* 현재는 한 번에 하나의 명령만 처리하므로 정적 버퍼를 재사용한다.
*/
uint8_t *buf = tx_echo_buf;
buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
buf[4] = (uint8_t)(num_samples >> 8);
buf[5] = (uint8_t)(num_samples & 0xFF);
for (uint16_t i = 0; i < num_samples; i++)
{
buf[6 + i * 2] = (uint8_t)(samples[i] >> 8);
buf[7 + i * 2] = (uint8_t)(samples[i] & 0xFF);
}
uint16_t payload_len = 4 + 2 + (num_samples * 2);
uint16_t crc = dr_crc16_compute(buf, payload_len);
buf[payload_len] = (uint8_t)(crc & 0xFF);
buf[payload_len + 1] = (uint8_t)(crc >> 8);
ble_data_send(buf, payload_len + 2);
}
static void send_response_bundle(uint16_t batt_mv,
const int16_t accel[3],
const int16_t gyro[3],
int16_t temp_cdeg)
{
uint8_t *buf = tx_bundle_buf;
buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
buf[4] = (uint8_t)(batt_mv >> 8);
buf[5] = (uint8_t)(batt_mv & 0xFF);
const int16_t imu_vals[6] = {
accel[0], accel[1], accel[2],
gyro[0], gyro[1], gyro[2]
};
for (int i = 0; i < 6; i++)
{
buf[6 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] >> 8);
buf[7 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] & 0xFF);
}
buf[18] = (uint8_t)((uint16_t)temp_cdeg >> 8);
buf[19] = (uint8_t)((uint16_t)temp_cdeg & 0xFF);
uint16_t crc = dr_crc16_compute(buf, 20);
buf[20] = (uint8_t)(crc & 0xFF);
buf[21] = (uint8_t)(crc >> 8);
ble_data_send(buf, sizeof(tx_bundle_buf));
}
static void send_response_piezo_config(uint16_t freq,
uint16_t cycles,
uint16_t avg,
uint16_t delay_us,
uint16_t samples)
{
uint8_t *buf = tx_cfg_buf;
buf[0] = 'r'; buf[1] = 'c'; buf[2] = 'f'; buf[3] = ':';
buf[4] = (uint8_t)(freq >> 8);
buf[5] = (uint8_t)(freq & 0xFF);
buf[6] = (uint8_t)(cycles >> 8);
buf[7] = (uint8_t)(cycles & 0xFF);
buf[8] = (uint8_t)(avg >> 8);
buf[9] = (uint8_t)(avg & 0xFF);
buf[10] = (uint8_t)(delay_us >> 8);
buf[11] = (uint8_t)(delay_us & 0xFF);
buf[12] = (uint8_t)(samples >> 8);
buf[13] = (uint8_t)(samples & 0xFF);
uint16_t crc = dr_crc16_compute(buf, 14);
buf[14] = (uint8_t)(crc & 0xFF);
buf[15] = (uint8_t)(crc >> 8);
ble_data_send(buf, sizeof(tx_cfg_buf));
}
static int start_piezo_session(void)
{
DBG_PRINTF("[MBB] piezo session start\r\n");
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("[MBB] piezo session ready\r\n");
return ECHO_STATUS_OK;
}
static int perform_piezo_sweep(void)
{
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
DBG_PRINTF("[MBB] sweep ch=%d start\r\n", ch);
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);
err = echo_adc_wake();
if (err)
{
DBG_PRINTF("[ECHO] dummy read fail ch=%d err=%d\r\n", ch, err);
return ECHO_STATUS_CAPTURE;
}
memset(echo_accum, 0, sizeof(echo_accum));
for (uint8_t avg = 0; avg < PIEZO_AVERAGE_COUNT; 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);
err = echo_adc_capture(echo_capture, ECHO_ADC_MAX_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++)
{
echo_accum[i] += echo_capture[i];
}
}
for (uint16_t i = 0; i < ECHO_ADC_MAX_SAMPLES; i++)
{
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / PIEZO_AVERAGE_COUNT);
}
DBG_PRINTF("[MBB] sweep ch=%d done\r\n", ch);
}
DBG_PRINTF("[MBB] sweep done\r\n");
return ECHO_STATUS_OK;
}
static int perform_single_piezo_capture(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 > ECHO_ADC_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);
}
piezo_burst_sw(cycles);
k_busy_wait(delay_us);
err = echo_adc_capture(echo_capture, num_samples);
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;
}
/*==============================================================================
@@ -95,6 +437,9 @@ static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
static int cmd_msn(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int mv = battery_read_mv();
if (mv < 0)
{
@@ -109,6 +454,9 @@ static int cmd_msn(const uint8_t *data, uint8_t data_len)
/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
static int cmd_msp(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int16_t accel[3], gyro[3];
int ret = imu_read(accel, gyro);
@@ -122,10 +470,464 @@ static int cmd_msp(const uint8_t *data, uint8_t data_len)
return 1;
}
/* mst? → 피에조 전원 ON → TMP235 온도 측정 → 전원 OFF → rso: + 온도(°C × 100, BE)
*
* TMP235가 피에조 레일을 공유하므로 ON/OFF 시퀀스를 한 커맨드에서 처리.
* 안정화 대기 10ms: TMP235 start-up(~2ms) + 레일 RC 필터 여유분.
* 에러 응답: 0xFFFF = ADC 읽기 실패 */
static int cmd_mst(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
/*
* mst?는 "온도만 읽는 명령"처럼 보이지만,
* 실제로는 TMP235가 piezo 전원 레일을 같이 쓰기 때문에
* 전원 ON/OFF 시퀀스까지 같이 처리해야 한다.
*/
power_button_suspend(true);
if (piezo_init() != 0)
{
power_button_suspend(false);
send_response_u16("rso:", 0xFFFF);
DBG_PRINTF("[CMD] mst: piezo init fail\r\n");
return 1;
}
/* 전원 ON → 센서 안정화 대기 */
piezo_power_on();
k_msleep(10);
int16_t t_cdeg = temp_read_cdeg();
/* 전원 OFF (측정 완료, 레일 끄기) */
piezo_power_off();
power_button_suspend(false);
/* ADC 읽기 실패 → 에러 코드 0xFFFF */
if (t_cdeg == INT16_MIN)
{
send_response_u16("rso:", 0xFFFF);
DBG_PRINTF("[CMD] mst: temp read fail\r\n");
return 1;
}
/* 음수 온도도 2's complement로 그대로 전송 (앱이 int16로 해석) */
send_response_u16("rso:", (uint16_t)t_cdeg);
DBG_PRINTF("[CMD] mst -> %d.%02d C\r\n",
t_cdeg / 100,
(t_cdeg < 0 ? -t_cdeg : t_cdeg) % 100);
return 1;
}
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");
send_response_u16("rpa:", 0);
return 1;
}
DBG_CORE("[MPA] piezo_init ok\r\n");
piezo_power_on();
DBG_CORE("[MPA] power on\r\n");
send_response_u16("rpa:", 1);
DBG_CORE("[MPA] response sent\r\n");
return 1;
}
static int cmd_mpb(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
/* mpb?: piezo TX/RX 전원 레일을 끈다. */
DBG_CORE("[MPB] enter\r\n");
if (piezo_init() != 0) {
DBG_ERR("[MPB] piezo_init failed\r\n");
send_response_u16("rpb:", 0);
return 1;
}
DBG_CORE("[MPB] piezo_init ok\r\n");
piezo_power_off();
DBG_CORE("[MPB] power off\r\n");
send_response_u16("rpb:", 1);
DBG_CORE("[MPB] response sent\r\n");
return 1;
}
static int cmd_mpc(const uint8_t *data, uint8_t data_len)
{
uint16_t cycles = 5;
uint16_t freq_option = 1;
uint16_t piezo_ch = 0;
get_data_u16_be(data, data_len, 0, &cycles);
get_data_u16_be(data, data_len, 1, &freq_option);
get_data_u16_be(data, data_len, 2, &piezo_ch);
ARG_UNUSED(freq_option);
/*
* mpc?: burst만 한 번 발생시키는 테스트 명령.
* echo를 읽지 않으므로 "초음파가 나가는지"만 빠르게 볼 때 쓴다.
*/
if ((cycles < 3U) || (cycles > 7U))
{
send_response_u16("rpc:", 2);
return 1;
}
power_button_suspend(true);
if (piezo_init() != 0) {
power_button_suspend(false);
send_response_u16("rpc:", 0);
return 1;
}
piezo_power_on();
if (piezo_select_channel((uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS)) != 0) {
piezo_power_off();
power_button_suspend(false);
send_response_u16("rpc:", 0);
return 1;
}
/*
* 현재 Zephyr 포팅본은 2.1MHz SW burst 하나만 구현되어 있다.
* 레거시의 freq_option 값은 받아두되, 아직은 같은 burst 함수로 처리한다.
*/
piezo_burst_sw((uint8_t)cycles);
piezo_power_off();
power_button_suspend(false);
send_response_u16("rpc:", cycles);
return 1;
}
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;
uint16_t piezo_ch = 0;
get_data_u16_be(data, data_len, 0, &freq_option);
get_data_u16_be(data, data_len, 1, &delay_us);
get_data_u16_be(data, data_len, 2, &num_samples);
get_data_u16_be(data, data_len, 3, &cycles);
get_data_u16_be(data, data_len, 4, &averaging);
get_data_u16_be(data, data_len, 5, &piezo_ch);
ARG_UNUSED(freq_option);
/*
* mec?: 단일 채널 burst + echo capture.
* maa?/mbb?보다 가벼워서 "한 채널만 먼저 살아 있는지" 확인하기 좋다.
*/
if (num_samples > ECHO_ADC_MAX_SAMPLES) {
num_samples = ECHO_ADC_MAX_SAMPLES;
}
if ((cycles < 3U) || (cycles > 7U)) {
cycles = PIEZO_SW_BURST_CYCLES;
}
if (averaging == 0U) {
averaging = 1U;
}
processing = true;
power_button_suspend(true);
int status = start_piezo_session();
if (status == ECHO_STATUS_OK)
{
status = perform_single_piezo_capture((uint8_t)cycles,
delay_us,
num_samples,
averaging,
(uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
}
if (status == ECHO_STATUS_OK)
{
send_response_echo(echo_capture, num_samples);
}
piezo_power_off();
power_button_suspend(false);
send_response_u16("raa:", (uint16_t)status);
processing = false;
return 1;
}
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);
int status = start_piezo_session();
if (status == ECHO_STATUS_OK)
{
status = perform_piezo_sweep();
}
if (status == ECHO_STATUS_OK)
{
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
send_response_echo(piezo_channels[ch], ECHO_ADC_MAX_SAMPLES);
}
}
piezo_power_off();
power_button_suspend(false);
send_response_u16("raa:", (uint16_t)status);
DBG_PRINTF("[CMD] maa status=0x%04X\r\n", status);
processing = false;
return 1;
}
static int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int16_t accel[3];
int16_t gyro[3];
/*
* mbb?는 이 프로젝트에서 가장 무거운 명령 중 하나다.
*
* 순서:
* 1. piezo/echo ADC 준비
* 2. 6채널 echo sweep
* 3. battery / imu / temp 추가 측정
* 4. rbb: 1개 전송
* 5. reb: 6개 전송
* 6. raa: 최종 상태 전송
*
* 중간에 하나라도 실패하면 status에 에러 코드를 넣고,
* 성공한 경우에만 묶음 응답(rbb + reb)을 보낸다.
*/
processing = true;
power_button_suspend(true);
DBG_PRINTF("[MBB] cmd start\r\n");
int status = start_piezo_session();
if (status == ECHO_STATUS_OK)
{
status = perform_piezo_sweep();
}
int batt_mv = -1;
int16_t temp_cdeg = INT16_MIN;
if (status == ECHO_STATUS_OK)
{
/* info 성격 데이터는 echo sweep이 정상 끝났을 때만 읽는다. */
DBG_PRINTF("[MBB] battery read\r\n");
batt_mv = battery_read_mv();
if (batt_mv < 0)
{
status = ECHO_STATUS_BATT;
}
}
if (status == ECHO_STATUS_OK)
{
DBG_PRINTF("[MBB] imu read\r\n");
if (imu_read(accel, gyro) != 0)
{
status = ECHO_STATUS_IMU;
}
}
if (status == ECHO_STATUS_OK)
{
DBG_PRINTF("[MBB] temp read\r\n");
temp_cdeg = temp_read_cdeg();
if (temp_cdeg == INT16_MIN)
{
status = ECHO_STATUS_TEMP;
}
}
if (status == ECHO_STATUS_OK)
{
/* rbb: 한 번에 보내는 "요약 정보 묶음" 패킷 */
DBG_PRINTF("[MBB] response tx start\r\n");
send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
/* reb: 채널별 raw echo 파형 */
DBG_PRINTF("[MBB] tx reb ch=%d\r\n", ch);
send_response_echo(piezo_channels[ch], ECHO_ADC_MAX_SAMPLES);
}
}
piezo_power_off();
power_button_suspend(false);
DBG_PRINTF("[MBB] power off\r\n");
send_response_u16("raa:", (uint16_t)status);
DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);
processing = false;
return 1;
}
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);
return 1;
}
/* mls? → LED 상태 변경 → rls: + state echo back
* 파라미터: [state(2B LE)] — led_state_t enum 값
* 0=OFF, 4=DETACH_WARNING, 5=ALIGN_SEARCHING, 6=ALIGN_COMPLETE, 7=ERROR
* 에러 응답: 0xFFFF=파라미터 없음, 0xFFFE=범위 초과 */
static int cmd_mfv(const uint8_t *data, uint8_t data_len)
{
char fw_version[SERIAL_NO_LENGTH];
int err;
ARG_UNUSED(data);
ARG_UNUSED(data_len);
copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));
err = send_response_ascii("rfv:", fw_version, sizeof(fw_version));
if (err)
{
DBG_ERR("[CMD] mfv tx failed err=%d\r\n", err);
}
DBG_PRINTF("[CMD] mfv read\r\n");
return 1;
}
static int cmd_mwh(const uint8_t *data, uint8_t data_len)
{
if (data_len < HW_NO_LENGTH)
{
send_response_u16("rwh:", 0xFFFF);
DBG_PRINTF("[CMD] mwh: insufficient data len=%u\r\n", data_len);
return 1;
}
memset(HW_NO, 0, sizeof(HW_NO));
memcpy(HW_NO, data, HW_NO_LENGTH);
send_response_ascii("rwh:", HW_NO, HW_NO_LENGTH);
DBG_PRINTF("[CMD] mwh updated\r\n");
return 1;
}
static int cmd_mrh(const uint8_t *data, uint8_t data_len)
{
int err;
ARG_UNUSED(data);
ARG_UNUSED(data_len);
err = send_response_ascii("rrh:", HW_NO, HW_NO_LENGTH);
if (err) {
DBG_ERR("[CMD] mrh tx failed err=%d\r\n", err);
}
DBG_PRINTF("[CMD] mrh read\r\n");
return 1;
}
static int cmd_mws(const uint8_t *data, uint8_t data_len)
{
if (data_len < SERIAL_NO_LENGTH)
{
send_response_u16("rws:", 0xFFFF);
DBG_PRINTF("[CMD] mws: insufficient data len=%u\r\n", data_len);
return 1;
}
memset(SERIAL_NO, 0, sizeof(SERIAL_NO));
memcpy(SERIAL_NO, data, SERIAL_NO_LENGTH);
send_response_ascii("rws:", SERIAL_NO, SERIAL_NO_LENGTH);
DBG_PRINTF("[CMD] mws updated\r\n");
return 1;
}
static int cmd_mrs(const uint8_t *data, uint8_t data_len)
{
int err;
ARG_UNUSED(data);
ARG_UNUSED(data_len);
err = send_response_ascii("rrs:", SERIAL_NO, SERIAL_NO_LENGTH);
if (err) {
DBG_ERR("[CMD] mrs tx failed err=%d\r\n", err);
}
DBG_PRINTF("[CMD] mrs read\r\n");
return 1;
}
static int cmd_mpz(const uint8_t *data, uint8_t data_len)
{
if (data_len < PASSKEY_LENGTH)
{
send_response_u16("rpz:", 0xFFFF);
DBG_PRINTF("[CMD] mpz: insufficient data len=%u\r\n", data_len);
return 1;
}
memset(m_static_passkey, 0, sizeof(m_static_passkey));
memcpy(m_static_passkey, data, PASSKEY_LENGTH);
send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH);
DBG_PRINTF("[CMD] mpz updated\r\n");
return 1;
}
static int cmd_mqz(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
send_response_ascii("rqz:", m_static_passkey, PASSKEY_LENGTH);
DBG_PRINTF("[CMD] mqz read\r\n");
return 1;
}
static int cmd_mls(const uint8_t *data, uint8_t data_len)
{
/* 파라미터 부족 → 에러 코드 0xFFFF 에코 */
@@ -167,6 +969,21 @@ static const cmd_entry_t cmd_table[] =
{ "msn?", cmd_msn },
{ "mls?", cmd_mls },
{ "msp?", cmd_msp },
{ "mst?", cmd_mst },
{ "mpa?", cmd_mpa },
{ "mpb?", cmd_mpb },
{ "mpc?", cmd_mpc },
{ "mec?", cmd_mec },
{ "maa?", cmd_maa },
{ "mbb?", cmd_mbb },
{ "mcf?", cmd_mcf },
{ "mfv?", cmd_mfv },
{ "mwh?", cmd_mwh },
{ "mrh?", cmd_mrh },
{ "mws?", cmd_mws },
{ "mrs?", cmd_mrs },
{ "mpz?", cmd_mpz },
{ "mqz?", cmd_mqz },
};
#define CMD_TABLE_SIZE (sizeof(cmd_table) / sizeof(cmd_table[0]))
@@ -176,28 +993,47 @@ static const cmd_entry_t cmd_table[] =
*============================================================================*/
int dr_parser(const uint8_t *buf, uint16_t len)
{
DBG_CORE("[PARSER] enter len=%u\r\n", len);
DBG_CORE("[CMD] RX len=%u\r\n", len);
/* 최소 4바이트 TAG 필요 */
if (len < 4)
{
DBG_PRINTF("[CMD] Too short (%d)\r\n", len);
DBG_ERR("[CMD] Too short (%u)\r\n", len);
return -1;
}
char raw_tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };
/* CRC16 검증 (6바이트 이상이면 마지막 2바이트가 CRC) */
if (len >= 6)
{
/*
* 이 프로토콜은 끝 2바이트에 CRC16이 붙는다.
* 값이 다르면 "명령 이름은 맞아 보여도 데이터가 깨졌다"는 뜻이므로 바로 버린다.
*/
uint16_t calc_crc = dr_crc16_compute(buf, len - 2);
uint16_t recv_crc = (uint16_t)(buf[len - 2]) | ((uint16_t)(buf[len - 1]) << 8);
if (calc_crc != recv_crc)
{
DBG_PRINTF("[CMD] CRC fail (calc=0x%04X recv=0x%04X)\r\n", calc_crc, recv_crc);
DBG_ERR("[CMD] CRC fail tag=%s calc=0x%04X recv=0x%04X\r\n",
raw_tag, calc_crc, recv_crc);
if (send_response_tag_echo("rxc:", raw_tag) != 0) {
DBG_ERR("[CMD] rxc tx failed\r\n");
}
return -1;
}
}
/* TAG 추출 (4바이트) */
char tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };
char tag[5] = {
ascii_to_lower((char)buf[0]),
ascii_to_lower((char)buf[1]),
ascii_to_lower((char)buf[2]),
ascii_to_lower((char)buf[3]),
'\0'
};
DBG_CORE("[CMD] tag=%s\r\n", tag);
/* 데이터 부분 (TAG 이후, CRC 이전) */
const uint8_t *data = buf + 4;
@@ -208,10 +1044,14 @@ int dr_parser(const uint8_t *buf, uint16_t len)
{
if (memcmp(tag, cmd_table[i].tag, 4) == 0)
{
DBG_CORE("[CMD] dispatch -> %s\r\n", cmd_table[i].tag);
return cmd_table[i].handler(data, data_len);
}
}
DBG_PRINTF("[CMD] Unknown: %s\r\n", tag);
DBG_ERR("[CMD] Unknown: raw=%s normalized=%s\r\n", raw_tag, tag);
if (send_response_tag_echo("rxx:", raw_tag) != 0) {
DBG_ERR("[CMD] rxx tx failed\r\n");
}
return 0;
}