diff --git a/src/command/cmd_common.c b/src/command/cmd_common.c index 117cf37..b8a1cc7 100644 --- a/src/command/cmd_common.c +++ b/src/command/cmd_common.c @@ -5,6 +5,7 @@ #include #include #include +#include #include "cmd_common.h" #include "main.h" @@ -288,4 +289,107 @@ void cmd_send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cyc buf[15] = (uint8_t)(crc >> 8); ble_data_send(buf, sizeof(tx_cfg_buf)); -} \ No newline at end of file +} + +static void cmd_put_u16_be(uint8_t *dst, uint16_t value) +{ + dst[0] = (uint8_t)(value >> 8); + dst[1] = (uint8_t)(value & 0xFF); +} + +static void cmd_put_u32_be(uint8_t *dst, uint32_t value) +{ + dst[0] = (uint8_t)(value >> 24); + dst[1] = (uint8_t)(value >> 16); + dst[2] = (uint8_t)(value >> 8); + dst[3] = (uint8_t)(value & 0xFF); +} + +static void cmd_put_crc(uint8_t *buf, uint16_t payload_len) +{ + uint16_t crc = cmd_crc16_compute(buf, payload_len); + buf[payload_len] = (uint8_t)(crc & 0xFF); + buf[payload_len + 1U] = (uint8_t)(crc >> 8); +} + +int cmd_send_response_rfq(uint32_t tick_start) +{ + static uint8_t buf[10]; + + buf[0] = 'r'; buf[1] = 'f'; buf[2] = 'q'; buf[3] = ':'; + cmd_put_u32_be(&buf[4], tick_start); + cmd_put_crc(buf, 8U); + + return ble_data_send(buf, sizeof(buf)); +} + +int cmd_send_response_rfe(uint16_t status, uint8_t session, uint32_t tick_end, uint16_t total_frames) +{ + static uint8_t buf[15]; + + buf[0] = 'r'; buf[1] = 'f'; buf[2] = 'e'; buf[3] = ':'; + cmd_put_u16_be(&buf[4], status); + buf[6] = session; + cmd_put_u32_be(&buf[7], tick_end); + cmd_put_u16_be(&buf[11], total_frames); + cmd_put_crc(buf, 13U); + + return ble_data_send(buf, sizeof(buf)); +} + +int cmd_send_response_rqh(const measure_queue_frame_header_t *header) +{ + static uint8_t buf[14]; + + if (header == NULL) + { + return -EINVAL; + } + + buf[0] = 'r'; buf[1] = 'q'; buf[2] = 'h'; buf[3] = ':'; + buf[4] = header->session; + cmd_put_u16_be(&buf[5], header->frame_idx); + cmd_put_u32_be(&buf[7], header->tick_ms); + buf[11] = header->ch_mask; + cmd_put_crc(buf, 12U); + + return ble_data_send(buf, sizeof(buf)); +} + +int cmd_send_response_rqb(uint16_t frame_idx, uint8_t channel, const uint16_t *samples, uint8_t num_samples) +{ + static uint8_t buf[4 + 2 + 1 + 1 + (PIEZO_MEASURE_MAX_SAMPLES * 2) + 2]; + uint16_t payload_len; + + if ((samples == NULL) || (num_samples > PIEZO_MEASURE_MAX_SAMPLES)) + { + return -EINVAL; + } + + buf[0] = 'r'; buf[1] = 'q'; buf[2] = 'b'; buf[3] = ':'; + cmd_put_u16_be(&buf[4], frame_idx); + buf[6] = channel; + buf[7] = num_samples; + + for (uint8_t i = 0; i < num_samples; i++) + { + cmd_put_u16_be(&buf[8U + (uint16_t)i * 2U], samples[i]); + } + + payload_len = (uint16_t)(8U + ((uint16_t)num_samples * 2U)); + cmd_put_crc(buf, payload_len); + + return ble_data_send(buf, (uint16_t)(payload_len + 2U)); +} + +int cmd_send_response_rqd(uint16_t status, uint16_t sent_frames) +{ + static uint8_t buf[10]; + + buf[0] = 'r'; buf[1] = 'q'; buf[2] = 'd'; buf[3] = ':'; + cmd_put_u16_be(&buf[4], status); + cmd_put_u16_be(&buf[6], sent_frames); + cmd_put_crc(buf, 8U); + + return ble_data_send(buf, sizeof(buf)); +} diff --git a/src/command/cmd_common.h b/src/command/cmd_common.h index 8077868..5ffee9f 100644 --- a/src/command/cmd_common.h +++ b/src/command/cmd_common.h @@ -11,6 +11,7 @@ #include "imu_i2c.h" #include "piezo_measure.h" +#include "measure_queue.h" void cmd_reboot_after_response(void); uint16_t cmd_crc16_compute(const uint8_t *p_data, uint32_t size); @@ -28,5 +29,10 @@ void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *sa 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); +int cmd_send_response_rfq(uint32_t tick_start); +int cmd_send_response_rfe(uint16_t status, uint8_t session, uint32_t tick_end, uint16_t total_frames); +int cmd_send_response_rqh(const measure_queue_frame_header_t *header); +int cmd_send_response_rqb(uint16_t frame_idx, uint8_t channel, const uint16_t *samples, uint8_t num_samples); +int cmd_send_response_rqd(uint16_t status, uint16_t sent_frames); #endif /* CMD_COMMON_H__ */ diff --git a/src/command/cmd_table.c b/src/command/cmd_table.c index 4548bee..088a9a5 100644 --- a/src/command/cmd_table.c +++ b/src/command/cmd_table.c @@ -28,6 +28,9 @@ const cmd_entry_t cmd_table[] = { "mtb?", cmd_mtb }, { "mcf?", cmd_mcf }, { "mcs?", cmd_mcs }, + { "mfq?", cmd_mfq }, + { "mfe?", cmd_mfe }, + { "mqb?", cmd_mqb }, { "mid?", cmd_mid }, { "mfv?", cmd_mfv }, { "mwh?", cmd_mwh }, diff --git a/src/command/handlers/cmd_piezo.c b/src/command/handlers/cmd_piezo.c index a90ce48..9b5c9a3 100644 --- a/src/command/handlers/cmd_piezo.c +++ b/src/command/handlers/cmd_piezo.c @@ -4,6 +4,7 @@ ******************************************************************************/ #include #include +#include #include "cmd_common.h" #include "main.h" @@ -12,6 +13,7 @@ #include "imu_i2c.h" #include "piezo.h" #include "piezo_measure.h" +#include "measure_queue.h" #include "cmd_piezo.h" static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES]; @@ -212,38 +214,138 @@ 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) + +static uint16_t cmd_queue_status_to_u16(int status) +{ + if (status == 0) + { + return ECHO_STATUS_OK; + } + + if (status > 0) + { + return (uint16_t)status; + } + + return (uint16_t)(0x8000U | ((uint16_t)(-status) & 0x7FFFU)); +} + +static uint16_t queue_tx_samples[PIEZO_NUM_CHANNELS][PIEZO_MEASURE_MAX_SAMPLES]; + +/* + * mfq?: start flash queue recording. + * Response: rfq: + tick_start. + */ +int cmd_mfq(const uint8_t *data, uint8_t data_len) { ARG_UNUSED(data); ARG_UNUSED(data_len); - uint16_t rim_count = 0U; - int imu_ret = imu_fifo_start(); - if (imu_ret != 0) + uint32_t tick_start = 0U; + DBG_PRINTF("[MFQ] start\r\n"); + int err = measure_queue_start(&tick_start); + if (err) { - DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret); - cmd_send_response_rim(tx_rim_samples, 0U); - 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); - rim_count = 0U; + DBG_ERR("[MFQ] start failed err=%d\r\n", err); } - cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 15 samples rim: 전송 - imu_fifo_stop(); + DBG_PRINTF("[MFQ] response rfq tick_start=%u err=%d\r\n", tick_start, err); + cmd_send_response_rfq(tick_start); return 1; } +/* + * mfe?: stop flash queue recording. + * Response: rfe: + status/session/tick_end/total_frames. + */ +int cmd_mfe(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + uint32_t tick_end = 0U; + uint8_t session = 0U; + DBG_PRINTF("[MFE] stop\r\n"); + uint16_t total_frames = 0U; + int status = measure_queue_stop(&tick_end, &session, &total_frames); + + DBG_PRINTF("[MFE] response rfe status=0x%04X session=%u tick_end=%u frames=%u\r\n", + cmd_queue_status_to_u16(status), session, tick_end, total_frames); + cmd_send_response_rfe(cmd_queue_status_to_u16(status), session, tick_end, total_frames); + return 1; +} + +/* + * mqb?: dump queued frames. + * Response sequence: (rqh + rqb * active channels) * frames, then rqd once. + */ +int cmd_mqb(const uint8_t *data, uint8_t data_len) +{ + ARG_UNUSED(data); + ARG_UNUSED(data_len); + + uint16_t total_frames = measure_queue_total_frames(); + uint16_t num_samples = measure_queue_num_samples(); + DBG_PRINTF("[MQB] cmd dump total_frames=%u samples=%u\r\n", total_frames, num_samples); + uint16_t sent_frames = 0U; + uint16_t status = ECHO_STATUS_OK; + + if (measure_queue_is_recording()) + { + DBG_PRINTF("[MQB] reject: recording busy\r\n"); + cmd_send_response_rqd(cmd_queue_status_to_u16(-EBUSY), 0U); + return 1; + } + + for (uint16_t frame_idx = 0; frame_idx < total_frames; frame_idx++) + { + measure_queue_frame_header_t header; + int err = measure_queue_read_frame(frame_idx, &header, queue_tx_samples); + if (err) + { + DBG_ERR("[MQB] read frame failed frame=%u err=%d\r\n", frame_idx, err); + status = cmd_queue_status_to_u16(err); + break; + } + + err = cmd_send_response_rqh(&header); + if (err) + { + DBG_ERR("[MQB] send rqh failed frame=%u err=%d\r\n", frame_idx, err); + status = cmd_queue_status_to_u16(err); + break; + } + + for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++) + { + if ((header.ch_mask & BIT(ch)) == 0U) + { + continue; + } + + err = cmd_send_response_rqb(frame_idx, ch, queue_tx_samples[ch], (uint8_t)num_samples); + if (err) + { + DBG_ERR("[MQB] send rqb failed frame=%u ch=%u err=%d\r\n", frame_idx, ch, err); + status = cmd_queue_status_to_u16(err); + break; + } + } + + if (status != ECHO_STATUS_OK) + { + break; + } + + sent_frames++; + } + + DBG_PRINTF("[MQB] response rqd status=0x%04X sent_frames=%u\r\n", status, sent_frames); + cmd_send_response_rqd(status, sent_frames); + return 1; +} + +// =============================== Test Command =============================== /* * TEST (이전 정렬모드) * maa?: piezo echo sweep diff --git a/src/command/handlers/cmd_piezo.h b/src/command/handlers/cmd_piezo.h index 86bf480..6a484ec 100644 --- a/src/command/handlers/cmd_piezo.h +++ b/src/command/handlers/cmd_piezo.h @@ -17,6 +17,9 @@ 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_mfq(const uint8_t *data, uint8_t data_len); +int cmd_mfe(const uint8_t *data, uint8_t data_len); +int cmd_mqb(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 cf49635..950be77 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_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES]; + /* * msn: battery voltage read */ @@ -31,7 +33,7 @@ int cmd_msn(const uint8_t *data, uint8_t data_len) return 1; } -/* +/* 삭제 예정(mim?으로 대체) * msp: IMU data direct read */ int cmd_msp(const uint8_t *data, uint8_t data_len) @@ -53,6 +55,37 @@ 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); + + uint16_t rim_count = 0U; + int imu_ret = imu_fifo_start(); + if (imu_ret != 0) + { + DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret); + cmd_send_response_rim(tx_rim_samples, 0U); + 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); + rim_count = 0U; + } + + cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 15 samples rim: 전송 + imu_fifo_stop(); + return 1; +} + /* * mst: IMU temperature direct read */ diff --git a/src/measurement/measure_queue.c b/src/measurement/measure_queue.c new file mode 100644 index 0000000..a6e0f3c --- /dev/null +++ b/src/measurement/measure_queue.c @@ -0,0 +1,449 @@ +/******************************************************************************* + * @file measure_queue.c + * @brief Flash-backed queued piezo measurement frames + ******************************************************************************/ +#include +#include +#include +#include + +#include "measure_queue.h" +#include "debug_print.h" +#include "main.h" +#include "led_control.h" + +#define MEASURE_QUEUE_PERIOD_MS 100U +#define MEASURE_QUEUE_THREAD_STACK_SIZE 2048 +#define MEASURE_QUEUE_THREAD_PRIORITY 7 + +/* data_storage flash partition handle, opened once and reused */ +static const struct flash_area *queue_area; + +/* Recording runs in its own thread, separate from BLE command handling */ +static struct k_thread queue_thread; +static K_THREAD_STACK_DEFINE(queue_thread_stack, MEASURE_QUEUE_THREAD_STACK_SIZE); + +/* Lock for shared queue state */ +static struct k_mutex queue_lock; +static bool queue_lock_ready; + +/* Current recording state */ +static bool queue_recording; // true while the worker keeps storing frames +static bool queue_thread_running; // true while the worker thread is alive +static uint8_t queue_session; // measurement session id, incremented on each mfq? +static uint16_t queue_total_frames; // number of frames written to flash +static uint16_t queue_num_samples; // samples value latched at recording start +static uint32_t queue_tick_start; // actual recording start tick +static uint32_t queue_tick_end; // actual recording end tick +static int queue_status; // last stop status: 0=OK, negative=errno + +/* Work buffers sized for one max-size frame */ +static uint8_t frame_write_buf[4 + (PIEZO_NUM_CHANNELS * PIEZO_MEASURE_MAX_SAMPLES * 2)]; +static uint8_t frame_read_buf[4 + (PIEZO_NUM_CHANNELS * PIEZO_MEASURE_MAX_SAMPLES * 2)]; + +/* Lazy init for the mutex on first use */ +static void queue_init_lock_once(void) +{ + if (!queue_lock_ready) + { + k_mutex_init(&queue_lock); + queue_lock_ready = true; + } +} + +/* + * Byte size of one frame in flash + * Layout: tick_ms 4B + 6 channels * samples * uint16 2B + */ +static uint32_t queue_frame_size(uint16_t num_samples) +{ + return 4U + (uint32_t)PIEZO_NUM_CHANNELS * (uint32_t)num_samples * 2U; +} + +/* Max frame count that fits in the current data_storage area */ +static uint16_t queue_capacity_frames(uint16_t num_samples) +{ + if (queue_area == NULL) + { + return 0U; + } + + return (uint16_t)(queue_area->fa_size / queue_frame_size(num_samples)); +} + +/* Flash format uses big endian for tick and samples */ +static void put_u16_be(uint8_t *dst, uint16_t value) +{ + dst[0] = (uint8_t)(value >> 8); + dst[1] = (uint8_t)(value & 0xFF); +} + +static void put_u32_be(uint8_t *dst, uint32_t value) +{ + dst[0] = (uint8_t)(value >> 24); + dst[1] = (uint8_t)(value >> 16); + dst[2] = (uint8_t)(value >> 8); + dst[3] = (uint8_t)(value & 0xFF); +} + +static uint16_t get_u16_be(const uint8_t *src) +{ + return ((uint16_t)src[0] << 8) | (uint16_t)src[1]; +} + +static uint32_t get_u32_be(const uint8_t *src) +{ + return ((uint32_t)src[0] << 24) | ((uint32_t)src[1] << 16) | ((uint32_t)src[2] << 8) | (uint32_t)src[3]; +} + +/* + * Store the current piezo sweep result as one frame + * Order: tick_ms -> CH0 samples -> CH1 samples -> ... -> CH5 samples + */ +static int queue_write_current_frame(uint16_t frame_idx, uint32_t tick_ms) +{ + uint16_t num_samples = queue_num_samples; + uint32_t frame_size = queue_frame_size(num_samples); + uint8_t *p = frame_write_buf; + + put_u32_be(p, tick_ms); + p += 4; + + for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++) + { + const uint16_t *samples = piezo_measure_channel_buffer(ch); + if (samples == NULL) + { + return -EINVAL; + } + + for (uint16_t i = 0; i < num_samples; i++) + { + put_u16_be(p, samples[i]); + p += 2; + } + } + + return flash_area_write(queue_area, (off_t)((uint32_t)frame_idx * frame_size), frame_write_buf, frame_size); +} + +/* + * Worker thread started by mfq? + * Keeps 100 ms recording separate from BLE command handling + */ +static void queue_record_thread(void *a, void *b, void *c) +{ + ARG_UNUSED(a); + ARG_UNUSED(b); + ARG_UNUSED(c); + + int status = piezo_measure_start_session(); + + while (status == ECHO_STATUS_OK) + { + k_mutex_lock(&queue_lock, K_FOREVER); + bool should_record = queue_recording; + uint16_t frame_idx = queue_total_frames; + uint16_t capacity = queue_capacity_frames(queue_num_samples); + k_mutex_unlock(&queue_lock); + + if (!should_record) + { + break; + } + + if (frame_idx >= capacity) + { + // 저장 공간 full인 경우 측정 및 저장 자동 종료 + status = -ENOSPC; + break; + } + + /* Frame timestamp shared by all six channels */ + uint32_t tick = k_uptime_get_32(); + status = piezo_measure_sweep_once(); + if (status != ECHO_STATUS_OK) + { + break; + } + + int err = queue_write_current_frame(frame_idx, tick); + if (err) + { + status = err; + break; + } + + k_mutex_lock(&queue_lock, K_FOREVER); + queue_total_frames++; + k_mutex_unlock(&queue_lock); + + /* Keep roughly a 100 ms period if sweep/write finishes early */ + uint32_t elapsed = k_uptime_get_32() - tick; + if (elapsed < MEASURE_QUEUE_PERIOD_MS) + { + k_msleep(MEASURE_QUEUE_PERIOD_MS - elapsed); + } + } + + /* Common cleanup for manual stop, full storage, and errors */ + piezo_power_off(); + power_button_suspend(false); + + led_set_state(LED_STATE_OFF); // storage stopped + + k_mutex_lock(&queue_lock, K_FOREVER); + queue_tick_end = k_uptime_get_32(); + queue_status = status; + queue_recording = false; + queue_thread_running = false; + processing = false; + k_mutex_unlock(&queue_lock); + + DBG_PRINTF("[MFQ] record thread stop status=%d frames=%u\r\n", status, queue_total_frames); +} + +/* + * Start flash-backed recording + * Flow: duplicate check -> open data_storage -> erase -> init state -> start worker + * rfq: is sent by cmd_mfq() after this function returns + */ +int measure_queue_start(uint32_t *tick_start) +{ + int err; + + queue_init_lock_once(); + + k_mutex_lock(&queue_lock, K_FOREVER); + if (queue_recording || queue_thread_running) + { + if (tick_start != NULL) + { + *tick_start = queue_tick_start; + } + k_mutex_unlock(&queue_lock); + return -EALREADY; + } + k_mutex_unlock(&queue_lock); + + if (queue_area == NULL) + { + // Zephyr flash_map에서 data_storage 파티션 열기 + err = flash_area_open(FIXED_PARTITION_ID(data_storage), &queue_area); + if (err) + { + DBG_ERR("[MFQ] data_storage open failed err=%d\r\n", err); + return err; + } + } + + const piezo_config_t *cfg = piezo_config_get(); + uint32_t frame_size = queue_frame_size(cfg->samples); + uint16_t capacity = queue_capacity_frames(cfg->samples); + uint32_t usable_bytes = (uint32_t)capacity * frame_size; + uint32_t erase_start = k_uptime_get_32(); + + DBG_PRINTF("[MFQ] data record prepare area_off=0x%08x area_size=%uB samples=%u frame_size=%uB capacity=%u frames usable=%uB\r\n", + (uint32_t)queue_area->fa_off, + (uint32_t)queue_area->fa_size, + cfg->samples, + frame_size, + capacity, + usable_bytes); + DBG_PRINTF("[MFQ] data_storage erase start size=%uB\r\n", (uint32_t)queue_area->fa_size); + + led_set_state(LED_STATE_DATA_ERASING); // erasing: green LED blink + + // Clear previous recording before starting a new one + err = flash_area_erase(queue_area, 0, queue_area->fa_size); + uint32_t erase_ms = k_uptime_get_32() - erase_start; + if (err) + { + DBG_ERR("[MFQ] data_storage erase failed err=%d elapsed=%ums\r\n", err, erase_ms); + return err; + } + + DBG_PRINTF("[MFQ] data_storage erase done elapsed=%ums\r\n", erase_ms); + + led_set_state(LED_STATE_DATA_STORAGING); // storaging: green LED on + + k_mutex_lock(&queue_lock, K_FOREVER); + queue_session++; // session id + queue_total_frames = 0U; // 새로운 측정 시 프레임 카운트 리셋 + queue_num_samples = cfg->samples; // latch samples at recording start + queue_tick_start = k_uptime_get_32(); + queue_tick_end = queue_tick_start; + queue_status = ECHO_STATUS_OK; + queue_recording = true; + queue_thread_running = true; + processing = true; + power_button_suspend(true); + if (tick_start != NULL) + { + *tick_start = queue_tick_start; + } + k_mutex_unlock(&queue_lock); + + k_thread_create(&queue_thread, queue_thread_stack, K_THREAD_STACK_SIZEOF(queue_thread_stack), queue_record_thread, NULL, NULL, NULL, MEASURE_QUEUE_THREAD_PRIORITY, 0, K_NO_WAIT); + + DBG_PRINTF("[MFQ] data record start session=%u tick_start=%u samples=%u frame_size=%uB capacity=%u frames\r\n", + queue_session, + queue_tick_start, + queue_num_samples, + queue_frame_size(queue_num_samples), + queue_capacity_frames(queue_num_samples)); + return 0; +} + +/* 측정 종료 및 저장 완료 후 상태 확인 */ +int measure_queue_stop(uint32_t *tick_end, uint8_t *session, uint16_t *total_frames) +{ + queue_init_lock_once(); + + k_mutex_lock(&queue_lock, K_FOREVER); + bool was_running = queue_thread_running; + queue_recording = false; + k_mutex_unlock(&queue_lock); + + if (was_running) + { + // rfe: 응답 시 측정 중인 경우 측정 완료될 때까지 기다림 + (void)k_thread_join(&queue_thread, K_FOREVER); + } + + k_mutex_lock(&queue_lock, K_FOREVER); + if (tick_end != NULL) + { + *tick_end = queue_tick_end; + } + if (session != NULL) + { + *session = queue_session; + } + if (total_frames != NULL) + { + *total_frames = queue_total_frames; + } + int status = queue_status; + uint8_t stop_session = queue_session; + uint16_t stop_frames = queue_total_frames; + uint16_t num_samples = queue_num_samples; + uint32_t frame_size = queue_frame_size(num_samples); + uint16_t capacity = queue_capacity_frames(num_samples); + uint32_t usable_bytes = (uint32_t)capacity * frame_size; + uint32_t used_bytes = (uint32_t)stop_frames * frame_size; + uint32_t remain_bytes = (used_bytes < usable_bytes) ? (usable_bytes - used_bytes) : 0U; + k_mutex_unlock(&queue_lock); + + DBG_PRINTF("[MFE] data record stop status=%d session=%u frames=%u/%u samples=%u frame_size=%uB used=%uB usable=%uB remain=%uB\r\n", + status, + stop_session, + stop_frames, + capacity, + num_samples, + frame_size, + used_bytes, + usable_bytes, + remain_bytes); + + return status; +} + +bool measure_queue_is_recording(void) +{ + queue_init_lock_once(); + k_mutex_lock(&queue_lock, K_FOREVER); + bool recording = queue_recording; + k_mutex_unlock(&queue_lock); + return recording; +} + +uint8_t measure_queue_session(void) +{ + queue_init_lock_once(); + k_mutex_lock(&queue_lock, K_FOREVER); + uint8_t session = queue_session; + k_mutex_unlock(&queue_lock); + return session; +} + +uint16_t measure_queue_total_frames(void) +{ + queue_init_lock_once(); + k_mutex_lock(&queue_lock, K_FOREVER); + uint16_t total = queue_total_frames; + k_mutex_unlock(&queue_lock); + return total; +} + +uint16_t measure_queue_num_samples(void) +{ + queue_init_lock_once(); + k_mutex_lock(&queue_lock, K_FOREVER); + uint16_t num_samples = queue_num_samples; + k_mutex_unlock(&queue_lock); + return num_samples; +} + +int measure_queue_read_frame(uint16_t frame_idx, measure_queue_frame_header_t *header, uint16_t samples[PIEZO_NUM_CHANNELS][PIEZO_MEASURE_MAX_SAMPLES]) +{ + if ((header == NULL) || (samples == NULL)) + { + return -EINVAL; + } + + queue_init_lock_once(); + + k_mutex_lock(&queue_lock, K_FOREVER); + uint16_t total = queue_total_frames; + uint16_t num_samples = queue_num_samples; + uint8_t session = queue_session; + bool recording = queue_recording; + k_mutex_unlock(&queue_lock); + + if (recording) + { + // 아직 저장 측정이 진행 중일 때 mqb? 가 오는 경우 busy 에러 + return -EBUSY; + } + + if (frame_idx >= total) + { + return -ENOENT; + } + + if (queue_area == NULL) + { + int err = flash_area_open(FIXED_PARTITION_ID(data_storage), &queue_area); + if (err) + { + return err; + } + } + + uint32_t frame_size = queue_frame_size(num_samples); + int err = flash_area_read(queue_area, (off_t)((uint32_t)frame_idx * frame_size), frame_read_buf, frame_size); + if (err) + { + return err; + } + + /* Build header fields for rqh: */ + header->session = session; + header->frame_idx = frame_idx; + header->tick_ms = get_u32_be(frame_read_buf); + header->ch_mask = MEASURE_QUEUE_CH_MASK_ALL; + + /* Restore channel samples for rqb: */ + const uint8_t *p = &frame_read_buf[4]; + for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++) + { + for (uint16_t i = 0; i < num_samples; i++) + { + samples[ch][i] = get_u16_be(p); + p += 2; + } + } + + return 0; +} \ No newline at end of file diff --git a/src/measurement/measure_queue.h b/src/measurement/measure_queue.h new file mode 100644 index 0000000..874abbf --- /dev/null +++ b/src/measurement/measure_queue.h @@ -0,0 +1,33 @@ +/******************************************************************************* + * @file measure_queue.h + * @brief Flash-backed queued piezo measurement frames + ******************************************************************************/ +#ifndef MEASURE_QUEUE_H__ +#define MEASURE_QUEUE_H__ + +#include +#include + +#include "piezo_measure.h" + +#define MEASURE_QUEUE_CH_MASK_ALL 0x3FU + +typedef struct +{ + uint8_t session; + uint16_t frame_idx; + uint32_t tick_ms; + uint8_t ch_mask; +} measure_queue_frame_header_t; + +int measure_queue_start(uint32_t *tick_start); +int measure_queue_stop(uint32_t *tick_end, uint8_t *session, uint16_t *total_frames); +bool measure_queue_is_recording(void); +uint8_t measure_queue_session(void); +uint16_t measure_queue_total_frames(void); +uint16_t measure_queue_num_samples(void); +int measure_queue_read_frame(uint16_t frame_idx, + measure_queue_frame_header_t *header, + uint16_t samples[PIEZO_NUM_CHANNELS][PIEZO_MEASURE_MAX_SAMPLES]); + +#endif /* MEASURE_QUEUE_H__ */ diff --git a/src/measurement/piezo_measure.c b/src/measurement/piezo_measure.c index 6c10410..57084f8 100644 --- a/src/measurement/piezo_measure.c +++ b/src/measurement/piezo_measure.c @@ -20,7 +20,7 @@ /* 앱에서 저장/변경 가능한 piezo 측정 기본값 */ #define PIEZO_CFG_FREQ_DEFAULT PIEZO_CFG_FREQ_2_1MHZ -#define PIEZO_CFG_CYCLES_DEFAULT 7 +#define PIEZO_CFG_CYCLES_DEFAULT 3 #define PIEZO_CFG_DELAY_DEFAULT 10 #define PIEZO_CFG_SAMPLES_DEFAULT 100 #define PIEZO_CFG_AVG_DEFAULT 3 @@ -181,7 +181,7 @@ int piezo_measure_start_session(void) return ECHO_STATUS_ADC_INIT; } - DBG_PRINTF("[SWEEP] piezo session ready\r\n"); + //DBG_PRINTF("[SWEEP] piezo session ready\r\n"); return ECHO_STATUS_OK; } @@ -189,7 +189,7 @@ int piezo_measure_start_session(void) * 6채널 전체 sweep * 각 채널마다 dummy capture를 먼저 버린 뒤, cfg->avg회 real capture를 sample index별로 평균 */ -int piezo_measure_sweep(void) +static int piezo_measure_sweep_with_avg(uint8_t avg_override) { const piezo_config_t *cfg = piezo_config_get(); @@ -197,7 +197,7 @@ int piezo_measure_sweep(void) uint8_t cycles = cfg->cycles; uint16_t capture_delay_us = cfg->delay_us; uint16_t samples = cfg->samples; - uint8_t avg = cfg->avg; + uint8_t avg = (avg_override > 0U) ? avg_override : cfg->avg; // 너무 짧은 delay가 들어오면 burst 직후 ADC capture가 겹치므로 최소 delay 보장 if (capture_delay_us < PIEZO_BURST_TO_ADC_DELAY_US) @@ -205,7 +205,7 @@ int piezo_measure_sweep(void) capture_delay_us = PIEZO_BURST_TO_ADC_DELAY_US; } - DBG_PRINTF("[SWEEP] freq=0x%04X cycles=%u avg=%u delay_us=%u samples=%u\r\n", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples); + //DBG_PRINTF("[SWEEP] freq=0x%04X cycles=%u avg=%u delay_us=%u samples=%u\r\n", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples); for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++) { @@ -275,7 +275,6 @@ int piezo_measure_sweep(void) // IRQ 잠금 해제 irq_unlock(key); - if (err) { DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, a, err); @@ -302,6 +301,16 @@ int piezo_measure_sweep(void) return ECHO_STATUS_OK; } +int piezo_measure_sweep(void) +{ + return piezo_measure_sweep_with_avg(0U); +} + +int piezo_measure_sweep_once(void) +{ + return piezo_measure_sweep_with_avg(1U); +} + /* * 테스트용 * 단일 채널 burst + echo capture diff --git a/src/measurement/piezo_measure.h b/src/measurement/piezo_measure.h index 7a44281..c94140b 100644 --- a/src/measurement/piezo_measure.h +++ b/src/measurement/piezo_measure.h @@ -38,6 +38,7 @@ int piezo_config_set(const piezo_config_t *cfg); int piezo_measure_start_session(void); int piezo_measure_sweep(void); +int piezo_measure_sweep_once(void); int piezo_measure_single_capture(uint8_t freq, uint8_t cycles, uint16_t delay_us, uint16_t num_samples, uint16_t averaging, uint8_t channel); int piezo_measure_adc_only_capture(uint16_t num_samples, uint16_t averaging, uint8_t channel);