IMU FIFO 동시 진입 방지 및 MCLK ready timeout 추가

- IMU FIFO capture가 이미 활성화되어 있는 상태에서 msp?/mim? 등이 들어오는 경우 요청 무시
- MCLK_RDY 대기에 timeout 추가해 IMU/I2C 상태가 꼬였을 때 메인 루프가 멈추지 않도록 함

components/libraries/bootloader/nrf_bootloader.c

@@ -64,6 +64,7 @@ static volatile bool m_flash_write_done;
 
 #define SCHED_QUEUE_SIZE      32          /**< Maximum number of events in the scheduler queue. */
 #define SCHED_EVENT_DATA_SIZE NRF_DFU_SCHED_EVENT_DATA_SIZE /**< Maximum app_scheduler event size. */
+#define POST_DFU_APP_BOOT_GPREGRET2_MASK 0x02U
 
 #if !(defined(NRF_BL_DFU_ENTER_METHOD_BUTTON)    && \
       defined(NRF_BL_DFU_ENTER_METHOD_PINRESET)  && \
@@ -470,6 +471,7 @@ ret_code_t nrf_bootloader_init(nrf_dfu_observer_t observer)
             break;
 
         case ACTIVATION_SUCCESS:
+            nrf_power_gpregret2_set(nrf_power_gpregret2_get() | POST_DFU_APP_BOOT_GPREGRET2_MASK);
             bootloader_reset(true);
             NRF_LOG_ERROR("Unreachable");
             return NRF_ERROR_INTERNAL; // Should not reach this.

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

@@ -29,7 +29,7 @@ extern uint8_t ble_bin_buffer[];
 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, size_t length);
 extern void format_data(uint8_t *buffer, const char *tag, const uint16_t *data_array, size_t length);
-extern void dr_binary_tx_safe(const uint8_t *buffer, uint16_t length);  /* length: word count */
+extern uint32_t dr_binary_tx_safe(const uint8_t *buffer, uint16_t length);  /* length: word count */
 extern void dr_sd_delay_ms(uint32_t ms);
 extern volatile bool data_tx_in_progress;
 
@@ -46,8 +46,12 @@ extern uint8_t resetCount;
  *----------------------------------------------------------------------------*/
 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_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
+extern int  imu_fifo_capture_start(void);
+extern int  imu_fifo_capture_stop_and_send_rim(void);
+extern int  imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms);
+extern void imu_fifo_capture_disable(void);
 extern void battery_timer_stop(void);
 extern void main_timer_start(void);
 extern void hw_i2c_init_once(void);
@@ -63,7 +67,6 @@ extern bool motion_raw_data_enabled;
 extern volatile uint16_t info_batt;
 extern volatile uint16_t info_temp;
 extern volatile uint16_t info_imu[6];
-extern volatile bool tmp235_saadc_done;
 extern volatile bool battery_saadc_done;
 
 /*------------------------------------------------------------------------------

project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c

@@ -41,6 +41,7 @@ static const CmdEntry m_cmd_table[] = {
     { "msn?", true, Cmd_msn },
     { "mst?", true, Cmd_mst },
     { "msp?", true, Cmd_msp },
+    { "mim?", true, Cmd_mim },
 
     /* D. Piezo ultrasound */
     { "mpa?", true, Cmd_mpa },
@@ -49,6 +50,8 @@ static const CmdEntry m_cmd_table[] = {
     { "mec?", true, Cmd_mec },
     { "maa?", true, Cmd_maa },
     { "mbb?", true, Cmd_mbb },
+    { "mab?", true, Cmd_mab }, // B-Mode Test
+    { "mtb?", true, Cmd_mtb },
     { "mcf?", true, Cmd_mcf },
     { "mcs?", true, Cmd_mcs },
 

project/ble_peripheral/ble_app_bladder_patch/command/dr_util.c

@@ -12,7 +12,7 @@
 extern void single_format_data(uint8_t *buffer, const char *tag, uint16_t value);
 extern void format_data(uint8_t *buffer, const char *tag, uint16_t *data, uint8_t length);
 extern uint8_t ble_bin_buffer[];
-extern void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
+extern uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
 
 void dr_ble_return_1(const char *tag, uint16_t value)
 {

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

@@ -12,6 +12,7 @@
 #include "cmd_device.h"
 #include "fstorage.h"
 #include "led_control.h"
+#include "app_raw.h"
 
 /*==============================================================================
  * msq? -> rsq:  Device power OFF
@@ -172,6 +173,14 @@ int Cmd_mls(const ParsedCmd *cmd)
     }
 
     led_set_state((led_state_t)state);
+    if ((led_state_t)state == LED_STATE_OFF)
+    {
+        if (imu_fifo_capture_is_active())
+        {
+            imu_fifo_capture_disable();
+        }
+    }
+
     dr_ble_return_1("rls:", state);
     return 1;
 }

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

@@ -16,6 +16,100 @@
 #include "cmd_sensor.h"           /* all_sensors() */
 #include "dr_piezo.h"
 #include "dr_adc121s051.h"
+#include "led_control.h"
+#include "../../measurement/piezo/piezo_config.h"
+
+static void mtb_send_rim_after_piezo(void)
+{
+    send_imu_rim_fifo();
+}
+
+static void mab_led_off_after_piezo(void)
+{
+    led_set_state(LED_STATE_OFF);
+}
+
+/*------------------------------------------------------------------------------
+ * 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 < PIEZO_CONFIG_CYCLES_MIN)
+    {
+        return (uint8_t)PIEZO_CONFIG_CYCLES_MIN;
+    }
+    if (cycles > PIEZO_CONFIG_CYCLES_MAX)
+    {
+        return (uint8_t)PIEZO_CONFIG_CYCLES_MAX;
+    }
+    return (uint8_t)cycles;
+}
+
+static uint16_t clamp_piezo_averaging(uint16_t averaging)
+{
+    if (averaging < PIEZO_CONFIG_AVERAGING_MIN)
+    {
+        return PIEZO_CONFIG_AVERAGING_MIN;
+    }
+    if (averaging > PIEZO_CONFIG_AVERAGING_MAX)
+    {
+        return PIEZO_CONFIG_AVERAGING_MAX;
+    }
+    return averaging;
+}
+
+static uint16_t clamp_piezo_delay_us(uint16_t delay_us)
+{
+    if (delay_us > PIEZO_CONFIG_DELAY_US_MAX)
+    {
+        return PIEZO_CONFIG_DELAY_US_MAX;
+    }
+    return delay_us;
+}
+
+static uint16_t clamp_piezo_num_samples(uint16_t num_samples)
+{
+    if (num_samples < PIEZO_CONFIG_NUM_SAMPLES_MIN)
+    {
+        return PIEZO_CONFIG_NUM_SAMPLES_MIN;
+    }
+    if (num_samples > PIEZO_CONFIG_NUM_SAMPLES_MAX)
+    {
+        return PIEZO_CONFIG_NUM_SAMPLES_MAX;
+    }
+    return num_samples;
+}
 
 /*==============================================================================
  * mpa? -> rpa:  Enable piezo TX/RX circuit
@@ -71,8 +165,8 @@ int Cmd_mpb(const ParsedCmd *cmd)
  *============================================================================*/
 int Cmd_mpc(const ParsedCmd *cmd)
 {
-    uint16_t cycles = 5;
+    uint16_t cycles = DR_PIEZO_DEFAULT_CYCLES;
-    uint16_t freq_option = 1;
+    uint16_t freq_option = PIEZO_CONFIG_FREQ_OPTION_DEFAULT;
     uint16_t piezo_ch = 0;
 
     (void)dr_get_u16(cmd, 0, &cycles);
@@ -84,7 +178,7 @@ int Cmd_mpc(const ParsedCmd *cmd)
         piezo_ch = 0;
     }
 
-    if (cycles < 3 || cycles > 7)
+    if (cycles < PIEZO_CONFIG_CYCLES_MIN || cycles > PIEZO_CONFIG_CYCLES_MAX)
     {
         dr_ble_return_1("rpc:", 2);
         return 1;
@@ -131,7 +225,7 @@ int Cmd_mec(const ParsedCmd *cmd)
     uint16_t freq_option = 0;
     uint16_t delay_us = 20;
     uint16_t num_samples = 140;
-    uint16_t cycles = 5;
+    uint16_t cycles = DR_PIEZO_DEFAULT_CYCLES;
     uint16_t averaging = 1;
     uint16_t piezo_ch = 0;
 
@@ -196,11 +290,6 @@ int Cmd_maa(const ParsedCmd *cmd)
         return 1;
     }
 
-    if (!dr_piezo_is_power_on())
-    {
-        dr_piezo_power_on();
-    }
-
     err = maa_async_start(
         m_config.piezo_freq_option,
         m_config.piezo_delay_us,
@@ -277,6 +366,112 @@ 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();
+
+    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;
+}
+
+/*==============================================================================
+ * mab? -> reb:+raa:  6-channel asynchronous full capture + GR LED
+ *
+ * Request:  [TAG 4B "mab?"] [CRC 2B]
+ * Response: per channel  reb: [num_samples 2B] [raw_data...]
+ *           final         raa: [status 2B]
+ * Error:    raa: + 0xFFFE        (previous capture in progress)
+ *           raa: + (0xFF00|err)  (start failed)
+ *
+ * For B-Mode Test
+ *============================================================================*/
+int Cmd_mab(const ParsedCmd *cmd)
+{
+    dr_adc_err_t err;
+    (void)cmd;
+
+    if (maa_async_is_busy())
+    {
+        dr_ble_return_1("raa:", 0xFFFE);
+        return 1;
+    }
+
+    led_set_state(LED_STATE_ALIGN_COMPLETE);
+
+    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_mab] 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();
+        led_set_state(LED_STATE_OFF);
+        return 1;
+    }
+
+    maa_async_set_on_complete(mab_led_off_after_piezo);
+
+    return 1;
+}
+
 /*==============================================================================
  * mcf? -> rcf:  Read piezo parameters from FDS
  *
@@ -303,6 +498,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 +526,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_freq_option = clamp_piezo_freq_option(freq);
-    m_config.piezo_cycles      = (uint8_t)cycles;
+    m_config.piezo_cycles      = clamp_piezo_cycles(cycles);
-    m_config.piezo_averaging   = averaging;
+    m_config.piezo_averaging   = clamp_piezo_averaging(averaging);
-    m_config.piezo_delay_us    = delay_us;
+    m_config.piezo_delay_us    = clamp_piezo_delay_us(delay_us);
-    m_config.piezo_num_samples = num_samples;
+    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,8 @@ 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_mab(const ParsedCmd *cmd);   /* mab? -> reb:+raa:  6-channel async capture + green LED */
+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

@@ -2,14 +2,15 @@
  * cmd_sensor.c - Sensor measurement handlers
  *
  * msn? -> rsn:  battery ADC measurement
- * mso? -> rso:  TMP235 temperature reading
+ * mst? -> rso:  IMU die temperature reading
  * msp? -> rsp:  IMU 6-axis single read
+ * mim? -> rim:  IMU FIFO 15-sample read
  * all_sensors()  bulk-measurement helper used by the mbb? handler
  *============================================================================*/
 
 #include "cmd_common.h"
 #include "cmd_sensor.h"
-#include "dr_piezo.h"
+#include "app_raw.h"
 
 /*==============================================================================
  * msn? -> rsn:  Battery level ADC measurement
@@ -25,40 +26,30 @@ int Cmd_msn(const ParsedCmd *cmd)
 }
 
 /*==============================================================================
- * mst? -> rso:  Temperature with piezo power cycle
+ * mst? -> rso:  IMU die temperature
  *
  * Request:  [TAG 4B "mst?"] [CRC 2B]
  * Response: [TAG 4B "rso:"] [temp_x100 2B BE] [CRC 2B]
- *
- * TMP235 shares the piezo TX/RX power rail. This command handles the full
- * sequence: power ON -> measure -> power OFF, so the caller doesn't need
- * to send mpa?/mpb? separately.
- * Response is sent from the TMP235 SAADC callback (tmp235_voltage_handler).
  *============================================================================*/
 int Cmd_mst(const ParsedCmd *cmd)
 {
-    uint32_t timeout_cnt;
+    uint16_t imu_temp_x100 = 0xFFFF;
     (void)cmd;
 
-    if (!dr_piezo_is_power_on())
+    if (imu_read_temperature_x100(&imu_temp_x100, NULL) != 0)
     {
-        dr_piezo_power_on();
+        imu_temp_x100 = 0xFFFF;
     }
 
-    tmp235_saadc_done = false;
+    single_format_data(ble_bin_buffer, "rso:", imu_temp_x100);
-    tmp235_voltage_level_meas();
+    dr_binary_tx_safe(ble_bin_buffer, 3);
-    for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
-    {
-        dr_sd_delay_ms(1);
-    }
-
-    dr_piezo_power_off();
 
     return 1;
 }
 
 /*==============================================================================
  * msp? -> rsp:  IMU 6-axis single read
+ * mim? -> rim:  IMU FIFO 15-sample read
  *
  * Request:  [TAG 4B "msp?"] [CRC 2B]
  * Response: rsp: + accel(xyz) + gyro(xyz)  (transmitted inside imu_read_direct)
@@ -68,11 +59,49 @@ int Cmd_mst(const ParsedCmd *cmd)
 int Cmd_msp(const ParsedCmd *cmd)
 {
     (void)cmd;
+
+    if (imu_fifo_capture_is_active())
+    {
+        return 1;   // already owned by mtb?/mim?, ignore duplicate request
+    }
+
     hw_i2c_init_once();
     imu_read_direct();
     return 1;
 }
 
+/*==============================================================================
+ * mim? -> rim:  IMU FIFO-only capture
+ *
+ * Request:  [TAG 4B "mim?"] [CRC 2B]
+ * Response: rim: [total_sample_count u16 BE]
+ *           [samples: 12B each ax,ay,az,gx,gy,gz ...]
+ *
+ * Uses the same FIFO/rim path as mtb? but without piezo reb:/raa: packets.
+ * At 50 Hz, 15 samples are about 300 ms; timeout leaves margin for startup.
+ *============================================================================*/
+int Cmd_mim(const ParsedCmd *cmd)
+{
+    int rc;
+    (void)cmd;
+
+    if (imu_fifo_capture_is_active())
+    {
+        return 1;   // already owned by mtb?/mim?, ignore duplicate request
+    }
+
+    rc = imu_fifo_capture_start();
+    if (rc != 0)
+    {
+        (void)imu_fifo_capture_stop_and_send_rim();
+        imu_fifo_capture_disable();
+        return 1;
+    }
+
+    (void)imu_fifo_capture_wait_samples_and_send_rim(15, 500);
+    return 1;
+}
+
 /*==============================================================================
  * all_sensors() - Bulk-measurement helper for the mbb? handler
  *
@@ -80,7 +109,7 @@ int Cmd_msp(const ParsedCmd *cmd)
  * single rbb: packet. SAADC measurements run asynchronously (callback), so
  * dr_sd_delay_ms() is used to wait for completion.
  *
- * Order: battery -> IMU -> (Piezo TX/RX ON) -> temperature
+ * Order: battery -> IMU 6-axis + IMU temperature
  * Response: rbb: [batt 2B] [IMU 6x2B] [temp 2B] = 20 bytes = 10 words
  *============================================================================*/
 void all_sensors(void)
@@ -98,23 +127,11 @@ void all_sensors(void)
         dr_sd_delay_ms(1);
     }
 
-    /* 2. IMU 6-axis single read -> info_imu[6] */
+    /* 2. IMU 6-axis single read + IMU die temperature -> info_imu[6], info_temp */
+    info_temp = 0xFFFF;
     hw_i2c_init_once();
     imu_read_direct();
 
-    /* 3. Temperature -> info_temp  (TMP235 needs Piezo TX/RX power) */
-    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;
 
     /* Assemble and transmit the rbb: packet (dedicated buffer to avoid
@@ -136,3 +153,55 @@ 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 -> IMU 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;
+    uint16_t imu_temp_x100;
+
+    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 (imu_read_temperature_x100(&imu_temp_x100, NULL) == 0)
+    {
+        info_temp = imu_temp_x100;
+    }
+    else
+    {
+        info_temp = 0xFFFF;
+    }
+
+    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

@@ -7,10 +7,18 @@
 #include "parser.h"
 
 int Cmd_msn(const ParsedCmd *cmd);   /* msn? -> rsn:  battery ADC measurement */
-int Cmd_mst(const ParsedCmd *cmd);   /* mst? -> rso:  TMP235 with piezo power cycle */
+int Cmd_mst(const ParsedCmd *cmd);   /* mst? -> rso:  IMU die temperature */
 int Cmd_msp(const ParsedCmd *cmd);   /* msp? -> rsp:  IMU 6-axis single read */
+int Cmd_mim(const ParsedCmd *cmd);   /* mim? -> rim:  IMU FIFO 15 samples */
 /* Helper for the mbb? handler: sequentially measures battery / IMU / temperature, then emits a single rbb: response.
  * 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/command/parser.h

@@ -24,7 +24,7 @@
  *----------------------------------------------------------------------------*/
 typedef struct {
     void (*log)(const char *fmt, ...);
-    void (*tx_bin)(const uint8_t *buf, uint16_t len);  /* len: word (uint16) count */
+    uint32_t (*tx_bin)(const uint8_t *buf, uint16_t len);  /* len: word (uint16) count */
     bool crc_check;
 } dr_platform_if_t;
 

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 ('
+set "SERIALNUMBER="
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
 
-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 ('
+set "SERIALNUMBER="
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
 
-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 ('
+set "SERIALNUMBER="
-    powershell -Command "(nrfutil device list --json | Select-String '\"type\":\"info\"' | ConvertFrom-Json).data.devices[0].serialNumber"
-') do set SERIALNUMBER=%%A
 
-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/download/generate_dfu_zip_with_bootloader.bat

@@ -0,0 +1,66 @@
+@echo off
+chcp 437
+setlocal EnableExtensions
+
+cd /d "%~dp0"
+
+set "APP_HEX=medithings_bladder_patch_0001.hex"
+set "BOOT_HEX=medithings_bladder_patch_bootloader.hex"
+set "OUT_APP_ZIP=medithings_bladder_patch_dfu.zip"
+set "OUT_BOOT_ZIP=medithings_bladder_patch_bootloader_dfu.zip"
+
+REM Bootloader DFU requires a version greater than the currently installed bootloader_version.
+REM cpd/cpd_eraseALL currently install bootloader-version 1, so use 2 for this update.
+set "APP_VERSION=1"
+set "BOOTLOADER_VERSION=2"
+set "HW_VERSION=52"
+set "SD_REQ=0x0100"
+set "SD_ID=0x0100"
+
+echo [1/4] Copying application HEX...
+copy /Y "..\pca10056\s140\arm5_no_packs\_build\nrf52840_xxaa.hex" "%APP_HEX%"
+if errorlevel 1 goto fail
+
+echo [2/4] Copying bootloader HEX...
+copy /Y "..\..\..\dfu\secure_bootloader\pca10056_s140_ble\arm5_no_packs\_build\nrf52840_xxaa_s140.hex" "%BOOT_HEX%"
+if errorlevel 1 goto fail
+
+echo [3/4] Generating application DFU package...
+nrfutil pkg generate ^
+    --application "%APP_HEX%" ^
+    --application-version %APP_VERSION% ^
+    --hw-version %HW_VERSION% ^
+    --sd-req %SD_REQ% ^
+    --sd-id %SD_ID% ^
+    --key-file private.key ^
+    "%OUT_APP_ZIP%"
+if errorlevel 1 goto fail
+
+echo [4/4] Generating bootloader DFU package...
+nrfutil pkg generate ^
+    --bootloader "%BOOT_HEX%" ^
+    --bootloader-version %BOOTLOADER_VERSION% ^
+    --hw-version %HW_VERSION% ^
+    --sd-req %SD_REQ% ^
+    --key-file private.key ^
+    "%OUT_BOOT_ZIP%"
+if errorlevel 1 goto fail
+
+echo.
+echo Done:
+echo   %OUT_APP_ZIP%
+echo   %OUT_BOOT_ZIP%
+echo Bootloader version: %BOOTLOADER_VERSION%
+echo.
+echo Update order for both app and bootloader changes:
+echo   1. Send %OUT_BOOT_ZIP%
+echo   2. Power on again if the device powers off
+echo   3. Send %OUT_APP_ZIP%
+pause
+exit /b 0
+
+:fail
+echo.
+echo ERROR: Failed to generate DFU package(s).
+pause
+exit /b 1

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

@@ -40,6 +40,7 @@
 #include "nrf_pwr_mgmt.h"
 #include "main.h"
 #include "debug_print.h"
+#include "../../measurement/piezo/piezo_config.h"
 
 
 /* FDS record identifiers */
@@ -47,7 +48,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;
@@ -107,11 +108,11 @@ void fds_default_value_set(void)
     m_config.life_cycle  = 0;
 
     /* Piezo measurement parameter defaults */
-    m_config.piezo_freq_option = 1;      /* 2.1 MHz */
+    m_config.piezo_freq_option = PIEZO_CONFIG_FREQ_OPTION_DEFAULT;  /* 2.1 MHz */
-    m_config.piezo_delay_us    = 10;     /* 10 us after burst */
+    m_config.piezo_delay_us    = PIEZO_CONFIG_DELAY_US_DEFAULT;     /* 10 us after burst */
-    m_config.piezo_num_samples = 100;    /* 100 samples */
+    m_config.piezo_num_samples = PIEZO_CONFIG_NUM_SAMPLES_DEFAULT;  /* 100 samples */
-    m_config.piezo_cycles      = 3;      /* 7 cycles */
+    m_config.piezo_cycles      = PIEZO_CONFIG_CYCLES_DEFAULT;       /* 3 cycles */
-    m_config.piezo_averaging   = 3;      /* 3x averaging */
+    m_config.piezo_averaging   = PIEZO_CONFIG_AVERAGING_DEFAULT;    /* 3x averaging */
 
     /* Factory provisioning — default: not provisioned */
     m_config.factory_provisioned = 0;

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_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) */
+    uint8_t     piezo_cycles;       /* 1B  - burst pulse cycle count (3~7) */
-    uint16_t    piezo_averaging;    /* 2B  - averages per channel (1..10) */
+    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_delay_us;     /* 2B  - delay from TX pulse to ADC start (us) (0~50) */
-    uint16_t    piezo_num_samples;  /* 2B  - ADC sample count (80..140) */
+    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.h

@@ -38,8 +38,25 @@
 *      : 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.
+*   - VBTFW0115 260518 jhChun
+*      : Added mtb? command (reb:+raa:+rim:, 6ch piezo + ICM42670 FIFO at 50 Hz).
+*      : rim: packet format and FIFO sample cap/filter options (app_raw).
+*   - VBTFW0116 260522 jhChun : Expanded BLE TX pending slots from 1 → 8 to reduce ADC data packet loss when the TX queue is full
+*   - VBTFW0117 260527 jhChun : Prevent battery/temperature safety checks from powering off piezo during MAA capture, and lock BLE PHY to 1M.
+*   - VBTFW0118 260601 jhChun
+*      : Added channel information(2B) to the ADC raw data response packet (reb:).
+*      : Replaced TMP235 temperature with IMU register direct-read temperature in the rbb: response.
+*   - VBTFW0119 260615 jhChun
+*      : Replaced all TMP235 temperature paths with IMU register direct-read temperature and removed legacy TMP235 build references.
+*      : Unify both IMU direct-read and FIFO outputs as raw data in datasheet axis convention.
+*      : Added mim? command (rim:, IMU FIFO-only 15 samples at 50 Hz).
+*   - VBTFW0120 260615 jhChun : Add EMC mitigation logic for BLE link stability: extended supervision timeout and dynamic TX power control based on RSSI, RSSI silence, HVN latency, and TX queue congestion.
+*   - VBTFW0121 260703 jhChun : Add mim? cammand.
+*   - VBTFW0122 260708 jhChun : Prevent IMU FIFO reentry and add MCLK ready timeout.
 ------------------------------------------------------------------------- */
-#define FIRMWARE_VERSION                "VBTFW0112"
+#define FIRMWARE_VERSION                "VBTFW0122"
 
 /*==============================================================================
  * Data Length Constants
@@ -136,7 +153,7 @@ void data_tx_handler(char const *p_data_to_send);
 /* Safe binary data BLE transmission (CRC16 appended, with retry logic)
  * @param ble_bin_buff  Binary buffer to transmit
  * @param length        Data length in uint16_t words (actual bytes = length x 2) */
-void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
+uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
 
 /* SoftDevice-compatible delay (nrf_delay_ms wrapper) */
 void dr_sd_delay_ms(uint32_t ms);

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

@@ -53,10 +53,80 @@ extern void dr_piezo_burst_sw_20mhz(uint8_t cycles);
 extern void dr_piezo_burst_sw_17mhz(uint8_t cycles);
 extern void dr_piezo_burst_sw_22mhz(uint8_t cycles);
 extern void dr_piezo_burst_sw_19mhz(uint8_t cycles);
+extern void dr_piezo_power_on(void);
 extern void dr_piezo_power_off(void);
+extern bool dr_piezo_is_power_on(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
  *============================================================================*/
@@ -82,7 +152,14 @@ extern void dr_piezo_power_off(void);
 #define BLE_RDB_HEADER_LEN      8       /* "rdb:" tag(4) + num_samples(2) + compressed_size(2) */
 #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 MAA_REB_HEADER_LEN      8       /* "reb:" tag(4) + ch_info(2) + num_samples(2) */
+#define MAA_REB_DATA_LEN        (BLE_MTU_SIZE - MAA_REB_HEADER_LEN)   /* 236 bytes = 118 samples */
 #define BLE_PACKET_DELAY_MS     100     /* Inter-packet delay - allow BLE TX buffer to drain */
+/* maa_async: inter-packet busy-wait (0 = rely on BLE_NUS_EVT_TX_RDY only). */
+#define MAA_ASYNC_POST_REB_MS   0U
+#define MAA_ASYNC_POST_RED_MS   0U
+#define MAA_ASYNC_PRE_RAA_MS    0U
+#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 */
@@ -532,7 +609,7 @@ uint32_t dr_adc_get_vref(void)
  *============================================================================*/
 
 /* External BLE NUS functions and variables from main.c */
-extern void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
+extern uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length);
 extern void dr_sd_delay_ms(uint32_t ms);
 
 /* Platform interface (log function) */
@@ -548,24 +625,10 @@ extern void dr_piezo_burst_sw(uint8_t cycles);
 /*==============================================================================
  * PIEZO CHANNEL SELECTION
  *============================================================================*/
- /*
- * Channel mapping (8ch)
 
- *         | EN MUXA | EN MUXB |  SEL 0  |  SEL 1
+/* Channel selection is split into GPIO switch and timer-backed settling wait. */
- *   ----------------------------------------------
+extern void dr_piezo_select_channel_start(uint8_t channel);
- *   CH A0 |    1    |    0    |    0    |    0
+extern void dr_piezo_wait_mux_settled(void);
- *   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);
 
 /*==============================================================================
  * INTEGRATED BURST + CAPTURE + TRANSMIT
@@ -592,26 +655,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)
+    freq_option = normalize_freq_option(freq_option);
-    {
+    delay_us = clamp_measure_delay_us(delay_us);
-        return DR_ADC_ERR_INVALID_PARAM;
+    num_samples = clamp_measure_num_samples(num_samples);
-    }
+    cycles = clamp_measure_cycles(cycles);
-    if (freq_option > 9)
+    averaging = clamp_measure_averaging(averaging);
-    {
-        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 */
-    }
     if (piezo_ch >= MAA_NUM_CHANNELS)
     {
         piezo_ch = 0;  /* clamp channel range */
@@ -638,7 +686,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 +743,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
+            /* Wait for previous echo and ringing to decay before next measurement.
-             * 1ms = ~77cm round-trip decay time (sound speed 1.54mm/us)
+             * Skip delay on first iteration. */
-             * 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 +882,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)
+    freq_option = normalize_freq_option(freq_option);
-    {
+    delay_us = clamp_measure_delay_us(delay_us);
-        return DR_ADC_ERR_INVALID_PARAM;
+    num_samples = clamp_measure_num_samples(num_samples);
-    }
+    cycles = clamp_measure_cycles(cycles);
-    if (freq_option > 3)
+    averaging = clamp_measure_averaging(averaging);
-    {
-        freq_option = 0;
-    }
-    if (cycles < 3 || cycles > 7)
-    {
-        cycles = 5;
-    }
-    if (averaging == 0)
-    {
-        averaging = 1;
-    }
-    if (averaging > 1000)
-    {
-        averaging = 1000;
-    }
     if (piezo_ch > (MAA_NUM_CHANNELS - 1))
     {
         piezo_ch = 0;
@@ -874,7 +908,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 +917,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 +939,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 */
@@ -1117,9 +1164,21 @@ dr_adc_err_t dr_adc_transmit_channel_delta(const dr_maa_channel_t *ch_data, uint
 
 /* Global async context */
 static maa_async_ctx_t g_maa_ctx = { .state = MAA_ASYNC_IDLE };
+static uint8_t g_maa_ch_session = 0;
 
 /* (reb+red merged protocol) */
 
+typedef enum {
+    MAA_ASYNC_TX_RETRY = 0,
+    MAA_ASYNC_TX_DONE,
+    MAA_ASYNC_TX_MORE
+} maa_async_tx_result_t;
+
+static bool maa_async_tx_accepted(uint32_t err_code)
+{
+    return (err_code == NRF_SUCCESS || err_code == NRF_ERROR_RESOURCES);
+}
+
 /**
  * @brief Capture one channel (internal helper)
  */
@@ -1146,33 +1205,43 @@ static dr_adc_err_t maa_async_capture_channel(uint8_t ch)
 /**
  * @brief Send reb: header + data merged for current channel
  *
- * reb: tag(4) + num_samples(2) + data(up to 238 bytes)
+ * reb: tag(4) + ch_info(2) + num_samples(2) + data(up to 236 bytes)
+ * ch_info: session(1) + channel(1)
  * If <= 100 samples (200B), the channel completes in this single packet
  */
-static void maa_async_send_header(void)
+static bool maa_async_send_header(void)
 {
     dr_maa_channel_t *ch = &g_maa_ctx.channels[g_maa_ctx.current_ch];
     uint8_t *buf = g_maa_ctx.ble_buffer;
+    uint32_t tx_err;
 
     uint16_t total_data_bytes = ch->num_samples * 2;
+    uint16_t ch_info = ((uint16_t)g_maa_ctx.ch_session << 8) | g_maa_ctx.current_ch;
 
-    /* reb: header + data merged (Big-Endian) */
+    /* reb: header + channel info + data merged (Big-Endian) */
     buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
-    buf[4] = (uint8_t)(ch->num_samples >> 8);
+    buf[4] = (uint8_t)(ch_info >> 8);
-    buf[5] = (uint8_t)(ch->num_samples & 0xFF);
+    buf[5] = (uint8_t)(ch_info & 0xFF);
+    buf[6] = (uint8_t)(ch->num_samples >> 8);
+    buf[7] = (uint8_t)(ch->num_samples & 0xFF);
 
-    uint16_t dst_idx = BLE_REB_HEADER_LEN;
+    uint16_t dst_idx = MAA_REB_HEADER_LEN;
-    uint16_t first_chunk = (total_data_bytes > BLE_REB_DATA_LEN) ? BLE_REB_DATA_LEN : total_data_bytes;
+    uint16_t first_chunk = (total_data_bytes > MAA_REB_DATA_LEN) ? MAA_REB_DATA_LEN : total_data_bytes;
     uint16_t src_idx = 0;
-    while (src_idx < ch->num_samples && (dst_idx - BLE_REB_HEADER_LEN) < first_chunk)
+    while (src_idx < ch->num_samples && (dst_idx - MAA_REB_HEADER_LEN) < first_chunk)
     {
         uint16_t sample = ch->samples[src_idx++];
         buf[dst_idx++] = (uint8_t)(sample >> 8);
         buf[dst_idx++] = (uint8_t)(sample & 0xFF);
     }
 
-    dr_binary_tx_safe(buf, dst_idx / 2);
+    tx_err = dr_binary_tx_safe(buf, dst_idx / 2);
-    dr_sd_delay_ms(5);  /* minimal delay; dr_binary_tx_safe retries internally (40 ms) */
+    if (!maa_async_tx_accepted(tx_err))
+    {
+        return false;
+    }
+
+    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 */
@@ -1186,21 +1255,24 @@ static void maa_async_send_header(void)
     {
         g_maa_ctx.state = MAA_ASYNC_TX_DATA;
     }
+
+    return true;
 }
 
 /**
  * @brief Send next red: data packet
- * @return true if more packets to send, false if done with current channel
+ * @return TX result: retry, current channel done, or more packets remain
  */
-static bool maa_async_send_data_packet(void)
+static maa_async_tx_result_t maa_async_send_data_packet(void)
 {
     dr_maa_channel_t *ch = &g_maa_ctx.channels[g_maa_ctx.current_ch];
     uint8_t *buf = g_maa_ctx.ble_buffer;
     uint16_t total_data_bytes = ch->num_samples * 2;
+    uint32_t tx_err;
 
     if (g_maa_ctx.data_offset >= total_data_bytes)
     {
-        return false;  /* All data sent */
+        return MAA_ASYNC_TX_DONE;  /* All data sent */
     }
 
     uint16_t pkt_idx = g_maa_ctx.current_pkt;
@@ -1222,32 +1294,42 @@ static bool maa_async_send_data_packet(void)
         buf[dst_idx++] = (uint8_t)(sample & 0xFF);
     }
 
-    dr_binary_tx_safe(buf, dst_idx / 2);
+    tx_err = dr_binary_tx_safe(buf, dst_idx / 2);
-    dr_sd_delay_ms(50);  /* Allow BLE stack to process TX */
+    if (!maa_async_tx_accepted(tx_err))
+    {
+        return MAA_ASYNC_TX_RETRY;
+    }
+
+    dr_sd_delay_ms(MAA_ASYNC_POST_RED_MS);
 
     g_maa_ctx.data_offset += chunk_size;
     g_maa_ctx.current_pkt++;
 
     //ADC_LOG("maa_async: CH%u red:%u (%u/%u bytes)", g_maa_ctx.current_ch, pkt_idx, g_maa_ctx.data_offset, total_data_bytes);
 
-    return (g_maa_ctx.data_offset < total_data_bytes);
+    return (g_maa_ctx.data_offset < total_data_bytes) ? MAA_ASYNC_TX_MORE : MAA_ASYNC_TX_DONE;
 }
 
 /**
  * @brief Send raa: completion marker
  */
-static void maa_async_send_completion(uint16_t status)
+static bool maa_async_send_completion(uint16_t status)
 {
     uint8_t *buf = g_maa_ctx.ble_buffer;
+    uint32_t tx_err;
 
     /* 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);
     buf[5] = (uint8_t)(status & 0xFF);
 
-    dr_binary_tx_safe(buf, 3);
+    tx_err = dr_binary_tx_safe(buf, 3);
+    if (!maa_async_tx_accepted(tx_err))
+    {
+        return false;
+    }
 
     //if (g_plat.log) g_plat.log("-------------------------------------------------------------------------------------\r\n");
 
@@ -1264,6 +1346,8 @@ static void maa_async_send_completion(uint16_t status)
         g_maa_ctx.on_complete_cb = NULL;  /* one-shot: prevent re-entry */
         cb();
     }
+
+    return true;
 }
 
 /*==============================================================================
@@ -1286,31 +1370,33 @@ 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.freq_option = normalize_freq_option(freq_option);
-    g_maa_ctx.delay_us = delay_us;
+    g_maa_ctx.delay_us = clamp_measure_delay_us(delay_us);
-    g_maa_ctx.num_samples = num_samples;
+    g_maa_ctx.num_samples = clamp_measure_num_samples(num_samples);
-    g_maa_ctx.cycles = (cycles < 3 || cycles > 7) ? 5 : cycles;
+    g_maa_ctx.cycles = clamp_measure_cycles(cycles);
-    g_maa_ctx.averaging = (averaging == 0) ? 1 : ((averaging > 1000) ? 1000 : averaging);
+    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;
+    g_maa_ctx.ch_session = g_maa_ch_session++;
     g_maa_ctx.data_offset = 0;
     g_maa_ctx.on_complete_cb = NULL;  /* default: no callback (maa?) */
 
     /* capture all channels without BLE TX, then start transmission */
     g_maa_ctx.state = MAA_ASYNC_CAPTURING;
 
-    /* Warm up CH0 6 times and discard the results before the real capture starts. */
+    if (!dr_piezo_is_power_on())
-    for (uint8_t warmup = 0; warmup < 6; warmup++)
+    {
+        dr_piezo_power_on();
+    }
+
+    /* Warm up CH0 before real capture (MAA_ASYNC_WARMUP_COUNT). */
+    for (uint8_t warmup = 0; warmup < MAA_ASYNC_WARMUP_COUNT; warmup++)
     {
         err = dr_adc_capture_channel_only(
             g_maa_ctx.freq_option,
@@ -1358,7 +1444,8 @@ dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us, uint16_t nu
     dr_piezo_power_off();
 
     /* Send CH0 header - this will trigger TX_RDY for subsequent packets */
-    maa_async_send_header();
+    g_maa_ctx.state = MAA_ASYNC_TX_HEADER;
+    (void)maa_async_send_header();
 
     return DR_ADC_OK;
 }
@@ -1373,8 +1460,16 @@ bool maa_async_on_tx_ready(void)
     switch (g_maa_ctx.state)
     {
         case MAA_ASYNC_TX_DATA:
+        {
+            maa_async_tx_result_t tx_result;
+
             /* Send next data packet */
-            if (!maa_async_send_data_packet())
+            tx_result = maa_async_send_data_packet();
+            if (tx_result == MAA_ASYNC_TX_RETRY)
+            {
+                return true;
+            }
+            if (tx_result == MAA_ASYNC_TX_DONE)
             {
                 /* Current channel done, move to next */
                 g_maa_ctx.current_ch++;
@@ -1383,22 +1478,20 @@ bool maa_async_on_tx_ready(void)
                 {
                     /* All channels done */
                     g_maa_ctx.state = MAA_ASYNC_COMPLETE;
-                    maa_async_send_completion(0x0000);
+                    return maa_async_send_completion(0x0000) ? false : true;
-                    return false;
                 }
                 else
                 {
                     /* all channels already captured, send header directly */
-                    g_maa_ctx.state = MAA_ASYNC_CAPTURING;
+                    g_maa_ctx.state = MAA_ASYNC_TX_HEADER;
-                    maa_async_send_header();
+                    (void)maa_async_send_header();
                 }
             }
             return true;
+        }
 
         case MAA_ASYNC_TX_HEADER:
-            /* Header sent, start sending data */
+            (void)maa_async_send_header();
-            g_maa_ctx.state = MAA_ASYNC_TX_DATA;
-            maa_async_send_data_packet();
             return true;
 
         case MAA_ASYNC_CAPTURING:
@@ -1406,6 +1499,8 @@ bool maa_async_on_tx_ready(void)
             return true;
 
         case MAA_ASYNC_COMPLETE:
+            return maa_async_send_completion(0x0000) ? false : true;
+
         case MAA_ASYNC_IDLE:
         default:
             return false;

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

@@ -30,6 +30,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "nrf_gpio.h"
+#include "../piezo/piezo_config.h"
 
 /*==============================================================================
  * PIN CONFIGURATION
@@ -73,8 +74,8 @@
  *============================================================================*/
 #define DR_ADC_SCLK_MHZ             8.6f    /**< SPI bit-bang SCLK frequency */
 #define DR_ADC_CLOCKS_PER_SAMPLE    16      /**< ADC121S051: 16 SCLK per sample */
-#define DR_ADC_ECHO_SAMPLES_MAX     119     /**< Maximum samples */
+#define DR_ADC_ECHO_SAMPLES_MAX     PIEZO_CONFIG_NUM_SAMPLES_MAX     /**< Maximum samples */
-#define DR_ADC_ECHO_SAMPLES_DEFAULT 100     /**< Default samples */
+#define DR_ADC_ECHO_SAMPLES_DEFAULT PIEZO_CONFIG_NUM_SAMPLES_DEFAULT /**< Default samples */
 #define DR_ADC_SAMPLE_INTERVAL_US   1.86f   /**< 16 / 8.6MHz = 1.86us per sample */
 #define DR_ADC_SOUND_SPEED_MM_US    1.54f   /**< Sound speed in tissue (mm/us) */
 
@@ -326,6 +327,11 @@ void dr_piezo_select_channel(uint8_t channel);
 #define MAA_NUM_CHANNELS    6       /* 4 -> 8 -> 6  jhChun 26.03.17*/
 #define MAA_SAMPLES_MAX     200
 
+/* CH0 warm-up captures before maa/mbb/mtb (each costs ~1 MUX settle + burst + ADC). */
+#ifndef MAA_ASYNC_WARMUP_COUNT
+#define MAA_ASYNC_WARMUP_COUNT  5U
+#endif
+
 /**
  * @brief Echo data for one channel
  */
@@ -429,6 +435,7 @@ typedef struct {
     maa_async_state_t state;      /**< Current state */
     uint8_t     current_ch;       /**< Current channel (0~7) */
     uint8_t     current_pkt;      /**< Current packet index */
+    uint8_t     ch_session;       /**< reb: channel session id for one measurement set */
     uint16_t    data_offset;      /**< Bytes sent so far for current channel */
     uint8_t     freq_option;      /**< Frequency option */
     uint16_t    delay_us;         /**< Capture delay */

project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c

@@ -4,7 +4,7 @@
  * Measures battery voltage via nRF52840 SAADC on AIN2:
  *   - 12-bit resolution, 4x oversampling
  *   - Periodic safety check via battery_loop timer (60 s interval)
- *   - Sequential: battery -> temperature measurement
+ *   - Sequential: battery -> IMU temperature measurement
  *   - Auto power-off after 5 consecutive readings below 3500 mV or above 40 C
  *   - In info4 mode (bulk sensor collection): stores to info_batt
  *
@@ -27,7 +27,7 @@
 #include "app_timer.h"
 #include "battery_saadc.h"
 #include "main_timer.h"
-#include "tmp235_q1.h"
+#include "app_raw.h"
 #include "dr_piezo.h"
 #include "debug_print.h"
 
@@ -59,9 +59,6 @@ APP_TIMER_DEF(m_battery_loop_timer_id);
 /* Low-battery check flag — set by battery_loop, consumed by handler */
 bool low_battery_check = false;
 
-/* Safety check mode flag — set by battery handler, consumed by tmp235 handler */
-bool safety_check_mode = false;
-
 /* SAADC callback completion flag — used by all_sensors() to wait */
 volatile bool battery_saadc_done = false;
 
@@ -84,15 +81,15 @@ extern uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN];
 volatile uint16_t info_batt;
 
 /* info4 sequential measurement control flags */
-extern bool go_temp;
 extern bool go_batt;
 
 extern bool motion_raw_data_enabled;
 extern bool ble_got_new_data;
 extern bool motion_data_once;
+extern bool maa_async_is_busy(void);
 
 /*==============================================================================
- * safety_check_complete - Called by tmp235 handler after temperature measurement
+ * safety_check_complete - Called after IMU die temperature measurement
  *
  * Checks both battery voltage and temperature against thresholds.
  * 5 consecutive readings exceeding either threshold triggers power OFF.
@@ -102,6 +99,11 @@ void safety_check_complete(float temp_c)
     //DBG_PRINTF("[SAFETY] Batt=%d mV, Temp=%d.%d C\r\n",
     //           (int)safety_batt_mv, (int)temp_c, ((int)(temp_c * 10)) % 10);
 
+    if (maa_async_is_busy())
+    {
+        return;
+    }
+
     /* Battery check */
     if (safety_batt_mv <= LOW_BATTERY_VOLTAGE)
     {
@@ -161,7 +163,7 @@ void battery_event_handler(nrf_drv_saadc_evt_t const * p_event)
 
         register_val = p_event->data.done.p_buffer[0];
 
-        /* Release SAADC — shared with temperature / pressure ADC */
+        /* Release SAADC — shared with pressure ADC */
         nrf_drv_saadc_channel_uninit(0);
         nrf_drv_saadc_uninit();
 
@@ -173,19 +175,25 @@ void battery_event_handler(nrf_drv_saadc_evt_t const * p_event)
         /* Resistor divider correction factor 1.42 */
         batt_lvl_in_milli_volt_1 = batt_lvl_in_milli_volt_0 * 1.42f;
 
-        /* --- Safety check mode: store voltage, chain temperature measurement --- */
+        /* --- Safety check mode: store voltage, then check IMU die temperature --- */
         if (low_battery_check == true)
         {
+            float imu_temp_c = -273.15f;
+
             low_battery_check = false;
             safety_batt_mv = batt_lvl_in_milli_volt_1;
-            safety_check_mode = true;
 
-            /* TMP235 shares piezo TX/RX power rail */
+            if (imu_fifo_capture_is_active())
-            if (!dr_piezo_is_power_on())
             {
-                dr_piezo_power_on();
+                return;
             }
-            tmp235_voltage_level_meas();
+
+            if (imu_read_temperature_x100(NULL, &imu_temp_c) != 0)
+            {
+                DBG_PRINTF("[SAFETY] IMU temp read failed\r\n");
+            }
+
+            safety_check_complete(imu_temp_c);
         }
 
         /* --- info4 mode: store value for mbb? bulk response --- */
@@ -268,6 +276,10 @@ void battery_loop(void * p_context)
     {
         return;
     }
+    if (maa_async_is_busy())
+    {
+        return;
+    }
 
     low_battery_check = true;
     battery_level_meas();

project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h

@@ -8,7 +8,7 @@
  *   battery_timer_init/start/stop() : 60-second periodic monitoring timer
  *
  * Periodic safety check (every 60 s):
- *   Battery -> Temperature sequential measurement via SAADC.
+ *   Battery SAADC -> IMU die temperature sequential measurement.
  *   Auto power-off after 5 consecutive readings below LOW_BATTERY_VOLTAGE (3500 mV)
  *   or above OVER_TEMPERATURE_THRESHOLD (40 C).
  *============================================================================*/
@@ -25,10 +25,7 @@
 /* SAADC callback completion flag (used by all_sensors() to wait) */
 extern volatile bool battery_saadc_done;
 
-/* Safety check mode flag — set by battery_loop, consumed by tmp235 handler */
+/* Called when safety check temperature measurement completes */
-extern bool safety_check_mode;
-
-/* Called by tmp235 handler when safety check temperature measurement completes */
 void safety_check_complete(float temp_c);
 
 /* Start a single async battery measurement. Result handled in callback. */

project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_transport.c

@@ -101,6 +101,7 @@ int inv_imu_switch_on_mclk(struct inv_imu_device *s)
 	int status = 0;
 	uint8_t data;
 	struct inv_imu_transport *t = (struct inv_imu_transport *)s;
+	uint64_t start;
 
 	/* set IDLE bit only if it is not set yet */
 	if (t->need_mclk_cnt == 0) {
@@ -112,9 +113,23 @@ int inv_imu_switch_on_mclk(struct inv_imu_device *s)
 		if (status)
 			return status;
 
+		start = inv_imu_get_time_us();
+		
 		/* Check if MCLK is ready */
 		do {
 			status = inv_imu_read_reg(s, MCLK_RDY, 1, &data); 
+
+			/* Bound the MCLK wait so a bad IMU/I2C state cannot hang the main loop. */
+			if ((inv_imu_get_time_us() - start) >= 50000U)
+			{
+				status = 0;
+				if (inv_imu_read_reg(s, PWR_MGMT0, 1, &data) == 0)
+				{
+					data &= ~PWR_MGMT0_IDLE_MASK;
+					(void)inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
+				}
+				return INV_ERROR_TIMEOUT;
+			}
 		} while ((status != 0) || !(data & MCLK_RDY_MCLK_RDY_MASK));
 	} else {
 

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

@@ -20,22 +20,22 @@
  *      - FIFO disabled (direct register read mode)
  *   3) get_imu_data()         - Read sensor data from FIFO or registers
  *   4) imu_callback()         - Sensor data receive callback
- *      - Applies mounting matrix (board orientation correction)
+ *      - Keeps raw sensor axes (ICM42670P datasheet coordinate system)
  *      - info4 mode: stores data in info_imu[6]
  *      - BLE mode:   sends 6-axis data via BLE with "rsp:" tag
  *      - UART mode:  outputs text format to serial
  *   5) imu_read_direct()      - Direct I2C register read bypassing driver API
- *      - Configure sensor -> power ON -> wait 80ms -> read 12 bytes -> sleep
+ *      - Configure sensor -> power ON -> wait 80ms -> read temp + 6-axis -> sleep
  *
- * Mounting matrix:
+ * Axis convention:
- *   3x3 rotation matrix in Q30 fixed-point format, correcting the sensor's
+ *   Raw ICM42670P datasheet X/Y/Z axes. No mounting matrix is applied.
- *   physical mounting orientation to match the software coordinate system.
  ******************************************************************************/
 
 #include "sdk_config.h"
 #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,10 +45,11 @@
 #include "main.h"
 #include "debug_print.h"
 #include "nrf_delay.h"
+#include <string.h>
 
 
 /*
- * Data output format selection
+ * SCALED_DATA_G_DPS : Data output format selection
  * 0 : Raw data output (raw accel, gyro, temp)
  * 1 : Scaled data output (g, dps, Celsius)
  */
@@ -65,25 +66,6 @@ static struct inv_imu_device icm_driver;
 /* Binary buffer for BLE transmission */
  uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN] = {0};
 
-/*
- * ICM42670P mounting matrix (Q30 fixed-point)
- *
- * Coordinate transform based on the sensor's physical mounting orientation.
- * Q30 format: 1.0 = (1 << 30) = 0x40000000
- *
- * SM_REVB_DB (dev board): X->-Y, Y->X transform (90-degree rotation)
- * Default (SmartMotion): identity matrix (no transform)
- */
-#if (SM_BOARD_REV == SM_REVB_DB) /* when DB or EVB are used */
-static int32_t icm_mounting_matrix[9] = {  0,     -(1<<30), 0,
-                                          (1<<30), 0,       0,
-                                           0,      0,      (1<<30) };
-#else /* For SmartMotion */
-static int32_t icm_mounting_matrix[9] = {(1<<30),  0,       0,
-                                          0,      (1<<30),  0,
-                                          0,       0,      (1<<30)};
-#endif
-
 bool custom_add_data;                              		/* Custom data append flag (BLE transmission control) */
 extern bool motion_raw_data_enabled;               		/* Flag requesting raw data read from external module */
 extern char		ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; 	/* BLE text transmit buffer */
@@ -91,11 +73,7 @@ extern which_cmd_t cmd_type_t;                     		/* Current command source (
 uint16_t ssp_data[6]={0,};                         		/* 6-axis data array for BLE (accel XYZ + gyro XYZ) */
 extern bool info4;                                 		/* info4 mode flag (set by cmd_parse) */
  volatile uint16_t info_imu[6];                    		/* Global array storing IMU data in info4 mode */
-
+volatile uint16_t info_temp;                		/* Global temperature cache (deg C x 100) */
-/* --------------------------------------------------------------------------------------
- *  static function declaration
- * -------------------------------------------------------------------------------------- */
-static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
 
 /* --------------------------------------------------------------------------------------
  *  Functions definition
@@ -168,9 +146,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) {
@@ -260,7 +238,7 @@ static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
  *   1) Extract raw accel/gyro data from event
  *      - FIFO mode: handles timestamp rollover, supports high-res (20-bit)
  *      - Register mode: uses 16-bit data directly
- *   2) Apply mounting matrix (board orientation correction)
+ *   2) Keep raw ICM42670P datasheet axes
  *   3) Output data (branches by mode):
  *      - info4 mode: stores in info_imu[6] global array (polled externally)
  *      - UART mode: text output with "Tp" prefix for 6-axis data
@@ -328,10 +306,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
 	event->sensor_mask |= (1 << INV_SENSOR_GYRO);
 #endif
 
-	/* Apply mounting matrix — correct sensor physical orientation to software coordinates */
-	apply_mounting_matrix(icm_mounting_matrix, accel);
-	apply_mounting_matrix(icm_mounting_matrix, gyro);
-
 #if SCALED_DATA_G_DPS
 	/*
 	 * Convert raw data to physical units (g, dps)
@@ -351,15 +325,10 @@ void imu_callback(inv_imu_sensor_event_t *event)
 	else
 		temp_degc = 25 + ((float)event->temperature / 2);
 
-	/*
+	(void)timestamp;
-	 * Output scaled data via UART
+	(void)accel_g;
-	 */
+	(void)gyro_dps;
-	if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
+	(void)temp_degc;
-		DBG_PRINTF("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n",
-		        (uint32_t)timestamp,
-		        accel_g[0], accel_g[1], accel_g[2],
-		        temp_degc,
-		        gyro_dps[0], gyro_dps[1], gyro_dps[2]);
 #else
 
 	/*
@@ -380,11 +349,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
 			info_imu[5] = (uint16_t)gyro[2];
 		}
 
-		/* UART mode: output 6-axis data in text format with "Tp" prefix */
-		else if(cmd_type_t == CMD_UART) {
-			//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
-		}
-
 		/*
 			* BLE mode: send 6-axis data as binary packet via BLE
 			* ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ
@@ -400,7 +364,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
 			ssp_data[5] = (uint16_t)gyro[2];
 
 			format_data(imu_bin_buffer, "rsp:", ssp_data,12);
-			//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
 			dr_binary_tx_safe(imu_bin_buffer,8);
 
 			if(custom_add_data==true) {
@@ -414,37 +377,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
 #endif
 }
 
-/* --------------------------------------------------------------------------------------
- *  Static functions definition
- * -------------------------------------------------------------------------------------- */
-
-/*
- * apply_mounting_matrix()
- * Applies a Q30 fixed-point rotation matrix to a 3-axis vector.
- *
- * Calculation:
- *   result[i] = matrix[i*3+0]*raw[0] + matrix[i*3+1]*raw[1] + matrix[i*3+2]*raw[2]
- *   Right-shift result by 30 bits for Q30 -> integer conversion.
- *
- * Ensures a consistent coordinate system regardless of physical sensor orientation.
- */
-static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
-{
-	unsigned i;
-	int64_t data_q30[3];
-
-	for(i = 0; i < 3; i++) {
-		data_q30[i] =  ((int64_t)matrix[3*i+0] * raw[0]);
-		data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
-		data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
-	}
-	/* Q30 -> integer conversion: right-shift by 30 bits */
-	raw[0] = (int32_t)(data_q30[0]>>30);
-	raw[1] = (int32_t)(data_q30[1]>>30);
-	raw[2] = (int32_t)(data_q30[2]>>30);
-}
-
-
 /*
  * imu_read_direct()
  * Reads IMU registers directly via I2C, bypassing the driver API.
@@ -453,12 +385,12 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
  * Flow:
  *   1) Check TWI initialization (first call only)
  *   2) Gyro config: +/-2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
- *   3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
+ *   3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x49)
  *   4) Power ON: accel+gyro low-noise mode (PWR_MGMT0 = 0x0F)
  *   5) Wait 80ms (gyro startup: min 45ms + margin)
- *   6) Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B) (accel 6 + gyro 6)
+ *   6) Read 14 consecutive bytes from TEMP_DATA1 (0x09) (temp 2 + accel 6 + gyro 6)
  *   7) Big-endian -> int16_t conversion
- *   8) Apply mounting matrix
+ *   8) Keep raw ICM42670P datasheet axes
  *   9) Send via BLE with "rsp:" tag
  *  10) Switch IMU to sleep mode (power saving)
  *
@@ -471,7 +403,27 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
 extern const nrfx_twi_t m_twi_icm42670;
 
 #define IMU_I2C_ADDR   0x68
-#define REG_ACCEL_X1   0x0B  /* ACCEL_DATA_X1 — accel X-axis upper byte register */
+#define REG_TEMP_DATA1 0x09  /* TEMP_DATA1 — temperature upper byte register */
+#define IMU_TEMP_AVG_SAMPLES 4U
+
+static bool s_direct_twi_ready = false;
+static bool s_fifo_capture_active = false;
+
+static void imu_direct_twi_init_once(void)
+{
+	/* TWI (I2C) init — performed only once (re-init ensures clean state) */
+	if (!s_direct_twi_ready) {
+		inv_i2c_master_uninitialize();
+		inv_i2c_master_initialize();
+		s_direct_twi_ready = true;
+	}
+}
+
+static uint16_t imu_temp_raw_to_x100(int16_t temp_raw)
+{
+	float temp_c = 25.0f + ((float)temp_raw / 128.0f);
+	return (uint16_t)(temp_c * 100.0f);
+}
 
 /* --------------------------------------------------------------------------------------
  * Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
@@ -479,19 +431,16 @@ extern const nrfx_twi_t m_twi_icm42670;
  * -------------------------------------------------------------------------------------- */
 int imu_read_direct(void)
 {
-	uint8_t raw[12];  /* accel 6 bytes + gyro 6 bytes */
+	uint8_t raw[14];  /* temp 2 bytes + accel 6 bytes + gyro 6 bytes */
+	uint8_t temp_raw_bytes[2];
 	int32_t accel[3], gyro[3];
+	int16_t temp_raw;
+	int32_t temp_sum;
 	uint8_t reg;
 	uint32_t ret;
+	uint8_t i;
 
-	static bool twi_ready = false;
+	imu_direct_twi_init_once();
-
-	/* TWI (I2C) init — performed only once (re-init ensures clean state) */
-	if (!twi_ready) {
-		inv_i2c_master_uninitialize();
-		inv_i2c_master_initialize();
-		twi_ready = true;
-	}
 
 	/* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */
 	{
@@ -499,9 +448,9 @@ int imu_read_direct(void)
 		icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false);
 	}
 
-	/* Accel config: ACCEL_CONFIG0(0x21) = 0x29 -> +/-4g FSR, 100Hz ODR */
+	/* Accel config: ACCEL_CONFIG0(0x21) = 0x49 -> +/-4g FSR, 100Hz ODR */
 	{
-		uint8_t accel_cfg[2] = { 0x21, 0x29 };
+		uint8_t accel_cfg[2] = { 0x21, 0x49 };
 		icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
 	}
 
@@ -513,8 +462,8 @@ int imu_read_direct(void)
 		dr_sd_delay_ms(80);
 	}
 
-	/* Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B~0x16) */
+	/* Read 14 consecutive bytes from TEMP_DATA1 (0x09~0x16): temp + accel + gyro */
-	reg = REG_ACCEL_X1;
+	reg = REG_TEMP_DATA1;
 	ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);  /* Send register address (no STOP) */
 	
 	if (ret)
@@ -523,7 +472,7 @@ int imu_read_direct(void)
 		return -1;
 	}
 
-	ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12);  /* Receive 12 bytes of data */
+	ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));
 
 	if (ret)
 	{
@@ -533,19 +482,18 @@ int imu_read_direct(void)
 
 	/*
 	 * Convert big-endian register layout to int16_t
-	 * raw[0..5] = accel X,Y,Z (2 bytes each, MSB first)
+	 * raw[0..1] = temperature
-	 * raw[6..11] = gyro X,Y,Z (2 bytes each, MSB first)
+	 * raw[2..7] = accel X,Y,Z (2 bytes each, MSB first)
+	 * raw[8..13] = gyro X,Y,Z (2 bytes each, MSB first)
 	 */
-	accel[0] = (int16_t)((raw[0] << 8) | raw[1]);
+	temp_raw = (int16_t)((raw[0] << 8) | raw[1]);
-	accel[1] = (int16_t)((raw[2] << 8) | raw[3]);
+	temp_sum = temp_raw;
-	accel[2] = (int16_t)((raw[4] << 8) | raw[5]);
+	accel[0] = (int16_t)((raw[2] << 8) | raw[3]);
-	gyro[0]  = (int16_t)((raw[6] << 8) | raw[7]);
+	accel[1] = (int16_t)((raw[4] << 8) | raw[5]);
-	gyro[1]  = (int16_t)((raw[8] << 8) | raw[9]);
+	accel[2] = (int16_t)((raw[6] << 8) | raw[7]);
-	gyro[2]  = (int16_t)((raw[10] << 8) | raw[11]);
+	gyro[0]  = (int16_t)((raw[8] << 8) | raw[9]);
-
+	gyro[1]  = (int16_t)((raw[10] << 8) | raw[11]);
-	/* Apply mounting matrix — board orientation correction */
+	gyro[2]  = (int16_t)((raw[12] << 8) | raw[13]);
-	apply_mounting_matrix(icm_mounting_matrix, accel);
-	apply_mounting_matrix(icm_mounting_matrix, gyro);
 
 	/* Pack data */
 	ssp_data[0] = (uint16_t)accel[0];
@@ -557,6 +505,32 @@ int imu_read_direct(void)
 
 	if (info4 == true)
 	{
+		reg = REG_TEMP_DATA1;
+		for (i = 1; i < IMU_TEMP_AVG_SAMPLES; i++)
+		{
+			ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);
+			if (ret)
+			{
+				DBG_PRINTF("[IMU] temp avg tx FAIL %u\r\n", ret);
+				break;
+			}
+
+			ret = icm42670_twi_rx(IMU_I2C_ADDR, temp_raw_bytes, sizeof(temp_raw_bytes));
+			if (ret)
+			{
+				DBG_PRINTF("[IMU] temp avg rx FAIL %u\r\n", ret);
+				break;
+			}
+
+			temp_sum += (int16_t)((temp_raw_bytes[0] << 8) | temp_raw_bytes[1]);
+		}
+		if (i == IMU_TEMP_AVG_SAMPLES)
+		{
+			temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
+		}
+
+		uint16_t temp_x100 = imu_temp_raw_to_x100(temp_raw);
+
 		/* info4 mode: store in global array (sent as rbb: packet by mbb?) */
 		info_imu[0] = ssp_data[0];
 		info_imu[1] = ssp_data[1];
@@ -564,6 +538,7 @@ int imu_read_direct(void)
 		info_imu[3] = ssp_data[3];
 		info_imu[4] = ssp_data[4];
 		info_imu[5] = ssp_data[5];
+		info_temp = temp_x100;
 	}
 	else
 	{
@@ -580,3 +555,479 @@ int imu_read_direct(void)
 
 	return 0;
 }
+
+int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
+{
+	uint8_t raw[2];
+	uint8_t reg = REG_TEMP_DATA1;
+	int16_t temp_raw;
+	int32_t temp_sum = 0;
+	uint32_t ret;
+	uint8_t i;
+
+	if (temp_x100 == NULL && temp_c == NULL)
+	{
+		return -1;
+	}
+
+	if (s_fifo_capture_active)
+	{
+		return -4;
+	}
+
+	imu_direct_twi_init_once();
+
+	/* Power ON briefly so the temperature register is refreshed before reading. */
+	{
+		uint8_t pwr_cmd[2] = { 0x1F, 0x0F };
+		icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
+		dr_sd_delay_ms(80);
+	}
+
+	for (i = 0; i < IMU_TEMP_AVG_SAMPLES; i++)
+	{
+		ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);
+		if (ret)
+		{
+			uint8_t pwr_off[2] = { 0x1F, 0x00 };
+			icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
+			DBG_PRINTF("[IMU] temp tx FAIL %u\r\n", ret);
+			return -2;
+		}
+
+		ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));
+		if (ret)
+		{
+			uint8_t pwr_off[2] = { 0x1F, 0x00 };
+			icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
+			DBG_PRINTF("[IMU] temp rx FAIL %u\r\n", ret);
+			return -3;
+		}
+
+		temp_sum += (int16_t)((raw[0] << 8) | raw[1]);
+	}
+
+	temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
+	if (temp_x100 != NULL)
+	{
+		*temp_x100 = imu_temp_raw_to_x100(temp_raw);
+	}
+	if (temp_c != NULL)
+	{
+		*temp_c = 25.0f + ((float)temp_raw / 128.0f);
+	}
+
+	{
+		uint8_t pwr_off[2] = { 0x1F, 0x00 };
+		icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
+	}
+
+	return 0;
+}
+
+bool imu_fifo_capture_is_active(void)
+{
+	return s_fifo_capture_active;
+}
+
+/* --------------------------------------------------------------------------------------
+ * mtb? FIFO capture support
+ *
+ * Uses the ICM42670P internal FIFO at 50 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   20U
+#define IMU_FIFO_RIM_POST_TX_MS     0U
+#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 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;
+
+	inv_i2c_master_uninitialize();
+	inv_i2c_master_initialize();
+
+	imu_serif_make(&icm_serif);
+	rc = setup_imu_device(&icm_serif);
+	return rc;
+}
+
+static void imu_fifo_power_off(void)
+{
+	(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;
+}
+
+void imu_fifo_capture_disable(void)
+{
+	if (s_fifo_capture_active)
+	{
+		imu_fifo_power_off();
+	}
+}
+
+int imu_fifo_capture_start(void)
+{
+	int rc;
+
+	if (s_fifo_capture_active)
+	{
+		return 0;
+	}
+
+	rc = imu_fifo_driver_prepare();
+	if (rc != 0)
+	{
+		DBG_PRINTF("[IMU FIFO] prepare fail %d\r\n", rc);
+		return rc;
+	}
+
+	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_50_HZ); // FIFO ODR Accel 50Hz Setting
+	rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_50_HZ);	 // FIFO ODR Gyro 50Hz Setting
+	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); // FIFO Accel Low Noise Mode
+	//rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);	// FIFO Accel Low Power Mode TEST
+	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);	// FIFO Gyro Low Noise Mode
+	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;
+	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;
+
+	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;
+		}
+
+		dr_binary_tx_safe(rim_buf, dst_idx / 2);
+		record_idx += sample_count;
+		if (record_idx < total_sample_count)
+		{
+			dr_sd_delay_ms(IMU_FIFO_RIM_POST_TX_MS);
+		}
+	} while (record_idx < total_sample_count);
+}
+
+static int imu_fifo_read_records(uint16_t record_count)
+{
+	int rc = 0;
+	uint16_t record_idx = 0;
+
+	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;
+		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;
+	}
+
+	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;
+	}
+	(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++;
+		}
+	}
+#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;
+
+	if (!s_fifo_capture_active)
+	{
+		imu_fifo_send_rim_packets(0);
+		return -1;
+	}
+
+	rc |= inv_imu_switch_on_mclk(&icm_driver);
+
+	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);
+
+	if (packet_count > IMU_FIFO_MAX_PACKET_COUNT)
+	{
+		packet_count = IMU_FIFO_MAX_PACKET_COUNT;
+	}
+
+	if ((rc == 0) && (packet_count > 0))
+	{
+		rc |= imu_fifo_read_records(packet_count);
+		if (rc == 0)
+		{
+			packet_count = imu_fifo_compact_placeholder_records(icm_driver.fifo_data, packet_count);
+			packet_count = imu_fifo_compact_invalid_gyro_records(icm_driver.fifo_data, 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);
+	}
+
+	if (rc == 0)
+	{
+		packet_count = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
+	}
+
+	rc |= inv_imu_switch_off_mclk(&icm_driver);
+
+	if (rc != 0)
+	{
+		DBG_PRINTF("[IMU FIFO] drain fail %d\r\n", rc);
+		imu_fifo_send_rim_packets(0);
+		imu_fifo_power_off();
+		return rc;
+	}
+
+	imu_fifo_send_rim_packets(packet_count);
+	return 0;
+}
+
+int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms)
+{
+	int rc = 0;
+	uint8_t count_raw[2] = {0};
+	uint16_t packet_count = 0;
+	uint32_t waited_ms = 0;
+	const uint32_t poll_ms = 10U;
+
+	if (target_samples == 0U)
+	{
+		return imu_fifo_capture_stop_and_send_rim();
+	}
+
+	if (!s_fifo_capture_active)
+	{
+		imu_fifo_send_rim_packets(0);
+		return -1;
+	}
+
+	while (waited_ms <= timeout_ms)
+	{
+		rc = inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
+		if (rc != 0)
+		{
+			DBG_PRINTF("[IMU FIFO] count read fail %d\r\n", rc);
+			imu_fifo_send_rim_packets(0);
+			imu_fifo_power_off();
+			return rc;
+		}
+
+		packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
+		if (packet_count > target_samples)
+		{
+			break;
+		}
+
+		dr_sd_delay_ms(poll_ms);
+		waited_ms += poll_ms;
+	}
+
+	rc = imu_fifo_capture_stop_and_send_rim();
+	imu_fifo_capture_disable();
+	return rc;
+}

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

@@ -21,6 +21,7 @@
 
 #ifndef _APP_RAW_H_
 #define _APP_RAW_H_
+#include <stdbool.h>
 #include "sdk_config.h"
 
 #include <stdint.h>
@@ -97,5 +98,75 @@ void imu_callback(inv_imu_sensor_event_t *event);
  */
 int imu_read_direct(void);
 
+/**
+ * \brief Read IMU die temperature directly and return deg C x 100.
+ * \param[out] temp_x100 Optional temperature output in Celsius x 100.
+ * \param[out] temp_c Optional temperature output in Celsius.
+ * \return 0=success, negative=error
+ */
+int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
+
+/**
+ * \brief Return true while IMU FIFO capture is active.
+ */
+bool imu_fifo_capture_is_active(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);
+
+/**
+ * \brief Wait until at least target_samples FIFO records are available, then drain
+ *        FIFO and send rim: packets.
+ * \return 0=success, negative=error
+ */
+int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms);
+
+/**
+ * \brief Stop IMU FIFO capture without sending rim: packets.
+ */
+void imu_fifo_capture_disable(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 (50Hz in mtb? FIFO mode).
+ */
+#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;
@@ -399,6 +462,13 @@ void dr_piezo_uninit(void)
     nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
     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)
+ *   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,0,1)
+ *   CH1 = MUXA input2 (1,0,1,0)   CH5 = MUXB input1 (0,1,1,0)
- *   CH2 = MUXA input1 (1,0,0,1)   CH6 = MUXB input2 (0,1,1,0)
+ *   CH2 = MUXA input1 (1,0,0,1)
- *   CH3 = MUXA input3 (1,0,1,1)   CH7 = MUXB input3 (0,1,0,0)
+ *   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
+        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_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
             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) */
+    /* Start one-shot timer for MUX settling (> 1.2ms). */
-    nrf_delay_us(DR_PIEZO_MUX_SETTLING_US);
+    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

@@ -32,6 +32,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "nrf_gpio.h"
+#include "piezo_config.h"
 
 /*==============================================================================
  * Power control pin (+/-20V DC/DC converter)
@@ -64,9 +65,9 @@
  * Configuration
  *============================================================================*/
 #define DR_PIEZO_FREQ_HZ        2100000     /**< Target frequency (set PIEZO_FREQ_MHZ in .c) */
-#define DR_PIEZO_DEFAULT_CYCLES 5           /**< Default burst cycles */
+#define DR_PIEZO_DEFAULT_CYCLES PIEZO_CONFIG_CYCLES_DEFAULT /**< Default burst cycles */
-#define DR_PIEZO_MIN_CYCLES     3
+#define DR_PIEZO_MIN_CYCLES     PIEZO_CONFIG_CYCLES_MIN
-#define DR_PIEZO_MAX_CYCLES     7
+#define DR_PIEZO_MAX_CYCLES     PIEZO_CONFIG_CYCLES_MAX
 #define DR_PIEZO_MUX_SETTLING_US 1300       /**< MUX settling delay (us) */
 
 /*==============================================================================
@@ -95,11 +96,27 @@ void dr_piezo_test_pins(void);
 void dr_piezo_mux_init(void);
 
 /**
- * @brief Select piezo channel (0..7) via 8ch MUX
+ * @brief Select piezo channel and wait for MUX settling
- * @note MUX settling time: ~1.3 ms delay after switching
+ * @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/measurement/piezo/piezo_config.h

@@ -0,0 +1,29 @@
+/*==============================================================================
+ * piezo_config.h - Shared piezo measurement parameter limits/defaults
+ *============================================================================*/
+#ifndef PIEZO_CONFIG_H
+#define PIEZO_CONFIG_H
+
+/* Frequency option protocol:
+ *   0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 9=1.9MHz
+ */
+#define PIEZO_CONFIG_FREQ_OPTION_DEFAULT        1U
+
+#define PIEZO_CONFIG_CYCLES_MIN                 3U
+#define PIEZO_CONFIG_CYCLES_MAX                 7U
+#define PIEZO_CONFIG_CYCLES_DEFAULT             7U
+
+#define PIEZO_CONFIG_AVERAGING_MIN              1U
+#define PIEZO_CONFIG_AVERAGING_MAX              10U
+#define PIEZO_CONFIG_AVERAGING_DEFAULT          10U
+
+#define PIEZO_CONFIG_DELAY_US_MIN               0U
+#define PIEZO_CONFIG_DELAY_US_MAX               50U
+#define PIEZO_CONFIG_DELAY_US_DEFAULT           10U
+
+#define PIEZO_CONFIG_NUM_SAMPLES_MIN            80U
+#define PIEZO_CONFIG_NUM_SAMPLES_MAX            118U
+#define PIEZO_CONFIG_NUM_SAMPLES_DEFAULT        100U
+
+
+#endif /* PIEZO_CONFIG_H */

project/ble_peripheral/ble_app_bladder_patch/measurement/temperature/tmp235_q1.c

@@ -1,6 +1,9 @@
 /*==============================================================================
  * tmp235_q1.c - TMP235-Q1 analogue temperature sensor driver
  *
+ * Deprecated: temperature measurement has been replaced by IMU register direct read paths.
+ * This file is kept only as a legacy reference and is not built.
+ *
  * Reads the TMP235-Q1 analogue output via SAADC AIN3 and converts to deg C.
  *
  * Temperature conversion (piecewise linear, per datasheet):

project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/RTE/Device/nRF52840_xxAA/system_nrf52.c

@@ -32,6 +32,8 @@ NOTICE: This file has been modified by Nordic Semiconductor ASA.
 #include "system_nrf52_approtect.h"
 
 #define __SYSTEM_CLOCK_64M      (64000000UL)
+#define POWER_HOLD_PIN          8UL
+#define POWER_HOLD_PIN_MASK     (1UL << POWER_HOLD_PIN)
 
 
 #if defined ( __CC_ARM )
@@ -89,6 +91,15 @@ void SystemCoreClockUpdate(void)
 
 void SystemInit(void)
 {
+    /* Keep the external power latch asserted immediately after reset. */
+    NRF_P0->OUTSET = POWER_HOLD_PIN_MASK;
+    NRF_P0->DIRSET = POWER_HOLD_PIN_MASK;
+    NRF_P0->PIN_CNF[POWER_HOLD_PIN] =
+        (GPIO_PIN_CNF_DIR_Output << GPIO_PIN_CNF_DIR_Pos) |
+        (GPIO_PIN_CNF_INPUT_Disconnect << GPIO_PIN_CNF_INPUT_Pos) |
+        (GPIO_PIN_CNF_PULL_Disabled << GPIO_PIN_CNF_PULL_Pos) |
+        (GPIO_PIN_CNF_DRIVE_H0H1 << GPIO_PIN_CNF_DRIVE_Pos) |
+        (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
     /* Enable SWO trace functionality. If ENABLE_SWO is not defined, SWO pin will be used as GPIO (see Product
        Specification to see which one). */
     #if defined (ENABLE_SWO) && defined(CLOCK_TRACECONFIG_TRACEMUX_Pos)

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

@@ -408,11 +408,6 @@
               <FileType>1</FileType>
               <FilePath>..\..\..\system\power\power_control.c</FilePath>
             </File>
-            <File>
-              <FileName>tmp235_q1.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\..\..\measurement\temperature\tmp235_q1.c</FilePath>
-            </File>
             <File>
               <FileName>fstorage.c</FileName>
               <FileType>1</FileType>
@@ -4616,11 +4611,6 @@
               <FileType>1</FileType>
               <FilePath>..\..\..\system\power\power_control.c</FilePath>
             </File>
-            <File>
-              <FileName>tmp235_q1.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\..\..\measurement\temperature\tmp235_q1.c</FilePath>
-            </File>
             <File>
               <FileName>fstorage.c</FileName>
               <FileType>1</FileType>

project/ble_peripheral/ble_app_bladder_patch/system/main_timer.c

@@ -14,15 +14,10 @@
  *     - motion_data_once == true: one-shot read (after HW I2C init)
  *     - motion_data_once == false: continuous read (when not waiting for BLE TX)
  *   go_batt                -> Battery voltage measurement (battery_level_meas)
- *   go_temp                -> Temperature measurement (tmp235_voltage_level_meas)
  *   go_device_power_off    -> Device power OFF (device_power_off)
  *   go_sleep_mode_enter    -> Enter sleep mode (sleep_mode_enter)
  *   go_NVIC_SystemReset    -> NVIC system reset
  *
- * [info4 mode measurement order]
- *   IMU continuous read -> go_batt (battery) -> go_temp (temp) -> motion_data_once (IMU one-shot)
- *   After temperature measurement, motion_data_once=true to return to IMU.
- *
  * [Timer settings]
  *   - Normal mode: 10ms interval (MAIN_LOOP_INTERVAL)
  *   - FEATURE_DETAIL_VALUE_FULL mode: 80ms interval (for detail printout)
@@ -53,7 +48,6 @@
 #include "main.h"
 #include "app_raw_main.h"
 #include "main_timer.h"
-#include "tmp235_q1.h"
 //#include "fstorage.h"
 #include "power_control.h"
 #include "main.h"  /* 2026-03-17: cmd_parse.h removed, use main.h */
@@ -83,7 +77,6 @@ extern which_cmd_t cmd_type_t;
 /* Event flags (set by external modules, processed in main_loop)              */
 /* ========================================================================== */
 bool go_batt= false;                    /* Battery measurement request flag */
-bool go_temp= false;                    /* Temperature measurement request flag */
 bool go_device_power_off = false;       /* Device power OFF request flag */
 bool go_sleep_mode_enter = false;       /* Sleep mode entry request flag */
 bool go_NVIC_SystemReset = false;       /* System reset request flag */
@@ -101,10 +94,9 @@ bool motion_data_once = false;          /* IMU one-shot read mode (true: read on
  * [Processing priority] (checked in code order)
  *   1. IMU motion data (motion_raw_data_enabled)
  *   2. Battery measurement (go_batt)
- *   3. Temperature measurement (go_temp)
+ *   3. Power OFF (go_device_power_off)
- *   4. Power OFF (go_device_power_off)
+ *   4. Sleep mode (go_sleep_mode_enter)
- *   5. Sleep mode (go_sleep_mode_enter)
+ *   5. System reset (go_NVIC_SystemReset)
- *   6. System reset (go_NVIC_SystemReset)
  */
 void main_loop(void * p_context)	/* For x ms */
 {
@@ -192,33 +184,11 @@ void main_loop(void * p_context)	/* For x ms */
 		DBG_PRINTF("IMU BATT\r\n");
 		main_timer_stop();       /* Stop timer */
 		go_batt = false;         /* Consume flag (one-shot) */
-//		go_temp = true;
 			battery_level_meas();   /* Measure battery voltage via SAADC */
 //			nrf_delay_ms(20);
 //			m48_adc_start_init();
 //		main_timer_start();
 	}
-
-
-	/* ---- Temperature measurement ---- */
-	/*
-	 * If go_temp is true, measure temperature via TMP235-Q1 sensor.
-	 * Called after battery measurement in info4 mode.
-	 * After completion, sets motion_data_once=true so the next IMU read
-	 * uses one-shot mode (with HW I2C re-init).
-	 */
-	if (go_temp == true)
-	{
-		DBG_PRINTF("IMU Temp\r\n");
-		main_timer_stop();           /* Stop timer */
-//	go_batt = false;
-		go_temp = false;             /* Consume flag (one-shot) */
-		motion_data_once = true;     /* Switch next IMU read to one-shot mode */
-		tmp235_voltage_level_meas(); /* Measure TMP235-Q1 temperature sensor voltage */
-//	 motion_raw_data_enabled = true;
-//		main_timer_start();
-
-	}
 	
 	/* ---- System control event handling ---- */
 
@@ -285,4 +255,3 @@ void main_timer_init(void)
 {
     APP_ERROR_CHECK(app_timer_create(&m_main_loop_timer_id, APP_TIMER_MODE_SINGLE_SHOT, main_loop));
 }
-

project/dfu/secure_bootloader/main.c

@@ -76,6 +76,8 @@ static void dfu_observer(nrf_dfu_evt_type_t evt_type)
             break;
         case NRF_DFU_EVT_DFU_STARTED:
             break;
+        case NRF_DFU_EVT_DFU_COMPLETED:
+            break;
         default:
             break;
     }

project/dfu/secure_bootloader/pca10056_s140_ble/arm5_no_packs/RTE/Device/nRF52840_xxAA/system_nrf52.c

@@ -32,6 +32,8 @@ NOTICE: This file has been modified by Nordic Semiconductor ASA.
 #include "system_nrf52_approtect.h"
 
 #define __SYSTEM_CLOCK_64M      (64000000UL)
+#define POWER_HOLD_PIN          8UL
+#define POWER_HOLD_PIN_MASK     (1UL << POWER_HOLD_PIN)
 
 
 #if defined ( __CC_ARM )
@@ -89,6 +91,15 @@ void SystemCoreClockUpdate(void)
 
 void SystemInit(void)
 {
+    /* Keep the external power latch asserted immediately after reset. */
+    NRF_P0->OUTSET = POWER_HOLD_PIN_MASK;
+    NRF_P0->DIRSET = POWER_HOLD_PIN_MASK;
+    NRF_P0->PIN_CNF[POWER_HOLD_PIN] =
+        (GPIO_PIN_CNF_DIR_Output << GPIO_PIN_CNF_DIR_Pos) |
+        (GPIO_PIN_CNF_INPUT_Disconnect << GPIO_PIN_CNF_INPUT_Pos) |
+        (GPIO_PIN_CNF_PULL_Disabled << GPIO_PIN_CNF_PULL_Pos) |
+        (GPIO_PIN_CNF_DRIVE_H0H1 << GPIO_PIN_CNF_DRIVE_Pos) |
+        (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
     /* Enable SWO trace functionality. If ENABLE_SWO is not defined, SWO pin will be used as GPIO (see Product
        Specification to see which one). */
     #if defined (ENABLE_SWO) && defined(CLOCK_TRACECONFIG_TRACEMUX_Pos)

project/dfu/secure_bootloader/pca10056_s140_ble/arm5_no_packs/secure_bootloader_ble_s140_pca10056.uvoptx

@@ -73,7 +73,7 @@
         <LExpSel>0</LExpSel>
       </OPTXL>
       <OPTFL>
-        <tvExp>0</tvExp>
+        <tvExp>1</tvExp>
         <tvExpOptDlg>0</tvExpOptDlg>
         <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>

project/dfu/secure_bootloader/pca10056_s140_ble/config/sdk_config.h

@@ -141,6 +141,7 @@
 // <i> to immediately transfer a new application if it wishes.
 
 #ifndef NRF_BL_DFU_CONTINUATION_TIMEOUT_MS
+/* Time to wait for an expected DFU update immediately after entering DFU mode from the application. (10 seconds) */
 #define NRF_BL_DFU_CONTINUATION_TIMEOUT_MS 10000
 #endif
 
@@ -150,7 +151,8 @@
 // <i> If 0, no inactivity timer will be used. Values 1-99 are invalid.
 
 #ifndef NRF_BL_DFU_INACTIVITY_TIMEOUT_MS
-#define NRF_BL_DFU_INACTIVITY_TIMEOUT_MS 120000
+/* Time to stay in DFU mode after the last DFU activity before returning to the application. (60 seconds) */
+#define NRF_BL_DFU_INACTIVITY_TIMEOUT_MS 60000
 #endif
 
 // </h>