커맨드 구조 분리
This commit is contained in:
@@ -0,0 +1,500 @@
|
||||
/*******************************************************************************
|
||||
* @file cmd_piezo.c
|
||||
* @brief BLE command handlers
|
||||
******************************************************************************/
|
||||
#include <zephyr/sys/util.h>
|
||||
#include <limits.h>
|
||||
|
||||
#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=범위 초과 */
|
||||
Reference in New Issue
Block a user