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.

project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h

@@ -46,11 +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);
@@ -66,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,7 @@ 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/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,12 +16,19 @@
 #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
  *----------------------------------------------------------------------------*/
@@ -58,48 +65,48 @@ static uint8_t clamp_piezo_freq_option(uint16_t raw_freq)
 
 static uint8_t clamp_piezo_cycles(uint16_t cycles)
 {
-    if (cycles < 3)
+    if (cycles < PIEZO_CONFIG_CYCLES_MIN)
     {
-        return 3;
+        return (uint8_t)PIEZO_CONFIG_CYCLES_MIN;
     }
-    if (cycles > 7)
+    if (cycles > PIEZO_CONFIG_CYCLES_MAX)
     {
-        return 7;
+        return (uint8_t)PIEZO_CONFIG_CYCLES_MAX;
     }
     return (uint8_t)cycles;
 }
 
 static uint16_t clamp_piezo_averaging(uint16_t averaging)
 {
-    if (averaging < 1)
+    if (averaging < PIEZO_CONFIG_AVERAGING_MIN)
     {
-        return 1;
+        return PIEZO_CONFIG_AVERAGING_MIN;
     }
-    if (averaging > 10)
+    if (averaging > PIEZO_CONFIG_AVERAGING_MAX)
     {
-        return 10;
+        return PIEZO_CONFIG_AVERAGING_MAX;
     }
     return averaging;
 }
 
 static uint16_t clamp_piezo_delay_us(uint16_t delay_us)
 {
-    if (delay_us > 50)
+    if (delay_us > PIEZO_CONFIG_DELAY_US_MAX)
     {
-        return 50;
+        return PIEZO_CONFIG_DELAY_US_MAX;
     }
     return delay_us;
 }
 
 static uint16_t clamp_piezo_num_samples(uint16_t num_samples)
 {
-    if (num_samples < 80)
+    if (num_samples < PIEZO_CONFIG_NUM_SAMPLES_MIN)
     {
-        return 80;
+        return PIEZO_CONFIG_NUM_SAMPLES_MIN;
     }
-    if (num_samples > 119)
+    if (num_samples > PIEZO_CONFIG_NUM_SAMPLES_MAX)
     {
-        return 119;
+        return PIEZO_CONFIG_NUM_SAMPLES_MAX;
     }
     return num_samples;
 }
@@ -158,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);
@@ -171,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;
@@ -218,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;
 
@@ -414,6 +421,57 @@ int Cmd_mtb(const ParsedCmd *cmd)
     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
  *

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

@@ -12,6 +12,7 @@ int Cmd_mpc(const ParsedCmd *cmd);   /* mpc? -> rpc:  burst generation */
 int Cmd_mec(const ParsedCmd *cmd);   /* mec? -> reb:+raa:  single-channel capture */
 int Cmd_maa(const ParsedCmd *cmd);   /* maa? -> reb:+raa:  6-channel async capture */
 int Cmd_mbb(const ParsedCmd *cmd);   /* mbb? -> rbb:+reb:+raa:  sensors + capture */
+int Cmd_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
  *

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

@@ -7,8 +7,9 @@
 #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);

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 */
@@ -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/main.c

@@ -12,7 +12,7 @@
  * [Hardware Configuration]
  *   - MCU: nRF52840 (SoftDevice S140 v7.x)
  *   - IMU: ICM42670P 6-axis accel/gyro (I2C, 0x68)
- *   - Temperature: TMP235-Q1 analog sensor (SAADC AIN3)
+ *   - Temperature: IMU register direct read
  *   - Battery: voltage divider (SAADC AIN2, 1/3 prescale)
  *   - Power: POWER_HOLD(P0.8) self-latch circuit, POWER_BUTTON(P1.8) input
  *
@@ -94,7 +94,6 @@
 #include "app_raw_main.h"       /* Sensor raw data processing module */
 #include "main_timer.h"         /* Main event loop timer (10ms period) */
 #include "power_control.h"      /* Power sequence control (ON/OFF/sleep) */
-#include "tmp235_q1.h"          /* TMP235-Q1 temperature sensor driver */
 #include "fds.h"                /* Flash Data Storage (non-volatile config) */
 #include "battery_saadc.h"      /* Battery voltage measurement (SAADC) */
 
@@ -143,11 +142,35 @@
 #define MIN_CONN_INTERVAL               MSEC_TO_UNITS(15, UNIT_1_25_MS)   /* Min connection interval: 15ms */
 #define MAX_CONN_INTERVAL               MSEC_TO_UNITS(15, UNIT_1_25_MS)   /* Max connection interval: 15ms */
 #define SLAVE_LATENCY                   0                                 /* Slave latency: 0 (respond every connection event) */
-#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(4000, UNIT_10_MS)   /* Connection supervision timeout: 4s */
+#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(10000, UNIT_10_MS)  /* EMC: extend supervision timeout 4s -> 10s to tolerate short RF fades */
 #define FIRST_CONN_PARAMS_UPDATE_DELAY  APP_TIMER_TICKS(5000)             /* Wait 5s before first param update request */
 #define NEXT_CONN_PARAMS_UPDATE_DELAY   APP_TIMER_TICKS(30000)            /* Subsequent param update interval: 30s */
 #define MAX_CONN_PARAMS_UPDATE_COUNT    3                                 /* Max param update attempts */
 
+/* EMC BLE link protection:
+ * - Start each connection at normal +4 dBm.
+ * - Boost to +8 dBm when RSSI is repeatedly poor, RSSI events stop, HVN complete is slow/stuck,
+ *   or the NUS pending queue backs up.
+ * - Restore to +4 dBm only after RSSI is stably good and TX is idle.
+ */
+#define BLE_TX_POWER_NORMAL_DBM         4       /* Normal connected TX power */
+#define BLE_TX_POWER_BOOST_DBM          8       /* Temporary TX power boost during link stress */
+#define BLE_TX_POWER_UNKNOWN_DBM        127     /* Sentinel to force first SoftDevice TX power apply */
+#define BLE_RSSI_BAD_DBM                (-80)   /* Weak central signal threshold: count toward boost */
+#define BLE_RSSI_GOOD_DBM               (-65)   /* Good central signal threshold: count toward restore */
+#define BLE_RSSI_BAD_COUNT_LIMIT        3       /* Poor RSSI events needed before RSSI-based boost */
+#define BLE_RSSI_GOOD_COUNT_LIMIT       10      /* Good RSSI events needed before restore */
+#define BLE_RSSI_CHANGE_THRESHOLD_DBM   1       /* Request RSSI event on >=1 dB change */
+#define BLE_RSSI_SKIP_COUNT             0       /* Report RSSI immediately; useful during EMC validation */
+
+#define BLE_LINK_STRESS_TICK_MS             200     /* Periodic link-health poll interval */
+#define BLE_LINK_STRESS_RSSI_SILENCE_MS     500     /* No RSSI event for this long -> suspect downlink stress */
+#define BLE_LINK_STRESS_HVN_SLOW_MS         50      /* HVN complete slower than this counts as TX stress */
+#define BLE_LINK_STRESS_HVN_VERY_SLOW_MS    100     /* HVN complete slower than this boosts immediately */
+#define BLE_LINK_STRESS_HVN_STUCK_MS        200     /* No HVN complete after submit -> boost immediately */
+#define BLE_LINK_STRESS_BOOST_COUNT         3       /* Consecutive slow HVN events before boost */
+#define BLE_LINK_STRESS_RECOVERY_MS         10000   /* Clear stress counter after quiet recovery period */
+
 /*==============================================================================
  * System Constants
  *============================================================================*/
@@ -158,6 +181,7 @@
 #define POWER_OFF_DELAY                 3000        /* Power-off delay: 3s after LED indication */
 #define POWER_RESET_DELAY               2000        /* Reset delay: 2s */
 #define LED_NUM                         24          /* LED pin number */
+#define POST_DFU_APP_BOOT_GPREGRET2_MASK 0x02U       /* Bootloader marker after DFU activation */
 
 /*==============================================================================
  * BLE Instances (statically allocated via SoftDevice macros)
@@ -173,6 +197,7 @@ BLE_ADVERTISING_DEF(m_advertising);                /* Advertising module instanc
 APP_TIMER_DEF(m_power_on_delay_timer_id);          /* Power button polling timer (5ms single-shot, main_s callback) */
 APP_TIMER_DEF(m_power_off_delay_timer_id);         /* Power-off delay timer (physical power cut after 3s) */
 APP_TIMER_DEF(m_PM_timer_id);                      /* Peer Manager timer (forced disconnect) */
+APP_TIMER_DEF(m_link_stress_timer_id);             /* EMC: BLE link stress monitor (periodic RSSI/HVN watchdog) */
 
 /*==============================================================================
  * Static Variables
@@ -184,6 +209,17 @@ static pm_peer_id_t m_peer_to_be_deleted = PM_PEER_ID_INVALID;
 static uint16_t   m_conn_handle          = BLE_CONN_HANDLE_INVALID;         /* Current BLE connection handle */
 static uint16_t   m_ble_nus_max_data_len = BLE_GATT_ATT_MTU_DEFAULT - 3;    /* NUS max data length (MTU - overhead) */
 static ble_uuid_t m_adv_uuids[]          = {{BLE_UUID_NUS_SERVICE, NUS_SERVICE_UUID_TYPE}};  /* UUIDs included in advertising */
+static int8_t     m_ble_tx_power_dbm     = BLE_TX_POWER_UNKNOWN_DBM;        /* Currently applied connected TX power */
+static uint8_t    m_rssi_bad_count       = 0;                               /* Consecutive poor RSSI event count */
+static uint8_t    m_rssi_good_count      = 0;                               /* Consecutive good RSSI event count */
+static uint8_t    m_link_stress_count    = 0;                               /* Consecutive slow-HVN stress count */
+static uint32_t   m_last_rssi_tick       = 0;                               /* Last RSSI event tick for silence detection */
+static uint32_t   m_last_stress_tick     = 0;                               /* Last stress tick for recovery reset */
+static uint32_t   m_hvn_send_tick        = 0;                               /* Last successful notification submit tick */
+static bool       m_hvn_send_pending     = false;                           /* True until BLE_GATTS_EVT_HVN_TX_COMPLETE */
+static bool       m_rssi_silence_latched = false;                           /* Prevent repeated RSSI silence logs/boosts */
+static bool       m_rssi_bad_latched     = false;                           /* Prevent repeated RSSI_BAD logs/boosts */
+static bool       m_hvn_stuck_latched    = false;                           /* Prevent repeated HVN_STUCK logs/boosts */
 
 static uint8_t  m_tx_buffer[BLE_NUS_MAX_DATA_LEN] = {0}; /* ASCII text transmission buffer */
 static uint16_t m_tx_len = 0;                            /* Data length to transmit */
@@ -195,7 +231,7 @@ static char * roles_str[] = {"INVALID_ROLE", "CENTRAL", "PERIPHERAL"};
 
 /*==============================================================================
  * Global Variables
- * IEC 62304 §5.5.3: all global buffers zero-initialized for deterministic startup
+ * IEC 62304 5.5.3: all global buffers zero-initialized for deterministic startup
  *============================================================================*/
 volatile uint8_t Sj_type;                     /* Command type identifier */
 bool power_off_duble_prohibit = false;        /* Power-off double prevention flag */
@@ -229,7 +265,8 @@ volatile bool ble_connection_st;                   /* BLE connection state (1=co
 volatile bool data_tx_in_progress = false;         /* Binary TX in progress flag */
 
 /* -- BLE TX async retry state -- */
-#define BLE_TX_PENDING_QUEUE_SIZE   8U
+#define BLE_TX_PENDING_QUEUE_SIZE       8U
+#define BLE_TX_PENDING_BOOST_THRESHOLD  4U   /* EMC: boost TX power if queued packets accumulate */
 static uint8_t          s_tx_pending_buf[BLE_TX_PENDING_QUEUE_SIZE][BLE_NUS_MAX_DATA_LEN] = {{0}};  /* Pending packets (with CRC) */
 static uint16_t         s_tx_pending_len[BLE_TX_PENDING_QUEUE_SIZE] = {0};                          /* Pending packet lengths */
 static volatile uint8_t s_tx_pending_head = 0;                                                       /* Next packet to retry */
@@ -395,6 +432,7 @@ static void load_flash_config(void)
 static void main_s(void * p_context);                       /* Power button state machine (forward declaration) */
 static void t_power_off_timeout_handler(void * p_context);  /* Power-off timeout */
 static void PM_s(void * p_context);                         /* Peer Manager timer */
+static void ble_link_stress_tick_handler(void * p_context); /* EMC: BLE RSSI/HVN link stress timer */
 
 /**
  * @brief Power-off timeout callback
@@ -441,6 +479,7 @@ static void PM_s(void * p_context)
  *   - m_power_on_delay_timer: power button polling (5ms single-shot, main_s callback)
  *   - m_power_off_delay_timer: power-off delay (3s single-shot)
  *   - m_PM_timer: Peer Manager disconnect (single-shot)
+ *   - m_link_stress_timer: EMC BLE link monitor (RSSI silence + HVN latency)
  *   - main_timer: main event loop (10ms single-shot, main_loop callback)
  *   - battery_timer: battery monitoring (5s repeating)
  *   - power_timer: power sequence (20ms single-shot, power_loop callback)
@@ -453,6 +492,7 @@ static void timers_init(void)
     APP_ERROR_CHECK(app_timer_create(&m_power_on_delay_timer_id, APP_TIMER_MODE_SINGLE_SHOT, main_s));
     APP_ERROR_CHECK(app_timer_create(&m_power_off_delay_timer_id, APP_TIMER_MODE_SINGLE_SHOT, t_power_off_timeout_handler));
     APP_ERROR_CHECK(app_timer_create(&m_PM_timer_id, APP_TIMER_MODE_SINGLE_SHOT, PM_s));
+    APP_ERROR_CHECK(app_timer_create(&m_link_stress_timer_id, APP_TIMER_MODE_REPEATED, ble_link_stress_tick_handler));
     
     main_timer_init();
     battery_timer_init();
@@ -513,7 +553,7 @@ static void ble_dfu_evt_handler(ble_dfu_buttonless_evt_type_t event)
  * @brief Initialize GAP (Generic Access Profile) parameters
  *
  * 1) Set device name to SERIAL_NO -> shown during BLE scan
- * 2) Configure connection parameters (20~75ms interval, slave latency 0, supervision timeout 4s)
+ * 2) Configure connection parameters (15ms interval, slave latency 0, EMC supervision timeout 10s)
  * 3) Set static passkey (when FEATURE_STATIC_PASSKEY enabled)
  */
 static void gap_params_init(void)
@@ -565,6 +605,11 @@ extern void maa_async_abort(void);
 static volatile uint8_t  pending_cmd_buf[BLE_NUS_MAX_DATA_LEN] = {0};
 static volatile uint8_t  pending_cmd_len = 0;
 
+static void ble_link_stress_on_hvn_send(void);
+static void ble_link_stress_on_hvn_complete(void);
+static void ble_link_stress_boost_now(const char * p_reason, uint32_t elapsed_ms);
+static void ble_link_stress_check_pending_depth(void);
+
 static bool ble_tx_pending_has_data(void)
 {
     return (s_tx_pending_count > 0);
@@ -626,6 +671,7 @@ static bool ble_retry_pending_tx(void)
 
         if (err == NRF_SUCCESS)
         {
+            ble_link_stress_on_hvn_send();
             ble_tx_pending_pop();
             freed_slot = true;
             continue;
@@ -1058,6 +1104,359 @@ static void peer_manager_init(void)
 }
 #endif
 
+/*==============================================================================
+ * BLE Link Stress Monitor + Dynamic TX Power Control (EMC)
+ *
+ * EMC noise can make the BLE link degrade before the connection actually drops.
+ * This monitor watches both directions:
+ *   - RSSI events from the central side indicate downlink quality.
+ *   - HVN TX complete latency and NUS pending depth indicate uplink congestion.
+ *
+ * Policy:
+ *   1. Use +4 dBm normally to keep RF output modest.
+ *   2. Boost to +8 dBm when the link shows repeated or immediate stress.
+ *   3. Restore to +4 dBm only after RSSI is stably good and TX is fully idle.
+ *============================================================================*/
+
+static void ble_tx_power_set_dynamic(int8_t tx_power_dbm);
+
+static uint32_t ble_link_stress_elapsed_ms(uint32_t start_tick, uint32_t end_tick)
+{
+    return (uint32_t)((app_timer_cnt_diff_compute(end_tick, start_tick) * 1000ULL) / APP_TIMER_CLOCK_FREQ);
+}
+
+static bool ble_link_stress_rssi_recent(void)
+{
+    if (m_last_rssi_tick == 0)
+    {
+        return false;
+    }
+
+    return (app_timer_cnt_diff_compute(app_timer_cnt_get(), m_last_rssi_tick) <
+            APP_TIMER_TICKS(BLE_LINK_STRESS_RSSI_SILENCE_MS));
+}
+
+static void ble_link_stress_try_restore(void)
+{
+    /* Restore only when every stress source is quiet; this prevents power flapping during EMC bursts. */
+    if (m_ble_tx_power_dbm == BLE_TX_POWER_BOOST_DBM &&
+        m_link_stress_count == 0 &&
+        m_rssi_good_count >= BLE_RSSI_GOOD_COUNT_LIMIT &&
+        !m_rssi_bad_latched &&
+        !m_rssi_silence_latched &&
+        ble_link_stress_rssi_recent() &&
+        !ble_tx_pending_has_data() &&
+        !m_tx_in_progress &&
+        !m_hvn_send_pending)
+    {
+        ble_tx_power_set_dynamic(BLE_TX_POWER_NORMAL_DBM);
+    }
+}
+
+static void ble_link_stress_boost_now(const char * p_reason, uint32_t elapsed_ms)
+{
+    m_last_stress_tick = app_timer_cnt_get();
+    UNUSED_PARAMETER(p_reason);
+
+    if (elapsed_ms > 0)
+    {
+        /* EMC RTT log disabled: DBG_PRINTF("[BLE] stress: %s %lums (boost now)\r\n", p_reason, (unsigned long)elapsed_ms); */
+    }
+    else
+    {
+        /* EMC RTT log disabled: DBG_PRINTF("[BLE] stress: %s (boost now)\r\n", p_reason); */
+    }
+
+    ble_tx_power_set_dynamic(BLE_TX_POWER_BOOST_DBM);
+}
+
+static void ble_link_stress_check_pending_depth(void)
+{
+    /* Queue growth means SoftDevice/NUS cannot accept packets fast enough; treat it as uplink stress. */
+    if (s_tx_pending_count >= BLE_TX_PENDING_BOOST_THRESHOLD)
+    {
+        ble_link_stress_boost_now("TX_QUEUE_DEPTH", 0);
+    }
+}
+
+static void ble_link_stress_bump(const char * p_reason, uint32_t elapsed_ms)
+{
+    m_last_stress_tick = app_timer_cnt_get();
+    UNUSED_PARAMETER(p_reason);
+    UNUSED_PARAMETER(elapsed_ms);
+
+    if (m_link_stress_count < BLE_LINK_STRESS_BOOST_COUNT)
+    {
+        m_link_stress_count++;
+    }
+
+    /* EMC RTT log disabled: DBG_PRINTF("[BLE] stress: %s %lums (count=%u/%u)\r\n",
+       p_reason, (unsigned long)elapsed_ms, m_link_stress_count, BLE_LINK_STRESS_BOOST_COUNT); */
+
+    if (m_link_stress_count >= BLE_LINK_STRESS_BOOST_COUNT)
+    {
+        ble_tx_power_set_dynamic(BLE_TX_POWER_BOOST_DBM);
+    }
+}
+
+static void ble_link_stress_reset(void)
+{
+    m_link_stress_count    = 0;
+    m_last_rssi_tick       = 0;
+    m_last_stress_tick     = 0;
+    m_hvn_send_tick        = 0;
+    m_hvn_send_pending     = false;
+    m_rssi_silence_latched = false;
+    m_hvn_stuck_latched    = false;
+}
+
+static void ble_link_stress_on_hvn_send(void)
+{
+    m_hvn_send_tick     = app_timer_cnt_get();
+    m_hvn_send_pending  = true;
+    m_hvn_stuck_latched = false;
+}
+
+static void ble_link_stress_on_hvn_complete(void)
+{
+    uint32_t now_tick;
+    uint32_t elapsed_ms;
+
+    if (!m_hvn_send_pending)
+    {
+        return;
+    }
+
+    now_tick    = app_timer_cnt_get();
+    elapsed_ms  = ble_link_stress_elapsed_ms(m_hvn_send_tick, now_tick);
+    m_hvn_send_pending  = false;
+    m_hvn_stuck_latched = false;
+
+    if (elapsed_ms >= BLE_LINK_STRESS_HVN_VERY_SLOW_MS)
+    {
+        ble_link_stress_boost_now("HVN_VERY_SLOW", elapsed_ms);
+    }
+    else if (elapsed_ms >= BLE_LINK_STRESS_HVN_SLOW_MS)
+    {
+        ble_link_stress_bump("HVN_SLOW", elapsed_ms);
+    }
+    else
+    {
+        m_link_stress_count = 0;
+    }
+}
+
+static void ble_link_stress_tick_handler(void * p_context)
+{
+    uint32_t now_tick;
+
+    UNUSED_PARAMETER(p_context);
+
+    if (m_conn_handle == BLE_CONN_HANDLE_INVALID || ble_connection_st != BLE_CONNECTED_ST)
+    {
+        return;
+    }
+
+    now_tick = app_timer_cnt_get();
+
+    /* If RSSI events disappear, assume the central/downlink side is being disturbed. */
+    if (m_last_rssi_tick != 0 &&
+        app_timer_cnt_diff_compute(now_tick, m_last_rssi_tick) >= APP_TIMER_TICKS(BLE_LINK_STRESS_RSSI_SILENCE_MS))
+    {
+        if (!m_rssi_silence_latched)
+        {
+            m_rssi_silence_latched = true;
+            ble_link_stress_boost_now("RSSI_SILENCE", 0);
+        }
+    }
+
+    /* If a notification was submitted but TX complete does not arrive, boost immediately. */
+    if (m_hvn_send_pending &&
+        app_timer_cnt_diff_compute(now_tick, m_hvn_send_tick) >= APP_TIMER_TICKS(BLE_LINK_STRESS_HVN_STUCK_MS))
+    {
+        if (!m_hvn_stuck_latched)
+        {
+            uint32_t elapsed_ms = ble_link_stress_elapsed_ms(m_hvn_send_tick, now_tick);
+
+            m_hvn_stuck_latched = true;
+            ble_link_stress_boost_now("HVN_STUCK", elapsed_ms);
+        }
+    }
+    else if (m_tx_in_progress &&
+             m_hvn_send_tick != 0 &&
+             app_timer_cnt_diff_compute(now_tick, m_hvn_send_tick) >= APP_TIMER_TICKS(BLE_LINK_STRESS_HVN_STUCK_MS))
+    {
+        if (!m_hvn_stuck_latched)
+        {
+            uint32_t elapsed_ms = ble_link_stress_elapsed_ms(m_hvn_send_tick, now_tick);
+
+            m_hvn_stuck_latched = true;
+            ble_link_stress_boost_now("HVN_STUCK", elapsed_ms);
+        }
+    }
+
+    if (m_link_stress_count > 0 &&
+        m_last_stress_tick != 0 &&
+        app_timer_cnt_diff_compute(now_tick, m_last_stress_tick) >= APP_TIMER_TICKS(BLE_LINK_STRESS_RECOVERY_MS))
+    {
+        m_link_stress_count = 0;
+        /* EMC RTT log disabled: DBG_PRINTF("[BLE] stress: recovered (count=0)\r\n"); */
+        ble_link_stress_try_restore();
+    }
+}
+
+static void ble_link_stress_start(void)
+{
+    ret_code_t err_code;
+
+    ble_link_stress_reset();
+    m_last_rssi_tick = app_timer_cnt_get();
+
+    err_code = app_timer_start(m_link_stress_timer_id, APP_TIMER_TICKS(BLE_LINK_STRESS_TICK_MS), NULL);
+    if (err_code != NRF_SUCCESS && err_code != NRF_ERROR_INVALID_STATE)
+    {
+        APP_ERROR_CHECK(err_code);
+    }
+}
+
+static void ble_link_stress_stop(void)
+{
+    (void)app_timer_stop(m_link_stress_timer_id);
+    ble_link_stress_reset();
+}
+
+static void ble_tx_power_set_dynamic(int8_t tx_power_dbm)
+{
+    ret_code_t err_code;
+    int8_t     prev_tx_power_dbm = m_ble_tx_power_dbm;
+
+    if (m_conn_handle == BLE_CONN_HANDLE_INVALID || m_ble_tx_power_dbm == tx_power_dbm)
+    {
+        return;
+    }
+
+    err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_CONN,
+                                       m_conn_handle,
+                                       tx_power_dbm);
+    if (err_code == NRF_SUCCESS)
+    {
+        m_ble_tx_power_dbm = tx_power_dbm;
+        if (tx_power_dbm == BLE_TX_POWER_BOOST_DBM)
+        {
+            m_rssi_good_count = 0;
+        }
+
+        if (prev_tx_power_dbm == BLE_TX_POWER_UNKNOWN_DBM)
+        {
+            /* EMC RTT log disabled: DBG_PRINTF("[BLE] TX power -> %d dBm\r\n", tx_power_dbm); */
+        }
+        else
+        {
+            /* EMC RTT log disabled: DBG_PRINTF("[BLE] TX power %d -> %d dBm\r\n", prev_tx_power_dbm, tx_power_dbm); */
+        }
+    }
+    else if (err_code != NRF_ERROR_INVALID_STATE && err_code != BLE_ERROR_INVALID_CONN_HANDLE)
+    {
+        APP_ERROR_CHECK(err_code);
+    }
+}
+
+static void ble_rssi_state_reset(void)
+{
+    m_rssi_bad_count   = 0;
+    m_rssi_good_count  = 0;
+    m_rssi_bad_latched = false;
+}
+
+static void ble_rssi_monitor_start(void)
+{
+    ret_code_t err_code;
+
+    if (m_conn_handle == BLE_CONN_HANDLE_INVALID)
+    {
+        return;
+    }
+
+    ble_rssi_state_reset();
+    m_ble_tx_power_dbm = BLE_TX_POWER_UNKNOWN_DBM;
+
+    /* Apply normal power first; stress monitor will boost only when needed. */
+    ble_tx_power_set_dynamic(BLE_TX_POWER_NORMAL_DBM);
+    ble_link_stress_start();
+
+    err_code = sd_ble_gap_rssi_start(m_conn_handle,
+                                     BLE_RSSI_CHANGE_THRESHOLD_DBM,
+                                     BLE_RSSI_SKIP_COUNT);
+    if (err_code == NRF_SUCCESS)
+    {
+        /* EMC RTT log disabled: DBG_PRINTF("[BLE] RSSI monitor start (threshold=%d, skip=%d)\r\n",
+           BLE_RSSI_CHANGE_THRESHOLD_DBM, BLE_RSSI_SKIP_COUNT); */
+    }
+    else if (err_code != NRF_ERROR_INVALID_STATE)
+    {
+        APP_ERROR_CHECK(err_code);
+    }
+    else
+    {
+        /* EMC RTT log disabled: DBG_PRINTF("[BLE] RSSI monitor already active\r\n"); */
+    }
+}
+
+static void ble_rssi_monitor_stop(void)
+{
+    if (m_conn_handle != BLE_CONN_HANDLE_INVALID)
+    {
+        (void)sd_ble_gap_rssi_stop(m_conn_handle);
+    }
+
+    ble_link_stress_stop();
+    ble_rssi_state_reset();
+    m_ble_tx_power_dbm = BLE_TX_POWER_UNKNOWN_DBM;
+}
+
+static void ble_rssi_update(int8_t rssi_dbm)
+{
+    m_last_rssi_tick = app_timer_cnt_get();
+
+    if (rssi_dbm <= BLE_RSSI_BAD_DBM)
+    {
+        if (m_rssi_bad_count < BLE_RSSI_BAD_COUNT_LIMIT)
+        {
+            m_rssi_bad_count++;
+        }
+        m_rssi_good_count = 0;
+
+        if (m_rssi_bad_count >= BLE_RSSI_BAD_COUNT_LIMIT && !m_rssi_bad_latched)
+        {
+            m_rssi_bad_latched = true;
+            /* EMC RTT log disabled: DBG_PRINTF("[BLE] stress: RSSI_BAD %d dBm (count=%u/%u, boost now)\r\n",
+               rssi_dbm, m_rssi_bad_count, BLE_RSSI_BAD_COUNT_LIMIT); */
+            ble_tx_power_set_dynamic(BLE_TX_POWER_BOOST_DBM);
+        }
+    }
+    else if (rssi_dbm >= BLE_RSSI_GOOD_DBM)
+    {
+        m_rssi_silence_latched = false;
+
+        if (m_rssi_good_count < BLE_RSSI_GOOD_COUNT_LIMIT)
+        {
+            m_rssi_good_count++;
+        }
+        m_rssi_bad_count   = 0;
+        m_rssi_bad_latched = false;
+
+        if (m_rssi_good_count >= BLE_RSSI_GOOD_COUNT_LIMIT)
+        {
+            ble_link_stress_try_restore();
+        }
+    }
+    else
+    {
+        m_rssi_bad_count  = 0;
+        m_rssi_good_count = 0;
+    }
+}
+
 /*==============================================================================
  * BLE Event Handler
  * Handles all BLE events from the SoftDevice.
@@ -1068,13 +1467,14 @@ static void peer_manager_init(void)
  *
  * Key events handled:
  *   - DISCONNECTED: connection lost -> device sleep, state reset
- *   - CONNECTED: connection established -> assign QWR handle, set TX power +8dBm
+ *   - CONNECTED: connection established -> assign QWR handle, start EMC link monitor
  *   - PHY_UPDATE_REQUEST: keep link on 1M PHY
+ *   - RSSI_CHANGED: RSSI input for dynamic TX power boost/restore
  *   - TIMEOUT: connection/GATT timeout -> force disconnect
  *   - SEC_PARAMS_REQUEST: security parameter request (reject if security unused)
  *   - PASSKEY_DISPLAY: display passkey (debug log)
  *   - AUTH_KEY_REQUEST: respond with static passkey
- *   - HVN_TX_COMPLETE: TX complete -> clear transmission flag
+ *   - HVN_TX_COMPLETE: TX complete -> clear transmission flag and measure HVN latency
  */
 static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
 {
@@ -1110,6 +1510,8 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
             DBG_PRINTF("[BLE] Disconnected (reason 0x%02X)%s\r\n",
                        disc_reason, unintended_disc ? " [UNINTENDED]" : "");
 
+            ble_rssi_monitor_stop();  /* EMC: stop RSSI/HVN stress timer before invalidating handle */
+
             ble_connection_st = 0;
             pending_cmd_len = 0;    // Clear pending command buffer
             m_conn_handle = BLE_CONN_HANDLE_INVALID;
@@ -1144,7 +1546,7 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
             DBG_PRINTF("[BLE] Connected\r\n");
 
 #if BLE_DEV_MODE
-            /* Dev: no passkey/SEC — allow NUS TX/RX as soon as GAP is up (prod uses PM_EVT_CONN_SEC_SUCCEEDED). */
+            /* Dev: no passkey/SEC - allow NUS TX/RX as soon as GAP is up (prod uses PM_EVT_CONN_SEC_SUCCEEDED). */
             ble_connection_st = BLE_CONNECTED_ST;
             battery_timer_start();
 #endif
@@ -1153,7 +1555,7 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
             err_code = nrf_ble_qwr_conn_handle_assign(&m_qwr, m_conn_handle);
             APP_ERROR_CHECK(err_code);
             
-            sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_CONN, m_conn_handle, 4);
+            ble_rssi_monitor_start();  /* EMC: apply +4 dBm normal power and start dynamic boost monitor */
 
             led_set_state(LED_STATE_OFF);  /* Connection complete -> LED OFF */
 
@@ -1171,6 +1573,10 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
         }
         break;
 
+        case BLE_GAP_EVT_RSSI_CHANGED:
+            ble_rssi_update(p_ble_evt->evt.gap_evt.params.rssi_changed.rssi);
+            break;
+
         case BLE_GATTC_EVT_TIMEOUT:
         case BLE_GATTS_EVT_TIMEOUT:
             DBG_PRINTF("[BLE] GATT Timeout -> disconnect\r\n");
@@ -1223,6 +1629,7 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
 #endif
 
         case BLE_GATTS_EVT_HVN_TX_COMPLETE:
+            ble_link_stress_on_hvn_complete();  /* EMC: HVN latency feeds dynamic TX power control */
             m_tx_in_progress = false;
             m_tx_complete_pending = false;  /* Notify waiting functions of TX completion */
             break;
@@ -1425,11 +1832,11 @@ void data_tx_handler(char const *p_data_to_send)
 
         if (err_code == NRF_SUCCESS)
         {
-            // OK
+            ble_link_stress_on_hvn_send();  /* EMC: start HVN latency stopwatch */
         }
         else if (err_code == NRF_ERROR_RESOURCES)
         {
-            // Retry later
+            /* Retry later; pending queue depth will trigger boost if congestion persists. */
         }
         else if (err_code == NRF_ERROR_INVALID_STATE || err_code == NRF_ERROR_NOT_FOUND)
         {
@@ -1608,6 +2015,7 @@ uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
                 return NRF_ERROR_NO_MEM;
             }
 
+            ble_link_stress_check_pending_depth();
             return NRF_ERROR_RESOURCES;
         }
 
@@ -1616,6 +2024,7 @@ uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
 
         if (err_code == NRF_SUCCESS)
         {
+            ble_link_stress_on_hvn_send();  /* EMC: start HVN latency stopwatch */
             return NRF_SUCCESS;
         }
         else if (err_code == NRF_ERROR_RESOURCES)
@@ -1626,6 +2035,7 @@ uint32_t dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
                 return NRF_ERROR_NO_MEM;
             }
 
+            ble_link_stress_check_pending_depth();
             return NRF_ERROR_RESOURCES;
         }
         else if (err_code == NRF_ERROR_INVALID_STATE || err_code == NRF_ERROR_NOT_FOUND)
@@ -1667,6 +2077,7 @@ static void main_s(void * p_context)
 
     bool button_released = nrf_gpio_pin_read(POWER_BUTTON);
 
+
     /* ---- Running phase: post-boot button polling ---- */
     if (booted)
     {
@@ -1711,7 +2122,7 @@ static void main_s(void * p_context)
             go_device_power_off = true;
             main_timer_start();
         }
-        else if (cnt_s > 250 || (m_reset_status == 2))  /* 250 x 5ms = 1.25s + α */
+        else if (cnt_s > 250 || (m_reset_status == 2))  /* 250 x 5ms = 1.25s + ¥á */
         {
             DBG_PRINTF("[BTN] Boot (cnt=%d)\r\n", cnt_s);
             device_reset = false;
@@ -1740,7 +2151,7 @@ static void main_s(void * p_context)
         cnt_s++;
         device_reset = false;
 
-        if (cnt_s == 250)   /* 250 x 5ms = 1.25s + α */
+        if (cnt_s == 250)   /* 250 x 5ms = 1.25s + ¥á */
         {
             led_set_state(LED_STATE_POWER_ON);
             DBG_PRINTF("[BTN] 2.0s\r\n");
@@ -1761,7 +2172,7 @@ int main(void)
 {
     bool erase_bonds_local = false;
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Phase 1: Basic hardware init (BLE-independent)
      *
      *  - Power self-latch (P0.8 HIGH)
@@ -1770,9 +2181,20 @@ int main(void)
      *  - App timers (power polling, battery, main loop)
      *  - Default config (serial number, passkey)
      *  - Buttons/LEDs
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     power_hold_init();
 
+    if ((NRF_POWER->GPREGRET2 & POST_DFU_APP_BOOT_GPREGRET2_MASK) != 0)
+    {
+        NRF_POWER->GPREGRET2 &= ~POST_DFU_APP_BOOT_GPREGRET2_MASK;
+        NRF_P0->OUTCLR = (1UL << 8);
+        NRF_P0->DIRSET = (1UL << 8);
+        while (true)
+        {
+            __WFE();
+        }
+    }
+
     if (power_off_duble_prohibit)
     {
         return 0;
@@ -1782,7 +2204,7 @@ int main(void)
     log_init();
 
     DBG_PRINTF("\r\n========================================\r\n");
-    DBG_PRINTF(" Medithings v1.17\r\n");
+    DBG_PRINTF(" Medithings VesiScan-Basic\r\n");
     DBG_PRINTF("========================================\r\n");
 
     DBG_PRINTF("[1] HW Init\r\n");
@@ -1796,33 +2218,33 @@ int main(void)
 #endif
     DBG_PRINTF("  gpio/timer/config/btn OK\r\n");
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Phase 2: BLE stack init
      *
      *  - Power management module (WFE/sleep when idle)
      *  - SoftDevice S140 (BLE 5.0 protocol stack)
      *  - DC-DC converter enable (required after SoftDevice)
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     DBG_PRINTF("[2] BLE Stack\r\n");
     power_management_init();
     ble_stack_init();
     APP_ERROR_CHECK(sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE));
     DBG_PRINTF("  pwr/stack/dcdc OK\r\n");
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Phase 3: Internal flash config (must init after BLE stack)
      *
      *  - FDS (Flash Data Storage) init
      *  - Read stored config from flash (serial, passkey, piezo, etc.)
      *  - Overwrite defaults with flash values
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     DBG_PRINTF("[3] FDS\r\n");
     fs_storage_init();
     config_load();
     load_flash_config();
     DBG_PRINTF("  fds OK\r\n");
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Phase 4: BLE protocol setup
      *
      *  - GAP: device name (serial number), connection params, passkey
@@ -1831,7 +2253,7 @@ int main(void)
      *  - Advertising: advertising data (name, UUID)
      *  - Connection parameter negotiation module
      *  - Security: Peer Manager (bonding/passkey)
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     DBG_PRINTF("[4] BLE Protocol\r\n");
     gap_params_init();
     gatt_init();
@@ -1866,14 +2288,14 @@ int main(void)
         DBG_PRINTF("  fds defaults saved\r\n");
     }
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Phase 5: Application init
      *
      *  - Command parser: process BLE received commands (log, BLE TX, CRC)
      *  - Piezo driver: GPIO/Timer/PPI setup for ultrasound measurement
      *  - IMU (ICM42670P) is NOT initialized here
      *    -> imu_read_direct() self-configures/reads/sleeps on each msp? command
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     DBG_PRINTF("[5] App\r\n");
     g_plat.log       = log_printf;
     g_plat.tx_bin    = dr_binary_tx_safe;
@@ -1883,9 +2305,9 @@ int main(void)
     dr_piezo_init();
     DBG_PRINTF("  parser/piezo OK\r\n");
 
-    /*──────────────────────────────────────────────────────────────
+    /*--------------------------------------------------------------------------
      *  Boot complete -> start power button state machine
-     *────────────────────────────────────────────────────────────*/
+     *--------------------------------------------------------------------------*/
     DBG_PRINTF("\r\n========================================\r\n");
     DBG_PRINTF(" READY [%s]\r\n", SERIAL_NO);
     DBG_PRINTF("========================================\r\n\r\n");

project/ble_peripheral/ble_app_bladder_patch/main.h

@@ -45,8 +45,18 @@
 *      : 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                "VBTFW0118"
+#define FIRMWARE_VERSION                "VBTFW0122"
 
 /*==============================================================================
  * Data Length Constants

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) */
 

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

@@ -27,7 +27,6 @@
 #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"
@@ -60,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 — legacy TMP235 path */
-bool safety_check_mode = false;
-
 /* SAADC callback completion flag — used by all_sensors() to wait */
 volatile bool battery_saadc_done = false;
 
@@ -85,7 +81,6 @@ 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;
@@ -94,7 +89,7 @@ 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.
@@ -168,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();
 
@@ -188,6 +183,11 @@ void battery_event_handler(nrf_drv_saadc_evt_t const * p_event)
             low_battery_check = false;
             safety_batt_mv = batt_lvl_in_milli_volt_1;
 
+            if (imu_fifo_capture_is_active())
+            {
+                return;
+            }
+
             if (imu_read_temperature_x100(NULL, &imu_temp_c) != 0)
             {
                 DBG_PRINTF("[SAFETY] IMU temp read failed\r\n");

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

@@ -25,9 +25,6 @@
 /* SAADC callback completion flag (used by all_sensors() to wait) */
 extern volatile bool battery_saadc_done;
 
-/* Safety check mode flag — legacy TMP235 path */
-extern bool safety_check_mode;
-
 /* Called when safety check temperature measurement completes */
 void safety_check_complete(float temp_c);
 

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,16 +20,15 @@
  *      - 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 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"
@@ -67,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 */
@@ -93,12 +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 */
-extern volatile uint16_t info_temp;                		/* Global temperature cache (deg C x 100) */
+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
@@ -263,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
@@ -331,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)
@@ -406,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.
@@ -445,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 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)
  *
@@ -467,6 +407,7 @@ extern const nrfx_twi_t m_twi_icm42670;
 #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)
 {
@@ -507,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);
 	}
 
@@ -554,10 +495,6 @@ int imu_read_direct(void)
 	gyro[1]  = (int16_t)((raw[10] << 8) | raw[11]);
 	gyro[2]  = (int16_t)((raw[12] << 8) | raw[13]);
 
-	/* Apply mounting matrix — board orientation correction */
-	apply_mounting_matrix(icm_mounting_matrix, accel);
-	apply_mounting_matrix(icm_mounting_matrix, gyro);
-
 	/* Pack data */
 	ssp_data[0] = (uint16_t)accel[0];
 	ssp_data[1] = (uint16_t)accel[1];
@@ -633,6 +570,11 @@ int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
 		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. */
@@ -683,6 +625,11 @@ int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
 	return 0;
 }
 
+bool imu_fifo_capture_is_active(void)
+{
+	return s_fifo_capture_active;
+}
+
 /* --------------------------------------------------------------------------------------
  * mtb? FIFO capture support
  *
@@ -706,8 +653,6 @@ int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
 #define RIM_MAX_SAMPLE_BYTES        (BLE_NUS_MAX_DATA_LEN - 2 - RIM_PACKET_HEADER_BYTES)
 #define RIM_SAMPLES_PER_PACKET      (RIM_MAX_SAMPLE_BYTES / RIM_SAMPLE_SIZE_BYTES)
 
-static bool s_fifo_capture_active = false;
-
 static void imu_serif_make(struct inv_imu_serif *serif)
 {
 	serif->context    = 0;
@@ -739,6 +684,14 @@ static void imu_fifo_power_off(void)
 	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;
@@ -757,8 +710,8 @@ int imu_fifo_capture_start(void)
 
 	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);
+	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);
+	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);
@@ -771,8 +724,9 @@ int imu_fifo_capture_start(void)
 		rc |= inv_imu_write_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
 	}
 	rc |= inv_imu_reset_fifo(&icm_driver);
-	rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
+	rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver); // FIFO Accel Low Noise Mode
-	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
+	//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);
 
@@ -1025,9 +979,55 @@ int imu_fifo_capture_stop_and_send_rim(void)
 	{
 		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>
@@ -105,6 +106,11 @@ int imu_read_direct(void);
  */
 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.
@@ -116,6 +122,18 @@ int imu_fifo_capture_start(void);
  */
 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 ... ]

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) */
 
 /*==============================================================================

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,34 +184,12 @@ 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 ---- */
 
 	/* Device power OFF 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>