ADC까지, 신호 확인 필요(반사 신호 약함 or 뒤쪽을 찍고 있음)
This commit is contained in:
+850
-10
@@ -9,6 +9,7 @@
|
||||
******************************************************************************/
|
||||
#include <zephyr/kernel.h>
|
||||
#include <string.h>
|
||||
#include <limits.h>
|
||||
|
||||
#include "parser.h"
|
||||
#include "main.h"
|
||||
@@ -16,7 +17,41 @@
|
||||
#include "ble_service.h"
|
||||
#include "battery_adc.h"
|
||||
#include "led_control.h"
|
||||
#include "echo_adc.h"
|
||||
#include "imu_i2c.h"
|
||||
#include "piezo.h"
|
||||
#include "tmp235.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 10
|
||||
#define PIEZO_POST_SELECT_SETTLE_US 500
|
||||
#define PIEZO_AVG_INTER_BURST_GAP_US 500
|
||||
|
||||
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 + (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];
|
||||
|
||||
/*==============================================================================
|
||||
* CRC16 (CRC-CCITT, Nordic SDK 호환)
|
||||
@@ -36,14 +71,47 @@ static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size)
|
||||
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 void send_response_u16(const char *tag, uint16_t value)
|
||||
static int send_response_u16(const char *tag, uint16_t value)
|
||||
{
|
||||
uint8_t buf[8];
|
||||
uint8_t *buf = tx_u16_buf;
|
||||
|
||||
buf[0] = tag[0];
|
||||
buf[1] = tag[1];
|
||||
@@ -56,7 +124,44 @@ static void send_response_u16(const char *tag, uint16_t value)
|
||||
buf[6] = (uint8_t)(crc & 0xFF);
|
||||
buf[7] = (uint8_t)(crc >> 8);
|
||||
|
||||
ble_data_send(buf, 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_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);
|
||||
}
|
||||
|
||||
/*==============================================================================
|
||||
@@ -65,9 +170,9 @@ static void send_response_u16(const char *tag, uint16_t value)
|
||||
|
||||
/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
|
||||
* 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
|
||||
static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
|
||||
static int send_response_imu(const int16_t accel[3], const int16_t gyro[3])
|
||||
{
|
||||
uint8_t buf[18];
|
||||
uint8_t *buf = tx_imu_buf;
|
||||
|
||||
buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':';
|
||||
|
||||
@@ -84,7 +189,244 @@ static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
|
||||
buf[16] = (uint8_t)(crc & 0xFF);
|
||||
buf[17] = (uint8_t)(crc >> 8);
|
||||
|
||||
ble_data_send(buf, 18);
|
||||
return ble_data_send(buf, 18);
|
||||
}
|
||||
|
||||
static void send_response_echo(const uint16_t *samples, uint16_t num_samples)
|
||||
{
|
||||
/*
|
||||
* reb: 패킷은 최대 208바이트라서 스택 지역변수로 두면
|
||||
* mbb?처럼 반복 호출할 때 워커 스택을 꽤 먹는다.
|
||||
* 현재는 한 번에 하나의 명령만 처리하므로 정적 버퍼를 재사용한다.
|
||||
*/
|
||||
uint8_t *buf = tx_echo_buf;
|
||||
|
||||
buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
|
||||
buf[4] = (uint8_t)(num_samples >> 8);
|
||||
buf[5] = (uint8_t)(num_samples & 0xFF);
|
||||
|
||||
for (uint16_t i = 0; i < num_samples; i++)
|
||||
{
|
||||
buf[6 + i * 2] = (uint8_t)(samples[i] >> 8);
|
||||
buf[7 + i * 2] = (uint8_t)(samples[i] & 0xFF);
|
||||
}
|
||||
|
||||
uint16_t payload_len = 4 + 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_piezo_config(uint16_t freq,
|
||||
uint16_t cycles,
|
||||
uint16_t avg,
|
||||
uint16_t delay_us,
|
||||
uint16_t samples)
|
||||
{
|
||||
uint8_t *buf = tx_cfg_buf;
|
||||
|
||||
buf[0] = 'r'; buf[1] = 'c'; buf[2] = 'f'; buf[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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
memset(echo_accum, 0, sizeof(echo_accum));
|
||||
|
||||
for (uint8_t avg = 0; avg < PIEZO_AVERAGE_COUNT; avg++)
|
||||
{
|
||||
if (avg > 0U)
|
||||
{
|
||||
k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
|
||||
}
|
||||
|
||||
piezo_burst_sw(PIEZO_SW_BURST_CYCLES);
|
||||
k_busy_wait(PIEZO_BURST_TO_ADC_DELAY_US);
|
||||
|
||||
err = echo_adc_capture(echo_capture, ECHO_ADC_MAX_SAMPLES);
|
||||
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 < ECHO_ADC_MAX_SAMPLES; i++)
|
||||
{
|
||||
echo_accum[i] += echo_capture[i];
|
||||
}
|
||||
}
|
||||
|
||||
for (uint16_t i = 0; i < ECHO_ADC_MAX_SAMPLES; i++)
|
||||
{
|
||||
piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / PIEZO_AVERAGE_COUNT);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
piezo_burst_sw(cycles);
|
||||
k_busy_wait(delay_us);
|
||||
|
||||
err = echo_adc_capture(echo_capture, num_samples);
|
||||
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;
|
||||
}
|
||||
|
||||
/*==============================================================================
|
||||
@@ -95,6 +437,9 @@ static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
|
||||
|
||||
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)
|
||||
{
|
||||
@@ -109,6 +454,9 @@ static int cmd_msn(const uint8_t *data, uint8_t data_len)
|
||||
/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
|
||||
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);
|
||||
@@ -122,10 +470,464 @@ static int cmd_msp(const uint8_t *data, uint8_t data_len)
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* mst? → 피에조 전원 ON → TMP235 온도 측정 → 전원 OFF → rso: + 온도(°C × 100, BE)
|
||||
*
|
||||
* TMP235가 피에조 레일을 공유하므로 ON/OFF 시퀀스를 한 커맨드에서 처리.
|
||||
* 안정화 대기 10ms: TMP235 start-up(~2ms) + 레일 RC 필터 여유분.
|
||||
* 에러 응답: 0xFFFF = ADC 읽기 실패 */
|
||||
static int cmd_mst(const uint8_t *data, uint8_t data_len)
|
||||
{
|
||||
ARG_UNUSED(data);
|
||||
ARG_UNUSED(data_len);
|
||||
|
||||
/*
|
||||
* mst?는 "온도만 읽는 명령"처럼 보이지만,
|
||||
* 실제로는 TMP235가 piezo 전원 레일을 같이 쓰기 때문에
|
||||
* 전원 ON/OFF 시퀀스까지 같이 처리해야 한다.
|
||||
*/
|
||||
power_button_suspend(true);
|
||||
|
||||
if (piezo_init() != 0)
|
||||
{
|
||||
power_button_suspend(false);
|
||||
send_response_u16("rso:", 0xFFFF);
|
||||
DBG_PRINTF("[CMD] mst: piezo init fail\r\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* 전원 ON → 센서 안정화 대기 */
|
||||
piezo_power_on();
|
||||
k_msleep(10);
|
||||
|
||||
int16_t t_cdeg = temp_read_cdeg();
|
||||
|
||||
/* 전원 OFF (측정 완료, 레일 끄기) */
|
||||
piezo_power_off();
|
||||
power_button_suspend(false);
|
||||
|
||||
/* ADC 읽기 실패 → 에러 코드 0xFFFF */
|
||||
if (t_cdeg == INT16_MIN)
|
||||
{
|
||||
send_response_u16("rso:", 0xFFFF);
|
||||
DBG_PRINTF("[CMD] mst: temp read fail\r\n");
|
||||
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;
|
||||
}
|
||||
|
||||
static int cmd_mpa(const uint8_t *data, uint8_t data_len)
|
||||
{
|
||||
ARG_UNUSED(data);
|
||||
ARG_UNUSED(data_len);
|
||||
|
||||
/* mpa?: piezo TX/RX 전원 레일만 켠다. 측정은 하지 않는다. */
|
||||
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;
|
||||
}
|
||||
|
||||
static int cmd_mpc(const uint8_t *data, uint8_t data_len)
|
||||
{
|
||||
uint16_t cycles = 5;
|
||||
uint16_t freq_option = 1;
|
||||
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);
|
||||
|
||||
/*
|
||||
* mpc?: burst만 한 번 발생시키는 테스트 명령.
|
||||
* echo를 읽지 않으므로 "초음파가 나가는지"만 빠르게 볼 때 쓴다.
|
||||
*/
|
||||
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)
|
||||
{
|
||||
uint16_t freq_option = 1;
|
||||
uint16_t delay_us = PIEZO_BURST_TO_ADC_DELAY_US;
|
||||
uint16_t num_samples = ECHO_ADC_MAX_SAMPLES;
|
||||
uint16_t cycles = PIEZO_SW_BURST_CYCLES;
|
||||
uint16_t averaging = 1;
|
||||
uint16_t piezo_ch = 0;
|
||||
|
||||
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.
|
||||
* maa?/mbb?보다 가벼워서 "한 채널만 먼저 살아 있는지" 확인하기 좋다.
|
||||
*/
|
||||
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(echo_capture, num_samples);
|
||||
}
|
||||
|
||||
piezo_power_off();
|
||||
power_button_suspend(false);
|
||||
send_response_u16("raa:", (uint16_t)status);
|
||||
processing = false;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int cmd_maa(const uint8_t *data, uint8_t data_len)
|
||||
{
|
||||
ARG_UNUSED(data);
|
||||
ARG_UNUSED(data_len);
|
||||
|
||||
/*
|
||||
* maa?:
|
||||
* - piezo 6채널을 순서대로 쏘고
|
||||
* - echo 샘플을 채널별로 모은 뒤
|
||||
* - reb: 패킷 6개를 보낸다.
|
||||
*
|
||||
* 마지막 raa: 상태값은 "전체 작업 성공/실패 요약"이다.
|
||||
*/
|
||||
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(piezo_channels[ch], ECHO_ADC_MAX_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);
|
||||
|
||||
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 read\r\n");
|
||||
if (imu_read(accel, gyro) != 0)
|
||||
{
|
||||
status = ECHO_STATUS_IMU;
|
||||
}
|
||||
}
|
||||
|
||||
if (status == ECHO_STATUS_OK)
|
||||
{
|
||||
DBG_PRINTF("[MBB] temp read\r\n");
|
||||
temp_cdeg = temp_read_cdeg();
|
||||
if (temp_cdeg == INT16_MIN)
|
||||
{
|
||||
status = ECHO_STATUS_TEMP;
|
||||
}
|
||||
}
|
||||
|
||||
if (status == ECHO_STATUS_OK)
|
||||
{
|
||||
/* rbb: 한 번에 보내는 "요약 정보 묶음" 패킷 */
|
||||
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(piezo_channels[ch], ECHO_ADC_MAX_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;
|
||||
}
|
||||
|
||||
static int cmd_mcf(const uint8_t *data, uint8_t data_len)
|
||||
{
|
||||
ARG_UNUSED(data);
|
||||
ARG_UNUSED(data_len);
|
||||
|
||||
send_response_piezo_config(PIEZO_CFG_FREQ_DEFAULT,
|
||||
PIEZO_CFG_CYCLES_DEFAULT,
|
||||
PIEZO_CFG_AVG_DEFAULT,
|
||||
PIEZO_CFG_DELAY_DEFAULT,
|
||||
PIEZO_CFG_SAMPLES_DEFAULT);
|
||||
DBG_PRINTF("[CMD] mcf -> freq=%d cycles=%d avg=%d delay=%d samples=%d\r\n",
|
||||
PIEZO_CFG_FREQ_DEFAULT,
|
||||
PIEZO_CFG_CYCLES_DEFAULT,
|
||||
PIEZO_CFG_AVG_DEFAULT,
|
||||
PIEZO_CFG_DELAY_DEFAULT,
|
||||
PIEZO_CFG_SAMPLES_DEFAULT);
|
||||
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_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);
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
memset(m_static_passkey, 0, sizeof(m_static_passkey));
|
||||
memcpy(m_static_passkey, data, PASSKEY_LENGTH);
|
||||
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 에코 */
|
||||
@@ -167,6 +969,21 @@ static const cmd_entry_t cmd_table[] =
|
||||
{ "msn?", cmd_msn },
|
||||
{ "mls?", cmd_mls },
|
||||
{ "msp?", cmd_msp },
|
||||
{ "mst?", cmd_mst },
|
||||
{ "mpa?", cmd_mpa },
|
||||
{ "mpb?", cmd_mpb },
|
||||
{ "mpc?", cmd_mpc },
|
||||
{ "mec?", cmd_mec },
|
||||
{ "maa?", cmd_maa },
|
||||
{ "mbb?", cmd_mbb },
|
||||
{ "mcf?", cmd_mcf },
|
||||
{ "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]))
|
||||
@@ -176,28 +993,47 @@ static const cmd_entry_t cmd_table[] =
|
||||
*============================================================================*/
|
||||
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);
|
||||
/* 최소 4바이트 TAG 필요 */
|
||||
if (len < 4)
|
||||
{
|
||||
DBG_PRINTF("[CMD] Too short (%d)\r\n", len);
|
||||
DBG_ERR("[CMD] Too short (%u)\r\n", len);
|
||||
return -1;
|
||||
}
|
||||
|
||||
char raw_tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };
|
||||
|
||||
/* CRC16 검증 (6바이트 이상이면 마지막 2바이트가 CRC) */
|
||||
if (len >= 6)
|
||||
{
|
||||
/*
|
||||
* 이 프로토콜은 끝 2바이트에 CRC16이 붙는다.
|
||||
* 값이 다르면 "명령 이름은 맞아 보여도 데이터가 깨졌다"는 뜻이므로 바로 버린다.
|
||||
*/
|
||||
uint16_t calc_crc = dr_crc16_compute(buf, len - 2);
|
||||
uint16_t recv_crc = (uint16_t)(buf[len - 2]) | ((uint16_t)(buf[len - 1]) << 8);
|
||||
|
||||
if (calc_crc != recv_crc)
|
||||
{
|
||||
DBG_PRINTF("[CMD] CRC fail (calc=0x%04X recv=0x%04X)\r\n", 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;
|
||||
}
|
||||
}
|
||||
|
||||
/* TAG 추출 (4바이트) */
|
||||
char tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };
|
||||
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);
|
||||
|
||||
/* 데이터 부분 (TAG 이후, CRC 이전) */
|
||||
const uint8_t *data = buf + 4;
|
||||
@@ -208,10 +1044,14 @@ int dr_parser(const uint8_t *buf, uint16_t len)
|
||||
{
|
||||
if (memcmp(tag, cmd_table[i].tag, 4) == 0)
|
||||
{
|
||||
DBG_CORE("[CMD] dispatch -> %s\r\n", cmd_table[i].tag);
|
||||
return cmd_table[i].handler(data, data_len);
|
||||
}
|
||||
}
|
||||
|
||||
DBG_PRINTF("[CMD] Unknown: %s\r\n", tag);
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user