497 lines
13 KiB
C
497 lines
13 KiB
C
/*******************************************************************************
|
|
* @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];
|
|
|
|
/*
|
|
* 주기 측정 명령
|
|
* mbb?: piezo echo sweep + 배터리 + IMU + IMU 온도 측정
|
|
* - 성공 시 rbb 1개, reb 6개, raa 상태값 전송
|
|
* - processing=true 구간에서는 내부 모니터의 I2C 접근을 막는 기준으로 사용
|
|
*
|
|
* - 순서:
|
|
* 1. battery / imu / temp 측정
|
|
* 2. rbb: 1개 전송
|
|
* 3. piezo / echo ADC 준비
|
|
* 4. piezo 6채널 burst
|
|
* 5. echo 샘플 채널별로 모으기
|
|
* 6. reb: 6개 전송
|
|
* 7. raa: 상태값 전송
|
|
*/
|
|
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];
|
|
|
|
processing = true;
|
|
DBG_PRINTF("[MBB] cmd start\r\n");
|
|
|
|
int batt_mv = -1;
|
|
int16_t temp_cdeg = INT16_MIN;
|
|
|
|
/* Read battery first (idle measurement) so mbb? result matches msn? */
|
|
DBG_PRINTF("[MBB] battery read (idle)\r\n");
|
|
batt_mv = battery_read_mv();
|
|
|
|
int status = piezo_measure_start_session();
|
|
|
|
if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
|
|
{
|
|
DBG_PRINTF("[MBB] imu/temp read\r\n");
|
|
imu_read_with_temperature(accel, gyro, &temp_cdeg);
|
|
|
|
battery_protection_record_continuous(batt_mv, temp_cdeg); // update protection counters for continuous monitoring
|
|
|
|
DBG_PRINTF("[MBB] response rbb: tx start\r\n");
|
|
cmd_send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);
|
|
|
|
DBG_PRINTF("[MBB] piezo sweep start\r\n");
|
|
status = piezo_measure_sweep();
|
|
if (status == ECHO_STATUS_OK)
|
|
{
|
|
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
|
|
{
|
|
DBG_PRINTF("[MBB] response reb: tx start ch=%d\r\n", ch);
|
|
cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
|
|
}
|
|
}
|
|
}
|
|
|
|
piezo_power_off();
|
|
DBG_PRINTF("[MBB] done\r\n");
|
|
cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
|
|
DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);
|
|
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* 정렬모드
|
|
* mtb?: IMU FIFO + piezo echo sweep
|
|
* - 순서:
|
|
* 1. IMU FIFO start
|
|
* 2. piezo 6채널 burst
|
|
* 3. echo 샘플 채널별로 모으기
|
|
* 4. reb: 패킷 6개 전송
|
|
* 5. raa: 상태값 전송
|
|
* 6. rim: IMU FIFO 15 샘플 전송
|
|
*/
|
|
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;
|
|
DBG_PRINTF("[MTB] cmd start\r\n");
|
|
|
|
int imu_ret = imu_fifo_start();
|
|
if (imu_ret != 0)
|
|
{
|
|
DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
|
|
}
|
|
else
|
|
{
|
|
fifo_started = true;
|
|
}
|
|
|
|
int status = piezo_measure_start_session();
|
|
|
|
if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
|
|
{
|
|
DBG_PRINTF("[MTB] piezo sweep start\r\n");
|
|
status = piezo_measure_sweep();
|
|
if (status == ECHO_STATUS_OK)
|
|
{
|
|
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
|
|
{
|
|
DBG_PRINTF("[MTB] response reb: tx start ch=%d\r\n", ch);
|
|
cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
|
|
}
|
|
}
|
|
}
|
|
|
|
cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
|
|
|
|
if (status == ECHO_STATUS_OK)
|
|
{
|
|
if (fifo_started)
|
|
{
|
|
imu_ret = imu_fifo_wait_samples(IMU_FIFO_RIM_TARGET_SAMPLES, 500U);
|
|
if (imu_ret != 0)
|
|
{
|
|
DBG_PRINTF("[MTB] fifo wait ret=%d\r\n", imu_ret);
|
|
}
|
|
|
|
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);
|
|
rim_count = 0U;
|
|
}
|
|
}
|
|
|
|
cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 샘플 rim: 전송
|
|
}
|
|
DBG_PRINTF("[CMD] mtb status=0x%04X\r\n", status);
|
|
|
|
piezo_power_off();
|
|
DBG_PRINTF("[CMD] mtb status=0x%04X rim=%u\r\n", status, rim_count);
|
|
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* mcf?: piezo measurement parameter read
|
|
*/
|
|
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 measurement parameter write
|
|
*/
|
|
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;
|
|
}
|
|
|
|
// =============================== Test Command ===============================
|
|
/* TEST
|
|
* mim? : IMU FIFO 15 samples
|
|
*/
|
|
int cmd_mim(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
ARG_UNUSED(data);
|
|
ARG_UNUSED(data_len);
|
|
|
|
if (processing)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
processing = true;
|
|
|
|
uint16_t rim_count = 0U;
|
|
int imu_ret = imu_fifo_start();
|
|
if (imu_ret != 0)
|
|
{
|
|
DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
|
|
cmd_send_response_rim(tx_rim_samples, 0U);
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
k_msleep(320); // IMU FIFO is 50Hz, 15 samples need about 300ms
|
|
|
|
imu_ret = imu_fifo_read_latest(tx_rim_samples, IMU_FIFO_RIM_TARGET_SAMPLES, &rim_count);
|
|
if (imu_ret != 0)
|
|
{
|
|
DBG_PRINTF("[MIM] fifo read fail ret=%d\r\n", imu_ret);
|
|
rim_count = 0U;
|
|
}
|
|
|
|
cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 15 samples rim: 전송
|
|
imu_fifo_stop();
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* TEST (이전 정렬모드)
|
|
* maa?: piezo echo sweep
|
|
* - 순서:
|
|
* 1. piezo 6채널 burst
|
|
* 2. echo 샘플 채널별로 모으기
|
|
* 3. reb: 패킷 6개 전송
|
|
* 4. 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;
|
|
|
|
int status = piezo_measure_start_session();
|
|
|
|
if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
|
|
{
|
|
DBG_PRINTF("[MAA] piezo sweep start\r\n");
|
|
status = piezo_measure_sweep();
|
|
if (status == ECHO_STATUS_OK)
|
|
{
|
|
for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
|
|
{
|
|
DBG_PRINTF("[MBB] response reb: tx start ch=%d\r\n", ch);
|
|
cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
|
|
}
|
|
}
|
|
}
|
|
|
|
piezo_power_off();
|
|
cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
|
|
DBG_PRINTF("[CMD] maa status=0x%04X\r\n", status);
|
|
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* TEST
|
|
* mpa?: piezo power ON
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/*
|
|
* TEST
|
|
* mpb?: piezo power OFF
|
|
*/
|
|
int cmd_mpb(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
ARG_UNUSED(data);
|
|
ARG_UNUSED(data_len);
|
|
|
|
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;
|
|
}
|
|
|
|
/*
|
|
* TEST
|
|
* mpc?: burst only
|
|
*/
|
|
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);
|
|
|
|
if ((cycles < 3U) || (cycles > 7U))
|
|
{
|
|
cmd_send_response_u16("rpc:", 2);
|
|
return 1;
|
|
}
|
|
|
|
|
|
if (piezo_init() != 0)
|
|
{
|
|
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();
|
|
cmd_send_response_u16("rpc:", 0);
|
|
return 1;
|
|
}
|
|
|
|
piezo_burst_sw_freq((uint8_t)freq_option, cycles);
|
|
piezo_power_off();
|
|
|
|
cmd_send_response_u16("rpc:", cycles);
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* TEST
|
|
* mec?: 1 channel 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);
|
|
|
|
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;
|
|
|
|
int status = piezo_measure_start_session();
|
|
if (status == ECHO_STATUS_OK)
|
|
{
|
|
status = piezo_measure_single_capture((uint8_t)freq_option, 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();
|
|
cmd_send_response_u16("raa:", (uint16_t)status);
|
|
processing = false;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* TEST
|
|
* mad?: 1 channel echo capture only
|
|
*/
|
|
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;
|
|
|
|
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();
|
|
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;
|
|
}
|