1063 lines
29 KiB
C
1063 lines
29 KiB
C
/*
|
|
* 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_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);
|
|
|
|
|
|
/* 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);
|
|
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 },
|
|
/* 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;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|