/* * 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 #include "nrf_gpio.h" #include "nrf_delay.h" #include "dr_util.h" #include "imu_stub.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); /* AGC functions */ extern void full_agc_mesurement_start(void); extern bool is_power_up_done(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 binary_tx_handler(const uint8_t *buffer, uint16_t length); extern void dr_binary_tx_safe(const uint8_t *buffer, uint16_t word_count); 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]; // ======================================== // External variables // ======================================== extern volatile bool processing; extern bool device_status; extern uint8_t resetCount; extern uint8_t ble_bin_buffer[]; extern uint8_t simple_samples_in_buffer; extern uint8_t m_pd_adc_cnt; extern bool con_single; extern bool lock_check; extern bool info4; // addtional info extern bool ble_got_new_data; // BLE data flag extern uint8_t m_pd_adc_cnt; // PD ADC count 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 uint8_t ADC_PD_MODE; // PD ADC mode extern bool pd_adc_m48_start; // PD ADC M48 start flag extern uint8_t m48_samples_in_buffer; // M48 sample count 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 extern int imu_read_cached(void); // IMU cached memory read + BLE send extern volatile bool g_imu_active; // IMU active streaming flag extern void imu_active_timer_start(void); // start 1-sec amu: timer extern void imu_active_timer_stop(void); // stop 1-sec amu: timer /* 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) /* ---- Global variable definitions (extern in header) ---- */ dr_platform_if_t g_plat = { 0, 0, 0 }; bool g_log_enable = false; /* ---- 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); /* B. AGC / Gain Measurement */ static int Cmd_mag(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_ssv(const ParsedCmd *cmd); static int Cmd_msp(const ParsedCmd *cmd); /* IMU 6-axis raw data (single shot) */ static int Cmd_cmd(const ParsedCmd *cmd); 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_mrc(const ParsedCmd *cmd); /* Read Measurement Config from FDS */ static int Cmd_mwc(const ParsedCmd *cmd); /* Write Measurement Config to FDS */ static int Cmd_mas(const ParsedCmd *cmd); /* IMU Active Streaming Start */ static int Cmd_max(const ParsedCmd *cmd); /* IMU Active Streaming Stop */ /* ---- Command Table ---- */ static CmdEntry g_cmd_table[] = { /* sudo command */ { "cmd?", true, Cmd_cmd }, // Piezo Activate { "msp?", true, Cmd_msp }, // IMU 6-axis raw data (single shot) /* Config: HW/Serial Number (FDS) */ { "mwh?", true, Cmd_mwh }, // Write HW Number { "mws?", true, Cmd_mws }, // Write Serial Number { "mrh?", true, Cmd_mrh }, // Read HW Number { "mrs?", true, Cmd_mrs }, // Read Serial Number { "mrc?", true, Cmd_mrc }, // Read Measurement Config (freq_idx, cycles, avg_count) { "mwc?", true, Cmd_mwc }, // Write Measurement Config (freq_idx, cycles, avg_count) { "mas?", true, Cmd_mas }, // IMU Active Streaming Start (1-sec amu: timer) { "max?", true, Cmd_max }, // IMU Active Streaming Stop /* A. Device Status */ { "mta?", true, Cmd_mta }, { "sta?", true, Cmd_sta }, { "str?", false, Cmd_str }, /* B. AGC / Gain Measurement */ { "mag?", true, Cmd_mag }, { "sag?", true, Cmd_mag }, /* 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 }, { "ssv?", false, Cmd_ssv }, }; 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); binary_tx_handler(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); binary_tx_handler(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); binary_tx_handler(ble_bin_buffer, 3); } return 1; } /* B. AGC / Gain Measurement */ static int Cmd_mag(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_mag] Full AGC measurement request\r\n"); } if (device_status != true || !is_power_up_done()) { if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_mag] ERROR: Device not activated or power-up not done\r\n"); } if (g_plat.tx_bin) { param_error("mag?"); } return 1; } processing = true; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_mag] full_agc_mesurement_start()\r\n"); } full_agc_mesurement_start(); 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; ADC_PD_MODE = 4; info4 = true; ble_got_new_data = false; processing = true; AGC_GAIN_SW(false); m48_samples_in_buffer = m_pd_adc_cnt; pd_adc_m48_start = true; battery_timer_stop(); go_batt = true; main_timer_start(); if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_sej] MODE=4 (M48 + batt + IMU) started\r\n"); } return 1; } 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 */ static int Cmd_ssq(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_ssq] Power off\n"); } return 1; } static int Cmd_ssr(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_ssr] Bond delete\n"); } return 1; } static int Cmd_sss(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_sss] Device reset\n"); } return 1; } static int Cmd_sst(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_sst] Ready\n"); } return 1; } static int Cmd_ssv(const ParsedCmd *cmd) { (void)cmd; if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_ssv] Read firmware version\n"); } 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; } */ 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; } /*============================================================================== * 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); binary_tx_handler(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); binary_tx_handler(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); binary_tx_handler(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); binary_tx_handler(ble_bin_buffer, 8); return 1; } /*============================================================================== * Measurement Config: Read/Write (FDS) * * mrc? - Read measurement config * Response: rrc: freq_idx(uint16), cycles(uint16), avg_count(uint16) * freq_idx : Piezo frequency index * 0=2.1MHz, 1=1.8MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 5=1.9MHz * cycles : Piezo burst cycles (3~7: 3,4,5,6,7) * avg_count: mec measurement averaging count (1~20) * * mwc? - Write measurement config * Data: 6 bytes (3 x uint16_t LE) * data[0-1]: freq_idx (0~5, see above) * data[2-3]: cycles (3~10) * data[4-5]: avg_count (1~20) * Response: rwc: freq_idx(uint16), cycles(uint16), avg_count(uint16) *============================================================================*/ /* mrc? - Read Measurement Config from FDS */ static int Cmd_mrc(const ParsedCmd *cmd) { (void)cmd; /* Return current measurement config: freq_idx, cycles, avg_count */ dr_ble_return_3("rrc:", (uint16_t)meas_config_get_freq_idx(), (uint16_t)meas_config_get_cycles(), (uint16_t)meas_config_get_avg_count()); if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_mrc] freq=%u cycles=%u avg=%u\r\n", meas_config_get_freq_idx(), meas_config_get_cycles(), meas_config_get_avg_count()); } return 1; } /* mwc? - Write Measurement Config to FDS * Data: 6 bytes = 3 x uint16_t (LE) * [0-1] freq_idx : 0=2.1MHz, 1=1.8MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 5=1.9MHz * [2-3] cycles : Piezo burst cycles (3~7: 3,4,5,6,7) * [4-5] avg_count : mec averaging count (1~20) */ static int Cmd_mwc(const ParsedCmd *cmd) { uint16_t freq_idx, cycles, avg_count; if (cmd->data_len < 6) { dr_ble_return_1("rwc:", 0xFFFF); /* Error: insufficient data */ return 1; } /* Parse Little Endian uint16_t */ freq_idx = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8); cycles = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8); avg_count = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8); /* Validate and set */ meas_config_set_freq_idx((uint8_t)freq_idx); meas_config_set_cycles((uint8_t)cycles); meas_config_set_avg_count((uint8_t)avg_count); /* Save to FDS */ meas_config_save(); if (g_plat.log && g_log_enable) { g_plat.log("[Cmd_mwc] freq=%u cycles=%u avg=%u saved\r\n", meas_config_get_freq_idx(), meas_config_get_cycles(), meas_config_get_avg_count()); } /* Response: confirmed values (after validation) */ dr_ble_return_3("rwc:", (uint16_t)meas_config_get_freq_idx(), (uint16_t)meas_config_get_cycles(), (uint16_t)meas_config_get_avg_count()); return 1; } /*============================================================================== * IMU: 6-axis raw data (single shot) *============================================================================*/ /*============================================================================== * msp? — Read IMU accel(xyz) + gyro(xyz) raw data * * Behavior depends on active streaming state (g_imu_active): * - g_imu_active == true : read from cached memory (g_imu_latest[]) * No I2C access. Fast. No conflict with amu: timer. * - g_imu_active == false : direct I2C register read (original behavior) * Initializes I2C, reads sensor, responds. * * Response tag : "rsp:" + 6 x uint16_t BE (accel_x/y/z, gyro_x/y/z) + CRC16 * Packet size : 18 bytes * * Author : Charles Kwon (Medithings) * Date : 2026-03-17 * * Usage : Client sends "msp?" via BLE NUS * Firmware responds with "rsp:" packet (same format regardless of source) *============================================================================*/ static int Cmd_msp(const ParsedCmd *cmd) { (void)cmd; if (g_imu_active) { /* Active timer running — read from cached memory */ if (g_plat.log) g_plat.log("[MSP] cached read (active mode)\r\n"); int rc = imu_read_cached(); if (rc < 0) { if (g_plat.log) g_plat.log("[MSP] no cache yet, fallback to direct\r\n"); hw_i2c_init_once(); imu_read_direct(); } } else { /* No active timer — direct I2C read */ if (g_plat.log) g_plat.log("[MSP] direct read\r\n"); hw_i2c_init_once(); imu_read_direct(); } return 1; } /*============================================================================== * mas? — Measure Active Start * * Starts the 1-second periodic IMU active streaming timer. * After this command, firmware sends "amu:" packets every 1 second via BLE NUS * Notification containing accel(xyz) + gyro(xyz) raw data. * * Sets g_imu_active = true so that: * - imu_active_timer_handler() fires every 1 sec * - msp? reads from cached memory instead of direct I2C * * Request : "mas?" (4 bytes + CRC16) * Response : "ras:" (4 bytes + CRC16) — confirmation that streaming started * Then : "amu:" packets every 1 second (until max? is received) * * Author : Charles Kwon (Medithings) * Date : 2026-03-17 * * Usage : * 1. Client connects via BLE * 2. Client sends "mas?" + CRC16 * 3. Firmware responds "ras:" + CRC16 * 4. Firmware begins sending "amu:" every 1 sec * 5. Client sends "max?" + CRC16 to stop *============================================================================*/ static int Cmd_mas(const ParsedCmd *cmd) { (void)cmd; uint16_t status; if (g_imu_active) { if (g_plat.log) g_plat.log("[MAS] already active\r\n"); status = 0x0002; /* already running */ } else { g_imu_active = true; imu_active_timer_start(); if (g_plat.log) g_plat.log("[MAS] streaming started\r\n"); status = 0x0001; /* newly started */ } /* Response: "ras:" + status (0x0001=started, 0x0002=already active) */ dr_ble_return_1("ras:", status); return 1; } /*============================================================================== * max? — Measure Active eXit (stop) * * Stops the 1-second periodic IMU active streaming timer. * After this command, firmware stops sending "amu:" packets. * IMU is powered off and I2C is released for power saving. * * Sets g_imu_active = false so that: * - Timer stops firing * - msp? reverts to direct I2C read * * Request : "max?" (4 bytes + CRC16) * Response : "rax:" (4 bytes + CRC16) — confirmation that streaming stopped * * Author : Charles Kwon (Medithings) * Date : 2026-03-17 * * Usage : * 1. Client sends "max?" + CRC16 while streaming is active * 2. Firmware stops amu: timer * 3. Firmware responds "rax:" + CRC16 * 4. No more "amu:" packets sent * 5. msp? now does direct I2C read again *============================================================================*/ static int Cmd_max(const ParsedCmd *cmd) { (void)cmd; uint16_t status; if (!g_imu_active) { if (g_plat.log) g_plat.log("[MAX] already stopped\r\n"); status = 0x0002; /* already stopped */ } else { g_imu_active = false; imu_active_timer_stop(); if (g_plat.log) g_plat.log("[MAX] streaming stopped\r\n"); status = 0x0001; /* newly stopped */ } /* Response: "rax:" + status (0x0001=stopped, 0x0002=already stopped) */ dr_ble_return_1("rax:", status); return 1; }