IMU direct read -> FIFO 방식 변경

- mtb? 커맨드

package-lock.json

@@ -0,0 +1,6 @@
+{
+  "name": "Vesiscan-Basic_imu",
+  "lockfileVersion": 3,
+  "requires": true,
+  "packages": {}
+}

project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h

@@ -48,6 +48,8 @@ extern void battery_level_meas(void);
 extern void pressure_all_level_meas(void);
 extern void tmp235_voltage_level_meas(void);
 extern int  imu_read_direct(void);
+extern int  imu_fifo_capture_start(void);
+extern int  imu_fifo_capture_stop_and_send_rim(void);
 extern void battery_timer_stop(void);
 extern void main_timer_start(void);
 extern void hw_i2c_init_once(void);

project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c

@@ -49,6 +49,7 @@ static const CmdEntry m_cmd_table[] = {
     { "mec?", true, Cmd_mec },
     { "maa?", true, Cmd_maa },
     { "mbb?", true, Cmd_mbb },
+    { "mtb?", true, Cmd_mtb },
     { "mcf?", true, Cmd_mcf },
     { "mcs?", true, Cmd_mcs },
 

project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c

@@ -17,6 +17,93 @@
 #include "dr_piezo.h"
 #include "dr_adc121s051.h"
 
+static void mtb_send_rim_after_piezo(void)
+{
+    send_imu_rim_fifo();
+}
+
+/*------------------------------------------------------------------------------
+ * Internal clamp helpers for persisted piezo configuration
+ *----------------------------------------------------------------------------*/
+static uint8_t clamp_piezo_freq_option(uint16_t raw_freq)
+{
+    /* Keep the existing option-code protocol:
+     *   0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 9=1.9MHz
+     * Unknown values fall back to 2.1MHz (code 1).
+     */
+    switch (raw_freq)
+    {
+        case 3:   /* 1.7MHz */
+            return 3;
+
+        case 0:   /* 1.8MHz */
+            return 0;
+
+        case 9:   /* 1.9MHz */
+            return 9;
+
+        case 2:   /* 2.0MHz */
+            return 2;
+
+        case 1:   /* 2.1MHz */
+            return 1;
+
+        case 4:   /* 2.2MHz */
+            return 4;
+
+        default:
+            return 1;      /* unknown code -> default to 2.1MHz */
+    }
+}
+
+static uint8_t clamp_piezo_cycles(uint16_t cycles)
+{
+    if (cycles < 3)
+    {
+        return 3;
+    }
+    if (cycles > 7)
+    {
+        return 7;
+    }
+    return (uint8_t)cycles;
+}
+
+static uint16_t clamp_piezo_averaging(uint16_t averaging)
+{
+    if (averaging < 1)
+    {
+        return 1;
+    }
+    if (averaging > 10)
+    {
+        return 10;
+    }
+    return averaging;
+}
+
+static uint16_t clamp_piezo_delay_us(uint16_t delay_us)
+{
+    if (delay_us > 50)
+    {
+        return 50;
+    }
+    return delay_us;
+}
+
+static uint16_t clamp_piezo_num_samples(uint16_t num_samples)
+{
+    if (num_samples < 80)
+    {
+        return 80;
+    }
+    if (num_samples > 119)
+    {
+        return 119;
+    }
+    return num_samples;
+}
+
 /*==============================================================================
  * mpa? -> rpa:  Enable piezo TX/RX circuit
  *
@@ -277,6 +364,66 @@ int Cmd_mbb(const ParsedCmd *cmd)
     return 1;
 }
 
+/*==============================================================================
+ * mtb? -> reb:+raa:+rim:  Piezo ADC + IMU FIFO (no rbb:)
+ *
+ * Request:  [TAG 4B "mtb?"] [CRC 2B]
+ * Response: reb: [num_samples 2B] [raw_data...]   (per channel; same maa_async as mbb?)
+ *           raa: [status 2B]
+ *           rim: [total_sample_count u16 BE] [samples: 12B each ax,ay,az,gx,gy,gz ...] (may span BLE packets)
+ * Error:    raa: + 0xFFFE       (previous capture in progress)
+ *           raa: + (0xFF00|err) (start failed)
+ *
+ * reb/raa use the same maa_async_start path as mbb?; no rbb: / all_sensors().
+ *============================================================================*/
+int Cmd_mtb(const ParsedCmd *cmd)
+{
+    dr_adc_err_t err;
+    (void)cmd;
+
+    if (maa_async_is_busy())
+    {
+        dr_ble_return_1("raa:", 0xFFFE);
+        return 1;
+    }
+
+    (void)imu_fifo_capture_start();
+
+    if (!dr_piezo_is_power_on())
+    {
+        dr_piezo_power_on();
+    }
+
+    maa_async_set_pre_capture_all(true);
+
+    err = maa_async_start(
+        m_config.piezo_freq_option,
+        m_config.piezo_delay_us,
+        m_config.piezo_num_samples,
+        m_config.piezo_cycles,
+        m_config.piezo_averaging,
+        ble_bin_buffer
+    );
+
+    if (err != DR_ADC_OK)
+    {
+        if (g_plat.log)
+        {
+            g_plat.log("[Cmd_mtb] start failed err=%d\r\n", err);
+        }
+        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
+        dr_binary_tx_safe(ble_bin_buffer, 3);
+        dr_piezo_power_off();
+        maa_async_set_on_complete(NULL);
+        send_imu_rim_fifo();
+        return 1;
+    }
+
+    maa_async_set_on_complete(mtb_send_rim_after_piezo);
+
+    return 1;
+}
+
 /*==============================================================================
  * mcf? -> rcf:  Read piezo parameters from FDS
  *
@@ -303,6 +450,13 @@ int Cmd_mcf(const ParsedCmd *cmd)
  *
  * Request:  [TAG 4B "mcs?"] [freq 2B] [cycles 2B] [avg 2B] [delay_us 2B] [num_samples 2B] [CRC 2B]
  * Response: [TAG 4B "rcs:"] [stored 5 values] [CRC 2B]
+ * Notes:
+ *   - Stored values are clamped before saving
+ *   - freq      : 1.8/1.9/2.0/2.1/2.2 MHz only
+ *   - cycles    : 3..7
+ *   - avg       : 1..10
+ *   - delay_us  : 0..50
+ *   - samples   : 80..119
  * Error:    rcs: + 0xFFFF  (insufficient data)
  *============================================================================*/
 int Cmd_mcs(const ParsedCmd *cmd)
@@ -324,11 +478,11 @@ int Cmd_mcs(const ParsedCmd *cmd)
     dr_get_u16(cmd, 3, &delay_us);
     dr_get_u16(cmd, 4, &num_samples);
 
-    m_config.piezo_freq_option = (uint8_t)freq;
-    m_config.piezo_cycles      = (uint8_t)cycles;
-    m_config.piezo_averaging   = averaging;
-    m_config.piezo_delay_us    = delay_us;
-    m_config.piezo_num_samples = num_samples;
+    m_config.piezo_freq_option = clamp_piezo_freq_option(freq);
+    m_config.piezo_cycles      = clamp_piezo_cycles(cycles);
+    m_config.piezo_averaging   = clamp_piezo_averaging(averaging);
+    m_config.piezo_delay_us    = clamp_piezo_delay_us(delay_us);
+    m_config.piezo_num_samples = clamp_piezo_num_samples(num_samples);
     config_save();
 
     uint8_t *buf = ble_bin_buffer;

project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h

@@ -12,6 +12,7 @@ int Cmd_mpc(const ParsedCmd *cmd);   /* mpc? -> rpc:  burst generation */
 int Cmd_mec(const ParsedCmd *cmd);   /* mec? -> reb:+raa:  single-channel capture */
 int Cmd_maa(const ParsedCmd *cmd);   /* maa? -> reb:+raa:  6-channel async capture */
 int Cmd_mbb(const ParsedCmd *cmd);   /* mbb? -> rbb:+reb:+raa:  sensors + capture */
+int Cmd_mtb(const ParsedCmd *cmd);   /* mtb? -> reb:+raa:+rim:  piezo + IMU FIFO (no rbb:) */
 int Cmd_mcf(const ParsedCmd *cmd);   /* mcf? -> rcf:  read piezo parameters */
 int Cmd_mcs(const ParsedCmd *cmd);   /* mcs? -> rcs:  write piezo parameters */
 

project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c

@@ -136,3 +136,57 @@ void all_sensors(void)
 
     dr_binary_tx_safe(buf, 10);  /* 20 bytes = 10 words */
 }
+
+/*==============================================================================
+ * all_sensors_batt_temp() - Battery + temperature only, then short rbb:
+ *
+ * Emits rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words (no IMU).
+ * Not used by mtb? anymore; kept for optional host/tests.
+ *
+ * Order: battery -> (Piezo TX/RX ON) -> temperature
+ * Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words
+ *           TX layer appends CRC 2B, so the BLE packet is 10B total.
+ *============================================================================*/
+void all_sensors_batt_temp(void)
+{
+    uint8_t *buf;
+    uint32_t timeout_cnt;
+
+    info4 = true;
+
+    battery_saadc_done = false;
+    battery_level_meas();
+    for (timeout_cnt = 0; !battery_saadc_done && timeout_cnt < 100; timeout_cnt++)
+    {
+        dr_sd_delay_ms(1);
+    }
+
+    if (!dr_piezo_is_power_on())
+    {
+        dr_piezo_power_on();
+    }
+
+    tmp235_saadc_done = false;
+    tmp235_voltage_level_meas();
+    for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
+    {
+        dr_sd_delay_ms(1);
+    }
+
+    info4 = false;
+
+    static uint8_t rbb_buf[8];
+    buf = rbb_buf;
+    buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
+    buf[4] = (uint8_t)(info_batt >> 8);
+    buf[5] = (uint8_t)(info_batt & 0xFF);
+    buf[6] = (uint8_t)(info_temp >> 8);
+    buf[7] = (uint8_t)(info_temp & 0xFF);
+
+    dr_binary_tx_safe(buf, 4);  /* 8 bytes = 4 words, CRC appended by TX layer */
+}
+
+void send_imu_rim_fifo(void)
+{
+    (void)imu_fifo_capture_stop_and_send_rim();
+}

project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h

@@ -13,4 +13,11 @@ int Cmd_msp(const ParsedCmd *cmd);   /* msp? -> rsp:  IMU 6-axis single read */
  * Called from Cmd_mbb() in cmd_piezo.c. */
 void all_sensors(void);
 
+/* Optional helper: battery / temperature only, then rbb: [batt 2B] [temp 2B].
+ * (mtb? no longer calls this; kept for reuse / tooling.) */
+void all_sensors_batt_temp(void);
+
+/* Test helper for mtb?: drains IMU FIFO and emits rim: packet(s). */
+void send_imu_rim_fifo(void);
+
 #endif /* CMD_SENSOR_H */

project/ble_peripheral/ble_app_bladder_patch/download/cp.bat

@@ -1,11 +1,17 @@
 @echo off
-setlocal enabledelayedexpansion
+setlocal EnableExtensions EnableDelayedExpansion
 
 echo ==========================================
 echo     MEDiThings Bladder Patch Programming
 echo     (mergehex + nrfutil 8.x)
 echo ==========================================
 
+REM -----------------------------------------------------
+REM Create hex output folder
+REM -----------------------------------------------------
+cd /d "%~dp0"
+echo Working directory: %CD%
+
 REM -----------------------------------------------------
 REM Create hex output folder
 REM -----------------------------------------------------
@@ -85,18 +91,29 @@ REM 6. Detect device SERIAL NUMBER
 REM -----------------------------------------------------
 echo [6/7] Detecting device serial number...
 
-for /f %%A in ('
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
+set "SERIALNUMBER="
 
-if "%SERIALNUMBER%"=="" (
+for /f %%A in ('nrfjprog --ids 2^>nul') do (
+    if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+)
+
+if not defined SERIALNUMBER (
+    for /f %%A in ('
+        powershell -NoProfile -Command "(nrfutil device list --json | ConvertFrom-Json).data.devices[0].serialNumber"
+    ') do (
+        if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+    )
+)
+
+if not defined SERIALNUMBER (
     echo ERROR: No serial number found.
+    echo        Check USB connection and Nordic tools installation.
     pause
     exit /b 1
 )
 
 echo Using Serial Number: %SERIALNUMBER%
-echo Flashing: recover → erase → program → reset
+echo Flashing: recover ??erase ??program ??reset
 
 REM recover
 nrfutil device recover --serial-number %SERIALNUMBER%

project/ble_peripheral/ble_app_bladder_patch/download/cpd.bat

@@ -1,5 +1,5 @@
 @echo off
-setlocal enabledelayedexpansion
+setlocal EnableExtensions EnableDelayedExpansion
 
 echo ==========================================
 echo     MEDiThings Bladder Patch Programming
@@ -116,23 +116,34 @@ REM 6. Detect device SERIAL NUMBER and Flash
 REM -----------------------------------------------------
 echo [6/6] Detecting device serial number...
 
-for /f %%A in ('
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
+set "SERIALNUMBER="
 
-if "%SERIALNUMBER%"=="" (
+for /f %%A in ('nrfjprog --ids 2^>nul') do (
+    if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+)
+
+if not defined SERIALNUMBER (
+    for /f %%A in ('
+        powershell -NoProfile -Command "(nrfutil device list --json | ConvertFrom-Json).data.devices[0].serialNumber"
+    ') do (
+        if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+    )
+)
+
+if not defined SERIALNUMBER (
     echo ERROR: No serial number found.
+    echo        Check USB connection and Nordic tools installation.
     pause
     exit /b 1
 )
 
 echo Using Serial Number: %SERIALNUMBER%
-echo Flashing: program → reset (NO erase, FDS preserved)
+echo Flashing: program ??reset (NO erase, FDS preserved)
 
 REM recover - SKIP to preserve internal flash data
 REM erase   - SKIP to preserve FDS/fstorage data
 
-REM program (hex 데이터가 있는 영역만 erase, FDS 보존)
+REM program (hex ?곗씠?곌? ?덈뒗 ?곸뿭留?erase, FDS 蹂댁〈)
 nrfutil device program --firmware hex\firmware_all.hex --options chip_erase_mode=ERASE_RANGES_TOUCHED_BY_FIRMWARE --serial-number %SERIALNUMBER%
 if %errorlevel% neq 0 (
     echo.

project/ble_peripheral/ble_app_bladder_patch/download/cpd_eraseALL.bat

@@ -1,5 +1,5 @@
 @echo off
-setlocal enabledelayedexpansion
+setlocal EnableExtensions EnableDelayedExpansion
 
 echo ==========================================
 echo     MEDiThings Bladder Patch Programming
@@ -116,12 +116,23 @@ REM 6. Detect device SERIAL NUMBER and Flash
 REM -----------------------------------------------------
 echo [6/6] Detecting device serial number...
 
-for /f %%A in ('
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
+set "SERIALNUMBER="
 
-if "%SERIALNUMBER%"=="" (
+for /f %%A in ('nrfjprog --ids 2^>nul') do (
+    if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+)
+
+if not defined SERIALNUMBER (
+    for /f %%A in ('
+        powershell -NoProfile -Command "(nrfutil device list --json | ConvertFrom-Json).data.devices[0].serialNumber"
+    ') do (
+        if not defined SERIALNUMBER set "SERIALNUMBER=%%A"
+    )
+)
+
+if not defined SERIALNUMBER (
     echo ERROR: No serial number found.
+    echo        Check USB connection and Nordic tools installation.
     pause
     exit /b 1
 )

project/ble_peripheral/ble_app_bladder_patch/hal/fds/fstorage.c

@@ -47,7 +47,7 @@
 #define CONFIG_REC_KEY  (0x7010)
 
 /* Magic number used to validate stored data */
-#define CONFIG_MAGIC_NUMBER_VALUE          (0x20260319)
+#define CONFIG_MAGIC_NUMBER_VALUE          (0x20260428)
 
 /* Global configuration instance */
 config_data_t m_config;

project/ble_peripheral/ble_app_bladder_patch/hal/fds/fstorage.h

@@ -61,11 +61,11 @@ typedef struct
     uint32_t    life_cycle;         /* 4B  - device usage count */
 
     /* Piezo measurement parameters - 8B */
-    uint8_t     piezo_freq_option;  /* 1B  - TX pulse frequency (0=1.8M, 1=2.1M, 2=2.0M, 3=1.7M) */
-    uint8_t     piezo_cycles;       /* 1B  - burst pulse cycle count (3..7) */
-    uint16_t    piezo_averaging;    /* 2B  - averages per channel (1..10) */
-    uint16_t    piezo_delay_us;     /* 2B  - delay from TX pulse to ADC start (us) (0..30) */
-    uint16_t    piezo_num_samples;  /* 2B  - ADC sample count (80..140) */
+    uint8_t     piezo_freq_option;  /* 1B  - TX pulse frequency option (9=1.9M, 2=2.0M, 1=2.1M, 4=2.2M) */
+    uint8_t     piezo_cycles;       /* 1B  - burst pulse cycle count (3~7) */
+    uint16_t    piezo_averaging;    /* 2B  - averages per channel (1~10) */
+    uint16_t    piezo_delay_us;     /* 2B  - delay from TX pulse to ADC start (us) (0~50) */
+    uint16_t    piezo_num_samples;  /* 2B  - ADC sample count (80~119) */
 
     /* Factory provisioning lock */
     uint8_t     factory_provisioned; /* 1B  - 0=passkey not set, 1=passkey set (locked) */

project/ble_peripheral/ble_app_bladder_patch/main.c

@@ -562,6 +562,49 @@ extern void maa_async_abort(void);
 static volatile uint8_t  pending_cmd_buf[BLE_NUS_MAX_DATA_LEN] = {0};
 static volatile uint8_t  pending_cmd_len = 0;
 
+static bool ble_retry_pending_tx(void)
+{
+    uint16_t send_len;
+    uint32_t err;
+
+    if (!s_tx_pending)
+    {
+        return true;
+    }
+    if (ble_connection_st != BLE_CONNECTED_ST)
+    {
+        s_tx_pending = false;
+        s_tx_pending_len = 0;
+        return false;
+    }
+
+    send_len = s_tx_pending_len;
+    err = ble_nus_data_send(&m_nus, s_tx_pending_buf, &send_len, m_conn_handle);
+
+    if (err == NRF_SUCCESS)
+    {
+        s_tx_pending = false;
+        s_tx_pending_len = 0;
+        return true;
+    }
+    if (err == NRF_ERROR_RESOURCES)
+    {
+        return false;
+    }
+    if (err == NRF_ERROR_INVALID_STATE || err == NRF_ERROR_NOT_FOUND)
+    {
+        DBG_PRINTF("[BLE TX] Pending send aborted\r\n");
+        s_tx_pending = false;
+        s_tx_pending_len = 0;
+        return false;
+    }
+
+    DBG_PRINTF("[BLE TX] Pending err:0x%X\r\n", err);
+    s_tx_pending = false;
+    s_tx_pending_len = 0;
+    return false;
+}
+
 /**
  * @brief NUS (Nordic UART Service) data receive handler
  *
@@ -601,22 +644,20 @@ static void nus_data_handler(ble_nus_evt_t * p_evt)
     }
     else if (p_evt->type == BLE_NUS_EVT_TX_RDY)
     {
+        /* First drain any queued packet that previously hit NRF_ERROR_RESOURCES. */
+        if (s_tx_pending)
+        {
+            if (!ble_retry_pending_tx())
+            {
+                return;
+            }
+        }
+
         /* BLE TX buffer has space - continue async MAA transmission */
         if (maa_async_is_busy())
         {
             maa_async_on_tx_ready();
         }
-        /* Retry pending packet (dr_binary_tx_safe async) */
-        else if (s_tx_pending)
-        {
-            uint16_t send_len = s_tx_pending_len;
-            uint32_t err = ble_nus_data_send(&m_nus, s_tx_pending_buf, &send_len, m_conn_handle);
-            if (err == NRF_SUCCESS)
-            {
-                s_tx_pending = false;
-            }
-            /* NRF_ERROR_RESOURCES -> retry on next TX_RDY */
-        }
     }
 }
 
@@ -1458,7 +1499,8 @@ void ascii_format_data(uint8_t *buffer, const char *tag, const char *data_ascii,
 /**
  * @brief BLE binary safe transmission (preamble for dr_binary_tx_safe)
  *
- * On NRF_ERROR_RESOURCES (TX queue full), retries at 5ms intervals.
+ * On NRF_ERROR_RESOURCES (TX queue full), stores one pending packet and retries
+ * only when BLE_NUS_EVT_TX_RDY is received.
  * Returns gracefully on connection errors.
  *
  * @param ble_bin_buff Data buffer to transmit
@@ -1507,8 +1549,8 @@ void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
 {
     uint32_t err_code;
     static uint8_t tx_buffer[BLE_NUS_MAX_DATA_LEN] = {0};
-    uint16_t retry_count = 0;
-    const uint16_t MAX_RETRIES = 20;  /* Max retries: 2ms x 20 = 40ms (was 5ms x 100 = 500ms) */
+    uint16_t send_len;
+    uint16_t total_len;
 
     if (ble_connection_st == 0) return;
 
@@ -1521,11 +1563,8 @@ void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
         tx_buffer[length * sizeof(uint16_t)]     = (uint8_t)(crc & 0xFF);
         tx_buffer[length * sizeof(uint16_t) + 1] = (uint8_t)((crc >> 8) & 0xFF);
 
-        uint16_t total_len = length * sizeof(uint16_t) + 2;
-
-        do
-        {
-            uint16_t send_len = total_len;
+        total_len = length * sizeof(uint16_t) + 2;
+        send_len = total_len;
         err_code = ble_nus_data_send(&m_nus, tx_buffer, &send_len, m_conn_handle);
 
         if (err_code == NRF_SUCCESS)
@@ -1534,8 +1573,16 @@ void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
         }
         else if (err_code == NRF_ERROR_RESOURCES)
         {
-                nrf_delay_ms(2);  /* Was 5ms -> 2ms */
-                retry_count++;
+            if (s_tx_pending)
+            {
+                DBG_PRINTF("[BLE TX] Pending queue busy, dropping new packet\r\n");
+                return;
+            }
+
+            memcpy(s_tx_pending_buf, tx_buffer, total_len);
+            s_tx_pending_len = total_len;
+            s_tx_pending = true;
+            return;
         }
         else if (err_code == NRF_ERROR_INVALID_STATE || err_code == NRF_ERROR_NOT_FOUND)
         {
@@ -1547,12 +1594,6 @@ void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
             DBG_PRINTF("[BLE TX] Err:0x%X\r\n", err_code);
             return;
         }
-        } while (retry_count < MAX_RETRIES);
-
-        if (retry_count >= MAX_RETRIES)
-        {
-            DBG_PRINTF("[BLE TX] FAIL %u retries\r\n", retry_count);
-        }
     }
 }
 

project/ble_peripheral/ble_app_bladder_patch/main.h

@@ -38,8 +38,10 @@
 *      : Cleaned up command parsing and removed unused project files.
 *   - VBTFW0111 260422 jhChun : Updated firmware version for test.
 *   - VBTFW0112 260428 jhChun : Added extra CH0 settling delay after MUX selection for maa capture.
+*   - VBTFW0113 260430 jhChun : Changed piezo MUX settling delay after channel selection from busy-wait to TIMER4-based wait.
+*   - VBTFW0114 260430 jhChun : Added delay time between averaged bursts to reduce residual echo carry-over.
 ------------------------------------------------------------------------- */
-#define FIRMWARE_VERSION                "VBTFW0112"
+#define FIRMWARE_VERSION                "VBTFW0114"
 
 /*==============================================================================
  * Data Length Constants

project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c

@@ -57,6 +57,74 @@ extern void dr_piezo_power_off(void);
 
 #include "parser.h"
 
+/*------------------------------------------------------------------------------
+ * Internal helpers
+ *----------------------------------------------------------------------------*/
+static uint8_t normalize_freq_option(uint8_t freq_option)
+{
+    switch (freq_option)
+    {
+        case 0: /* 1.8MHz */
+        case 1: /* 2.1MHz */
+        case 2: /* 2.0MHz */
+        case 3: /* 1.7MHz */
+        case 4: /* 2.2MHz */
+        case 9: /* 1.9MHz */
+            return freq_option;
+
+        default:
+            return 1; /* Invalid -> default to 2.1MHz */
+    }
+}
+
+static uint8_t clamp_measure_cycles(uint8_t cycles)
+{
+    if (cycles < 3)
+    {
+        return 3;
+    }
+    if (cycles > 7)
+    {
+        return 7;
+    }
+    return cycles;
+}
+
+static uint16_t clamp_measure_averaging(uint16_t averaging)
+{
+    if (averaging < 1)
+    {
+        return 1;
+    }
+    if (averaging > 10)
+    {
+        return 10;
+    }
+    return averaging;
+}
+
+static uint16_t clamp_measure_num_samples(uint16_t num_samples)
+{
+    if (num_samples < 80)
+    {
+        return 80;
+    }
+    if (num_samples > 119)
+    {
+        return 119;
+    }
+    return num_samples;
+}
+
+static uint16_t clamp_measure_delay_us(uint16_t delay_us)
+{
+    if (delay_us > 50)
+    {
+        return 50;
+    }
+    return delay_us;
+}
+
 /*==============================================================================
  * DEBUG CONFIGURATION
  *============================================================================*/
@@ -83,6 +151,11 @@ extern void dr_piezo_power_off(void);
 #define BLE_REB_DATA_LEN        (BLE_MTU_SIZE - BLE_REB_HEADER_LEN)   /* 238 bytes = 119 samples */
 #define BLE_RED_DATA_LEN        (BLE_MTU_SIZE - BLE_RED_HEADER_LEN)   /* 238 bytes = 119 samples */
 #define BLE_PACKET_DELAY_MS     100     /* Inter-packet delay - allow BLE TX buffer to drain */
+/* maa_async: gaps between BLE notifications (sync MEC path still uses BLE_PACKET_DELAY_MS). */
+#define MAA_ASYNC_POST_REB_MS   3U      /* after reb: (was 5) */
+#define MAA_ASYNC_POST_RED_MS   18U     /* after each red: (was 50) */
+#define MAA_ASYNC_PRE_RAA_MS    15U     /* before raa: completion (was 50) */
+#define DR_ADC_AVG_INTER_BURST_GAP_US 500   /* Gap between averaged bursts to reduce residual echo carry-over */
 
 /* Piezo MUX pins (8ch) */
 #define DR_PIEZO_EN_MUXA    NRF_GPIO_PIN_MAP(0, 21)  /**< MUXA Enable */
@@ -548,24 +621,10 @@ extern void dr_piezo_burst_sw(uint8_t cycles);
 /*==============================================================================
  * PIEZO CHANNEL SELECTION
  *============================================================================*/
- /*
- * Channel mapping (8ch)
 
- *         | EN MUXA | EN MUXB |  SEL 0  |  SEL 1
- *   ----------------------------------------------
- *   CH A0 |    1    |    0    |    0    |    0
- *   CH A1 |    1    |    0    |    0    |    1
- *   CH A2 |    1    |    0    |    1    |    0
- *   CH A3 |    1    |    0    |    1    |    1
- *   ----------------------------------------------
- *   CH B3 |    0    |    1    |    0    |    0
- *   CH B2 |    0    |    1    |    0    |    1
- *   CH B1 |    0    |    1    |    1    |    0
- *   CH B0 |    0    |    1    |    1    |    1
- */
-
-/* dr_piezo_select_channel is defined in dr_piezo.c */
-extern void dr_piezo_select_channel(uint8_t channel);
+/* Channel selection is split into GPIO switch and timer-backed settling wait. */
+extern void dr_piezo_select_channel_start(uint8_t channel);
+extern void dr_piezo_wait_mux_settled(void);
 
 /*==============================================================================
  * INTEGRATED BURST + CAPTURE + TRANSMIT
@@ -592,26 +651,11 @@ dr_adc_err_t dr_adc_burst_capture_transmit(uint8_t freq_option, uint16_t delay_u
     {
         return DR_ADC_ERR_INVALID_PARAM;
     }
-    if (num_samples == 0 || num_samples > DR_ADC_ECHO_SAMPLES_MAX)
-    {
-        return DR_ADC_ERR_INVALID_PARAM;
-    }
-    if (freq_option > 9)
-    {
-        freq_option = 0;       /* Invalid -> default 1.8MHz */
-    }
-    if (cycles < 3 || cycles > 7)
-    {
-        cycles = 5;   /* Valid range: 3~7, default 5 */
-    }
-    if (averaging == 0)
-    {
-        averaging = 1;          /* Minimum 1 */
-    }
-    if (averaging > 1000)
-    {
-        averaging = 1000;     /* Maximum 1000 */
-    }
+    freq_option = normalize_freq_option(freq_option);
+    delay_us = clamp_measure_delay_us(delay_us);
+    num_samples = clamp_measure_num_samples(num_samples);
+    cycles = clamp_measure_cycles(cycles);
+    averaging = clamp_measure_averaging(averaging);
     if (piezo_ch >= MAA_NUM_CHANNELS)
     {
         piezo_ch = 0;  /* clamp channel range */
@@ -638,7 +682,8 @@ dr_adc_err_t dr_adc_burst_capture_transmit(uint8_t freq_option, uint16_t delay_u
     nrf_delay_us(100);
 
     /*--- Step 2: Select piezo channel ---*/
-    dr_piezo_select_channel(piezo_ch);
+    dr_piezo_select_channel_start(piezo_ch);
+    dr_piezo_wait_mux_settled();
 
     /* dummy read after MUX switch to settle S/H capacitor */
     (void)spim_read_raw();
@@ -694,17 +739,17 @@ dr_adc_err_t dr_adc_burst_capture_transmit(uint8_t freq_option, uint16_t delay_u
 
         for (uint16_t avg_iter = 0; avg_iter < averaging; avg_iter++)
         {
-            /* Wait for previous echo to decay before next measurement
-             * 1ms = ~77cm round-trip decay time (sound speed 1.54mm/us)
-             * Skip delay on first iteration */
+            /* Wait for previous echo and ringing to decay before next measurement.
+             * Skip delay on first iteration. */
             if (avg_iter > 0)
             {
-                nrf_delay_us(500);  /* 500us between measurements */
+                nrf_delay_us(DR_ADC_AVG_INTER_BURST_GAP_US);
             }
 
             /* Re-select piezo channel before each burst
              * (burst functions may modify P1 port state) */
-            dr_piezo_select_channel(piezo_ch);
+            dr_piezo_select_channel_start(piezo_ch);
+            dr_piezo_wait_mux_settled();
 
             /* Execute piezo burst based on frequency option */
             switch (freq_option)
@@ -833,26 +878,11 @@ dr_adc_err_t dr_adc_capture_channel_only(uint8_t freq_option, uint16_t delay_us,
     {
         return DR_ADC_ERR_INVALID_PARAM;
     }
-    if (num_samples == 0 || num_samples > MAA_SAMPLES_MAX)
-    {
-        return DR_ADC_ERR_INVALID_PARAM;
-    }
-    if (freq_option > 3)
-    {
-        freq_option = 0;
-    }
-    if (cycles < 3 || cycles > 7)
-    {
-        cycles = 5;
-    }
-    if (averaging == 0)
-    {
-        averaging = 1;
-    }
-    if (averaging > 1000)
-    {
-        averaging = 1000;
-    }
+    freq_option = normalize_freq_option(freq_option);
+    delay_us = clamp_measure_delay_us(delay_us);
+    num_samples = clamp_measure_num_samples(num_samples);
+    cycles = clamp_measure_cycles(cycles);
+    averaging = clamp_measure_averaging(averaging);
     if (piezo_ch > (MAA_NUM_CHANNELS - 1))
     {
         piezo_ch = 0;
@@ -874,7 +904,8 @@ dr_adc_err_t dr_adc_capture_channel_only(uint8_t freq_option, uint16_t delay_us,
     //nrf_delay_us(100);
 
     /* channel select (MUX) : ~1.3 ms */
-    dr_piezo_select_channel(piezo_ch);
+    dr_piezo_select_channel_start(piezo_ch);
+    dr_piezo_wait_mux_settled();
 
     /* dummy read after MUX switch to settle S/H capacitor */
     (void)spim_read_raw();
@@ -882,6 +913,12 @@ dr_adc_err_t dr_adc_capture_channel_only(uint8_t freq_option, uint16_t delay_us,
     /* repeat measurement 'averaging' times per channel */
     for (uint16_t avg_iter = 0; avg_iter < averaging; avg_iter++)
     {
+        /* Let the previous burst decay before the next averaged shot. */
+        if (avg_iter > 0)
+        {
+            nrf_delay_us(DR_ADC_AVG_INTER_BURST_GAP_US);
+        }
+
         /* TX burst pulse */
         switch (freq_option)
         {
@@ -898,6 +935,12 @@ dr_adc_err_t dr_adc_capture_channel_only(uint8_t freq_option, uint16_t delay_us,
             case 3:
                 dr_piezo_burst_sw_17mhz(cycles);
                 break;
+            case 4:
+                dr_piezo_burst_sw_22mhz(cycles);
+                break;
+            case 9:
+                dr_piezo_burst_sw_19mhz(cycles);
+                break;
         }
 
         /* delay from TX burst to ADC start */
@@ -1172,7 +1215,7 @@ static void maa_async_send_header(void)
     }
 
     dr_binary_tx_safe(buf, dst_idx / 2);
-    dr_sd_delay_ms(5);  /* minimal delay; dr_binary_tx_safe retries internally (40 ms) */
+    dr_sd_delay_ms(MAA_ASYNC_POST_REB_MS);
 
     g_maa_ctx.current_pkt = 0;
     g_maa_ctx.data_offset = src_idx * 2;  /* bytes already sent */
@@ -1223,7 +1266,7 @@ static bool maa_async_send_data_packet(void)
     }
 
     dr_binary_tx_safe(buf, dst_idx / 2);
-    dr_sd_delay_ms(50);  /* Allow BLE stack to process TX */
+    dr_sd_delay_ms(MAA_ASYNC_POST_RED_MS);
 
     g_maa_ctx.data_offset += chunk_size;
     g_maa_ctx.current_pkt++;
@@ -1241,7 +1284,7 @@ static void maa_async_send_completion(uint16_t status)
     uint8_t *buf = g_maa_ctx.ble_buffer;
 
     /* Wait for previous TX to complete before sending raa: */
-    dr_sd_delay_ms(50);
+    dr_sd_delay_ms(MAA_ASYNC_PRE_RAA_MS);
 
     buf[0] = 'r'; buf[1] = 'a'; buf[2] = 'a'; buf[3] = ':';
     buf[4] = (uint8_t)(status >> 8);
@@ -1286,20 +1329,16 @@ dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us, uint16_t nu
     {
         return DR_ADC_ERR_INVALID_PARAM;
     }
-    if (num_samples == 0 || num_samples > DR_ADC_ECHO_SAMPLES_MAX)
-    {
-        return DR_ADC_ERR_INVALID_PARAM;
-    }
 
     /* Clear BLE buffer to prevent stale data from previous measurement */
     memset(ble_buffer, 0, BLE_MTU_SIZE);
 
     /* Initialize context */
-    g_maa_ctx.freq_option = freq_option;
-    g_maa_ctx.delay_us = delay_us;
-    g_maa_ctx.num_samples = num_samples;
-    g_maa_ctx.cycles = (cycles < 3 || cycles > 7) ? 5 : cycles;
-    g_maa_ctx.averaging = (averaging == 0) ? 1 : ((averaging > 1000) ? 1000 : averaging);
+    g_maa_ctx.freq_option = normalize_freq_option(freq_option);
+    g_maa_ctx.delay_us = clamp_measure_delay_us(delay_us);
+    g_maa_ctx.num_samples = clamp_measure_num_samples(num_samples);
+    g_maa_ctx.cycles = clamp_measure_cycles(cycles);
+    g_maa_ctx.averaging = clamp_measure_averaging(averaging);
     g_maa_ctx.ble_buffer = ble_buffer;
     g_maa_ctx.current_ch = 0;
     g_maa_ctx.current_pkt = 0;

project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c

@@ -36,6 +36,7 @@
 #include "app_raw.h"
 #include "inv_imu_extfunc.h"
 #include "inv_imu_driver.h"
+#include "inv_imu_transport.h"
 #include "ble_nus.h"
 #include "nrf_log.h"
 #include "nrf_log_ctrl.h"
@@ -45,6 +46,7 @@
 #include "main.h"
 #include "debug_print.h"
 #include "nrf_delay.h"
+#include <string.h>
 
 
 /*
@@ -168,9 +170,9 @@ int configure_imu_device(void)
 		/* High-resolution FIFO mode: 20-bit data, FSR locked to 16g/2000dps */
 		rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
 	} else {
-		/* Standard mode: accel +/-4g, gyro +/-2000dps FSR */
+		/* Standard mode: accel +/-4g, gyro +/-500dps FSR */
 		rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
-		rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_2000dps);
+		rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
 	}
 
 	if (USE_LOW_NOISE_MODE) {
@@ -580,3 +582,423 @@ int imu_read_direct(void)
 
 	return 0;
 }
+
+/* --------------------------------------------------------------------------------------
+ * mtb? FIFO capture support
+ *
+ * Uses the ICM42670P internal FIFO at 25 Hz. The FIFO is started when mtb?
+ * begins, then drained after piezo raa: completion and sent as rim: packets.
+ * rim payload: u16 BE total_sample_count, then per sample accel(6B)+gyro(6B) from each 16B FIFO record.
+ *
+ * Timing (tune for MTB latency vs BLE reliability):
+ *   - IMU_FIFO_ENABLE_SETTLE_MS: after accel/gyro on, before 2nd FIFO flush (first start only).
+ *   - IMU_FIFO_RIM_POST_TX_MS: gap between rim notifications only (not after final packet).
+ * -------------------------------------------------------------------------------------- */
+#define IMU_FIFO_PACKET_SIZE_BYTES  FIFO_16BYTES_PACKET_SIZE
+#define IMU_FIFO_MAX_PACKET_COUNT   258
+#define IMU_FIFO_READ_RECORDS_PER_BURST 14
+#define IMU_FIFO_ACCEL_OFFSET       1
+#define IMU_FIFO_GYRO_OFFSET        7
+#define IMU_FIFO_ENABLE_SETTLE_MS   35U
+#define IMU_FIFO_RIM_POST_TX_MS     8U
+#define RIM_SAMPLE_SIZE_BYTES       12
+#define RIM_PACKET_HEADER_BYTES     6  /* "rim:" + u16 BE total_sample_count */
+#define RIM_MAX_SAMPLE_BYTES        (BLE_NUS_MAX_DATA_LEN - 2 - RIM_PACKET_HEADER_BYTES)
+#define RIM_SAMPLES_PER_PACKET      (RIM_MAX_SAMPLE_BYTES / RIM_SAMPLE_SIZE_BYTES)
+
+static bool s_fifo_capture_active = false;
+
+static void imu_serif_make(struct inv_imu_serif *serif)
+{
+	serif->context    = 0;
+	serif->read_reg   = inv_io_hal_read_reg;
+	serif->write_reg  = inv_io_hal_write_reg;
+	serif->max_read   = 1024 * 32;
+	serif->max_write  = 1024 * 32;
+	serif->serif_type = SERIF_TYPE;
+}
+
+static int imu_fifo_driver_prepare(void)
+{
+	struct inv_imu_serif icm_serif;
+	int rc;
+
+	DBG_PRINTF("[IMU FIFO] prepare: TWI reinit + setup_imu_device\r\n");
+	inv_i2c_master_uninitialize();
+	inv_i2c_master_initialize();
+
+	imu_serif_make(&icm_serif);
+	rc = setup_imu_device(&icm_serif);
+	DBG_PRINTF("[IMU FIFO] prepare: setup_imu_device rc=%d\r\n", rc);
+	return rc;
+}
+
+static void imu_fifo_power_off(void)
+{
+	DBG_PRINTF("[IMU FIFO] power_off: disable gyro/accel, FIFO off, active=0\r\n");
+	(void)inv_imu_disable_gyro(&icm_driver);
+	(void)inv_imu_disable_accel(&icm_driver);
+	(void)inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
+	s_fifo_capture_active = false;
+}
+
+int imu_fifo_capture_start(void)
+{
+	int rc;
+
+	DBG_PRINTF("[IMU FIFO] capture_start: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
+
+	if (s_fifo_capture_active)
+	{
+		DBG_PRINTF("[IMU FIFO] capture_start: skip (already active)\r\n");
+		return 0;
+	}
+
+	rc = imu_fifo_driver_prepare();
+	if (rc != 0)
+	{
+		DBG_PRINTF("[IMU FIFO] prepare fail %d\r\n", rc);
+		return rc;
+	}
+
+	DBG_PRINTF("[IMU FIFO] capture_start: FSR/ODR/FIFO stream, settle %ums\r\n",
+			   (unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
+	rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
+	rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
+	rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_25_HZ);
+	rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_25_HZ);
+	rc |= inv_imu_set_accel_ln_bw(&icm_driver, IMU_FIFO_MTB_ACCEL_LN_BW);
+	rc |= inv_imu_set_gyro_ln_bw(&icm_driver, IMU_FIFO_MTB_GYRO_LN_BW);
+	rc |= inv_imu_disable_high_resolution_fifo(&icm_driver);
+	rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_ENABLED);
+	{
+		uint8_t fifo_cfg1;
+		rc |= inv_imu_read_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
+		fifo_cfg1 &= ~FIFO_CONFIG1_FIFO_MODE_MASK;
+		fifo_cfg1 |= FIFO_CONFIG1_FIFO_MODE_STREAM;
+		rc |= inv_imu_write_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
+	}
+	rc |= inv_imu_reset_fifo(&icm_driver);
+	rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
+	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
+	DBG_PRINTF("[IMU FIFO] capture_start: delay %ums\r\n", (unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
+	dr_sd_delay_ms(IMU_FIFO_ENABLE_SETTLE_MS);
+	rc |= inv_imu_reset_fifo(&icm_driver);
+
+	if (rc != 0)
+	{
+		DBG_PRINTF("[IMU FIFO] start fail %d\r\n", rc);
+		imu_fifo_power_off();
+		return rc;
+	}
+
+	s_fifo_capture_active = true;
+	DBG_PRINTF("[IMU FIFO] capture_start: OK active=1 rc_accum=%d\r\n", rc);
+	return 0;
+}
+
+static void imu_fifo_send_rim_packets(uint16_t total_sample_count)
+{
+	static uint8_t rim_buf[BLE_NUS_MAX_DATA_LEN];
+	uint16_t record_idx = 0;
+	uint16_t pkt = 0;
+
+	DBG_PRINTF("[IMU FIFO] rim_send: total_samples=%u max_per_pkt=%u\r\n",
+			   (unsigned)total_sample_count, (unsigned)RIM_SAMPLES_PER_PACKET);
+
+	do
+	{
+		uint16_t sample_count = total_sample_count - record_idx;
+		uint16_t dst_idx = RIM_PACKET_HEADER_BYTES;
+		uint16_t i;
+
+		if (sample_count > RIM_SAMPLES_PER_PACKET)
+		{
+			sample_count = RIM_SAMPLES_PER_PACKET;
+		}
+
+		rim_buf[0] = 'r'; rim_buf[1] = 'i'; rim_buf[2] = 'm'; rim_buf[3] = ':';
+		rim_buf[4] = (uint8_t)(total_sample_count >> 8);
+		rim_buf[5] = (uint8_t)(total_sample_count & 0xFF);
+
+		for (i = 0; i < sample_count; i++)
+		{
+			const uint8_t *record = &icm_driver.fifo_data[(record_idx + i) * IMU_FIFO_PACKET_SIZE_BYTES];
+			memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_ACCEL_OFFSET], 6);
+			dst_idx += 6;
+			memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_GYRO_OFFSET], 6);
+			dst_idx += 6;
+		}
+
+		DBG_PRINTF("[IMU FIFO] rim_send: pkt=%u samples=%u bytes=%u words=%u\r\n",
+				   (unsigned)pkt, (unsigned)sample_count, (unsigned)dst_idx, (unsigned)(dst_idx / 2));
+		dr_binary_tx_safe(rim_buf, dst_idx / 2);
+		record_idx += sample_count;
+		pkt++;
+		if (record_idx < total_sample_count)
+		{
+			dr_sd_delay_ms(IMU_FIFO_RIM_POST_TX_MS);
+		}
+	} while (record_idx < total_sample_count);
+
+	DBG_PRINTF("[IMU FIFO] rim_send: done packets=%u\r\n", (unsigned)pkt);
+}
+
+static int imu_fifo_read_records(uint16_t record_count)
+{
+	int rc = 0;
+	uint16_t record_idx = 0;
+	uint16_t burst_n = 0;
+
+	DBG_PRINTF("[IMU FIFO] fifo_read: records=%u burst_max=%u\r\n",
+			   (unsigned)record_count, (unsigned)IMU_FIFO_READ_RECORDS_PER_BURST);
+
+	while ((record_idx < record_count) && (rc == 0))
+	{
+		uint16_t burst_records = record_count - record_idx;
+		uint16_t burst_bytes;
+
+		if (burst_records > IMU_FIFO_READ_RECORDS_PER_BURST)
+		{
+			burst_records = IMU_FIFO_READ_RECORDS_PER_BURST;
+		}
+
+		burst_bytes = burst_records * IMU_FIFO_PACKET_SIZE_BYTES;
+		DBG_PRINTF("[IMU FIFO] fifo_read: burst=%u idx=%u n=%u bytes=%u\r\n",
+				   (unsigned)burst_n, (unsigned)record_idx, (unsigned)burst_records, (unsigned)burst_bytes);
+		rc |= inv_imu_read_reg(&icm_driver,
+							   FIFO_DATA,
+							   burst_bytes,
+							   &icm_driver.fifo_data[record_idx * IMU_FIFO_PACKET_SIZE_BYTES]);
+
+		record_idx += burst_records;
+		burst_n++;
+	}
+
+	DBG_PRINTF("[IMU FIFO] fifo_read: done bursts=%u rc=%d\r\n", (unsigned)burst_n, rc);
+	return rc;
+}
+
+/* Invensense inv_imu_get_data_from_fifo(): header 0x80 with all payload bytes zero = invalid placeholder. */
+static bool imu_fifo_record_is_invalid_placeholder(const uint8_t *rec)
+{
+	uint16_t i;
+
+	if (rec[0] != 0x80u)
+	{
+		return false;
+	}
+	for (i = 1u; i < IMU_FIFO_PACKET_SIZE_BYTES; i++)
+	{
+		if (rec[i] != 0u)
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static uint16_t imu_fifo_compact_placeholder_records(uint8_t *fifo, uint16_t record_count)
+{
+	uint16_t w;
+	uint16_t r;
+	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
+
+	for (r = 0, w = 0; r < record_count; r++)
+	{
+		uint8_t *rec = fifo + (r * sz);
+
+		if (imu_fifo_record_is_invalid_placeholder(rec))
+		{
+			continue;
+		}
+		if (w != r)
+		{
+			memcpy(fifo + (w * sz), rec, sz);
+		}
+		w++;
+	}
+	return w;
+}
+
+static int16_t imu_fifo_record_gyro_axis(const uint8_t *rec, uint8_t axis)
+{
+	const uint8_t *p = &rec[IMU_FIFO_GYRO_OFFSET + ((uint16_t)axis * 2u)];
+
+	if (icm_driver.endianness_data == INTF_CONFIG0_DATA_BIG_ENDIAN)
+	{
+		return (int16_t)((uint16_t)p[0] << 8 | p[1]);
+	}
+	return (int16_t)((uint16_t)p[1] << 8 | p[0]);
+}
+
+/* Invensense: gyro axis == INVALID_VALUE_FIFO (0x8000) when not valid in this FIFO record. */
+static bool imu_fifo_record_is_invalid_gyro(const uint8_t *rec)
+{
+	uint8_t axis;
+
+	for (axis = 0; axis < 3u; axis++)
+	{
+		if (imu_fifo_record_gyro_axis(rec, axis) != INVALID_VALUE_FIFO)
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static uint16_t imu_fifo_compact_invalid_gyro_records(uint8_t *fifo, uint16_t record_count)
+{
+	uint16_t w;
+	uint16_t r;
+	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
+
+	for (r = 0, w = 0; r < record_count; r++)
+	{
+		uint8_t *rec = fifo + (r * sz);
+
+		if (imu_fifo_record_is_invalid_gyro(rec))
+		{
+			continue;
+		}
+		if (w != r)
+		{
+			memcpy(fifo + (w * sz), rec, sz);
+		}
+		w++;
+	}
+	return w;
+}
+
+static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n)
+{
+	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
+
+#if (IMU_FIFO_RIM_TARGET_SAMPLES > 0)
+	if (n > (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES)
+	{
+		const uint16_t drop = (uint16_t)(n - (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES);
+
+		memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)IMU_FIFO_RIM_TARGET_SAMPLES * sz);
+		n = (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES;
+		DBG_PRINTF("[IMU FIFO] normalize: capped to newest %u samples\r\n",
+				   (unsigned)IMU_FIFO_RIM_TARGET_SAMPLES);
+	}
+	(void)sz;
+#elif (IMU_FIFO_RIM_MIN_SAMPLES > 0)
+	if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
+	{
+		while (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES && n < IMU_FIFO_MAX_PACKET_COUNT)
+		{
+			memset(fifo + ((uint32_t)n * sz), 0, sz);
+			n++;
+		}
+		if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
+		{
+			DBG_PRINTF("[IMU FIFO] normalize: MIN=%u unreachable, using %u\r\n",
+					   (unsigned)IMU_FIFO_RIM_MIN_SAMPLES, (unsigned)n);
+		}
+	}
+#else
+	(void)sz;
+#endif
+	return n;
+}
+
+int imu_fifo_capture_stop_and_send_rim(void)
+{
+	int rc = 0;
+	uint8_t count_raw[2] = {0};
+	uint16_t packet_count;
+
+	DBG_PRINTF("[IMU FIFO] stop_send: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
+
+	if (!s_fifo_capture_active)
+	{
+		DBG_PRINTF("[IMU FIFO] stop_send: not active -> empty rim\r\n");
+		imu_fifo_send_rim_packets(0);
+		return -1;
+	}
+
+	rc |= inv_imu_switch_on_mclk(&icm_driver);
+	DBG_PRINTF("[IMU FIFO] stop_send: mclk on rc=%d\r\n", rc);
+
+	rc |= inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
+	packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
+	DBG_PRINTF("[IMU FIFO] stop_send: FIFO_COUNT raw[0]=0x%02X [1]=0x%02X -> records=%u\r\n",
+			   count_raw[0], count_raw[1], (unsigned)packet_count);
+
+	if (packet_count > IMU_FIFO_MAX_PACKET_COUNT)
+	{
+		DBG_PRINTF("[IMU FIFO] stop_send: clamp %u -> %u\r\n",
+				   (unsigned)packet_count, (unsigned)IMU_FIFO_MAX_PACKET_COUNT);
+		packet_count = IMU_FIFO_MAX_PACKET_COUNT;
+	}
+
+	if ((rc == 0) && (packet_count > 0))
+	{
+		uint16_t before_count = packet_count;
+
+		rc |= imu_fifo_read_records(packet_count);
+		if (rc == 0)
+		{
+			packet_count = imu_fifo_compact_placeholder_records(icm_driver.fifo_data, packet_count);
+			if (before_count != packet_count)
+			{
+				DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid FIFO placeholder record(s)\r\n",
+						   (unsigned)(before_count - packet_count));
+			}
+			before_count = packet_count;
+			packet_count = imu_fifo_compact_invalid_gyro_records(icm_driver.fifo_data, packet_count);
+			if (before_count != packet_count)
+			{
+				DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid-gyro FIFO record(s)\r\n",
+						   (unsigned)(before_count - packet_count));
+			}
+		}
+	}
+	else
+	{
+		DBG_PRINTF("[IMU FIFO] stop_send: skip read (rc=%d count=%u)\r\n", rc, (unsigned)packet_count);
+	}
+
+	if ((rc == 0) && (IMU_FIFO_RIM_DROP_LEADING_SAMPLES > 0U) && (packet_count > 0U))
+	{
+		const uint16_t drop_req = (uint16_t)IMU_FIFO_RIM_DROP_LEADING_SAMPLES;
+		const uint16_t drop = (drop_req < packet_count) ? drop_req : packet_count;
+		const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
+		uint8_t *const fifo = icm_driver.fifo_data;
+
+		packet_count = (uint16_t)(packet_count - drop);
+		memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)packet_count * sz);
+		DBG_PRINTF("[IMU FIFO] stop_send: dropped %u leading sample(s), remain=%u\r\n",
+				   (unsigned)drop, (unsigned)packet_count);
+	}
+
+	if (rc == 0)
+	{
+		const uint16_t after_norm = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
+
+		if (after_norm != packet_count)
+		{
+			DBG_PRINTF("[IMU FIFO] stop_send: normalize %u -> %u samples (TARGET/MIN)\r\n",
+					   (unsigned)packet_count, (unsigned)after_norm);
+		}
+		packet_count = after_norm;
+	}
+
+	rc |= inv_imu_switch_off_mclk(&icm_driver);
+	DBG_PRINTF("[IMU FIFO] stop_send: mclk off rc=%d\r\n", rc);
+
+	if (rc != 0)
+	{
+		DBG_PRINTF("[IMU FIFO] drain fail %d\r\n", rc);
+		imu_fifo_send_rim_packets(0);
+		return rc;
+	}
+
+	DBG_PRINTF("[IMU FIFO] stop_send: rim TX records=%u\r\n", (unsigned)packet_count);
+	imu_fifo_send_rim_packets(packet_count);
+	DBG_PRINTF("[IMU FIFO] stop_send: OK\r\n");
+	return 0;
+}

project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h

@@ -97,5 +97,50 @@ void imu_callback(inv_imu_sensor_event_t *event);
  */
 int imu_read_direct(void);
 
+/**
+ * \brief Start IMU internal FIFO capture for mtb? test flow.
+ *        Configures accel/gyro 100 Hz and flushes FIFO before capture.
+ */
+int imu_fifo_capture_start(void);
+
+/**
+ * \brief Stop IMU FIFO capture, drain FIFO, and send raw FIFO bytes as rim: packets.
+ */
+int imu_fifo_capture_stop_and_send_rim(void);
+
+/*
+ * mtb? / rim: binary layout (every BLE fragment)
+ *   [ 'r' 'i' 'm' ':' ] [ total_sample_count u16 BE ] [ 12 * total_sample_count bytes ... ]
+ *   total_sample_count is the same in each fragment; per-fragment sample count = (payload_len - 6) / 12.
+ *
+ * IMU_FIFO_RIM_TARGET_SAMPLES: if > 0, cap at newest N samples (drop older when FIFO has more).
+ *     When fewer than N valid samples remain, send the actual count in the rim header (no zero pad).
+ *     Set to 0 to send all valid samples after filtering (up to the driver buffer limit).
+ * IMU_FIFO_RIM_DROP_LEADING_SAMPLES: drop the oldest N FIFO-derived samples before MIN/TARGET
+ *     (reduces startup + early-window transients; 0 = off).
+ * Override before including this header or via -D from the toolchain.
+ */
+#ifndef IMU_FIFO_RIM_TARGET_SAMPLES
+#define IMU_FIFO_RIM_TARGET_SAMPLES 15U
+#endif
+#ifndef IMU_FIFO_RIM_MIN_SAMPLES
+#define IMU_FIFO_RIM_MIN_SAMPLES 0U
+#endif
+#ifndef IMU_FIFO_RIM_DROP_LEADING_SAMPLES
+#define IMU_FIFO_RIM_DROP_LEADING_SAMPLES 0U
+#endif
+
+/*
+ * mtb? FIFO path — ICM42670 UI low-noise filter bandwidth (inv_imu_set_*_ln_bw).
+ * Enum suffix is approximate -3dB BW in Hz; smaller => smoother, more phase lag.
+ * Match to imu_fifo_capture_start() ODR (e.g. 25Hz → _16 or _25 typical).
+ */
+#ifndef IMU_FIFO_MTB_ACCEL_LN_BW
+#define IMU_FIFO_MTB_ACCEL_LN_BW ACCEL_CONFIG1_ACCEL_FILT_BW_16
+#endif
+#ifndef IMU_FIFO_MTB_GYRO_LN_BW
+#define IMU_FIFO_MTB_GYRO_LN_BW GYRO_CONFIG1_GYRO_FILT_BW_16
+#endif
+
 
 #endif /* !_APP_RAW_H_ */

project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c

@@ -60,6 +60,8 @@
 #include "nrf_gpiote.h"
 #include "nrf_ppi.h"
 #include "nrf_delay.h"
+#include "nrfx_timer.h"
+#include "nrf_pwr_mgmt.h"
 #include "power_control.h"
 #include "app_util_platform.h"
 
@@ -126,6 +128,9 @@ static volatile uint8_t  m_remaining_cycles = 0;        /* remaining pulse cycle
 static uint32_t          m_period_ticks = PERIOD_TICKS_2MHZ; /* current period (timer ticks) */
 static bool              m_power_enabled = false;       /* DC/DC converter state */
 static bool              m_initialized = false;         /* driver initialised flag */
+static nrfx_timer_t      m_mux_settle_timer = NRFX_TIMER_INSTANCE(4);
+static volatile bool     m_mux_settle_done = true;
+static bool              m_mux_settle_timer_ready = false;
 
 /*==============================================================================
  * Timer2 IRQ handler
@@ -363,6 +368,63 @@ static void dr_piezo_ppi_init(void)
     nrf_ppi_channel_enable((nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_1);
 }
 
+static void dr_piezo_mux_settle_timeout_handler(nrf_timer_event_t event_type, void *p_context)
+{
+    (void)p_context;
+
+    if (event_type != NRF_TIMER_EVENT_COMPARE0)
+    {
+        return;
+    }
+
+    nrfx_timer_disable(&m_mux_settle_timer);
+    m_mux_settle_done = true;
+}
+
+static void dr_piezo_mux_timer_init(void)
+{
+    nrfx_err_t err;
+    nrfx_timer_config_t config = NRFX_TIMER_DEFAULT_CONFIG;
+
+    config.frequency = NRF_TIMER_FREQ_1MHz;
+    config.mode = NRF_TIMER_MODE_TIMER;
+    config.bit_width = NRF_TIMER_BIT_WIDTH_16;
+    config.interrupt_priority = APP_IRQ_PRIORITY_LOWEST;
+
+    err = nrfx_timer_init(&m_mux_settle_timer, &config, dr_piezo_mux_settle_timeout_handler);
+    if (err == NRFX_SUCCESS)
+    {
+        m_mux_settle_timer_ready = true;
+    }
+    else
+    {
+        m_mux_settle_timer_ready = false;
+        DBG_PRINTF("[DR_PIEZO] MUX settle timer init failed: %lu\r\n", (unsigned long)err);
+    }
+}
+
+static void dr_piezo_mux_timer_start(void)
+{
+    if (!m_mux_settle_timer_ready)
+    {
+        nrf_delay_us(DR_PIEZO_MUX_SETTLING_US);
+        m_mux_settle_done = true;
+        return;
+    }
+
+    m_mux_settle_done = false;
+    nrfx_timer_disable(&m_mux_settle_timer);
+    nrfx_timer_clear(&m_mux_settle_timer);
+    nrfx_timer_extended_compare(
+        &m_mux_settle_timer,
+        NRF_TIMER_CC_CHANNEL0,
+        nrfx_timer_us_to_ticks(&m_mux_settle_timer, DR_PIEZO_MUX_SETTLING_US),
+        NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK,
+        true
+    );
+    nrfx_timer_enable(&m_mux_settle_timer);
+}
+
 /*==============================================================================
  * TX driver public functions
  *============================================================================*/
@@ -374,6 +436,7 @@ void dr_piezo_init(void)
     dr_piezo_gpiote_init();
     dr_piezo_timer_init();
     dr_piezo_ppi_init();
+    dr_piezo_mux_timer_init();
     
     m_tx_active = false;
     m_remaining_cycles = 0;
@@ -400,6 +463,13 @@ void dr_piezo_uninit(void)
     nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
     nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);
 
+    if (m_mux_settle_timer_ready)
+    {
+        nrfx_timer_disable(&m_mux_settle_timer);
+        nrfx_timer_uninit(&m_mux_settle_timer);
+        m_mux_settle_timer_ready = false;
+    }
+    
     m_tx_active = false;
     m_initialized = false;
 }
@@ -583,13 +653,13 @@ void dr_piezo_mux_init(void)
 /*
  * Select piezo channel (0~7)
  * Channel mapping (EN_MUXA, EN_MUXB, SEL0, SEL1):
- *   CH0 = MUXA input0 (1,0,0,0)   CH4 = MUXB input0 (0,1,1,1)
- *   CH1 = MUXA input2 (1,0,1,0)   CH5 = MUXB input1 (0,1,0,1)
- *   CH2 = MUXA input1 (1,0,0,1)   CH6 = MUXB input2 (0,1,1,0)
- *   CH3 = MUXA input3 (1,0,1,1)   CH7 = MUXB input3 (0,1,0,0)
+ *   CH0 = MUXA input0 (1,0,0,0)   CH4 = MUXB input0 (0,1,0,0)
+ *   CH1 = MUXA input2 (1,0,1,0)   CH5 = MUXB input1 (0,1,1,0)
+ *   CH2 = MUXA input1 (1,0,0,1)
+ *   CH3 = MUXA input3 (1,0,1,1)
  * MUX settling time (~1.3ms) required after channel switch.
  */
-void dr_piezo_select_channel(uint8_t channel)
+void dr_piezo_select_channel_start(uint8_t channel)
 {
     channel = channel & 0x07;  /* Mask to 0~7 range */
 
@@ -611,26 +681,37 @@ void dr_piezo_select_channel(uint8_t channel)
             nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB); nrf_gpio_pin_set(DR_PIEZO_EN_MUXA);
             nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0);   nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);
             break;
-        /*case 4: // B0: EN_MUXA=0, EN_MUXB=1, SEL0=1, SEL1=1
-            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
-            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0);   nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);
-            break;
-        case 5: // B1: EN_MUXA=0, EN_MUXB=1, SEL0=0, SEL1=1
-            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
-            nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);
-            break;*/
-        case 5: // B2: EN_MUXA=0, EN_MUXB=1, SEL0=1, SEL1=0
-            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
-            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
-            break;
-        case 4: // B3: EN_MUXA=0, EN_MUXB=1, SEL0=0, SEL1=0
+        case 4: // B0: EN_MUXA=0, EN_MUXB=1, SEL0=0, SEL1=0
             nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
             nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
             break;
+        case 5: // B1: EN_MUXA=0, EN_MUXB=1, SEL0=1, SEL1=0
+            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
+            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
+            break;
     }
 
-    /* MUX settling time required after channel change (> 1.2ms) */
-    nrf_delay_us(DR_PIEZO_MUX_SETTLING_US);
+    /* Start one-shot timer for MUX settling (> 1.2ms). */
+    dr_piezo_mux_timer_start();
+}
+
+void dr_piezo_wait_mux_settled(void)
+{
+    while (!m_mux_settle_done)
+    {
+        nrf_pwr_mgmt_run();
+    }
+}
+
+bool dr_piezo_is_mux_settled(void)
+{
+    return m_mux_settle_done;
+}
+
+void dr_piezo_select_channel(uint8_t channel)
+{
+    dr_piezo_select_channel_start(channel);
+    dr_piezo_wait_mux_settled();
 }
 
 /* Pin test: toggle each signal pin HIGH/LOW in sequence (for oscilloscope verification) */
@@ -1532,4 +1613,3 @@ void dr_piezo_burst_sw_17mhz(uint8_t cycles)
 
     __enable_irq();
 }
-

project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.h

@@ -95,11 +95,27 @@ void dr_piezo_test_pins(void);
 void dr_piezo_mux_init(void);
 
 /**
- * @brief Select piezo channel (0..7) via 8ch MUX
- * @note MUX settling time: ~1.3 ms delay after switching
+ * @brief Select piezo channel and wait for MUX settling
+ * @note Uses a one-shot timer for the ~1.3 ms settling interval.
  */
 void dr_piezo_select_channel(uint8_t channel);
 
+/**
+ * @brief Select piezo channel and start MUX settling timer
+ * @note Does not wait for settling to complete.
+ */
+void dr_piezo_select_channel_start(uint8_t channel);
+
+/**
+ * @brief Wait until the outstanding MUX settling interval completes
+ */
+void dr_piezo_wait_mux_settled(void);
+
+/**
+ * @brief Check whether the current MUX settling interval has completed
+ */
+bool dr_piezo_is_mux_settled(void);
+
 /*==============================================================================
  * System functions (power + TX combined)
  *============================================================================*/

project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx

@@ -3148,7 +3148,7 @@
 
   <Group>
     <GroupName>ICM42670</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>