1653 lines
43 KiB
C
1653 lines
43 KiB
C
/*******************************************************************************
|
||
* @file parser.c
|
||
* @brief BLE command parser
|
||
*
|
||
* Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
|
||
******************************************************************************/
|
||
#include <zephyr/kernel.h>
|
||
#include <zephyr/sys/util.h>
|
||
#include <zephyr/sys/reboot.h>
|
||
#if IS_ENABLED(CONFIG_BT_SMP)
|
||
#include <zephyr/bluetooth/bluetooth.h>
|
||
#include <zephyr/bluetooth/conn.h>
|
||
#endif
|
||
#include <string.h>
|
||
#include <limits.h>
|
||
|
||
#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 "echo_adc.h"
|
||
#include "imu_i2c.h"
|
||
#include "piezo.h"
|
||
|
||
/*==============================================================================
|
||
* Piezo / echo measurement constants
|
||
*============================================================================*/
|
||
#define PIEZO_AVERAGE_COUNT 10
|
||
#define ECHO_STATUS_OK 0x0000
|
||
#define ECHO_STATUS_PIEZO 0x0001
|
||
#define ECHO_STATUS_ADC_INIT 0x0002
|
||
#define ECHO_STATUS_MUX 0x0003
|
||
#define ECHO_STATUS_CAPTURE 0x0004
|
||
#define ECHO_STATUS_BATT 0x0005
|
||
#define ECHO_STATUS_IMU 0x0006
|
||
#define ECHO_STATUS_TEMP 0x0007
|
||
|
||
#define PIEZO_CFG_FREQ_DEFAULT 1
|
||
#define PIEZO_CFG_CYCLES_DEFAULT 7
|
||
#define PIEZO_CFG_DELAY_DEFAULT 10
|
||
#define PIEZO_CFG_SAMPLES_DEFAULT 100
|
||
#define PIEZO_CFG_AVG_DEFAULT 3
|
||
|
||
#define PIEZO_CFG_AVG_MAX 10
|
||
#define PIEZO_CFG_SAMPLES_MIN 80
|
||
#define PIEZO_CFG_SAMPLES_MAX 117
|
||
#define PIEZO_CFG_DELAY_MAX_US 50
|
||
#define PIEZO_POST_SELECT_SETTLE_US 500
|
||
#define PIEZO_AVG_INTER_BURST_GAP_US 500
|
||
#define PIEZO_DUMMY_CAPTURE_COUNT 5
|
||
|
||
static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][ECHO_ADC_MAX_SAMPLES];
|
||
static uint16_t echo_capture[ECHO_ADC_MAX_SAMPLES];
|
||
static uint32_t echo_accum[ECHO_ADC_MAX_SAMPLES];
|
||
static uint8_t tx_u16_buf[8];
|
||
static uint8_t tx_imu_buf[18];
|
||
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_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 piezo_config_t g_piezo_config = {
|
||
.freq = PIEZO_CFG_FREQ_DEFAULT,
|
||
.cycles = PIEZO_CFG_CYCLES_DEFAULT,
|
||
.avg = PIEZO_CFG_AVG_DEFAULT,
|
||
.delay_us = PIEZO_CFG_DELAY_DEFAULT,
|
||
.samples = PIEZO_CFG_SAMPLES_DEFAULT,
|
||
};
|
||
|
||
static bool piezo_config_validate(const piezo_config_t *cfg)
|
||
{
|
||
if (cfg == NULL)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (cfg->freq != PIEZO_CFG_FREQ_DEFAULT)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if ((cfg->cycles < 3U) || (cfg->cycles > 7U))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if ((cfg->avg == 0U) || (cfg->avg > PIEZO_CFG_AVG_MAX))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (cfg->delay_us > PIEZO_CFG_DELAY_MAX_US)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if ((cfg->samples < PIEZO_CFG_SAMPLES_MIN) ||
|
||
(cfg->samples > PIEZO_CFG_SAMPLES_MAX))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
static void piezo_config_log(const char *prefix, const piezo_config_t *cfg)
|
||
{
|
||
DBG_PRINTF("%s freq=%u cycles=%u avg=%u delay=%u samples=%u\r\n",
|
||
prefix,
|
||
cfg->freq,
|
||
cfg->cycles,
|
||
cfg->avg,
|
||
cfg->delay_us,
|
||
cfg->samples);
|
||
}
|
||
|
||
const piezo_config_t *piezo_config_get(void)
|
||
{
|
||
return &g_piezo_config;
|
||
}
|
||
|
||
int piezo_config_init(void)
|
||
{
|
||
int err = app_nvs_init(&g_piezo_config, piezo_config_validate, piezo_config_log);
|
||
if (err)
|
||
{
|
||
return err;
|
||
}
|
||
|
||
piezo_config_log("[CFG] piezo active", &g_piezo_config);
|
||
return 0;
|
||
}
|
||
|
||
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)
|
||
{
|
||
/*
|
||
* reb: 패킷은 최대 210바이트라서 스택 지역변수로 두면
|
||
* mbb?처럼 반복 호출할 때 워커 스택을 꽤 먹는다.
|
||
* 현재는 한 번에 하나의 명령만 처리하므로 정적 버퍼를 재사용한다.
|
||
*/
|
||
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));
|
||
}
|
||
|
||
static int start_piezo_session(void)
|
||
{
|
||
DBG_PRINTF("[MBB] piezo session start\r\n");
|
||
|
||
int err = piezo_init();
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[PIEZO] init fail err=%d\r\n", err);
|
||
return ECHO_STATUS_PIEZO;
|
||
}
|
||
|
||
piezo_power_on();
|
||
k_msleep(PIEZO_POWER_STABILIZE_MS);
|
||
|
||
err = echo_adc_init();
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
|
||
return ECHO_STATUS_ADC_INIT;
|
||
}
|
||
|
||
err = echo_adc_wake();
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[ECHO] wake fail err=%d\r\n", err);
|
||
return ECHO_STATUS_ADC_INIT;
|
||
}
|
||
|
||
DBG_PRINTF("[MBB] piezo session ready\r\n");
|
||
return ECHO_STATUS_OK;
|
||
}
|
||
|
||
static int perform_piezo_sweep(void)
|
||
{
|
||
const piezo_config_t *cfg = piezo_config_get();
|
||
uint16_t capture_delay_us = cfg->delay_us;
|
||
|
||
if (capture_delay_us < PIEZO_BURST_TO_ADC_DELAY_US)
|
||
{
|
||
capture_delay_us = PIEZO_BURST_TO_ADC_DELAY_US;
|
||
}
|
||
|
||
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
|
||
{
|
||
DBG_PRINTF("[MBB] sweep ch=%d start\r\n", ch);
|
||
|
||
int err = piezo_select_channel(ch);
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[PIEZO] mux fail ch=%d err=%d\r\n", ch, err);
|
||
return ECHO_STATUS_MUX;
|
||
}
|
||
|
||
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
||
|
||
err = echo_adc_wake();
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[ECHO] dummy read fail ch=%d err=%d\r\n", ch, err);
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
|
||
{
|
||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||
piezo_burst_sw((uint8_t)cfg->cycles);
|
||
k_busy_wait(capture_delay_us);
|
||
|
||
err = echo_adc_capture(echo_capture, cfg->samples);
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err);
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
}
|
||
|
||
memset(echo_accum, 0, sizeof(echo_accum));
|
||
|
||
for (uint16_t avg = 0; avg < cfg->avg; avg++)
|
||
{
|
||
if (avg > 0U)
|
||
{
|
||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||
}
|
||
|
||
unsigned int key = irq_lock();
|
||
piezo_burst_sw((uint8_t)cfg->cycles);
|
||
k_busy_wait(capture_delay_us);
|
||
|
||
err = echo_adc_capture(echo_capture, cfg->samples);
|
||
irq_unlock(key);
|
||
if (err)
|
||
{
|
||
DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, avg, err);
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
for (uint16_t i = 0; i < cfg->samples; i++)
|
||
{
|
||
echo_accum[i] += echo_capture[i];
|
||
}
|
||
}
|
||
|
||
for (uint16_t i = 0; i < cfg->samples; i++)
|
||
{
|
||
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / cfg->avg);
|
||
}
|
||
|
||
DBG_PRINTF("[MBB] sweep ch=%d done\r\n", ch);
|
||
}
|
||
|
||
DBG_PRINTF("[MBB] sweep done\r\n");
|
||
return ECHO_STATUS_OK;
|
||
}
|
||
|
||
static int perform_single_piezo_capture(uint8_t cycles,
|
||
uint16_t delay_us,
|
||
uint16_t num_samples,
|
||
uint16_t averaging,
|
||
uint8_t channel)
|
||
{
|
||
if (channel >= PIEZO_NUM_CHANNELS)
|
||
{
|
||
return ECHO_STATUS_MUX;
|
||
}
|
||
|
||
if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
if (averaging == 0U)
|
||
{
|
||
averaging = 1U;
|
||
}
|
||
|
||
int err = piezo_select_channel(channel);
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_MUX;
|
||
}
|
||
|
||
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
||
|
||
err = echo_adc_wake();
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
|
||
{
|
||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||
piezo_burst_sw(cycles);
|
||
k_busy_wait(delay_us);
|
||
|
||
err = echo_adc_capture(echo_capture, num_samples);
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
}
|
||
|
||
memset(echo_accum, 0, sizeof(echo_accum));
|
||
|
||
for (uint16_t avg = 0; avg < averaging; avg++)
|
||
{
|
||
if (avg > 0U)
|
||
{
|
||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||
}
|
||
|
||
unsigned int key = irq_lock();
|
||
piezo_burst_sw(cycles);
|
||
k_busy_wait(delay_us);
|
||
|
||
err = echo_adc_capture(echo_capture, num_samples);
|
||
irq_unlock(key);
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
for (uint16_t i = 0; i < num_samples; i++)
|
||
{
|
||
echo_accum[i] += echo_capture[i];
|
||
}
|
||
}
|
||
|
||
for (uint16_t i = 0; i < num_samples; i++)
|
||
{
|
||
echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
|
||
}
|
||
|
||
return ECHO_STATUS_OK;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* 커맨드 핸들러
|
||
*============================================================================*/
|
||
|
||
/* msn? → 배터리 전압 측정 → rsn: + mV */
|
||
|
||
static int perform_adc_only_capture(uint16_t num_samples,
|
||
uint16_t averaging,
|
||
uint8_t channel)
|
||
{
|
||
if (channel >= PIEZO_NUM_CHANNELS)
|
||
{
|
||
return ECHO_STATUS_MUX;
|
||
}
|
||
|
||
if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
if (averaging == 0U)
|
||
{
|
||
averaging = 1U;
|
||
}
|
||
|
||
int err = piezo_select_channel(channel);
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_MUX;
|
||
}
|
||
|
||
k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);
|
||
|
||
err = echo_adc_wake();
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
memset(echo_accum, 0, sizeof(echo_accum));
|
||
|
||
for (uint16_t avg = 0; avg < averaging; avg++)
|
||
{
|
||
if (avg > 0U)
|
||
{
|
||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||
}
|
||
|
||
unsigned int key = irq_lock();
|
||
err = echo_adc_capture(echo_capture, num_samples);
|
||
irq_unlock(key);
|
||
if (err)
|
||
{
|
||
return ECHO_STATUS_CAPTURE;
|
||
}
|
||
|
||
for (uint16_t i = 0; i < num_samples; i++)
|
||
{
|
||
echo_accum[i] += echo_capture[i];
|
||
}
|
||
}
|
||
|
||
for (uint16_t i = 0; i < num_samples; i++)
|
||
{
|
||
echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
|
||
}
|
||
|
||
return ECHO_STATUS_OK;
|
||
}
|
||
|
||
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);
|
||
DBG_PRINTF("[CMD] msn -> %d mV\r\n", 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;
|
||
}
|
||
|
||
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);
|
||
|
||
/*
|
||
* 테스트용
|
||
* mec?: 단일 채널 burst + echo capture
|
||
*/
|
||
if (num_samples > ECHO_ADC_MAX_SAMPLES)
|
||
{
|
||
num_samples = ECHO_ADC_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 = start_piezo_session();
|
||
if (status == ECHO_STATUS_OK)
|
||
{
|
||
status = perform_single_piezo_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), echo_capture, num_samples);
|
||
}
|
||
|
||
piezo_power_off();
|
||
power_button_suspend(false);
|
||
send_response_u16("raa:", (uint16_t)status);
|
||
processing = false;
|
||
return 1;
|
||
}
|
||
|
||
/**
|
||
* 테스트용(이전 정렬모드)
|
||
* maa?:
|
||
* - piezo 6채널 burst 순서대로 쏘고
|
||
* - echo 샘플을 채널별로 모은 뒤
|
||
* - reb: 패킷 6개 전송
|
||
* - 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 > ECHO_ADC_MAX_SAMPLES)
|
||
{
|
||
num_samples = ECHO_ADC_MAX_SAMPLES;
|
||
}
|
||
if (averaging == 0U)
|
||
{
|
||
averaging = 1U;
|
||
}
|
||
|
||
processing = true;
|
||
power_button_suspend(true);
|
||
|
||
int status = start_piezo_session();
|
||
if (status == ECHO_STATUS_OK)
|
||
{
|
||
status = perform_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), echo_capture, 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;
|
||
}
|
||
|
||
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 = 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);
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
/**
|
||
* 전체 측정 시 사용
|
||
*/
|
||
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 = start_piezo_session();
|
||
if (status == ECHO_STATUS_OK)
|
||
{
|
||
status = perform_piezo_sweep();
|
||
}
|
||
|
||
int batt_mv = -1;
|
||
int16_t temp_cdeg = INT16_MIN;
|
||
|
||
if (status == ECHO_STATUS_OK)
|
||
{
|
||
/* info 성격 데이터는 echo sweep이 정상 끝났을 때만 읽음 */
|
||
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)
|
||
{
|
||
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)
|
||
{
|
||
/* rbb: 센서(배터리+IMU+온도) 정보 패킷 */
|
||
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_channels[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 = 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
|
||
*/
|
||
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);
|
||
piezo_config_log("[CMD] mcf ->", cfg);
|
||
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);
|
||
DBG_PRINTF("[CMD] mcs: insufficient data len=%u\r\n", data_len);
|
||
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);
|
||
piezo_config_log("[CMD] mcs invalid", &cfg);
|
||
return 1;
|
||
}
|
||
|
||
g_piezo_config = cfg;
|
||
int err = app_nvs_save_piezo(&g_piezo_config);
|
||
if (err)
|
||
{
|
||
send_response_piezo_config("rcs:", 0xFFFD, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
|
||
piezo_config_log("[CMD] mcs save fail", &g_piezo_config);
|
||
return 1;
|
||
}
|
||
|
||
send_response_piezo_config("rcs:", g_piezo_config.freq, g_piezo_config.cycles, g_piezo_config.avg, g_piezo_config.delay_us, g_piezo_config.samples);
|
||
piezo_config_log("[CMD] mcs saved", &g_piezo_config);
|
||
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);
|
||
}
|
||
DBG_PRINTF("[CMD] mfv read\r\n");
|
||
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);
|
||
}
|
||
DBG_PRINTF("[CMD] mid read\r\n");
|
||
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);
|
||
DBG_PRINTF("[CMD] mwh: insufficient data len=%u\r\n", data_len);
|
||
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);
|
||
DBG_PRINTF("[CMD] mwh updated\r\n");
|
||
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);
|
||
}
|
||
DBG_PRINTF("[CMD] mrh read\r\n");
|
||
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);
|
||
DBG_PRINTF("[CMD] mws: insufficient data len=%u\r\n", data_len);
|
||
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);
|
||
DBG_PRINTF("[CMD] mws updated\r\n");
|
||
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);
|
||
}
|
||
DBG_PRINTF("[CMD] mrs read\r\n");
|
||
return 1;
|
||
}
|
||
|
||
static int cmd_mpz(const uint8_t *data, uint8_t data_len)
|
||
{
|
||
if (data_len < PASSKEY_LENGTH)
|
||
{
|
||
send_response_u16("rpz:", 0xFFFF);
|
||
DBG_PRINTF("[CMD] mpz: insufficient data len=%u\r\n", data_len);
|
||
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);
|
||
DBG_PRINTF("[CMD] mpz updated\r\n");
|
||
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);
|
||
DBG_PRINTF("[CMD] mqz read\r\n");
|
||
return 1;
|
||
}
|
||
|
||
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 2B BE */
|
||
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);
|
||
if (state == LED_STATE_OFF)
|
||
{
|
||
int imu_ret = imu_fifo_stop();
|
||
DBG_PRINTF("[CMD] mls: fifo stop ret=%d\r\n", imu_ret);
|
||
}
|
||
|
||
send_response_u16("rls:", state);
|
||
DBG_PRINTF("[CMD] mls -> LED state=%d\r\n", 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 dr_parser(const uint8_t *buf, uint16_t len)
|
||
{
|
||
DBG_CORE("[PARSER] enter len=%u\r\n", len);
|
||
DBG_CORE("[CMD] RX len=%u\r\n", len);
|
||
|
||
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'
|
||
};
|
||
DBG_CORE("[CMD] tag=%s\r\n", tag);
|
||
|
||
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;
|
||
}
|
||
|
||
DBG_CORE("[CMD] dispatch -> %s\r\n", cmd_table[i].tag);
|
||
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;
|
||
}
|