jh.chun/VesiScan-Basic_Zephyr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
9b042dfef237b3ecd0061b2dd62a2bec5c53580a
VesiScan-Basic_Zephyr/src/parser.c
T
jh.chun 9b042dfef2 piezo_measure 파일 분리 
- parser.c: BLE 커맨드 파싱/측정 함수 호출/응담 패킷 생성 및 전송
- piezo.c: Piezo 관련 GPIO 제어
- piezo_measure.c: 실제 측정 시퀀스
2026-06-30 15:38:32 +09:00

1302 lines
35 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/*******************************************************************************
* @file parser.c
* @brief BLE command parser
*
* Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
#include <zephyr/sys/reboot.h>
#if IS_ENABLED(CONFIG_BT_SMP)
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#endif
#include <string.h>
#include <limits.h>
#include "parser.h"
#include "app_nvs.h"
#include "main.h"
#include "debug_print.h"
#include "ble_service.h"
#include "battery_adc.h"
#include "led_control.h"
#include "imu_i2c.h"
#include "piezo.h"
#include "piezo_measure.h"
/*==============================================================================
* Piezo / echo response state
*============================================================================*/
static uint8_t tx_u16_buf[8];
static uint8_t tx_imu_buf[18];
static uint8_t tx_echo_buf[4 + 2 + 2 + (PIEZO_MEASURE_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 uint8_t tx_id_buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH + SERIAL_NO_LENGTH + 2];
static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES];
static uint8_t tx_rim_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2];
static uint8_t g_echo_session;
static void reboot_after_response(void)
{
k_msleep(100);
sys_reboot(SYS_REBOOT_COLD);
}
/*==============================================================================
* CRC16 (CRC-CCITT, Nordic SDK 호환)
*============================================================================*/
static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size)
{
uint16_t crc = 0xFFFF;
for (uint32_t i = 0; i < size; i++)
{
crc = (uint8_t)(crc >> 8) | (crc << 8);
crc ^= p_data[i];
crc ^= (uint8_t)(crc & 0xFF) >> 4;
crc ^= (crc << 8) << 4;
crc ^= ((crc & 0xFF) << 4) << 1;
}
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 int send_response_u16(const char *tag, uint16_t value)
{
uint8_t *buf = tx_u16_buf;
buf[0] = tag[0];
buf[1] = tag[1];
buf[2] = tag[2];
buf[3] = tag[3];
buf[4] = (uint8_t)(value >> 8);
buf[5] = (uint8_t)(value & 0xFF);
uint16_t crc = dr_crc16_compute(buf, 6);
buf[6] = (uint8_t)(crc & 0xFF);
buf[7] = (uint8_t)(crc >> 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_identity(void)
{
uint8_t *buf = tx_id_buf;
char fw_version[SERIAL_NO_LENGTH];
copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));
buf[0] = 'r';
buf[1] = 'i';
buf[2] = 'd';
buf[3] = ':';
memcpy(&buf[4], HW_NO, HW_NO_LENGTH);
memcpy(&buf[4 + HW_NO_LENGTH], SERIAL_NO, SERIAL_NO_LENGTH);
memcpy(&buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH], fw_version, sizeof(fw_version));
uint16_t crc = dr_crc16_compute(buf, (uint32_t)(sizeof(tx_id_buf) - 2U));
buf[sizeof(tx_id_buf) - 2U] = (uint8_t)(crc & 0xFF);
buf[sizeof(tx_id_buf) - 1U] = (uint8_t)(crc >> 8);
return ble_data_send(buf, sizeof(tx_id_buf));
}
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);
}
/*==============================================================================
* 응답 패킷 전송 (IMU)
*============================================================================*/
/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
* 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
static int send_response_imu(const int16_t accel[3], const int16_t gyro[3])
{
uint8_t *buf = tx_imu_buf;
buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':';
const int16_t vals[6] ={
accel[0], accel[1], accel[2],
gyro[0], gyro[1], gyro[2]
};
for (int i = 0; i < 6; i++)
{
buf[4 + i * 2] = (uint8_t)((uint16_t)vals[i] >> 8); /* MSB */
buf[4 + i * 2 + 1] = (uint8_t)((uint16_t)vals[i] & 0xFF); /* LSB */
}
uint16_t crc = dr_crc16_compute(buf, 16);
buf[16] = (uint8_t)(crc & 0xFF);
buf[17] = (uint8_t)(crc >> 8);
return ble_data_send(buf, 18);
}
static void send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
{
/* 정적 버퍼 사용 (한 번에 한 명령만 처리) */
uint8_t *buf = tx_echo_buf;
buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
buf[4] = session;
buf[5] = channel;
buf[6] = (uint8_t)(num_samples >> 8);
buf[7] = (uint8_t)(num_samples & 0xFF);
for (uint16_t i = 0; i < num_samples; i++)
{
buf[8 + i * 2] = (uint8_t)(samples[i] >> 8);
buf[9 + i * 2] = (uint8_t)(samples[i] & 0xFF);
}
uint16_t payload_len = 4 + 2 + 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_rim(const uint8_t *sample_bytes, uint16_t sample_count)
{
uint8_t *buf = tx_rim_buf;
uint16_t payload_len;
if (sample_count > IMU_FIFO_RIM_TARGET_SAMPLES)
{
sample_count = IMU_FIFO_RIM_TARGET_SAMPLES;
}
buf[0] = 'r'; buf[1] = 'i'; buf[2] = 'm'; buf[3] = ':';
buf[4] = (uint8_t)(sample_count >> 8);
buf[5] = (uint8_t)(sample_count & 0xFF);
memcpy(&buf[6], sample_bytes, (size_t)sample_count * IMU_FIFO_SAMPLE_BYTES);
payload_len = (uint16_t)(6U + (sample_count * IMU_FIFO_SAMPLE_BYTES));
uint16_t crc = dr_crc16_compute(buf, payload_len);
buf[payload_len] = (uint8_t)(crc & 0xFF);
buf[payload_len + 1U] = (uint8_t)(crc >> 8);
DBG_PRINTF("[MTB] tx rim samples=%u len=%u\r\n", sample_count, (uint16_t)(payload_len + 2U));
ble_data_send(buf, (uint16_t)(payload_len + 2U));
}
static void send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cycles, uint16_t avg, uint16_t delay_us, uint16_t samples)
{
uint8_t *buf = tx_cfg_buf;
buf[0] = tag[0];
buf[1] = tag[1];
buf[2] = tag[2];
buf[3] = tag[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));
}
/*==============================================================================
* 커맨드 핸들러
*============================================================================*/
/* msn? → 배터리 전압 측정 → rsn: + mV */
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)
{
mv = 0;
}
send_response_u16("rsn:", (uint16_t)mv);
return 1;
}
static int cmd_msq(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int err = send_response_u16("rsq:", 0x0000);
if (err)
{
DBG_ERR("[CMD] msq tx failed err=%d\r\n", err);
}
DBG_PRINTF("[CMD] msq -> power off\r\n");
sleep_mode_enter_reason("cmd-msq");
return 1;
}
static int cmd_mss(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int err = send_response_u16("rss:", 0x0000);
if (err)
{
DBG_ERR("[CMD] mss tx failed err=%d\r\n", err);
}
DBG_PRINTF("[CMD] mss -> reboot\r\n");
reboot_after_response();
return 1;
}
static int cmd_msr(const uint8_t *data, uint8_t data_len)
{
uint16_t status = 0x0000;
ARG_UNUSED(data);
ARG_UNUSED(data_len);
int tx_err = send_response_u16("rsr:", status);
if (tx_err)
{
DBG_ERR("[CMD] msr tx failed err=%d\r\n", tx_err);
return 1;
}
/*
* 현재 연결의 bond/security 정보를 먼저 지우면 같은 연결에서 rsr: 응답을
* 보낼 수 없음. 응답 전송 완료 후 bond 삭제와 reboot 진행 필요
*/
k_msleep(100);
#if IS_ENABLED(CONFIG_BT_SMP)
int err = bt_unpair(BT_ID_DEFAULT, NULL);
if (err)
{
status = (uint16_t)(-err);
DBG_ERR("[CMD] msr bt_unpair failed err=%d\r\n", err);
}
else
{
bond_data_delete = true;
DBG_PRINTF("[CMD] msr bond data deleted\r\n");
}
#else
bond_data_delete = true;
DBG_PRINTF("[CMD] msr bond delete skipped (BT_SMP disabled)\r\n");
#endif
if (status == 0x0000)
{
DBG_PRINTF("[CMD] msr complete -> reboot\r\n");
}
reboot_after_response();
return 1;
}
/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
static int cmd_mdf(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
uint16_t status = ble_dfu_advertising_is_enabled() ? 0xFFFF : 0x0000;
/* DFU advertising 전환 중 연결이 끊겨도 전원 래치가 유지되도록 먼저 ON 상태를 확정한다. */
device_power_keep_on();
int tx_err = send_response_u16("rdf:", status);
if (tx_err)
{
DBG_ERR("[CMD] mdf tx failed err=%d\r\n", tx_err);
return 1;
}
if (status == 0x0000)
{
/* rdf: 응답이 나간 뒤 연결 해제와 SMP 광고 전환 실행 */
k_msleep(100);
int err = ble_dfu_advertising_enable();
if (err)
{
DBG_ERR("[CMD] mdf enable failed err=%d\r\n", err);
}
}
else
{
DBG_PRINTF("[CMD] mdf rejected: already in DFU advertising\r\n");
}
return 1;
}
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);
if (ret != 0)
{
send_response_u16("rsp:", 0xFFFF);
DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret);
return 1;
}
send_response_imu(accel, gyro);
return 1;
}
/* mst? -> IMU internal temperature -> rso: + temperature (degC x 100, BE)
* Error response: 0xFFFF = IMU temperature read failed. */
static int cmd_mst(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
power_button_suspend(true);
int16_t t_cdeg = INT16_MIN;
int ret = imu_read_temperature_cdeg(&t_cdeg);
power_button_suspend(false);
if ((ret != 0) || (t_cdeg == INT16_MIN))
{
send_response_u16("rso:", 0xFFFF);
DBG_PRINTF("[CMD] mst: imu temp read fail ret=%d\r\n", ret);
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;
}
/*
* 테스트용
* mpa?: piezo TX/RX 전원 레일만 켬
*/
static int cmd_mpa(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
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;
}
/*
* 테스트용
* mpc?: burst만 한 번 발생시키는 테스트 명령
* echo를 읽지 않고 초음파가 나가는지만 볼 때 사용
*/
static int cmd_mpc(const uint8_t *data, uint8_t data_len)
{
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);
get_data_u16_be(data, data_len, 1, &freq_option);
get_data_u16_be(data, data_len, 2, &piezo_ch);
ARG_UNUSED(freq_option);
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;
}
/*
* 테스트용
* mec?: 단일 채널 burst + echo capture
*/
static int cmd_mec(const uint8_t *data, uint8_t data_len)
{
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;
uint8_t session = g_echo_session++;
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);
if (num_samples > PIEZO_MEASURE_MAX_SAMPLES)
{
num_samples = PIEZO_MEASURE_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 = piezo_measure_start_session();
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_single_capture((uint8_t)cycles, delay_us, num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
}
if (status == ECHO_STATUS_OK)
{
send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples);
}
piezo_power_off();
power_button_suspend(false);
send_response_u16("raa:", (uint16_t)status);
processing = false;
return 1;
}
/*
* 테스트용
* mad?: 단일 채널 echo capture만 수행
* - piezo burst는 하지 않고 echo ADC만 수행
* - echo 샘플을 모은 뒤 reb: 패킷 1개 전송
* - raa: 상태값은 전체 작업 성공/실패 요약
*/
static int cmd_mad(const uint8_t *data, uint8_t data_len)
{
const piezo_config_t *cfg = piezo_config_get();
uint16_t num_samples = cfg->samples;
uint16_t averaging = cfg->avg;
uint16_t piezo_ch = 0;
uint8_t session = g_echo_session++;
get_data_u16_be(data, data_len, 2, &num_samples);
get_data_u16_be(data, data_len, 4, &averaging);
get_data_u16_be(data, data_len, 5, &piezo_ch);
if (num_samples > PIEZO_MEASURE_MAX_SAMPLES)
{
num_samples = PIEZO_MEASURE_MAX_SAMPLES;
}
if (averaging == 0U)
{
averaging = 1U;
}
processing = true;
power_button_suspend(true);
int status = piezo_measure_start_session();
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_adc_only_capture(num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
}
if (status == ECHO_STATUS_OK)
{
send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples);
}
piezo_power_off();
power_button_suspend(false);
send_response_u16("raa:", (uint16_t)status);
DBG_PRINTF("[CMD] mad status=0x%04X ch=%u samples=%u avg=%u\r\n",
status,
(uint16_t)(piezo_ch % PIEZO_NUM_CHANNELS),
num_samples,
averaging);
processing = false;
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);
uint8_t session = g_echo_session++;
processing = true;
power_button_suspend(true);
int status = piezo_measure_start_session();
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_sweep();
}
if (status == ECHO_STATUS_OK)
{
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->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;
}
/*
* 전체 측정 명령
* mbb?: piezo echo sweep + 배터리 + IMU + IMU 온도 측정
* - 성공 시 rbb 1개, reb 6개, raa 상태값 전송
* - 실패 시 rbb/reb 생략, raa에 실패 원인 코드 전송
* - processing=true 구간에서는 내부 모니터의 I2C 접근을 막는 기준으로 사용
*/
static int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
uint8_t session = g_echo_session++;
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 = piezo_measure_start_session();
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_sweep();
}
int batt_mv = -1;
int16_t temp_cdeg = INT16_MIN;
if (status == ECHO_STATUS_OK)
{
/* echo sweep 성공 후에만 부가 센서값 읽기
* - rbb에 포함되는 배터리 전압
* - 실패 시 ECHO_STATUS_BATT로 종료
*/
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)
{
/* rbb에 포함되는 IMU 6축 값과 IMU 내부 온도 읽기
* - direct read 경로 사용
* - 실패 시 ECHO_STATUS_IMU로 종료
*/
DBG_PRINTF("[MBB] imu/temp read\r\n");
if (imu_read_with_temperature(accel, gyro, &temp_cdeg) != 0)
{
status = ECHO_STATUS_IMU;
}
}
if (status == ECHO_STATUS_OK)
{
/* mbb? 성공 응답값으로 연속 측정 보호 판정 갱신
* - 실패한 측정은 호출하지 않아 기존 저전압/고온 카운트 유지
* - rbb에 실리는 배터리/IMU 온도 값과 같은 값을 사용
*/
battery_protection_record_continuous(batt_mv, temp_cdeg);
/* 측정 성공 응답 순서
* - rbb: 배터리 + IMU + IMU 온도 요약
* - reb: 채널별 echo raw 데이터
* - raa: 마지막 상태 코드
*/
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(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->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;
}
/*
* mtb?: piezo sweep + IMU FIFO
*/
static int cmd_mtb(const uint8_t *data, uint8_t data_len)
{
ARG_UNUSED(data);
ARG_UNUSED(data_len);
uint8_t session = g_echo_session++;
uint16_t rim_count = 0U;
bool fifo_started = false;
processing = true;
power_button_suspend(true);
DBG_PRINTF("[MTB] cmd start\r\n");
int status = ECHO_STATUS_OK;
int imu_ret = imu_fifo_start();
if (imu_ret != 0)
{
DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
status = ECHO_STATUS_IMU;
}
else
{
fifo_started = true;
}
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_start_session();
}
if (status == ECHO_STATUS_OK)
{
status = piezo_measure_sweep();
}
if (status == ECHO_STATUS_OK)
{
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
{
send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
}
}
if (fifo_started)
{
int imu_ret = imu_fifo_read_latest(tx_rim_samples,
IMU_FIFO_RIM_TARGET_SAMPLES,
&rim_count);
if (imu_ret != 0)
{
DBG_PRINTF("[MTB] fifo read fail ret=%d\r\n", imu_ret);
status = ECHO_STATUS_IMU;
rim_count = 0U;
}
}
send_response_rim(tx_rim_samples, rim_count);
piezo_power_off();
power_button_suspend(false);
send_response_u16("raa:", (uint16_t)status);
DBG_PRINTF("[CMD] mtb status=0x%04X rim=%u\r\n", status, rim_count);
processing = false;
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);
const piezo_config_t *cfg = piezo_config_get();
send_response_piezo_config("rcf:", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
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_piezo_config("rcs:", 0xFFFF, 0U, 0U, 0U, 0U);
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_piezo_config("rcs:", 0xFFFF, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
return 1;
}
int err = piezo_config_set(&cfg);
if (err)
{
send_response_piezo_config("rcs:", 0xFFFD, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
return 1;
}
const piezo_config_t *saved_cfg = piezo_config_get();
send_response_piezo_config("rcs:", saved_cfg->freq, saved_cfg->cycles, saved_cfg->avg, saved_cfg->delay_us, saved_cfg->samples);
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);
}
return 1;
}
static int cmd_mid(const uint8_t *data, uint8_t data_len)
{
int err;
ARG_UNUSED(data);
ARG_UNUSED(data_len);
err = send_response_identity();
if (err)
{
DBG_ERR("[CMD] mid tx failed err=%d\r\n", err);
}
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);
return 1;
}
memset(HW_NO, 0, sizeof(HW_NO));
memcpy(HW_NO, data, HW_NO_LENGTH);
int err = app_nvs_save_hw_no(HW_NO);
if (err)
{
send_response_u16("rwh:", 0xFFFD);
return 1;
}
send_response_ascii("rwh:", HW_NO, HW_NO_LENGTH);
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);
}
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);
return 1;
}
memset(SERIAL_NO, 0, sizeof(SERIAL_NO));
memcpy(SERIAL_NO, data, SERIAL_NO_LENGTH);
int err = app_nvs_save_serial_no(SERIAL_NO);
if (err)
{
send_response_u16("rws:", 0xFFFD);
return 1;
}
send_response_ascii("rws:", SERIAL_NO, SERIAL_NO_LENGTH);
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);
}
return 1;
}
static int cmd_mpz(const uint8_t *data, uint8_t data_len)
{
if (data_len < PASSKEY_LENGTH)
{
send_response_u16("rpz:", 0xFFFF);
return 1;
}
if (m_passkey_changed != 0U)
{
/* 패스키가 변경된 경우 다시 바꿀 수 없음(FFFF 반환) */
send_response_u16("rpz:", 0xFFFF);
DBG_PRINTF("[CMD] mpz: passkey already changed\r\n");
return 1;
}
/* NVS 저장이 모두 성공한 뒤 RAM 값 확정 */
char new_passkey[PASSKEY_BUF_SIZE];
memset(new_passkey, 0, sizeof(new_passkey));
memcpy(new_passkey, data, PASSKEY_LENGTH);
int err = app_nvs_save_passkey(new_passkey);
if (err)
{
send_response_u16("rpz:", 0xFFFD);
return 1;
}
/* 변경 완료 플래그 세트 : 다음 mpz? 명령 차단 */
err = app_nvs_save_passkey_changed(1U);
if (err)
{
send_response_u16("rpz:", 0xFFFD);
return 1;
}
memset(m_static_passkey, 0, sizeof(m_static_passkey));
memcpy(m_static_passkey, new_passkey, PASSKEY_LENGTH);
m_passkey_changed = 1U;
send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH);
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);
return 1;
}
/*
* LED 상태 변경 명령
* mls?: 앱에서 전달한 LED 상태값을 적용하고 rls로 echo back
* - 파라미터 부족: 0xFFFF
* - 범위 초과: 0xFFFE
* - LED_STATE_OFF 진입 시 IMU FIFO 정지
*/
static int cmd_mls(const uint8_t *data, uint8_t data_len)
{
/* 파라미터 부족: 에러 코드 0xFFFF 에코 */
if (data_len < 2)
{
send_response_u16("rls:", 0xFFFF);
DBG_PRINTF("[CMD] mls: no data\r\n");
return 1;
}
/* state 필드: 2바이트 big-endian */
uint16_t state = ((uint16_t)data[0] << 8) | (uint16_t)data[1];
/* 범위 초과: 에러 코드 0xFFFE 에코 */
if (state > LED_STATE_ERROR)
{
send_response_u16("rls:", 0xFFFE);
DBG_PRINTF("[CMD] mls: invalid state %d\r\n", state);
return 1;
}
led_set_state((led_state_t)state);
/* 정렬 LED 상태와 보호 판정 정책 연결
* - ALIGN_SEARCHING/ALIGN_COMPLETE 동안 보호 판정 차단
* - 다른 LED 상태에서는 기존 조건에 따라 보호 판정 재개
*/
battery_protection_set_alignment_mode((state == LED_STATE_ALIGN_SEARCHING) ||
(state == LED_STATE_ALIGN_COMPLETE));
/* OFF 상태에서는 정렬/측정용 FIFO를 함께 정리 */
if (state == LED_STATE_OFF)
{
(void)imu_fifo_stop();
}
send_response_u16("rls:", state);
return 1;
}
/*==============================================================================
* 커맨드 테이블
*============================================================================*/
typedef struct
{
char tag[5];
int (*handler)(const uint8_t *data, uint8_t data_len);
} cmd_entry_t;
static const cmd_entry_t cmd_table[] =
{
{ "msq?", cmd_msq },
{ "mss?", cmd_mss },
{ "msr?", cmd_msr },
{ "mdf?", cmd_mdf },
{ "msn?", cmd_msn },
{ "mls?", cmd_mls },
{ "msp?", cmd_msp },
{ "mst?", cmd_mst },
{ "mpa?", cmd_mpa },
{ "mpb?", cmd_mpb },
{ "mpc?", cmd_mpc },
{ "mec?", cmd_mec },
{ "mad?", cmd_mad },
{ "maa?", cmd_maa },
{ "mbb?", cmd_mbb },
{ "mtb?", cmd_mtb },
{ "mcf?", cmd_mcf },
{ "mcs?", cmd_mcs },
{ "mid?", cmd_mid },
{ "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]))
/*==============================================================================
* 파서 엔트리
*============================================================================*/
int ble_cmd_dispatch(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);
char raw_tag[5] = { '?', '?', '?', '?', '\0' };
for (uint16_t i = 0; i < len && i < 4U; i++)
{
raw_tag[i] = (char)buf[i];
}
if (len < 7U)
{
DBG_ERR("[CMD] Too short (%u)\r\n", len);
if (send_response_tag_echo("rxs:", raw_tag) != 0)
{
DBG_ERR("[CMD] rxs tx failed\r\n");
}
return -1;
}
uint16_t calc_crc = dr_crc16_compute(buf, len - 2U);
uint16_t recv_crc = (uint16_t)(buf[len - 2U]) | ((uint16_t)(buf[len - 1U]) << 8);
if (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;
}
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'
};
const uint8_t *data = buf + 4;
uint8_t data_len = (uint8_t)(len - 4U - 2U);
for (int i = 0; i < CMD_TABLE_SIZE; i++)
{
if (memcmp(tag, cmd_table[i].tag, 4) == 0)
{
if (cmd_table[i].handler == NULL)
{
DBG_ERR("[CMD] Null handler: %s\r\n", cmd_table[i].tag);
if (send_response_tag_echo("rxn:", raw_tag) != 0)
{
DBG_ERR("[CMD] rxn tx failed\r\n");
}
return -1;
}
return cmd_table[i].handler(data, data_len);
}
}
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;
}
Reference in New Issue View Git Blame Copy Permalink