diff --git a/src/command/handlers/cmd_piezo.c b/src/command/handlers/cmd_piezo.c index ff72876..9b08c2b 100644 --- a/src/command/handlers/cmd_piezo.c +++ b/src/command/handlers/cmd_piezo.c @@ -236,50 +236,6 @@ int cmd_mcs(const uint8_t *data, uint8_t data_len) return 1; } -// =============================== Test Command =============================== -/* TEST -* mim? : IMU FIFO 15 samples -*/ -int cmd_mim(const uint8_t *data, uint8_t data_len) -{ - ARG_UNUSED(data); - ARG_UNUSED(data_len); - - if (processing) - { - return 1; - } - - processing = true; - - uint16_t rim_count = 0U; - int imu_ret = imu_fifo_start(); - if (imu_ret != 0) - { - DBG_PRINTF("[MIM] fifo start fail ret=%d\r\n", imu_ret); - cmd_send_response_rim_error(); - processing = false; - return 1; - } - - k_msleep(320); // IMU FIFO is 50Hz, 15 samples need about 300ms - - imu_ret = imu_fifo_read_latest(tx_rim_samples, IMU_FIFO_RIM_TARGET_SAMPLES, &rim_count); - if (imu_ret != 0) - { - DBG_PRINTF("[MIM] fifo read fail ret=%d\r\n", imu_ret); - cmd_send_response_rim_error(); - imu_fifo_stop(); - processing = false; - return 1; - } - - cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 15 samples rim: 전송 - imu_fifo_stop(); - processing = false; - return 1; -} - /* * TEST (이전 정렬모드) * maa?: piezo echo sweep diff --git a/src/command/handlers/cmd_piezo.h b/src/command/handlers/cmd_piezo.h index 86bf480..a8a4ca4 100644 --- a/src/command/handlers/cmd_piezo.h +++ b/src/command/handlers/cmd_piezo.h @@ -17,6 +17,5 @@ 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); -int cmd_mim(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 index e658fe6..8a3bd75 100644 --- a/src/command/handlers/cmd_sensor.c +++ b/src/command/handlers/cmd_sensor.c @@ -13,6 +13,8 @@ #include "led_control.h" #include "cmd_sensor.h" +static uint8_t tx_mim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES]; + /* * msn: battery voltage read */ @@ -58,6 +60,49 @@ int cmd_msp(const uint8_t *data, uint8_t data_len) return 1; } +/* + * mim?: IMU FIFO 15 samples + */ +int cmd_mim(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + if (processing) + { + return 1; + } + + processing = true; + + uint16_t rim_count = 0U; + int imu_ret = imu_fifo_start(); + if (imu_ret != 0) + { + DBG_PRINTF("[MIM] fifo start fail ret=%d\r\n", imu_ret); + cmd_send_response_rim_error(); + processing = false; + return 1; + } + + k_msleep(320); // IMU FIFO is 50Hz, 15 samples need about 300ms + + imu_ret = imu_fifo_read_latest(tx_mim_samples, IMU_FIFO_RIM_TARGET_SAMPLES, &rim_count); + if (imu_ret != 0) + { + DBG_PRINTF("[MIM] fifo read fail ret=%d\r\n", imu_ret); + cmd_send_response_rim_error(); + imu_fifo_stop(); + processing = false; + return 1; + } + + cmd_send_response_rim(tx_mim_samples, rim_count); // IMU FIFO 15 samples rim: 전송 + imu_fifo_stop(); + processing = false; + return 1; +} + /* * mst: IMU temperature direct read */ diff --git a/src/command/handlers/cmd_sensor.h b/src/command/handlers/cmd_sensor.h index 5b7f0d8..906c15f 100644 --- a/src/command/handlers/cmd_sensor.h +++ b/src/command/handlers/cmd_sensor.h @@ -9,6 +9,7 @@ int cmd_msn(const uint8_t *data, uint8_t data_len); int cmd_msp(const uint8_t *data, uint8_t data_len); +int cmd_mim(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);