diff --git a/CHANGELOG.md b/CHANGELOG.md
index 47aa899..be69f57 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -6,6 +6,17 @@ For the detailed description, please explore nested folders and corresponding CH
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## [1.2.3] - 2026-04-02
+
+### Added
+
+- Added optional accelerometer streaming on the nRF52, enabled only when requested by the user configuration.
+
+### Fixed
+
+### Changed
+
+
 ## [1.2.2] - 2025-03-23
 
 ### Added
diff --git a/README.md b/README.md
index ed75403..5fb2a09 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 <img src="docs/images/wulpus_main.png" alt="WULPUS main" width="100%"/>
 
-# WULPUS v1.2.2
+# WULPUS v1.2.3
 ## Wearable Ultra Low-Power Ultrasound
 
 # Introduction
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/CHANGELOG.md b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/CHANGELOG.md
index fc6a774..d5f6cac 100644
--- a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/CHANGELOG.md
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/CHANGELOG.md
@@ -5,7 +5,21 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
-## [Unreleased]
+## [1.2.3] - 2026-04-02
+
+### Added
+
+- Optional accelerometer streaming on the nRF52, enabled only when requested by the user configuration.
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.c`: Added IIS2DH accelerometer driver, runtime enable/disable handling, and ACC/non-ACC frame finalization.
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.h`: Added IIS2DH register definitions, types, and public driver API.
+
+### Changed
+
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/main.c`: Added runtime accelerometer state handling and deferred mode application from the main loop.
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_ble.c`: Added config-packet parsing for user-requested accelerometer streaming and deferred update signaling.
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_spi.c`: Moved frame completion responsibility to the ACC/non-ACC post-processing path.
+- `fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/pca10040/s132/ses/US_probe_nRF52_firmware.emProject`: Added IIS2DH source files and TWI driver sources to the SES project.
+
 
 ## [1.1.0] - 2024-02-21
 
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.c b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.c
new file mode 100644
index 0000000..9bf6869
--- /dev/null
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.c
@@ -0,0 +1,314 @@
+#include "iis2dh.h"
+#include "us_spi.h"
+#include "us_defines.h"
+
+
+/* TWI instance */
+static const nrf_drv_twi_t IIS2DH_twi = NRF_DRV_TWI_INSTANCE(TWI_INSTANCE_ID);
+
+/* Flag to know when a I2C transfer has been completed */
+static volatile bool IIS2DH_xfer_done = false;
+
+extern volatile bool flag_add_IMU;
+
+extern int buffer_content;
+extern int buffer_counter;
+extern int BLE_packet_ready;
+extern ArrayList_type m_rx_buf[NUMBER_OF_XFERS*MAX_BUFFER_NUMBER_OF_US_FRAMES];
+
+uint8_t IIS2DH_buffer[805] = {};
+uint16_t IIS2DH_buffer_index =0;
+/* Frame counter for IIS2DH over bluetooth */
+uint16_t IIS2DH_frame_number =0;
+
+
+//Event Handler
+static void twi_handler(nrf_drv_twi_evt_t const * p_event, void * p_context)
+{
+    //Check the event to see what type of event occurred
+    switch (p_event->type)
+    {
+        //If data transmission or receiving is finished
+	case NRF_DRV_TWI_EVT_DONE:
+        IIS2DH_xfer_done = true;//Set the flag
+        break;
+        
+        default:
+        // do nothing
+          break;
+    }
+}
+
+
+/**
+ * @brief TWI initialization function for the MCP9808 sensor
+ *
+ */
+static void iis2dh_twi_init (void)
+{
+    ret_code_t err_code;
+
+    const nrf_drv_twi_config_t twi_config = {
+       .scl                = PIN_IIS2DH_SCL,
+       .sda                = PIN_IIS2DH_SDA,
+       .frequency          = NRF_DRV_TWI_FREQ_100K, 
+       .interrupt_priority = APP_IRQ_PRIORITY_LOWEST, 
+       .clear_bus_init     = false
+    };
+
+    err_code = nrf_drv_twi_init(&IIS2DH_twi, &twi_config, twi_handler, NULL);
+    APP_ERROR_CHECK(err_code);
+
+    nrf_drv_twi_enable(&IIS2DH_twi);
+}
+
+void IIS2DH_init()
+{
+    iis2dh_twi_init();
+    IIS2DH_buffer[0] = 0xFF;
+    IIS2DH_buffer[1] = 128;
+    //((uint16_t*)IIS2DH_buffer)[1] = IIS2DH_frame_number++;
+        IIS2DH_buffer[3] = 0;
+    IIS2DH_buffer[2] = IIS2DH_frame_number++;
+
+    IIS2DH_buffer_index = 4;
+    nrf_delay_ms(500);
+}
+
+bool IIS2DH_register_read(uint8_t register_address, uint8_t * rx_buffer, uint8_t number_of_bytes)
+{
+    ret_code_t err_code;
+
+    //Set the flag to false to show the receiving is not yet completed
+    IIS2DH_xfer_done = false;
+
+    // Send the register address where we want to read the data from
+    err_code = nrf_drv_twi_tx(&IIS2DH_twi, IIS2DH_ADDRESS, &register_address, 1, true);
+
+    //Wait for the transmission to be completed
+    while (IIS2DH_xfer_done == false){}
+    
+    // If transmission was not successful, exit the function and return false
+    if (NRF_SUCCESS != err_code)
+    {
+        return false;
+    }
+
+    //reset the flag so that we can read data from the IIS2DH's internal register
+    IIS2DH_xfer_done = false;
+	  
+    // Receive the data from the IIS2DH
+    err_code = nrf_drv_twi_rx(&IIS2DH_twi, IIS2DH_ADDRESS, rx_buffer, number_of_bytes);
+    //wait until the transmission is completed
+    while (IIS2DH_xfer_done == false){}
+	
+    // if data was successfully read, return true else return false
+    if (NRF_SUCCESS != err_code)
+    {
+        return false;
+    }
+    
+    return true;
+}
+bool IIS2DH_register_write(uint8_t register_address, uint8_t * wx_buffer, uint8_t number_of_bytes)
+{
+    ret_code_t err_code;
+
+    //Set the flag to false to show the receiving is not yet completed
+    IIS2DH_xfer_done = false;
+
+    uint8_t wxbuffer_with_address[10];
+    wxbuffer_with_address[0] = register_address;
+    if (number_of_bytes>8) return false;
+    for (int i = 0; i<number_of_bytes; i++){
+      wxbuffer_with_address[i+1] = wx_buffer[i];
+    }
+   
+    // Send the register address where we want to read the data from
+    err_code = nrf_drv_twi_tx(&IIS2DH_twi, IIS2DH_ADDRESS, wxbuffer_with_address, number_of_bytes+1, true);
+
+    //Wait for the transmission to be completed
+    while (IIS2DH_xfer_done == false){}
+    
+    // If transmission was not successful, exit the function and return false
+    if (NRF_SUCCESS != err_code)
+    {
+        return false;
+    }
+    return true;
+}
+
+bool setupTemp(){
+  uint8_t wx_buffer[1] = {0};
+  wx_buffer[0] = 0b11000000;
+  bool check = IIS2DH_register_write(IIS2DH_REG_TEMP_CFG_REG,wx_buffer , 1);
+  if (!check){
+    return false;
+  }
+  uint8_t rx_buffer[1] = {0};
+
+  check = IIS2DH_register_read(IIS2DH_REG_CTRL_REG4,rx_buffer , 1);
+  if (!check){
+    return false;
+  }
+  wx_buffer[0] = rx_buffer[0] | 0x80;
+  check = IIS2DH_register_write(IIS2DH_REG_CTRL_REG4,wx_buffer , 1);
+  if (!check){
+    return false;
+  }
+
+
+}
+
+int8_t getTemp(){
+  uint8_t rx_buffer[1] = {0};
+  bool check = IIS2DH_register_read(IIS2DH_REG_OUT_TEMP_H, rx_buffer, 1);
+  if (!check){
+    return 0;
+  }
+  int8_t res = rx_buffer[0];
+  check = IIS2DH_register_read(IIS2DH_REG_OUT_TEMP_L, rx_buffer, 1);
+  if (!check){
+    return 0;
+  }
+  //res|= rx_buffer[0];
+  return res;
+  
+
+}
+
+bool setupAccelormeter(IIS2DH_OperatingModes mode, IIS2DH_DataRate rate, IIS2DH_FullScale fs){
+  uint8_t wx_reg1_buffer[1];
+  uint8_t wx_reg4_buffer[1];
+
+  if (mode == IIS2DH_LowPowerMode){
+    wx_reg1_buffer[0] = 0x0F;
+  }else{
+    wx_reg1_buffer[0] = 0x07;
+  }
+  wx_reg1_buffer[0] = wx_reg1_buffer[0] | rate; // set the odr bits
+  if (mode == IIS2DH_HighResolutionMode){
+    wx_reg4_buffer[0] = 0b00001000;
+  }else{
+    wx_reg4_buffer[0] = 0b00000000;
+  }
+  wx_reg4_buffer[0] |= wx_reg4_buffer[0] | fs;
+  
+ 
+  bool res = IIS2DH_register_write(IIS2DH_REG_CTRL_REG1, wx_reg1_buffer, 1);
+  res &= IIS2DH_register_write(IIS2DH_REG_CTRL_REG4, wx_reg4_buffer, 1);
+  return true;
+}
+
+
+double convert2mg(uint16_t data_in, IIS2DH_FullScale range){
+  if (range == IIS2DH_Precision_2g){
+    return ((double) data_in)*0.06125;
+  }else if (range == IIS2DH_Precision_4g){
+    return ((double) data_in)*0.121875;
+  }else if (range == IIS2DH_Precision_8g){
+    return ((double) data_in)*0.244375;
+  }else if (range == IIS2DH_Precision_16g){
+    return ((double) data_in)*0.7325;
+  }
+}
+
+uint16_t getHighandLow(uint8_t register_address_L, uint8_t register_address_H, IIS2DH_OperatingModes mode){
+  uint8_t rx_buffer[1] ={0};
+
+  IIS2DH_register_read(register_address_H, rx_buffer, 1);
+  uint16_t res = rx_buffer[0]<<8;
+  rx_buffer[0] =0;
+  IIS2DH_register_read(register_address_L, rx_buffer, 1);
+  res += rx_buffer[0];
+
+  if (mode == IIS2DH_LowPowerMode){
+    res = res>>8;
+  }else if (mode == IIS2DH_NormalMode){
+    res = res>>6;
+  }else if (mode == IIS2DH_HighResolutionMode){
+    res = res>>4;
+  }
+  return res;
+
+}
+
+bool getAccelerationData(uint16_t* X, uint16_t* Y, uint16_t* Z, IIS2DH_OperatingModes mode, IIS2DH_FullScale range){
+  *X = getHighandLow(IIS2DH_REG_OUT_X_L, IIS2DH_REG_OUT_X_H, mode);  
+  *Y = getHighandLow(IIS2DH_REG_OUT_Y_L, IIS2DH_REG_OUT_Y_H, mode);
+  *Z = getHighandLow(IIS2DH_REG_OUT_Z_L, IIS2DH_REG_OUT_Z_H, mode);
+  //*X = convert2mg(X_in, range);
+  //*Y = convert2mg(Y_in, range);
+  //*Z = convert2mg(Z_in, range);
+
+
+  return true;
+
+}
+
+static void finalizeCurrentFrame(void)
+{
+    buffer_counter++;
+    if (buffer_counter == MAX_BUFFER_NUMBER_OF_US_FRAMES)
+    {
+        buffer_counter = 0;
+    }
+
+    BLE_packet_ready = 1;
+}
+
+bool IIS2DH_set_streaming_enabled(bool enable)
+{
+    if (enable)
+    {
+        return setupAccelormeter(IIS2DH_HighResolutionMode,
+                                 AllModes_400Hz,
+                                 IIS2DH_Precision_2g);
+    }
+    else
+    {
+        // Power-down mode: CTRL_REG1 = 0
+        uint8_t ctrl_reg1 = 0x00;
+        return IIS2DH_register_write(IIS2DH_REG_CTRL_REG1, &ctrl_reg1, 1);
+    }
+}
+
+void finalizeFrameWithoutIMU(void)
+{
+    while(flag_add_IMU == false) {}
+
+    flag_add_IMU = false;
+
+    // Zero out the 6 bytes that would normally carry accel data
+    uint8_t *dst = &m_rx_buf[3 + buffer_counter * NUMBER_OF_XFERS].buffer[0] + 202 - 6;
+    dst[0] = 0;
+    dst[1] = 0;
+    dst[2] = 0;
+    dst[3] = 0;
+    dst[4] = 0;
+    dst[5] = 0;
+
+    finalizeCurrentFrame();
+}
+
+void getIIS2DHData2Buffer(){
+
+  IIS2DH_buffer_index = 0;
+  IIS2DH_register_read(IIS2DH_REG_OUT_X_H, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  IIS2DH_register_read(IIS2DH_REG_OUT_X_L, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  IIS2DH_register_read(IIS2DH_REG_OUT_Y_H, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  IIS2DH_register_read(IIS2DH_REG_OUT_Y_L, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  IIS2DH_register_read(IIS2DH_REG_OUT_Z_H, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  IIS2DH_register_read(IIS2DH_REG_OUT_Z_L, &IIS2DH_buffer[IIS2DH_buffer_index++], 1); 
+  //NRF_LOG_INFO("Received: %u", IIS2DH_buffer_index);
+
+  while(flag_add_IMU == false) {}
+
+  flag_add_IMU = false;
+
+  memcpy(&m_rx_buf[3 + buffer_counter * NUMBER_OF_XFERS].buffer[0] + 202 - 6,
+         &IIS2DH_buffer[0],
+         6);
+
+  finalizeCurrentFrame();
+}
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.h b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.h
new file mode 100644
index 0000000..8142722
--- /dev/null
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/iis2dh.h
@@ -0,0 +1,127 @@
+#ifndef IIS2DH_Breakout
+#define IIS2DH_Breakout
+
+#include <stdio.h>
+#include "boards.h"
+#include "app_util_platform.h"
+#include "app_error.h"
+#include "nrf_drv_twi.h"
+#include "nrf_delay.h"
+#include "us_ble.h"
+
+
+#include "nrf_log_backend_rtt.h"
+#include "nrf_log.h"
+#include "nrf_log_default_backends.h"
+#include "nrf_log_ctrl.h"
+
+#define IIS2DH_REG_WHOAMI 0x0F
+#define IIS2DH_REG_OUT_X_L 0x28
+#define IIS2DH_REG_OUT_X_H 0x29
+#define IIS2DH_REG_OUT_Y_L 0x2A
+#define IIS2DH_REG_OUT_Y_H 0x2B
+#define IIS2DH_REG_OUT_Z_L 0x2C
+#define IIS2DH_REG_OUT_Z_H 0x2D
+
+#define IIS2DH_REG_CTRL_REG1  0x20
+#define IIS2DH_REG_CTRL_REG2  0x21
+#define IIS2DH_REG_CTRL_REG3  0x22
+#define IIS2DH_REG_CTRL_REG4  0x23
+#define IIS2DH_REG_CTRL_REG5  0x24
+#define IIS2DH_REG_CTRL_REG6  0x25
+
+#define IIS2DH_REG_STATUS_REG_AUX 0x07
+#define IIS2DH_REG_OUT_TEMP_L 0x0C
+#define IIS2DH_REG_OUT_TEMP_H 0x0D
+
+#define IIS2DH_REG_INT_COUNTER_REG
+#define IIS2DH_REG_TEMP_CFG_REG 0x1F
+#define IIS2DH_REG_REFERENCE
+#define IIS2DH_REG_STATUS_REG
+
+
+// 
+#define IIS2DH_ADDRESS     0x18
+
+
+#define PIN_IIS2DH_SCL     31
+#define PIN_IIS2DH_SDA     30
+
+#define PIN_IIS2DH_INT1   12
+#define PIN_IIS2DH_INT2   13
+
+// TWI instance ID
+#if TWI0_ENABLED
+#define TWI_INSTANCE_ID     0
+#elif TWI1_ENABLED
+#define TWI_INSTANCE_ID     1
+#endif
+
+
+extern uint8_t IIS2DH_buffer[805];
+extern uint16_t IIS2DH_buffer_index;
+/* Frame counter for IIS2DH over bluetooth */
+extern uint16_t IIS2DH_frame_number;
+
+
+typedef enum {
+  IIS2DH_LowPowerMode = 0,
+  IIS2DH_NormalMode = 1,
+  IIS2DH_HighResolutionMode = 2
+} IIS2DH_OperatingModes;
+
+typedef enum {
+  AllModes_1Hz = 0x10,
+  AllModes_10Hz = 0x20,
+  AllModes_25Hz = 0x30,
+  AllModes_50Hz = 0x40,
+  AllModes_100Hz = 0x50,
+  AllModes_200Hz = 0x60,
+  AllModes_400Hz = 0x70,
+  LowPower_1620Hz = 0x80,
+  LowPower_5376Hz = 0x90,
+  HighResolution_1344Hz = 0x90,
+  Normal_1344Hz = 0x90
+} IIS2DH_DataRate;
+
+typedef enum {
+  IIS2DH_Precision_2g = 0x00,
+  IIS2DH_Precision_4g = 0x10,
+  IIS2DH_Precision_8g = 0x20,
+  IIS2DH_Precision_16g = 0x30,
+}IIS2DH_FullScale;
+
+/**
+ * @brief Function for initializing the GPIO and TWI peripheral for the IIS2DH sensor
+ *
+ */
+void IIS2DH_init();
+
+/**
+ * @brief Function for reading a register from the IIS2DH sensor
+ *
+ * @param[in] register_address The address of the register to read from
+ * @param[out] rx_buffer The buffer to store the received data
+ * @param[in] number_of_bytes The number of bytes to read
+ *
+ * @retval true If the read was successful
+ * @retval false If the read was not successful
+ *
+ */
+bool IIS2DH_register_read(uint8_t register_address, uint8_t * rx_buffer, uint8_t number_of_bytes);
+
+bool IIS2DH_register_write(uint8_t register_address, uint8_t * wx_buffer, uint8_t number_of_bytes);
+
+bool setupTemp();
+int8_t getTemp();
+
+bool setupAccelormeter(IIS2DH_OperatingModes mode, IIS2DH_DataRate rate, IIS2DH_FullScale fs);
+bool getAccelerationData(uint16_t* X, uint16_t* Y, uint16_t* Z, IIS2DH_OperatingModes mode, IIS2DH_FullScale range);
+
+bool IIS2DH_set_streaming_enabled(bool enable);
+void finalizeFrameWithoutIMU(void);
+
+void getIIS2DHData2Buffer();
+#endif
+
+
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/main.c b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/main.c
index 84d9b46..2918c2e 100644
--- a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/main.c
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/main.c
@@ -52,6 +52,8 @@
 #include "us_ble.h"
 #include "us_defines.h"
 
+#include "iis2dh.h"
+
 // Buffers to store US data
 ArrayList_type m_rx_buf[NUMBER_OF_XFERS*MAX_BUFFER_NUMBER_OF_US_FRAMES] = {0};
 
@@ -69,6 +71,14 @@ volatile bool ble_connected = false;
 // A flag to signal that MSP config is received
 volatile bool msp_conf_received = false;
 
+volatile bool do_act_reading=false;
+
+
+// Handle accelerometer as requested by GUI/User (see us_ble.c)
+volatile bool accel_stream_enabled = false;
+volatile bool accel_stream_requested = false;
+volatile bool accel_stream_update_pending = false;
+
 /**@brief Function for initializing the timer module.
  */
 static void timers_init(void)
@@ -132,6 +142,7 @@ void in_pin_handler(nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
         // Enable timer and counter to start the four SPI transactions
         nrf_drv_timer_enable(&timer_timer);
         nrf_drv_timer_enable(&timer_counter);
+        do_act_reading =true;
     }
 }
 
@@ -167,6 +178,19 @@ static void gpio_init(void)
 
 }
 
+
+static void apply_accel_mode_if_pending(void)
+{
+    if (!accel_stream_update_pending)
+        return;
+
+    accel_stream_enabled = accel_stream_requested;
+    IIS2DH_set_streaming_enabled(accel_stream_enabled);
+    accel_stream_update_pending = false;
+}
+
+
+
 /**@brief Application main function.
  */
 int main(void)
@@ -178,11 +202,13 @@ int main(void)
     us_ble_init();
     gpio_init();
     us_spi_init();
+    IIS2DH_init();
 
 
     // Tell MSP430 that the BLE connection is not ready yet
     nrf_drv_gpiote_out_clear(PIN_BLE_CONN_READY);
 
+
     // Start BLE advertising
     advertising_start();
 
@@ -197,16 +223,35 @@ int main(void)
 
     while(msp_conf_received==false)
     {
-        // Wait for MSP430 config to be received from host PC
+        idle_state_handle();
     }
-    //msp_conf_received = false;
+    msp_conf_received = false;
+
+    apply_accel_mode_if_pending();
 
     // Now the BLE connection is ready to send US data
     nrf_drv_gpiote_out_set(PIN_BLE_CONN_READY);
 
     // Enter main loop.
     while(1)
-    {
+    { 
+        // Enable/disable accelerometer as requested by user-config
+        apply_accel_mode_if_pending();
+
+        if (do_act_reading)
+        {
+            if (accel_stream_enabled)
+            {
+                getIIS2DHData2Buffer();
+            }
+            else
+            {
+                finalizeFrameWithoutIMU();
+            }
+
+            do_act_reading = false;
+        }
+
         send_pending_frames();
         idle_state_handle();
     }
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/pca10040/s132/ses/US_probe_nRF52_firmware.emProject b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/pca10040/s132/ses/US_probe_nRF52_firmware.emProject
index 3199725..2506b6f 100644
--- a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/pca10040/s132/ses/US_probe_nRF52_firmware.emProject
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/pca10040/s132/ses/US_probe_nRF52_firmware.emProject
@@ -99,6 +99,11 @@
         <configuration Name="Release" build_exclude_from_build="No" />
       </file>
       <file file_name="../../../../../../modules/nrfx/drivers/src/nrfx_ppi.c" />
+      <file file_name="../../../../../../modules/nrfx/drivers/include/nrfx_twim.h" />
+      <file file_name="../../../../../../modules/nrfx/drivers/src/nrfx_twi.c" />
+      <file file_name="../../../../../../modules/nrfx/drivers/src/nrfx_twim.c" />
+      <file file_name="../../../../../../modules/nrfx/drivers/src/nrfx_twi_twim.c" />
+      <file file_name="../../../../../../integration/nrfx/legacy/nrf_drv_twi.c" />
     </folder>
     <folder Name="Board Support">
       <file file_name="../../../../../../components/libraries/bsp/bsp.c" />
@@ -112,6 +117,8 @@
       <file file_name="../../../us_defines.h" />
       <file file_name="../../../us_ble.c" />
       <file file_name="../../../us_ble.h" />
+      <file file_name="../../../iis2dh.c" />
+      <file file_name="../../../iis2dh.h" />
     </folder>
     <folder Name="nRF_Segger_RTT">
       <file file_name="../../../../../../external/segger_rtt/SEGGER_RTT.c" />
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_ble.c b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_ble.c
index 49bf1ed..cc5e639 100644
--- a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_ble.c
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_ble.c
@@ -73,6 +73,7 @@
 #include "nrf_delay.h"
 #include "us_ble.h"
 #include "us_defines.h"
+#include "iis2dh.h"
 
 
 #define APP_BLE_CONN_CFG_TAG            1                                           /**< A tag identifying the SoftDevice BLE configuration. */
@@ -102,6 +103,11 @@ extern volatile bool ble_connected;
 extern volatile bool msp_conf_received;
 extern int BLE_packet_ready;
 
+extern volatile bool accel_stream_enabled;
+extern volatile bool accel_stream_requested;
+extern volatile bool accel_stream_update_pending;
+
+
 void sleep_mode_enter(void);
 
 BLE_NUS_DEF(m_nus, NRF_SDH_BLE_TOTAL_LINK_COUNT);                                   /**< BLE NUS service instance. */
@@ -167,6 +173,8 @@ static void gap_params_init(void)
 }
 
 
+
+
 /**@brief Function for handling Queued Write Module errors.
  *
  * @details A pointer to this function will be passed to each service which may need to inform the
@@ -179,6 +187,18 @@ static void nrf_qwr_error_handler(uint32_t nrf_error)
     APP_ERROR_HANDLER(nrf_error);
 }
 
+
+// Helper to decode little-endian uint32_t
+static uint32_t read_u32_le(const uint8_t *p)
+{
+    return ((uint32_t)p[0]) |
+           ((uint32_t)p[1] << 8) |
+           ((uint32_t)p[2] << 16) |
+           ((uint32_t)p[3] << 24);
+}
+
+
+
 /**@brief Function for handling the data from the Nordic UART Service.
  *
  * @details This function will process the data received from the Nordic UART BLE Service and send
@@ -189,11 +209,23 @@ static void nrf_qwr_error_handler(uint32_t nrf_error)
 /**@snippet [Handling the data received over BLE] */
 static void nus_data_handler(ble_nus_evt_t * p_evt)
 {
-
     if (p_evt->type == BLE_NUS_EVT_RX_DATA)
     {
-        // Copy received command from python to the SPI transmit buffer
-        memcpy(m_tx_buf_1, p_evt->params.rx_data.p_data, p_evt->params.rx_data.length);
+        const uint8_t *rx  = p_evt->params.rx_data.p_data;
+        uint16_t       len = p_evt->params.rx_data.length;
+
+        // Only decode config packets: start byte 0xFA, transFreq at bytes 5..8
+        if ((len >= 9) && (rx[0] == 0xFA))
+        {
+            uint32_t transFreq = read_u32_le(&rx[5]);
+          
+            // If transFreq contains code 101, enable accelerometer
+            accel_stream_requested = (transFreq == 101u);
+            accel_stream_update_pending = true;
+        }
+
+        // Forward packet unchanged to MSP430
+        memcpy(m_tx_buf_1, rx, len);
         msp_conf_received = true;
 
         // Clear the BLE buffers to send US data with the received configuration
diff --git a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_spi.c b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_spi.c
index 876fe88..90b0272 100644
--- a/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_spi.c
+++ b/fw/nrf52/ble_peripheral/US_probe_nRF52_firmware/us_spi.c
@@ -60,6 +60,8 @@ extern volatile bool ble_connected;
 
 extern volatile bool msp_conf_received;
 
+volatile bool flag_add_IMU;
+
 extern int buffer_content;
 extern int buffer_counter;
 
@@ -173,11 +175,9 @@ void counter_cc0_event_handler(nrf_timer_event_t event_type, void* p_context)
         //APP_ERROR_CHECK(1);
     }
 
-    buffer_counter++;
-    if(buffer_counter == MAX_BUFFER_NUMBER_OF_US_FRAMES)
-        buffer_counter = 0;
-
-    BLE_packet_ready = 1;
+    // Buffer ready to add IMU data
+    flag_add_IMU = true;
+    
     //msp_conf_received = false;
     
     // Relay the received SPI data to the BLE dongle