From a01c0fb01b7619ac833f07c66ff19f38436dc3f8 Mon Sep 17 00:00:00 2001 From: jhchun Date: Tue, 30 Jun 2026 15:42:18 +0900 Subject: [PATCH] =?UTF-8?q?=EC=BB=A4=EB=A7=A8=EB=93=9C=20=EA=B5=AC?= =?UTF-8?q?=EC=A1=B0=20=EB=B6=84=EB=A6=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/command/cmd_common.c | 311 +++++++ src/command/cmd_common.h | 32 + src/command/cmd_table.c | 41 + src/command/cmd_table.h | 19 + src/command/handlers/cmd_device.c | 131 +++ src/command/handlers/cmd_device.h | 15 + src/command/handlers/cmd_info.c | 177 ++++ src/command/handlers/cmd_info.h | 19 + src/command/handlers/cmd_piezo.c | 500 +++++++++++ src/command/handlers/cmd_piezo.h | 21 + src/command/handlers/cmd_sensor.c | 123 +++ src/command/handlers/cmd_sensor.h | 15 + src/command/parser.c | 88 ++ src/{ => command}/parser.h | 0 src/parser.c | 1301 ----------------------------- 15 files changed, 1492 insertions(+), 1301 deletions(-) create mode 100644 src/command/cmd_common.c create mode 100644 src/command/cmd_common.h create mode 100644 src/command/cmd_table.c create mode 100644 src/command/cmd_table.h create mode 100644 src/command/handlers/cmd_device.c create mode 100644 src/command/handlers/cmd_device.h create mode 100644 src/command/handlers/cmd_info.c create mode 100644 src/command/handlers/cmd_info.h create mode 100644 src/command/handlers/cmd_piezo.c create mode 100644 src/command/handlers/cmd_piezo.h create mode 100644 src/command/handlers/cmd_sensor.c create mode 100644 src/command/handlers/cmd_sensor.h create mode 100644 src/command/parser.c rename src/{ => command}/parser.h (100%) delete mode 100644 src/parser.c diff --git a/src/command/cmd_common.c b/src/command/cmd_common.c new file mode 100644 index 0000000..317545c --- /dev/null +++ b/src/command/cmd_common.c @@ -0,0 +1,311 @@ +/******************************************************************************* + * @file cmd_common.c + * @brief BLE command common helpers + ******************************************************************************/ +#include +#include +#include + +#include "cmd_common.h" +#include "main.h" +#include "debug_print.h" +#include "ble_service.h" + +/*============================================================================== + * Command 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_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2]; +static uint8_t g_echo_session; + +uint8_t cmd_next_echo_session(void) +{ + return g_echo_session++; +} + +void cmd_reboot_after_response(void) +{ + k_msleep(100); + sys_reboot(SYS_REBOOT_COLD); +} + +/*============================================================================== + * CRC16 (CRC-CCITT, Nordic SDK 호환) + *============================================================================*/ +uint16_t cmd_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; +} + +bool cmd_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; +} + +void cmd_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); +} + +char cmd_ascii_to_lower(char ch) +{ + if ((ch >= 'A') && (ch <= 'Z')) + { + return (char)(ch - 'A' + 'a'); + } + + return ch; +} + +/*============================================================================== + * 응답 패킷 전송 + *============================================================================*/ + +/* TAG(4B) + uint16 값(2B) + CRC16(2B) = 8바이트 전송 */ +int cmd_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 = cmd_crc16_compute(buf, 6); + buf[6] = (uint8_t)(crc & 0xFF); + buf[7] = (uint8_t)(crc >> 8); + + return ble_data_send(buf, 8); +} + +int cmd_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 = cmd_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)); +} + +int cmd_send_response_identity(void) +{ + uint8_t *buf = tx_id_buf; + char fw_version[SERIAL_NO_LENGTH]; + + cmd_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 = cmd_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)); +} + +int cmd_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 = cmd_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) 방식과 동일한 레이아웃 */ +int cmd_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 = cmd_crc16_compute(buf, 16); + buf[16] = (uint8_t)(crc & 0xFF); + buf[17] = (uint8_t)(crc >> 8); + + return ble_data_send(buf, 18); +} + +void cmd_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 = cmd_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); +} + +void cmd_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 = cmd_crc16_compute(buf, 20); + buf[20] = (uint8_t)(crc & 0xFF); + buf[21] = (uint8_t)(crc >> 8); + + ble_data_send(buf, sizeof(tx_bundle_buf)); +} + +void cmd_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 = cmd_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)); +} + +void cmd_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 = cmd_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 */ diff --git a/src/command/cmd_common.h b/src/command/cmd_common.h new file mode 100644 index 0000000..8077868 --- /dev/null +++ b/src/command/cmd_common.h @@ -0,0 +1,32 @@ +/******************************************************************************* + * @file cmd_common.h + * @brief BLE command common helpers + ******************************************************************************/ +#ifndef CMD_COMMON_H__ +#define CMD_COMMON_H__ + +#include +#include +#include + +#include "imu_i2c.h" +#include "piezo_measure.h" + +void cmd_reboot_after_response(void); +uint16_t cmd_crc16_compute(const uint8_t *p_data, uint32_t size); +bool cmd_get_data_u16_be(const uint8_t *data, uint8_t data_len, uint8_t word_index, uint16_t *out); +void cmd_copy_fixed_ascii(char *dst, size_t dst_len, const char *src, size_t src_len); +char cmd_ascii_to_lower(char ch); +uint8_t cmd_next_echo_session(void); + +int cmd_send_response_u16(const char *tag, uint16_t value); +int cmd_send_response_ascii(const char *tag, const char *value, uint8_t value_len); +int cmd_send_response_identity(void); +int cmd_send_response_tag_echo(const char *tag, const char *echo_tag); +int cmd_send_response_imu(const int16_t accel[3], const int16_t gyro[3]); +void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples); +void cmd_send_response_bundle(uint16_t batt_mv, const int16_t accel[3], const int16_t gyro[3], int16_t temp_cdeg); +void cmd_send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count); +void cmd_send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cycles, uint16_t avg, uint16_t delay_us, uint16_t samples); + +#endif /* CMD_COMMON_H__ */ diff --git a/src/command/cmd_table.c b/src/command/cmd_table.c new file mode 100644 index 0000000..8974433 --- /dev/null +++ b/src/command/cmd_table.c @@ -0,0 +1,41 @@ +/******************************************************************************* + * @file cmd_table.c + * @brief BLE command table + ******************************************************************************/ +#include "cmd_table.h" +#include "handlers/cmd_device.h" +#include "handlers/cmd_info.h" +#include "handlers/cmd_piezo.h" +#include "handlers/cmd_sensor.h" + +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 }, +}; + +const int cmd_table_size = (int)(sizeof(cmd_table) / sizeof(cmd_table[0])); diff --git a/src/command/cmd_table.h b/src/command/cmd_table.h new file mode 100644 index 0000000..9786980 --- /dev/null +++ b/src/command/cmd_table.h @@ -0,0 +1,19 @@ +/******************************************************************************* + * @file cmd_table.h + * @brief BLE command table + ******************************************************************************/ +#ifndef CMD_TABLE_H__ +#define CMD_TABLE_H__ + +#include + +typedef struct +{ + char tag[5]; + int (*handler)(const uint8_t *data, uint8_t data_len); +} cmd_entry_t; + +extern const cmd_entry_t cmd_table[]; +extern const int cmd_table_size; + +#endif /* CMD_TABLE_H__ */ diff --git a/src/command/handlers/cmd_device.c b/src/command/handlers/cmd_device.c new file mode 100644 index 0000000..26b9d0c --- /dev/null +++ b/src/command/handlers/cmd_device.c @@ -0,0 +1,131 @@ +/******************************************************************************* + * @file cmd_device.c + * @brief BLE command handlers + ******************************************************************************/ +#include +#include +#if IS_ENABLED(CONFIG_BT_SMP) +#include +#include +#endif + +#include "cmd_common.h" +#include "main.h" +#include "debug_print.h" +#include "ble_service.h" +#include "cmd_device.h" + +int cmd_msq(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + int err = cmd_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; +} + +int cmd_mss(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + int err = cmd_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"); + cmd_reboot_after_response(); + return 1; +} + +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 = cmd_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"); + } + + cmd_reboot_after_response(); + return 1; +} + +/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */ +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 = cmd_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; +} + diff --git a/src/command/handlers/cmd_device.h b/src/command/handlers/cmd_device.h new file mode 100644 index 0000000..324357b --- /dev/null +++ b/src/command/handlers/cmd_device.h @@ -0,0 +1,15 @@ +/******************************************************************************* + * @file cmd_device.h + * @brief BLE command handlers + ******************************************************************************/ +#ifndef CMD_DEVICE_H__ +#define CMD_DEVICE_H__ + +#include + +int cmd_msq(const uint8_t *data, uint8_t data_len); +int cmd_mss(const uint8_t *data, uint8_t data_len); +int cmd_msr(const uint8_t *data, uint8_t data_len); +int cmd_mdf(const uint8_t *data, uint8_t data_len); + +#endif /* CMD_DEVICE_H__ */ diff --git a/src/command/handlers/cmd_info.c b/src/command/handlers/cmd_info.c new file mode 100644 index 0000000..02ea4ff --- /dev/null +++ b/src/command/handlers/cmd_info.c @@ -0,0 +1,177 @@ +/******************************************************************************* + * @file cmd_info.c + * @brief BLE command handlers + ******************************************************************************/ +#include +#include + +#include "cmd_common.h" +#include "main.h" +#include "debug_print.h" +#include "app_nvs.h" +#include "cmd_info.h" + +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); + + cmd_copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION)); + err = cmd_send_response_ascii("rfv:", fw_version, sizeof(fw_version)); + if (err) + { + DBG_ERR("[CMD] mfv tx failed err=%d\r\n", err); + } + return 1; +} + +int cmd_mid(const uint8_t *data, uint8_t data_len) +{ + int err; + + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + err = cmd_send_response_identity(); + if (err) + { + DBG_ERR("[CMD] mid tx failed err=%d\r\n", err); + } + return 1; +} + +int cmd_mwh(const uint8_t *data, uint8_t data_len) +{ + if (data_len < HW_NO_LENGTH) + { + cmd_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) + { + cmd_send_response_u16("rwh:", 0xFFFD); + return 1; + } + + cmd_send_response_ascii("rwh:", HW_NO, HW_NO_LENGTH); + return 1; +} + +int cmd_mrh(const uint8_t *data, uint8_t data_len) +{ + int err; + + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + err = cmd_send_response_ascii("rrh:", HW_NO, HW_NO_LENGTH); + if (err) + { + DBG_ERR("[CMD] mrh tx failed err=%d\r\n", err); + } + return 1; +} + +int cmd_mws(const uint8_t *data, uint8_t data_len) +{ + if (data_len < SERIAL_NO_LENGTH) + { + cmd_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) + { + cmd_send_response_u16("rws:", 0xFFFD); + return 1; + } + + cmd_send_response_ascii("rws:", SERIAL_NO, SERIAL_NO_LENGTH); + return 1; +} + +int cmd_mrs(const uint8_t *data, uint8_t data_len) +{ + int err; + + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + err = cmd_send_response_ascii("rrs:", SERIAL_NO, SERIAL_NO_LENGTH); + if (err) + { + DBG_ERR("[CMD] mrs tx failed err=%d\r\n", err); + } + return 1; +} + +int cmd_mpz(const uint8_t *data, uint8_t data_len) +{ + if (data_len < PASSKEY_LENGTH) + { + cmd_send_response_u16("rpz:", 0xFFFF); + return 1; + } + + if (m_passkey_changed != 0U) + { + /* 패스키가 변경된 경우 다시 바꿀 수 없음(FFFF 반환) */ + cmd_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) + { + cmd_send_response_u16("rpz:", 0xFFFD); + return 1; + } + + /* 변경 완료 플래그 세트 : 다음 mpz? 명령 차단 */ + err = app_nvs_save_passkey_changed(1U); + if (err) + { + cmd_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; + + cmd_send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH); + return 1; +} + +int cmd_mqz(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + cmd_send_response_ascii("rqz:", m_static_passkey, PASSKEY_LENGTH); + return 1; +} + +/* + * LED 상태 변경 명령 + * mls?: 앱에서 전달한 LED 상태값을 적용하고 rls로 echo back + * - 파라미터 부족: 0xFFFF + * - 범위 초과: 0xFFFE + * - LED_STATE_OFF 진입 시 IMU FIFO 정지 + */ diff --git a/src/command/handlers/cmd_info.h b/src/command/handlers/cmd_info.h new file mode 100644 index 0000000..ac0c3d1 --- /dev/null +++ b/src/command/handlers/cmd_info.h @@ -0,0 +1,19 @@ +/******************************************************************************* + * @file cmd_info.h + * @brief BLE command handlers + ******************************************************************************/ +#ifndef CMD_INFO_H__ +#define CMD_INFO_H__ + +#include + +int cmd_mfv(const uint8_t *data, uint8_t data_len); +int cmd_mid(const uint8_t *data, uint8_t data_len); +int cmd_mwh(const uint8_t *data, uint8_t data_len); +int cmd_mrh(const uint8_t *data, uint8_t data_len); +int cmd_mws(const uint8_t *data, uint8_t data_len); +int cmd_mrs(const uint8_t *data, uint8_t data_len); +int cmd_mpz(const uint8_t *data, uint8_t data_len); +int cmd_mqz(const uint8_t *data, uint8_t data_len); + +#endif /* CMD_INFO_H__ */ diff --git a/src/command/handlers/cmd_piezo.c b/src/command/handlers/cmd_piezo.c new file mode 100644 index 0000000..ff8e19a --- /dev/null +++ b/src/command/handlers/cmd_piezo.c @@ -0,0 +1,500 @@ +/******************************************************************************* + * @file cmd_piezo.c + * @brief BLE command handlers + ******************************************************************************/ +#include +#include + +#include "cmd_common.h" +#include "main.h" +#include "debug_print.h" +#include "battery_adc.h" +#include "imu_i2c.h" +#include "piezo.h" +#include "piezo_measure.h" +#include "cmd_piezo.h" + +static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES]; + +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"); + cmd_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"); + cmd_send_response_u16("rpa:", 1); + DBG_CORE("[MPA] response sent\r\n"); + return 1; +} + +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"); + cmd_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"); + cmd_send_response_u16("rpb:", 1); + DBG_CORE("[MPB] response sent\r\n"); + return 1; +} + +/* + * 테스트용 + * mpc?: burst만 한 번 발생시키는 테스트 명령 + * echo를 읽지 않고 초음파가 나가는지만 볼 때 사용 + */ +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; + + cmd_get_data_u16_be(data, data_len, 0, &cycles); + cmd_get_data_u16_be(data, data_len, 1, &freq_option); + cmd_get_data_u16_be(data, data_len, 2, &piezo_ch); + + ARG_UNUSED(freq_option); + + if ((cycles < 3U) || (cycles > 7U)) + { + cmd_send_response_u16("rpc:", 2); + return 1; + } + + power_button_suspend(true); + + if (piezo_init() != 0) + { + power_button_suspend(false); + cmd_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); + cmd_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); + + cmd_send_response_u16("rpc:", cycles); + return 1; +} + +/* + * 테스트용 + * mec?: 단일 채널 burst + echo capture + */ +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 = cmd_next_echo_session(); + + cmd_get_data_u16_be(data, data_len, 0, &freq_option); + cmd_get_data_u16_be(data, data_len, 1, &delay_us); + cmd_get_data_u16_be(data, data_len, 2, &num_samples); + cmd_get_data_u16_be(data, data_len, 3, &cycles); + cmd_get_data_u16_be(data, data_len, 4, &averaging); + cmd_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) + { + cmd_send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples); + } + + piezo_power_off(); + power_button_suspend(false); + cmd_send_response_u16("raa:", (uint16_t)status); + processing = false; + return 1; +} + +/* + * 테스트용 + * mad?: 단일 채널 echo capture만 수행 + * - piezo burst는 하지 않고 echo ADC만 수행 + * - echo 샘플을 모은 뒤 reb: 패킷 1개 전송 + * - raa: 상태값은 전체 작업 성공/실패 요약 + */ +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 = cmd_next_echo_session(); + + cmd_get_data_u16_be(data, data_len, 2, &num_samples); + cmd_get_data_u16_be(data, data_len, 4, &averaging); + cmd_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) + { + cmd_send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples); + } + + piezo_power_off(); + power_button_suspend(false); + cmd_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: 상태값은 전체 작업 성공/실패 요약 + */ +int cmd_maa(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + uint8_t session = cmd_next_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++) + { + cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples); + } + } + + piezo_power_off(); + power_button_suspend(false); + cmd_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 접근을 막는 기준으로 사용 + */ +int cmd_mbb(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + uint8_t session = cmd_next_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"); + cmd_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); + cmd_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"); + cmd_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 + */ +int cmd_mtb(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + uint8_t session = cmd_next_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++) + { + cmd_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; + } + } + + cmd_send_response_rim(tx_rim_samples, rim_count); + + piezo_power_off(); + power_button_suspend(false); + cmd_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 + */ +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(); + cmd_send_response_piezo_config("rcf:", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples); + return 1; +} + +/* + * mcs?: piezo 측정 파라미터 쓰기 + */ +int cmd_mcs(const uint8_t *data, uint8_t data_len) +{ + piezo_config_t cfg; + + if (data_len < 10U) + { + cmd_send_response_piezo_config("rcs:", 0xFFFF, 0U, 0U, 0U, 0U); + return 1; + } + + cmd_get_data_u16_be(data, data_len, 0, &cfg.freq); + cmd_get_data_u16_be(data, data_len, 1, &cfg.cycles); + cmd_get_data_u16_be(data, data_len, 2, &cfg.avg); + cmd_get_data_u16_be(data, data_len, 3, &cfg.delay_us); + cmd_get_data_u16_be(data, data_len, 4, &cfg.samples); + + if (!piezo_config_validate(&cfg)) + { + cmd_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) + { + cmd_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(); + cmd_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=범위 초과 */ diff --git a/src/command/handlers/cmd_piezo.h b/src/command/handlers/cmd_piezo.h new file mode 100644 index 0000000..a8a4ca4 --- /dev/null +++ b/src/command/handlers/cmd_piezo.h @@ -0,0 +1,21 @@ +/******************************************************************************* + * @file cmd_piezo.h + * @brief BLE command handlers + ******************************************************************************/ +#ifndef CMD_PIEZO_H__ +#define CMD_PIEZO_H__ + +#include + +int cmd_mpa(const uint8_t *data, uint8_t data_len); +int cmd_mpb(const uint8_t *data, uint8_t data_len); +int cmd_mpc(const uint8_t *data, uint8_t data_len); +int cmd_mec(const uint8_t *data, uint8_t data_len); +int cmd_mad(const uint8_t *data, uint8_t data_len); +int cmd_maa(const uint8_t *data, uint8_t data_len); +int cmd_mbb(const uint8_t *data, uint8_t data_len); +int cmd_mtb(const uint8_t *data, uint8_t data_len); +int cmd_mcf(const uint8_t *data, uint8_t data_len); +int cmd_mcs(const uint8_t *data, uint8_t data_len); + +#endif /* CMD_PIEZO_H__ */ diff --git a/src/command/handlers/cmd_sensor.c b/src/command/handlers/cmd_sensor.c new file mode 100644 index 0000000..b797ff2 --- /dev/null +++ b/src/command/handlers/cmd_sensor.c @@ -0,0 +1,123 @@ +/******************************************************************************* + * @file cmd_sensor.c + * @brief BLE command handlers + ******************************************************************************/ +#include +#include + +#include "cmd_common.h" +#include "main.h" +#include "debug_print.h" +#include "battery_adc.h" +#include "imu_i2c.h" +#include "led_control.h" +#include "cmd_sensor.h" + +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; + } + + cmd_send_response_u16("rsn:", (uint16_t)mv); + return 1; +} + +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) + { + cmd_send_response_u16("rsp:", 0xFFFF); + DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret); + return 1; + } + + cmd_send_response_imu(accel, gyro); + return 1; +} + +/* mst? -> IMU internal temperature -> rso: + temperature (degC x 100, BE) + * Error response: 0xFFFF = IMU temperature read failed. */ +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)) + { + cmd_send_response_u16("rso:", 0xFFFF); + DBG_PRINTF("[CMD] mst: imu temp read fail ret=%d\r\n", ret); + return 1; + } + + /* 음수 온도도 2's complement로 그대로 전송 (앱이 int16로 해석) */ + cmd_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 전원 레일만 켬 + */ +int cmd_mls(const uint8_t *data, uint8_t data_len) +{ + /* 파라미터 부족: 에러 코드 0xFFFF 에코 */ + if (data_len < 2) + { + cmd_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) + { + cmd_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(); + } + + cmd_send_response_u16("rls:", state); + return 1; +} + +/*============================================================================== + * 커맨드 테이블 + *============================================================================*/ diff --git a/src/command/handlers/cmd_sensor.h b/src/command/handlers/cmd_sensor.h new file mode 100644 index 0000000..5b7f0d8 --- /dev/null +++ b/src/command/handlers/cmd_sensor.h @@ -0,0 +1,15 @@ +/******************************************************************************* + * @file cmd_sensor.h + * @brief BLE command handlers + ******************************************************************************/ +#ifndef CMD_SENSOR_H__ +#define CMD_SENSOR_H__ + +#include + +int cmd_msn(const uint8_t *data, uint8_t data_len); +int cmd_msp(const uint8_t *data, uint8_t data_len); +int cmd_mst(const uint8_t *data, uint8_t data_len); +int cmd_mls(const uint8_t *data, uint8_t data_len); + +#endif /* CMD_SENSOR_H__ */ diff --git a/src/command/parser.c b/src/command/parser.c new file mode 100644 index 0000000..b89e991 --- /dev/null +++ b/src/command/parser.c @@ -0,0 +1,88 @@ +/******************************************************************************* + * @file parser.c + * @brief BLE command parser + * + * Packet format : [TAG 4B] [DATA NB] [CRC16 2B] + ******************************************************************************/ +#include +#include + +#include "parser.h" +#include "cmd_common.h" +#include "cmd_table.h" +#include "debug_print.h" + +/*============================================================================== + * 파서 엔트리 + *============================================================================*/ +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 (cmd_send_response_tag_echo("rxs:", raw_tag) != 0) + { + DBG_ERR("[CMD] rxs tx failed\r\n"); + } + return -1; + } + + uint16_t calc_crc = cmd_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 (cmd_send_response_tag_echo("rxc:", raw_tag) != 0) + { + DBG_ERR("[CMD] rxc tx failed\r\n"); + } + return -1; + } + + char tag[5] = + { + cmd_ascii_to_lower((char)buf[0]), + cmd_ascii_to_lower((char)buf[1]), + cmd_ascii_to_lower((char)buf[2]), + cmd_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 (cmd_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 (cmd_send_response_tag_echo("rxx:", raw_tag) != 0) + { + DBG_ERR("[CMD] rxx tx failed\r\n"); + } + return 0; +} diff --git a/src/parser.h b/src/command/parser.h similarity index 100% rename from src/parser.h rename to src/command/parser.h diff --git a/src/parser.c b/src/parser.c deleted file mode 100644 index e70464e..0000000 --- a/src/parser.c +++ /dev/null @@ -1,1301 +0,0 @@ -/******************************************************************************* - * @file parser.c - * @brief BLE command parser - * - * Packet format : [TAG 4B] [DATA NB] [CRC16 2B] - ******************************************************************************/ -#include -#include -#include -#if IS_ENABLED(CONFIG_BT_SMP) -#include -#include -#endif -#include -#include - -#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; -}