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

명령 오류 응답(rxs:, rxn:) 추가

Browse Source
This commit is contained in:
jhchun
2026-06-25 17:45:09 +09:00
parent dba33baf1c
commit 42f745e39b
1 changed files with 40 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/parser.c
+40 -31
View File
@@ -1,11 +1,8 @@
/*******************************************************************************
* @file parser.c
* @brief BLE command parser (Zephyr port)
* @brief BLE command parser
*
* 원본: pc_firm/parser.c
* 패킷 포맷: [TAG 4B] [DATA NB] [CRC16 2B]
*
* 현재 구현된 커맨드: msn? (배터리 측정), mls? (LED 상태 설정)
* Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
@@ -41,13 +38,14 @@
#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_CFG_AVG_MAX 20
#define PIEZO_CFG_DELAY_MAX_US 100
#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 3
#define PIEZO_CFG_AVG_MAX 10
#define PIEZO_CFG_DELAY_MAX_US 50
#define PIEZO_POST_SELECT_SETTLE_US 500
#define PIEZO_AVG_INTER_BURST_GAP_US 500
#define PIEZO_DUMMY_CAPTURE_COUNT 5
@@ -1580,33 +1578,36 @@ 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)
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;
}
char raw_tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };
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);
/* CRC16 검증 (6바이트 이상이면 마지막 2바이트가 CRC) */
if (len >= 6)
if (calc_crc != recv_crc)
{
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_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] 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;
DBG_ERR("[CMD] rxc tx failed\r\n");
}
return -1;
}
/* TAG 추출 (4바이트) */
char tag[5] =
{
ascii_to_lower((char)buf[0]),
@@ -1617,15 +1618,23 @@ int dr_parser(const uint8_t *buf, uint16_t len)
};
DBG_CORE("[CMD] tag=%s\r\n", tag);
/* 데이터 부분 (TAG 이후, CRC 이전) */
const uint8_t *data = buf + 4;
uint8_t data_len = (len >= 6) ? (len - 4 - 2) : (len - 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;
}
DBG_CORE("[CMD] dispatch -> %s\r\n", cmd_table[i].tag);
return cmd_table[i].handler(data, data_len);
}