diff --git a/src/parser.c b/src/parser.c index cd3597e..1194aba 100644 --- a/src/parser.c +++ b/src/parser.c @@ -57,6 +57,8 @@ static uint8_t tx_echo_buf[4 + 2 + 2 + (ECHO_ADC_MAX_SAMPLES * 2) + 2]; static uint8_t tx_bundle_buf[22]; static uint8_t tx_cfg_buf[16]; static uint8_t tx_ascii_buf[4 + HW_NO_LENGTH + 2]; +static 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 piezo_config_t g_piezo_config = { .freq = PIEZO_CFG_FREQ_DEFAULT, @@ -332,6 +334,33 @@ static void send_response_bundle(uint16_t batt_mv, 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, @@ -1058,6 +1087,76 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len) 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 = start_piezo_session(); + } + + if (status == ECHO_STATUS_OK) + { + status = perform_piezo_sweep(); + } + + if (status == ECHO_STATUS_OK) + { + for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++) + { + send_response_echo(session, ch, piezo_channels[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 @@ -1296,6 +1395,7 @@ static const cmd_entry_t cmd_table[] = { "mad?", cmd_mad }, { "maa?", cmd_maa }, { "mbb?", cmd_mbb }, + { "mtb?", cmd_mtb }, { "mcf?", cmd_mcf }, { "mcs?", cmd_mcs }, { "mfv?", cmd_mfv },