jh.chun/firmware-test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e0a504b96934f64d003b086ebfba33566a35a0df
firmware-test/pc_firm/parser.c

1467 lines
44 KiB
C
Raw Blame History

/*
* 2025-12-08 power loop bug fix
* 2025-12-07 msn, mta, mqq
* 2025-12-04 by Charles KWON
* parser.c : Common parser + command table + handlers
* - Firmware/PC shared
* - Hardware-dependent parts left as TODO
* - Added CRC16 validation support
*/
#include "parser.h"
#include <string.h>
#include "nrf_gpio.h"
#include "nrf_delay.h"
#include "dr_piezo.h"
#include "dr_util.h"
#include "dr_adc121s051.h"
// ========================================
// External function declarations
// ========================================
/* Sensor functions */
extern void battery_level_meas(void);
extern void pressure_all_level_meas(void);
extern void tmp235_voltage_level_meas(void);
/* Device control functions */
extern int device_activated(void);
extern int device_sleep_mode(void);
/* Error handling */
extern void param_error(const char *cmd);
/* BLE transmission */
extern void single_format_data(uint8_t *buffer, const char *tag, uint16_t value);
extern void ascii_format_data(uint8_t *buffer, const char *tag, const char *ascii, uint8_t len);
extern void dr_binary_tx_safe(const uint8_t *buffer, uint16_t length);
extern void dr_sd_delay_ms(uint32_t ms); /* Softdevice-friendly delay */
/* FDS config (fstorage) */
#include "fstorage.h"
extern char SERIAL_NO[12];
extern char HW_NO[12];
extern char m_static_passkey[6];
extern uint32_t m_life_cycle;
extern void format_data(uint8_t *buffer, const char *tag, const uint16_t *data_array, size_t length);
/* FW Version - must match cmd_parse.c DEVICE_VERSION */
#define DR_DEVICE_VERSION "FW25LIT2B102"
// ========================================
// External variables
// ========================================
extern volatile bool processing;
extern bool device_status;
extern uint8_t resetCount;
extern uint8_t ble_bin_buffer[];
extern bool con_single;
extern bool lock_check;
extern bool info4; // addtional info
extern bool ble_got_new_data; // BLE data flag
extern bool go_batt; // battery
extern bool motion_data_once; // IMU data flag
extern bool motion_raw_data_enabled; // IMU continuous flag
extern int imu_read_direct(void); // IMU direct register read + BLE send
extern void pressure_all_level_meas(void); // pressure sensor
extern void battery_timer_stop(void); // battery timer
extern void main_timer_start(void); // main timer
extern void hw_i2c_init_once(void); // I2C init for IMU
/* Power / Reset / Bond */
extern bool go_device_power_off;
extern bool go_NVIC_SystemReset;
extern bool bond_data_delete;
extern uint8_t m_reset_status;
extern void config_save(void);
extern config_data_t m_config;
/* AGC_GAIN_SW is a macro in measurements.h - replicate here */
#include "nrf_gpio.h"
#define GAIN_SW_PIN NRF_GPIO_PIN_MAP(0, 20)
#define AGC_GAIN_SW(x) do { if(x) nrf_gpio_pin_set(GAIN_SW_PIN); else nrf_gpio_pin_clear(GAIN_SW_PIN); } while(0)
extern void dr_piezo_power_on( void );
extern void dr_piezo_power_off( void );
extern void dr_piezo_burst_sw(uint8_t cycles);
extern void dr_piezo_burst_sw_18mhz(uint8_t cycles);
extern void dr_piezo_burst_sw_20mhz(uint8_t cycles);
extern void dr_piezo_burst_sw_17mhz(uint8_t cycles);
/* ---- Global variable definitions (extern in header) ---- */
dr_platform_if_t g_plat = { 0, 0, 0 };
bool g_log_enable = false;
/* ---- Internal constants/structures ---- */
#define DR_MAX_DATA 128 /* Max data length after TAG */
typedef struct {
char tag[5]; /* "sta?" etc 4 chars + '\0' */
uint8_t data[DR_MAX_DATA]; /* Raw data after TAG */
uint8_t data_len; /* Length of data[] */
} ParsedCmd;
/* ---- Internal utility functions ---- */
/* Copy TAG */
static void dr_copy_tag(const uint8_t *buf, char *tag_out)
{
tag_out[0] = (char)buf[0];
tag_out[1] = (char)buf[1];
tag_out[2] = (char)buf[2];
tag_out[3] = (char)buf[3];
tag_out[4] = '\0';
}
/* TAG comparison (4 chars) */
static bool dr_tag_eq(const char *tag, const char *key4)
{
return (tag[0] == key4[0] &&
tag[1] == key4[1] &&
tag[2] == key4[2] &&
tag[3] == key4[3]);
}
/* Extract uint16 little endian: word_index based (0 -> data[0], data[1]) */
/* Extract uint16 BIG endian: word_index based (0 -> data[0], data[1]) */
/* Extract uint16 LITTLE endian: word_index based (0 -> data[0], data[1]) */
static bool dr_get_u16(const ParsedCmd *cmd, uint8_t word_index, uint16_t *out)
{
uint8_t pos = (uint8_t)(word_index * 2);
if (cmd->data_len < (uint8_t)(pos + 2)) {
return false;
}
// Little Endian: data[pos] = low byte, data[pos+1] = high byte
*out = (uint16_t)cmd->data[pos]
| (uint16_t)((uint16_t)cmd->data[pos + 1] << 8);
return true;
}
/* Extract ASCII: data[offset..offset+len] -> out, '\0' terminated */
/* EEPROM에서 텍스트 쓸 때 ASCII 문자열 추출 -> 추후 Flash Memory 커맨드에서 재활용 가능 */
static void dr_get_ascii(const ParsedCmd *cmd, uint8_t offset,
char *out, uint8_t max_len)
{
uint8_t i;
uint8_t remain;
if (offset >= cmd->data_len) {
out[0] = '\0';
return;
}
remain = (uint8_t)(cmd->data_len - offset);
if (remain > max_len) {
remain = max_len;
}
for (i = 0; i < remain; i++) {
out[i] = (char)cmd->data[offset + i];
}
out[remain] = '\0';
}
/* ---- CRC16 functions ---- */
/* CRC16 computation - matches Nordic SDK crc16_compute */
static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size, const uint16_t *p_crc)
{
uint32_t i;
uint16_t crc = (p_crc == NULL) ? 0xFFFF : *p_crc;
for (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;
}
/* CRC16 check: compare computed vs expected */
static bool dr_crc16_check(const uint8_t *p_data, uint32_t data_len, uint16_t expected_crc)
{
uint16_t computed_crc = dr_crc16_compute(p_data, data_len, NULL);
return (computed_crc == expected_crc);
}
/* CRC16 packet check: last 2 bytes are CRC (little endian) */
static bool dr_crc16_check_packet(const uint8_t *packet, uint32_t packet_len)
{
uint16_t expected_crc;
uint32_t data_len;
if (packet_len < 2) {
return false;
}
data_len = packet_len - 2;
/* Extract CRC: little endian (low byte first) */
expected_crc = (uint16_t)packet[packet_len - 2]
| ((uint16_t)packet[packet_len - 1] << 8);
if (g_plat.log && g_log_enable) {
g_plat.log("CRC check: expected=0x%04X\n", expected_crc);
}
return dr_crc16_check(packet, data_len, expected_crc);
}
/* ---- Raw buffer -> ParsedCmd ---- */
static bool dr_parse_cmd(const uint8_t *buffer, uint8_t length, ParsedCmd *out)
{
uint8_t i;
if (length < 4) {
return false; /* Not even TAG received */
}
/* CRC check if enabled */
if (g_plat.crc_check) {
if (!dr_crc16_check_packet(buffer, length)) {
if (g_plat.log && g_log_enable) {
g_plat.log("CRC check FAILED!\n");
}
return false;
}
/* CRC validated - remove CRC bytes from data */
length = (uint8_t)(length - 2);
if (g_plat.log && g_log_enable) {
g_plat.log("CRC check OK\n");
}
}
dr_copy_tag(buffer, out->tag);
out->data_len = (length > 4) ? (uint8_t)(length - 4) : 0;
if (out->data_len > DR_MAX_DATA) {
out->data_len = DR_MAX_DATA;
}
for (i = 0; i < out->data_len; i++) {
out->data[i] = buffer[4 + i];
}
if (g_plat.log && g_log_enable) {
g_plat.log("parse_cmd: TAG='%s', data_len=%u\n",
out->tag, out->data_len);
}
return true;
}
/* ---- Handler prototypes (Harbour style: int return) ---- */
/* A. Device Status */
static int Cmd_mta(const ParsedCmd *cmd);
static int Cmd_sta(const ParsedCmd *cmd);
static int Cmd_str(const ParsedCmd *cmd);
/* F. PD-ADC M48 Full Measurement Series */
static int Cmd_mcj(const ParsedCmd *cmd);
static int Cmd_sej(const ParsedCmd *cmd);
static int Cmd_ssj(const ParsedCmd *cmd);
/* I. Sensor Measurements */
static int Cmd_msn(const ParsedCmd *cmd);
static int Cmd_spn(const ParsedCmd *cmd);
static int Cmd_sso(const ParsedCmd *cmd);
static int Cmd_ssp(const ParsedCmd *cmd);
/* J. Power / Reset / Version / Security */
static int Cmd_ssq(const ParsedCmd *cmd);
static int Cmd_ssr(const ParsedCmd *cmd);
static int Cmd_sss(const ParsedCmd *cmd);
static int Cmd_sst(const ParsedCmd *cmd);
static int Cmd_mfv(const ParsedCmd *cmd);
/* ------------------ Piezo ------------------ */
static int Cmd_msp(const ParsedCmd *cmd); /* IMU 6-axis raw data (single shot) */
static int Cmd_mpa(const ParsedCmd *cmd); /* Piezo TX/RX Activate */
static int Cmd_mpb(const ParsedCmd *cmd); /* Piezo TX/RX Deactivate */
static int Cmd_mpc(const ParsedCmd *cmd); /* Piezo Burst Capture */
static int Cmd_mdc(const ParsedCmd *cmd); /* Piezo ADC Capture */
static int Cmd_mec(const ParsedCmd *cmd); /* Piezo Burst + ADC capture */
static int Cmd_maa(const ParsedCmd *cmd); /* Piezo Burst + ADC all channel capture */
static int Cmd_cmd(const ParsedCmd *cmd); /* Pin Test (High / Low) */
/* ------------------ FDS ------------------ */
static int Cmd_mwh(const ParsedCmd *cmd); /* Write HW Number to FDS */
static int Cmd_mws(const ParsedCmd *cmd); /* Write Serial Number to FDS */
static int Cmd_mrh(const ParsedCmd *cmd); /* Read HW Number from FDS */
static int Cmd_mrs(const ParsedCmd *cmd); /* Read Serial Number from FDS */
static int Cmd_mpz(const ParsedCmd *cmd); /* Write Passkey to FDS */
static int Cmd_mqz(const ParsedCmd *cmd); /* Read Passkey from FDS */
static int Cmd_mxz(const ParsedCmd *cmd); /* Write Life Cycle to FDS */
static int Cmd_myz(const ParsedCmd *cmd); /* Read Life Cycle from FDS */
/* ---- Command Table ---- */
typedef struct {
char tag[5]; /* "sta?" */
bool enabled; /* false = handler won't be called */
int (*handler)(const ParsedCmd *cmd); /* 1=success, 0=fail */
} CmdEntry;
static CmdEntry g_cmd_table[] = {
/* sudo command */
{ "cmd?", true, Cmd_cmd }, // Piezo Activate
/* Piezo command */
{ "mpa?", true, Cmd_mpa }, // Piezo Activate
{ "mpb?", true, Cmd_mpb }, // Piezo Deactivate 26.03.13
{ "mpc?", true, Cmd_mpc }, // Piezo Cycles control command, 3,4,5,6,7
{ "mdc?", true, Cmd_mdc }, // Piezo burst + Echo capture (12-bit packed)
{ "mec?", true, Cmd_mec }, // Piezo burst + Echo capture (16-bit raw)
{ "maa?", true, Cmd_maa }, // 8-channel all capture (mode: 0=raw, 1=delta)
{ "msp?", true, Cmd_msp }, // IMU 6-axis raw data (single shot)
/* Config: HW/Serial Number (FDS) */
{ "mwh?", true, Cmd_mwh },
{ "mws?", true, Cmd_mws },
{ "mrh?", true, Cmd_mrh },
{ "mrs?", true, Cmd_mrs },
/* Config: Passkey / Life Cycle (FDS) */
{ "mpz?", true, Cmd_mpz }, /* 패스키 쓰기 (spz? → mpz?) */
{ "mqz?", true, Cmd_mqz }, /* 패스키 읽기 (sqz? → mqz?) */
{ "mxz?", true, Cmd_mxz }, /* life_cycle 쓰기 (sxz? → mxz?) */
{ "myz?", true, Cmd_myz }, /* life_cycle 읽기 (syz? → myz?) */
/* A. Device Status */
{ "mta?", true, Cmd_mta },
{ "sta?", true, Cmd_sta },
{ "str?", false, Cmd_str },
/* F. PD-ADC M48 Full Measurement Series */
{ "mcj?", true, Cmd_mcj },
{ "scj?", true, Cmd_mcj },
{ "sej?", true, Cmd_sej },
{ "ssj?", false, Cmd_ssj },
/* I. Sensor Measurements */
{ "msn?", true, Cmd_msn },
{ "ssn?", true, Cmd_msn }, // snn compatible command for battery check
{ "spn?", false, Cmd_spn },
{ "sso?", false, Cmd_sso },
{ "ssp?", true, Cmd_ssp },
/* J. Power / Reset / Version / Security */
{ "ssq?", false, Cmd_ssq },
{ "ssr?", false, Cmd_ssr },
{ "sss?", false, Cmd_sss },
{ "sst?", false, Cmd_sst },
{ "mfv?", true, Cmd_mfv }, /* Firmware Version Read (ssv -> mfv) 26.03.13 */
};
static const uint16_t g_cmd_count =
(uint16_t)(sizeof(g_cmd_table) / sizeof(g_cmd_table[0]));
/* ---- Command dispatcher ---- */
static int dr_cmd_dispatch(const ParsedCmd *cmd)
{
uint16_t i;
char tag_lower[5];
/* tag command convert to lower case */
for (i = 0; i < 4 && cmd->tag[i]; i++) {
tag_lower[i] = (cmd->tag[i] >= 'A' && cmd->tag[i] <= 'Z')
? (cmd->tag[i] + 32) : cmd->tag[i];
}
tag_lower[i] = '\0';
for (i = 0; i < g_cmd_count; i++) {
if (dr_tag_eq(tag_lower, g_cmd_table[i].tag)) {
if (!g_cmd_table[i].enabled) {
if (g_plat.log && g_log_enable) {
g_plat.log("Command '%s' disabled\n", cmd->tag);
}
return 0;
}
if (g_plat.log && g_log_enable) {
g_plat.log("Run handler '%s'\n", cmd->tag);
}
return g_cmd_table[i].handler(cmd);
}
}
if (g_plat.log && g_log_enable) {
g_plat.log("Unknown TAG '%s'\n", cmd->tag);
}
return 0;
}
/*
Main Parser called from external code
*/
int dr_cmd_parser(const uint8_t *buf, uint8_t len)
{
ParsedCmd cmd;
if (g_plat.log) g_plat.log("[PARSER] in len=%u crc=%u\r\n", len, g_plat.crc_check);
if (!dr_parse_cmd(buf, len, &cmd)) {
if (g_plat.log) g_plat.log("[PARSER] PARSE FAIL\r\n");
/* CRC 실패 시 에러 응답 전송 */
if (g_plat.crc_check && g_plat.tx_bin) {
single_format_data(ble_bin_buffer, "crc!", 65530);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
return -1; /* CRC 실패 또는 파싱 실패 → 음수로 old parser에 위임 */
}
if (g_plat.log) g_plat.log("[PARSER] tag=%s\r\n", cmd.tag);
return dr_cmd_dispatch(&cmd);
}
/* ---- Each Command Handler implementation (Stub) ---- */
/* In actual firmware, replace TODO sections with hardware/EEPROM integration */
/* A. Device Status */
static int Cmd_mta(const ParsedCmd *cmd)
{
uint16_t mode = 0;
// Reset count (sta? replacement )
resetCount = 0;
// Extract mode
(void)dr_get_u16(cmd, 0, &mode);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mta] mode=%u\r\n", mode);
}
// Process mode ( mta?1 )
if (mode == 1) {
if (device_activated() == 0) {
device_status = true;
}
}
else if (mode == 0) { // mta?0
if (device_status == true) {
if (device_sleep_mode() == 0) {
device_status = false;
}
}
}
if (g_plat.tx_bin) {
single_format_data(ble_bin_buffer, "rta:", mode);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
return 1;
}
/* ---- Each Command Handler implementation (Stub) ---- */
/* In actual firmware, replace TODO sections with hardware/EEPROM integration */
/* A. Device Status */
static int Cmd_sta(const ParsedCmd *cmd)
{
uint16_t mode = 0;
// Reset count (sta? replacement )
resetCount = 0;
// Extract mode
(void)dr_get_u16(cmd, 0, &mode);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mta] mode=%u\r\n", mode);
}
// Process mode ( mta?1 )
if (mode == 1) {
if (device_activated() == 0) {
device_status = true;
}
}
else if (mode == 0) { // mta?0
if (device_status == true) {
if (device_sleep_mode() == 0) {
device_status = false;
}
}
}
if (g_plat.tx_bin) {
single_format_data(ble_bin_buffer, "sta:", mode);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
return 1;
}
static int Cmd_str(const ParsedCmd *cmd)
{
(void)cmd;
/* TODO: read actual device_status */
uint8_t status = 1;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_str] read status=%u\n", status);
}
if (g_plat.tx_bin) {
uint8_t resp[4] = { 'r','t','r', status };
g_plat.tx_bin(resp, 4);
}
return 1;
}
/**
* @brief PD-ADC M48 Full Measurement - MODE 2 (scj? ? mcj?)
*
* Original: scj?
* New: mcj?
* Response: rcj: (from m48 measurement callback)
*
* MODE 2: Pressure + M48 Full Measurement
* - Pressure sensor measurement (pressure1 + pressure2)
* - 48 LED-PD ADC measurement
* - Battery, Temperature, IMU data
*
* Preconditions:
* - Device must be activated (device_status == true)
* - Not currently processing
*/
static int Cmd_mcj(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mcj] PD-ADC M48 MODE=2 (Press + M48)\r\n");
}
/* Check device activation status */
if (device_status != true) {
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mcj] ERROR: Device not activated\r\n");
}
if (g_plat.tx_bin) {
param_error("mcj?");
}
return 1;
}
info4 = true;
ble_got_new_data = false;
processing = true;
/* Start pressure measurement */
pressure_all_level_meas();
battery_timer_stop();
/* Enable battery, temperature, IMU measurement */
go_batt = true;
motion_data_once = true;
/* Start main timer */
main_timer_start();
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mcj] Measurement started\r\n");
}
return 1;
}
static int Cmd_sej(const ParsedCmd *cmd)
{
(void)cmd;
/* NIRS/optical PD-ADC M48 mode removed (VesiScan-Basic) */
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_sej] Disabled (NIRS removed)\r\n");
}
return 0;
}
static int Cmd_ssj(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_ssj] MODE=0 (M48 + batt/IMU combined)\n");
}
return 1;
}
/* I. Sensor Measurements */
static int Cmd_msn(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_msn] Measure battery level\n");
}
battery_level_meas();
return 1;
}
static int Cmd_spn(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_spn] Measure pressure1 & 2\n");
}
return 1;
}
static int Cmd_sso(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_sso] Measure LED temperature\n");
}
return 1;
}
static int Cmd_ssp(const ParsedCmd *cmd)
{
hw_i2c_init_once();
motion_raw_data_enabled = true;
ble_got_new_data = false;
/* 'c' = continuous, otherwise single shot */
if (cmd->data_len > 0 && (char)cmd->data[0] == 'c') {
motion_data_once = false;
} else {
motion_data_once = true;
}
main_timer_start();
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_ssp] Motion sensor raw, once=%u\r\n", motion_data_once);
}
return 1;
}
/* J. Power / Reset / Version / Security */
/* ssq? - Device Power Off */
static int Cmd_ssq(const ParsedCmd *cmd)
{
uint16_t val = 0;
dr_get_u16(cmd, 0, &val);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_ssq] Power off\r\n");
}
single_format_data(ble_bin_buffer, "rsq:", val);
dr_binary_tx_safe(ble_bin_buffer, 2);
go_device_power_off = true;
main_timer_start();
return 1;
}
/* ssr? - Bond Info Delete + System Reset */
static int Cmd_ssr(const ParsedCmd *cmd)
{
uint16_t val = 0;
dr_get_u16(cmd, 0, &val);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_ssr] Bond delete + reset\r\n");
}
single_format_data(ble_bin_buffer, "rsr:", val);
dr_binary_tx_safe(ble_bin_buffer, 2);
bond_data_delete = true;
m_config.bond_data_delete = (uint8_t)bond_data_delete;
m_reset_status = 2;
m_config.reset_status = m_reset_status;
config_save();
nrf_delay_ms(5);
go_NVIC_SystemReset = true;
main_timer_start();
return 1;
}
/* sss? - Device Reset (Reboot) */
static int Cmd_sss(const ParsedCmd *cmd)
{
uint16_t val = 0;
dr_get_u16(cmd, 0, &val);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_sss] Device reset\r\n");
}
single_format_data(ble_bin_buffer, "rss:", val);
dr_binary_tx_safe(ble_bin_buffer, 2);
m_reset_status = 2;
m_config.reset_status = m_reset_status;
config_save();
nrf_delay_ms(5);
go_NVIC_SystemReset = true;
main_timer_start();
return 1;
}
/* sst? - Security Ready */
static int Cmd_sst(const ParsedCmd *cmd)
{
uint16_t val = 0;
dr_get_u16(cmd, 0, &val);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_sst] Ready\r\n");
}
single_format_data(ble_bin_buffer, "rst:", val);
dr_binary_tx_safe(ble_bin_buffer, 2);
return 1;
}
static int Cmd_mfv(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mfv] FW=%s\r\n", DR_DEVICE_VERSION);
}
ascii_format_data(ble_bin_buffer, "rfv:", DR_DEVICE_VERSION, 12);
dr_binary_tx_safe(ble_bin_buffer, 8);
return 1;
}
static int Cmd_mpa(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mpa] Piezo Activation\n");
}
dr_piezo_power_on();
dr_piezo_system_init();
if (g_plat.tx_bin) {
single_format_data(ble_bin_buffer, "rpa:", 1);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
return 1;
}
/* mpb? - Piezo TX/RX Deactivate (Power Off) */
static int Cmd_mpb(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mpb] Piezo Deactivation\n");
}
dr_piezo_power_off();
if (g_plat.tx_bin) {
single_format_data(ble_bin_buffer, "rpb:", 1);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
return 1;
}
/**
* @brief Piezo burst command with frequency option
* @param cmd->data[0-1]: cycles (3~9), default=5
* @param cmd->data[2-3]: freq_option (0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz), default=1
* @return Response: rpc:cycles
*
* Usage from Harbour:
* mpc?[cycles][freq] where freq: 0=1.8MHz, 1=2.1MHz (default), 2=2.0MHz, 3=1.7MHz, 4=2.2MHz
* Example: mpc?0x05,0x00,0x01,0x00 -> 5 cycles at 2.1MHz
* Example: mpc?0x05,0x00,0x00,0x00 -> 5 cycles at 1.8MHz
* Example: mpc?0x05,0x00,0x02,0x00 -> 5 cycles at 2.0MHz
* Example: mpc?0x05,0x00,0x03,0x00 -> 5 cycles at 1.7MHz
* Example: mpc?0x05,0x00,0x04,0x00 -> 5 cycles at 2.2MHz
*/
static int Cmd_mpc(const ParsedCmd *cmd)
{
uint16_t cycles = 5; /* default */
uint16_t freq_option = 1; /* 0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz */
uint16_t piezo_ch = 0; /* Piezo channel 0~7, default 0 */
/* Extract cycles (word 0) */
(void)dr_get_u16(cmd, 0, &cycles);
/* Extract frequency option (word 1) */
(void)dr_get_u16(cmd, 1, &freq_option);
/* Extract piezo channel (word 2) */
(void)dr_get_u16(cmd, 2, &piezo_ch);
/* Validate piezo channel: 0~7 jhChun 26.01.29 */
if (piezo_ch > 7) piezo_ch = 0;
if (g_plat.log && g_log_enable) {
const char *freq_str = (freq_option == 0) ? "1.8MHz" :
(freq_option == 1) ? "2.1MHz" :
(freq_option == 2) ? "2.0MHz" :
(freq_option == 3) ? "1.7MHz" :
(freq_option == 4) ? "2.2MHz" :
(freq_option == 9) ? "1.9MHz" : "unknown";
g_plat.log("[Cmd_mpc] cycles=%u, freq=%u (%s), piezo=%u\r\n",
cycles, freq_option, freq_str, piezo_ch);
}
/* Range check: 3~9 */
if (cycles < 3 || cycles > 9) {
dr_ble_return_1("rpc:", 2); /* Error: out of range */
return 1;
}
/* Select piezo channel */
dr_piezo_select_channel((uint8_t)piezo_ch);
/* Execute burst based on frequency option */
switch (freq_option) {
case 0:
dr_piezo_burst_sw_18mhz((uint8_t)cycles);
break;
case 2:
dr_piezo_burst_sw_20mhz((uint8_t)cycles);
break;
case 3:
dr_piezo_burst_sw_17mhz((uint8_t)cycles);
break;
case 4:
dr_piezo_burst_sw_22mhz((uint8_t)cycles);
break;
/*case 9:
dr_piezo_burst_sw_19mhz((uint8_t)cycles);
break;*/
case 1:
default:
dr_piezo_burst_sw((uint8_t)cycles); /* 2.1MHz */
break;
}
/* Response */
dr_ble_return_1("rpc:", (uint8_t)cycles);
return 1;
}
/**
* @brief Piezo burst + Echo capture command
* @param cmd->data[0]: cycles (3~9), default=5
* @param cmd->data[1-2]: delay_us (0~65535), default=1000
* @param cmd->data[3-4]: num_samples (1~8192), default=4096
* @return Multiple packets with 12-bit packed data
*
* Response format (multi-packet, 12-bit packed):
*
* First packet ("rdc:"):
* [0-3]: "rdc:"
* [4-5]: peak_raw (uint16_t, little-endian)
* [6-7]: peak_index (uint16_t)
* [8-9]: baseline_raw (uint16_t)
* [10-11]: num_samples (uint16_t)
* [12-13]: total_packets (uint16_t)
* [14...]: 12-bit packed ADC data (first chunk)
*
* Continuation packets ("rdd:"):
* [0-3]: "rdd:"
* [4-5]: packet_index (uint16_t, 1-based)
* [6...]: 12-bit packed ADC data (continuation)
*
* Last packet marker ("rde:"):
* [0-3]: "rde:"
* [4-5]: total_bytes_sent (uint16_t)
*
* 12-bit packing: 2 samples (24 bits) -> 3 bytes
* Byte0 = sample0[7:0]
* Byte1 = sample1[3:0] << 4 | sample0[11:8]
* Byte2 = sample1[11:4]
*/
#define MDC_BLE_MTU_SIZE 240 /* Safe BLE packet size */
#define MDC_FIRST_HEADER_LEN 14 /* "rdc:" + header */
#define MDC_CONT_HEADER_LEN 6 /* "rdd:" + packet_index */
#define MDC_FIRST_DATA_LEN (MDC_BLE_MTU_SIZE - MDC_FIRST_HEADER_LEN)
#define MDC_CONT_DATA_LEN (MDC_BLE_MTU_SIZE - MDC_CONT_HEADER_LEN)
/**
* @brief Piezo burst + Echo capture command (12-bit packed)
*
* PROTOCOL v3: Small header packet + separate data packets with delays
* This avoids large packet loss in BLE stack.
*
* Response format:
* Packet 1 (rdb:): header only (14 bytes)
* Packet 2~N (rdd:): pkt_idx(2) + packed_data (up to 234 bytes)
* Final (rde:): total_packets(2)
*/
static int Cmd_mdc(const ParsedCmd *cmd)
{
uint16_t cycles = 5;
uint16_t delay_us = 1000;
uint16_t num_samples = 4096;
/* Extract parameters */
(void)dr_get_u16(cmd, 0, &cycles);
(void)dr_get_u16(cmd, 1, &delay_us);
(void)dr_get_u16(cmd, 2, &num_samples);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mdc] cycles=%u, delay=%uus, samples=%u\r\n",
cycles, delay_us, num_samples);
}
/* Range check */
if (cycles < 3) cycles = 3;
if (cycles > 9) cycles = 9;
if (num_samples > 8192) num_samples = 8192;
if (num_samples < 1) num_samples = 1;
/* Execute burst + echo capture */
dr_adc_echo_t echo;
dr_adc_err_t err = dr_adc_burst_and_capture(
(uint8_t)cycles, delay_us, num_samples, &echo);
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mdc] err=%d, peak=%u@%u, base=%u, n=%u\r\n",
err, echo.peak_raw, echo.peak_index, echo.baseline_raw, echo.num_samples);
}
if (g_plat.tx_bin) {
const uint16_t *raw_buffer = dr_adc_get_echo_buffer();
/* Calculate packed data size: 2 samples -> 3 bytes */
uint16_t packed_data_bytes = ((echo.num_samples + 1) / 2) * 3;
uint16_t data_per_packet = MDC_BLE_MTU_SIZE - MDC_CONT_HEADER_LEN; /* 234 bytes */
uint16_t data_packets = (packed_data_bytes + data_per_packet - 1) / data_per_packet;
if (data_packets == 0) data_packets = 1;
uint16_t total_packets = 1 + data_packets; /* header + data packets */
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mdc] packed=%u bytes, packets=%u\r\n", packed_data_bytes, total_packets);
}
/* Packet 1: rdb: header only (14 bytes) */
ble_bin_buffer[0] = 'r';
ble_bin_buffer[1] = 'd';
ble_bin_buffer[2] = 'b';
ble_bin_buffer[3] = ':';
ble_bin_buffer[4] = (uint8_t)(total_packets & 0xFF);
ble_bin_buffer[5] = (uint8_t)(total_packets >> 8);
ble_bin_buffer[6] = (uint8_t)(echo.peak_raw & 0xFF);
ble_bin_buffer[7] = (uint8_t)(echo.peak_raw >> 8);
ble_bin_buffer[8] = (uint8_t)(echo.peak_index & 0xFF);
ble_bin_buffer[9] = (uint8_t)(echo.peak_index >> 8);
ble_bin_buffer[10] = (uint8_t)(echo.baseline_raw & 0xFF);
ble_bin_buffer[11] = (uint8_t)(echo.baseline_raw >> 8);
ble_bin_buffer[12] = (uint8_t)(echo.num_samples & 0xFF);
ble_bin_buffer[13] = (uint8_t)(echo.num_samples >> 8);
dr_binary_tx_safe(ble_bin_buffer, 7); /* 14 bytes = 7 words */
nrf_delay_ms(100); /* Wait for BLE stack */
/* Packet 2~N: rdd: data packets with packed 12-bit samples */
uint16_t src_idx = 0;
for (uint16_t pkt = 0; pkt < data_packets; pkt++) {
ble_bin_buffer[0] = 'r';
ble_bin_buffer[1] = 'd';
ble_bin_buffer[2] = 'd';
ble_bin_buffer[3] = ':';
ble_bin_buffer[4] = (uint8_t)(pkt & 0xFF);
ble_bin_buffer[5] = (uint8_t)(pkt >> 8);
uint16_t dst_idx = 6;
uint16_t bytes_this_pkt = 0;
while (src_idx < echo.num_samples && bytes_this_pkt < data_per_packet) {
uint16_t s0 = raw_buffer[src_idx++] & 0x0FFF;
uint16_t s1 = (src_idx < echo.num_samples) ? (raw_buffer[src_idx++] & 0x0FFF) : 0;
ble_bin_buffer[dst_idx++] = (uint8_t)(s0 & 0xFF);
ble_bin_buffer[dst_idx++] = (uint8_t)(((s1 & 0x0F) << 4) | ((s0 >> 8) & 0x0F));
ble_bin_buffer[dst_idx++] = (uint8_t)(s1 >> 4);
bytes_this_pkt += 3;
}
/* Ensure even byte count for word alignment */
if (dst_idx & 1) ble_bin_buffer[dst_idx++] = 0;
dr_binary_tx_safe(ble_bin_buffer, dst_idx / 2); /* bytes to words */
nrf_delay_ms(100); /* Inter-packet delay */
}
/* Final packet: rde: end marker */
ble_bin_buffer[0] = 'r';
ble_bin_buffer[1] = 'd';
ble_bin_buffer[2] = 'e';
ble_bin_buffer[3] = ':';
ble_bin_buffer[4] = (uint8_t)(total_packets & 0xFF);
ble_bin_buffer[5] = (uint8_t)(total_packets >> 8);
dr_binary_tx_safe(ble_bin_buffer, 3); /* 6 bytes = 3 words */
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mdc] sent rdb+rdd*%u+rde (%u samples)\r\n",
total_packets, echo.num_samples);
}
}
return 1;
}
/*
// The following code demonstrates an example of how to receive data from a client
// and echo the same data back to the client.
static int Cmd_cmd(const ParsedCmd *cmd)
{
uint16_t v1, v2, v3;
if (cmd->data_len < 6) {
dr_ble_return_1("rmd:", 0);
return 1;
}
// Little Endian ?? (PC? LE? ??)
v1 = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8);
v2 = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8);
v3 = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8);
dr_ble_return_3("rmd:", v1, v2, v3);
return 1;
}
*/
/**
* @brief Piezo burst + Echo capture with 16-bit raw data (no compression)
* @param cmd->data[0]: cycles (3~9), default=5
* @param cmd->data[1-2]: delay_us (0~65535), default=100
* @param cmd->data[3-4]: num_samples (1~200), default=140 (20cm target)
* @return Multiple packets with 16-bit raw data
*
* Response format (multi-packet, 16-bit raw - no compression):
*
* First packet ("rec:"):
* [0-3]: "rec:"
* [4-5]: peak_raw (uint16_t, little-endian)
* [6-7]: peak_index (uint16_t)
* [8-9]: baseline_raw (uint16_t)
* [10-11]: num_samples (uint16_t)
* [12-13]: total_packets (uint16_t)
* [14...]: 16-bit raw ADC data (first chunk, little-endian)
*
* Continuation packets ("red:"):
* [0-3]: "red:"
* [4-5]: packet_index (uint16_t, 1-based)
* [6...]: 16-bit raw ADC data (continuation)
*
* Last packet marker ("ree:"):
* [0-3]: "ree:"
* [4-5]: total_bytes_sent (uint16_t)
*
* 16-bit raw format: Each sample = 2 bytes (uint16, little-endian)
* Byte0 = Sample[7:0]
* Byte1 = Sample[15:8]
*
* Example: 140 samples (20cm) = 280 bytes = 2 packets
*/
static int Cmd_mec(const ParsedCmd *cmd)
{
uint16_t freq_option = 0; /* 0=1.8MHz (default), 1=2.1MHz, 2=2.0MHz, 3=1.7MHz */
uint16_t delay_us = 20; /* Default 20us */
uint16_t num_samples = 140; /* Default for 20cm target */
uint16_t cycles = 5; /* Default 5 cycles (valid: 3~7) */
uint16_t averaging = 1; /* Default 1 (no averaging), max 1000 */
uint16_t piezo_ch = 0; /* Default piezo channel 0 (valid: 0~7) */
/* Extract parameters: mec [freq_option] [delay_us] [num_samples] [cycles] [averaging] [piezo_ch] */
(void)dr_get_u16(cmd, 0, &freq_option);
(void)dr_get_u16(cmd, 1, &delay_us);
(void)dr_get_u16(cmd, 2, &num_samples);
(void)dr_get_u16(cmd, 3, &cycles);
(void)dr_get_u16(cmd, 4, &averaging);
(void)dr_get_u16(cmd, 5, &piezo_ch);
/* Validate averaging: 1~1000, default 1 */
if (averaging == 0) averaging = 1;
if (averaging > 1000) averaging = 1000;
/* Validate piezo channel: 0~7 jhChun 26.01.29 */
if (piezo_ch > 7) piezo_ch = 0;
if (g_plat.log && g_log_enable) {
const char *freq_str = (freq_option == 0) ? "1.8MHz" :
(freq_option == 1) ? "2.1MHz" :
(freq_option == 2) ? "2.0MHz" :
(freq_option == 3) ? "1.7MHz" :
(freq_option == 4) ? "2.2MHz" :
(freq_option == 9) ? "1.9MHz" : "unknown";
g_plat.log("[Cmd_mec] freq=%u (%s), delay=%uus, samples=%u, cycles=%u, avg=%u, piezo=%u\r\n",
freq_option, freq_str, delay_us, num_samples, cycles, averaging, piezo_ch);
}
/* Use integrated burst + capture + transmit function
* This function handles:
* 1. ADC power on
* 2. Select piezo channel (0~7)
* 3. Piezo burst (frequency based on freq_option, cycles from param)
* 4. ADC capture (after delay_us) - repeated 'averaging' times
* 5. Average the captured samples
* 6. BLE transmission with proper packet timing
*/
dr_adc_err_t err = dr_adc_burst_capture_transmit(
(uint8_t)freq_option, delay_us, num_samples, (uint8_t)cycles,
(uint16_t)averaging, (uint8_t)piezo_ch, ble_bin_buffer, 0); /* 0=send raa */
if (err != DR_ADC_OK) {
dr_ble_return_2("rer:", 0xEE00 | (uint16_t)err, num_samples);
}
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mec] result=%d\r\n", err);
}
return 1;
}
static int Cmd_cmd(const ParsedCmd *cmd)
{
uint16_t v1, v2, v3;
uint32_t pin_number;
if (cmd->data_len < 6) {
dr_ble_return_1("rmd:", 0);
return 1;
}
// Little Endian from PC to LE
v1 = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8); // port
v2 = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8); // pin
v3 = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8); // 1=HIGH, 0=LOW
// -- GPIO Test
// Pin No: NRF_GPIO_PIN_MAP(port, pin)
pin_number = NRF_GPIO_PIN_MAP(v1, v2);
// output
nrf_gpio_cfg_output(pin_number);
// HIGH or LOW
if (v3 == 1) {
nrf_gpio_pin_set(pin_number); // HIGH
} else {
nrf_gpio_pin_clear(pin_number); // LOW
}
// return : port, pin, state
dr_ble_return_3("rmd:", v1, v2, v3);
return 1;
}
/**
* @brief 4-Channel All Capture Command (maa?) - ASYNC VERSION
* @param cmd->data[0-1]: mode (0=raw 16-bit async)
* @return Immediately. Data sent asynchronously via BLE_NUS_EVT_TX_RDY
*
* ASYNC ARCHITECTURE:
* - maa_async_start() initiates capture and sends CH0 header
* - BLE_NUS_EVT_TX_RDY callback drives remaining transmission
* - No blocking - SoftDevice can process events normally
* - Prevents BLE TX buffer overflow that caused firmware brick
*
* Hardcoded parameters:
* freq_option = 0 (1.8MHz)
* delay_us = 10
* num_samples = 140
* cycles = 7
* averaging = 5
*
* Response format:
* For each channel (CH0~CH3):
* reb: [total_pkts(2)] [peak(2)] [idx(2)] [baseline(2)] [samples(2)]
* red: [pkt_idx(2)] [data...]
* Final:
* raa: [status(2)]
*
* Version marker: 0xA000 (vA) = async 8-channel
*/
#define MAA_FREQ_OPTION 0 /* 1.8MHz */
#define MAA_DELAY_US 10 /* 10us post-burst delay */
#define MAA_NUM_SAMPLES 140 /* 140 samples (~25cm) */
#define MAA_CYCLES 7 /* 7 cycles burst */
#define MAA_AVERAGING 5 /* 5x averaging */
static int Cmd_maa(const ParsedCmd *cmd)
{
uint16_t mode = 0;
dr_adc_err_t err;
/* Extract mode parameter */
(void)dr_get_u16(cmd, 0, &mode);
/* Mode validation - only mode 0 (async raw) supported */
if (mode > 0) {
dr_ble_return_1("raa:", 0xFFFF);
return 1;
}
/* Check if already busy */
if (maa_async_is_busy()) {
dr_ble_return_1("raa:", 0xFFFE); /* Busy */
return 1;
}
/*=======================================================================
* ASYNC 4-CHANNEL CAPTURE
* - maa_async_start() captures CH0 and sends first header
* - Subsequent packets sent via BLE_NUS_EVT_TX_RDY callback
* - No blocking delays - SoftDevice can process events normally
*=======================================================================*/
err = maa_async_start(
(uint8_t)MAA_FREQ_OPTION,
MAA_DELAY_US,
MAA_NUM_SAMPLES,
(uint8_t)MAA_CYCLES,
(uint16_t)MAA_AVERAGING,
ble_bin_buffer
);
if (err != DR_ADC_OK) {
/* Start failed - error already sent by maa_async_start */
return 1;
}
/* Return immediately - async transmission in progress */
/* raa: will be sent by state machine when complete */
return 1;
}
/*==============================================================================
* CONFIG: HW/Serial Number (FDS)
*============================================================================*/
/* mwh? - Write HW Number to FDS
* Data: 12 bytes ASCII HW number
*/
static int Cmd_mwh(const ParsedCmd *cmd)
{
char buf[13];
if (cmd->data_len < 12) {
dr_ble_return_1("rwh:", 0xFFFF); /* Error: insufficient data */
return 1;
}
dr_get_ascii(cmd, 0, buf, 12);
memcpy(HW_NO, buf, 12);
memcpy(m_config.hw_no, buf, 12);
config_save();
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mwh] HW=%.12s saved to FDS\r\n", m_config.hw_no);
}
ascii_format_data(ble_bin_buffer, "rwh:", buf, 12);
dr_binary_tx_safe(ble_bin_buffer, 8);
return 1;
}
/* mws? - Write Serial Number to FDS
* Data: 12 bytes ASCII serial number
*/
static int Cmd_mws(const ParsedCmd *cmd)
{
char buf[13];
if (cmd->data_len < 12) {
dr_ble_return_1("rws:", 0xFFFF); /* Error: insufficient data */
return 1;
}
dr_get_ascii(cmd, 0, buf, 12);
memcpy(SERIAL_NO, buf, 12);
memcpy(m_config.serial_no, buf, 12);
config_save();
if (g_plat.log && g_log_enable) {
g_plat.log("[Cmd_mws] S/N=%.12s saved to FDS\r\n", m_config.serial_no);
}
ascii_format_data(ble_bin_buffer, "rws:", buf, 12);
dr_binary_tx_safe(ble_bin_buffer, 8);
return 1;
}
/* mrh? - Read HW Number from FDS */
static int Cmd_mrh(const ParsedCmd *cmd)
{
(void)cmd;
memcpy(HW_NO, m_config.hw_no, 12);
ascii_format_data(ble_bin_buffer, "rrh:", HW_NO, 12);
dr_binary_tx_safe(ble_bin_buffer, 8);
return 1;
}
/* mrs? - Read Serial Number from FDS */
static int Cmd_mrs(const ParsedCmd *cmd)
{
(void)cmd;
memcpy(SERIAL_NO, m_config.serial_no, 12);
ascii_format_data(ble_bin_buffer, "rrs:", SERIAL_NO, 12);
dr_binary_tx_safe(ble_bin_buffer, 8);
return 1;
}
/*==============================================================================
* Passkey / Life Cycle (FDS)
*============================================================================*/
/* mpz? - 패스키 쓰기 (spz? → mpz?)
* 패킷: mpz? + ASCII 6자리 패스키
* 응답: rpz: + ASCII 6바이트
*/
static int Cmd_mpz(const ParsedCmd *cmd)
{
char passkey[7] = {0};
dr_get_ascii(cmd, 0, passkey, 6);
memcpy(m_static_passkey, passkey, 6);
memcpy(m_config.static_passkey, m_static_passkey, 6);
config_save();
if (g_plat.log) g_plat.log("[mpz] Passkey saved: %.6s\r\n", m_static_passkey);
ascii_format_data(ble_bin_buffer, "rpz:", passkey, 6);
dr_binary_tx_safe(ble_bin_buffer, 5);
return 1;
}
/* mqz? - 패스키 읽기 (sqz? → mqz?)
* 패킷: mqz? (데이터 없음)
* 응답: rqz: + ASCII 6바이트
*/
static int Cmd_mqz(const ParsedCmd *cmd)
{
(void)cmd;
memcpy(m_static_passkey, m_config.static_passkey, 6);
ascii_format_data(ble_bin_buffer, "rqz:", m_static_passkey, 6);
dr_binary_tx_safe(ble_bin_buffer, 5);
return 1;
}
/* mxz? - life_cycle 쓰기 (sxz? → mxz?)
* 패킷: mxz? + uint16 x 2 (상위16비트, 하위16비트)
* 응답: rxz: + uint16 x 2
*/
static int Cmd_mxz(const ParsedCmd *cmd)
{
uint16_t hi = 0, lo = 0;
dr_get_u16(cmd, 0, &hi);
dr_get_u16(cmd, 1, &lo);
m_life_cycle = ((uint32_t)hi << 16) | (uint32_t)lo;
m_config.life_cycle = m_life_cycle;
config_save();
if (g_plat.log) g_plat.log("[mxz] life_cycle=%u saved\r\n", m_life_cycle);
uint16_t result[2] = { hi, lo };
format_data(ble_bin_buffer, "rxz:", result, 2);
dr_binary_tx_safe(ble_bin_buffer, 4);
return 1;
}
/* myz? - life_cycle 읽기 (syz? → myz?)
* 패킷: myz? (데이터 없음)
* 응답: ryz: + uint16 x 2
*/
static int Cmd_myz(const ParsedCmd *cmd)
{
(void)cmd;
m_life_cycle = m_config.life_cycle;
uint16_t result[2];
result[0] = (uint16_t)(m_life_cycle >> 16);
result[1] = (uint16_t)(m_life_cycle & 0xFFFF);
format_data(ble_bin_buffer, "ryz:", result, 2);
dr_binary_tx_safe(ble_bin_buffer, 4);
return 1;
}
/*==============================================================================
* IMU: 6-axis raw data (single shot)
*============================================================================*/
/* msp? - Read IMU accel(xyz) + gyro(xyz) raw data, single shot
* Response: rsp: + 6 x uint16_t (accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z)
*/
static int Cmd_msp(const ParsedCmd *cmd)
{
(void)cmd;
if (g_plat.log) g_plat.log("[MSP] enter\r\n");
hw_i2c_init_once();
/* Direct register read — no timer, no DRDY, no callback */
int rc = imu_read_direct();
if (g_plat.log) g_plat.log("[MSP] rc=%d\r\n", rc);
return 1;
}
Reference in New Issue View Git Blame Copy Permalink