diff --git a/components/bmi270/example/README.md b/components/bmi270/example/README.md
index 03d4e05e5..7e4cde5e8 100644
--- a/components/bmi270/example/README.md
+++ b/components/bmi270/example/README.md
@@ -21,6 +21,10 @@ This example shows how to use the `espp::Bmi270` component to initialize and com
     - [Debug Tips](#debug-tips)
   - [Performance Notes](#performance-notes)
   - [Advanced Features](#advanced-features)
+    - [Data Ready Interrupt](#data-ready-interrupt)
+    - [Fast Offset Compensation (FOC)](#fast-offset-compensation-foc)
+    - [Component Re-Trim (CRT)](#component-re-trim-crt)
+    - [Built-in Features](#built-in-features)
   - [References](#references)
 
 <!-- markdown-toc end -->
@@ -32,6 +36,8 @@ This example shows how to use the `espp::Bmi270` component to initialize and com
 - **Orientation Filtering**: Both Kalman and Madgwick filter implementations
 - **Real-time Data Logging**: CSV format output suitable for plotting
 - **Interrupt Configuration**: Data ready and motion detection interrupts
+- **Fast Offset Compensation (FOC)**: Calibrate accelerometer and gyroscope offsets
+- **Component Re-Trim (CRT)**: Compensate for gyroscope sensitivity changes
 - **Advanced Configuration**: Performance modes, bandwidth settings, power management
 
 ## How to use example
@@ -187,6 +193,16 @@ The CSV output can be imported into analysis tools like:
 - Consider magnetometer integration for yaw correction
 - Check for temperature effects
 
+**Stack Overflow during Initialization**:
+- Device crashes or resets while calling `bmi270.init()`.
+- Uploading the configuration file uses a large stack buffer by default.
+- Reduce the burst write size in the configuration to limit stack usage.
+  ```cpp
+  espp::Bmi270::Config config;
+  config.burst_write_size = 128; // Use 128-byte chunks (default is 0/unlimited)
+  espp::Bmi270 imu(config);
+  ```
+
 ### Debug Tips
 
 1. **Enable verbose logging:**
@@ -218,7 +234,46 @@ The CSV output can be imported into analysis tools like:
 
 ## Advanced Features
 
-The example can be extended to demonstrate:
+### Data Ready Interrupt
+
+The BMI270 can be configured to generate an interrupt when new data is available. This is more efficient than polling the sensor.
+
+```cpp
+Imu::InterruptConfig int_config{
+    .pin = Imu::InterruptPin::INT1,
+    .output_type = Imu::InterruptOutput::PUSH_PULL,
+    .active_level = Imu::InterruptLevel::ACTIVE_HIGH,
+    .enable_data_ready = true
+};
+imu.configure_interrupts(int_config, ec);
+```
+
+### Fast Offset Compensation (FOC)
+
+FOC allows for quick calibration of the accelerometer and gyroscope offsets.
+
+```cpp
+// Accelerometer FOC (e.g., device flat on table, Z axis pointing up)
+Imu::AccelFocGValue accel_foc_target = {
+    .x = 0, .y = 0, .z = 1, .sign = 0 // 1g on Z axis
+};
+imu.perform_accel_foc(accel_foc_target, ec);
+
+// Gyroscope FOC (device must be stationary)
+imu.perform_gyro_foc(ec);
+```
+
+### Component Re-Trim (CRT)
+
+CRT is a feature to compensate for sensitivity changes in the gyroscope that may occur during assembly or over time.
+
+```cpp
+imu.perform_crt(ec);
+```
+
+### Built-in Features
+
+The BMI270 also supports several on-chip features that can be demonstrated by extending this example:
 
 - **Motion Detection**: Any motion, no motion, significant motion
 - **Step Counting**: Pedometer functionality
diff --git a/components/bmi270/example/main/bmi270_example.cpp b/components/bmi270/example/main/bmi270_example.cpp
index d6a82490f..1aad443d5 100644
--- a/components/bmi270/example/main/bmi270_example.cpp
+++ b/components/bmi270/example/main/bmi270_example.cpp
@@ -108,6 +108,45 @@ extern "C" void app_main(void) {
   // create the IMU
   Imu imu(config);
 
+  // ---------------------------------------------------------------------------
+  // [Optional] Sensor Calibration (FOC & CRT)
+  // WARNING: 
+  // 1. The device must be COMPLETELY STATIONARY on a flat surface.
+  // 2. Enable these lines only when you need calibration (e.g., once after assembly).
+  // ---------------------------------------------------------------------------
+  #if 0
+  // Perform FOC (Fast Offset Compensation)
+  // Note: This assumes the device is flat on a table (Z-axis = 1g)
+  // For a real application, you might want to trigger this based on a user action
+  // or store the offsets in NVS.
+  std::error_code ec;
+  
+  logger.info("Performing Accelerometer FOC...");
+  Imu::AccelFocGValue accel_foc_target = {.x = 0, .y = 0, .z = 1, .sign = 0}; // 1g on Z axis
+  if (imu.perform_accel_foc(accel_foc_target, ec)) {
+    logger.info("Accelerometer FOC completed successfully");
+  } else {
+    logger.error("Accelerometer FOC failed: {}", ec.message());
+  }
+
+  logger.info("Performing Gyroscope FOC...");
+  // Note: Device must be stationary for Gyro FOC
+  if (imu.perform_gyro_foc(ec)) {
+    logger.info("Gyroscope FOC completed successfully");
+  } else {
+    logger.error("Gyroscope FOC failed: {}", ec.message());
+  }
+
+  // Perform CRT (Component Re-Trim)
+  // This feature compensates for sensitivity changes in the gyroscope.
+  logger.info("Performing CRT...");
+  if (imu.perform_crt(ec)) {
+    logger.info("CRT completed successfully");
+  } else {
+    logger.error("CRT failed: {}", ec.message());
+  }
+  #endif
+
   // make a task to read out the IMU data and print it to console
   auto task_fn = [&](std::mutex &m, std::condition_variable &cv) -> bool {
     // sleep first in case we don't get IMU data and need to exit early
@@ -142,24 +181,6 @@ extern "C" void app_main(void) {
         fmt::format("{:02.3f},{:02.3f},{:02.3f},", (float)accel.x, (float)accel.y, (float)accel.z);
     text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gyro.x, (float)gyro.y, (float)gyro.z);
     text += fmt::format("{:02.1f},", temp);
-    // print kalman filter outputs
-    text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)orientation.x, (float)orientation.y,
-                        (float)orientation.z);
-    text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gravity_vector.x,
-                        (float)gravity_vector.y, (float)gravity_vector.z);
-
-    auto madgwick_orientation = madgwick_filter_fn(dt, accel, gyro);
-    float roll = madgwick_orientation.roll;
-    float pitch = madgwick_orientation.pitch;
-    float yaw = madgwick_orientation.yaw;
-    float vx = sin(pitch);
-    float vy = -cos(pitch) * sin(roll);
-    float vz = -cos(pitch) * cos(roll);
-
-    // print madgwick filter outputs
-    text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", roll, pitch, yaw);
-    text += fmt::format("{:03.3f},{:03.3f},{:03.3f}", vx, vy, vz);
-
     fmt::print("{}\n", text);
 
     return false;
diff --git a/components/bmi270/include/bmi270.hpp b/components/bmi270/include/bmi270.hpp
index 675991247..8495a3e10 100644
--- a/components/bmi270/include/bmi270.hpp
+++ b/components/bmi270/include/bmi270.hpp
@@ -1,5 +1,10 @@
 #pragma once
 
+#include <algorithm>
+#include <cmath>
+#include <thread>
+#include <chrono>
+
 #include "base_peripheral.hpp"
 #include "bmi270_detail.hpp"
 
@@ -68,6 +73,7 @@ class Bmi270 : public espp::BasePeripheral<uint8_t, Interface == bmi270::Interfa
   using WristData = bmi270::WristData;
   using InterruptConfig = bmi270::InterruptConfig;
   using FifoConfig = bmi270::FifoConfig;
+  using AccelFocGValue = bmi270::AccelFocGValue;
 
   /// Filter function for filtering 6-axis data into 3-axis orientation data
   /// @param dt The time step in seconds
@@ -446,6 +452,517 @@ class Bmi270 : public espp::BasePeripheral<uint8_t, Interface == bmi270::Interfa
     return !ec;
   }
 
+  /// Perform accelerometer Fast Offset Compensation (FOC)
+  /// @param accel_g_value Target values for FOC
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool perform_accel_foc(const AccelFocGValue &accel_g_value, std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+
+    // Check for input validity
+    if ((std::abs(accel_g_value.x) + std::abs(accel_g_value.y) + std::abs(accel_g_value.z)) != 1) {
+      logger_.error("Invalid AccelFocGValue: exactly one axis must be 1");
+      return false;
+    }
+
+    // Perform simplified verification consistent with the C driver
+    ImuConfig saved_config = imu_config_;
+    bool saved_acc_en = (read_u8_from_register(static_cast<uint8_t>(Register::PWR_CTRL), ec) & ACC_ENABLE) != 0;
+    if (ec) return false;
+    uint8_t pwr_conf = read_u8_from_register(static_cast<uint8_t>(Register::PWR_CONF), ec);
+    if (ec) return false;
+    bool saved_aps = (pwr_conf & 0x01) != 0;
+
+    // Prepare for FOC
+    if (saved_aps) {
+      clear_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+      if (ec) return false;
+    }
+    if (!saved_acc_en) {
+      set_bits_in_register(static_cast<uint8_t>(Register::PWR_CTRL), ACC_ENABLE, ec);
+      if (ec) return false;
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    write_u8_to_register(static_cast<uint8_t>(Register::ACC_CONF), 0xB7, ec); // 50Hz, CIC_AVG8, Perf Opt
+    if (ec) return false;
+    write_u8_to_register(static_cast<uint8_t>(Register::ACC_RANGE), 0x00, ec); // 2G
+    if (ec) return false;
+
+    // Read 128 samples
+    int32_t x_sum = 0, y_sum = 0, z_sum = 0;
+    for (int i = 0; i < 128; ++i) {
+        // Wait for data ready (20ms for 50Hz)
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        auto raw = get_accelerometer_raw(ec);
+        if (ec) return false;
+        x_sum += raw.x;
+        y_sum += raw.y;
+        z_sum += raw.z;
+    }
+
+    // Average
+    RawValue avg = {
+        static_cast<int16_t>(x_sum / 128),
+        static_cast<int16_t>(y_sum / 128),
+        static_cast<int16_t>(z_sum / 128)
+    };
+
+    // Calculate target
+    int16_t target_x = 0, target_y = 0, target_z = 0;
+    int16_t ref_1g = 16384;
+    int16_t target_g = (accel_g_value.sign == 1) ? -ref_1g : ref_1g;
+
+    if (accel_g_value.x) target_x = target_g;
+    if (accel_g_value.y) target_y = target_g;
+    if (accel_g_value.z) target_z = target_g;
+
+    // Calculate delta (offset)
+    // Offset = Average - Target
+    // However, the offset register subtracts the value from the signal?
+    // C driver: comp_data = data - target.
+    // scale_accel_offset: scale it.
+    // invert_accel_offset: multiply by -1.
+    // So register value = -(Average - Target) = Target - Average.
+
+    int32_t delta_x = avg.x - target_x;
+    int32_t delta_y = avg.y - target_y;
+    int32_t delta_z = avg.z - target_z;
+
+    // Invert
+    int8_t off_x = static_cast<int8_t>(-(delta_x / 64));
+    int8_t off_y = static_cast<int8_t>(-(delta_y / 64));
+    int8_t off_z = static_cast<int8_t>(-(delta_z / 64));
+
+    // Write offset registers
+    uint8_t data[3] = {static_cast<uint8_t>(off_x), static_cast<uint8_t>(off_y), static_cast<uint8_t>(off_z)};
+    write_many_to_register(static_cast<uint8_t>(Register::ACC_OFF_COMP_0), data, 3, ec);
+    if (ec) return false;
+
+    // Enable Accelerometer offset compensation
+    // NV_CONF (0x70) bit 3 (BMI2_NV_ACC_OFFSET_MASK = 0x08)
+    set_bits_in_register(static_cast<uint8_t>(Register::NV_CONF), 0x08, ec);
+    if (ec) return false;
+
+    // Restore configuration
+    // Restore config
+    set_config(saved_config, ec);
+    if (ec) return false;
+
+    if (!saved_acc_en) {
+        enable_accelerometer(false, ec);
+        if (ec) return false;
+    }
+
+    if (saved_aps) {
+        set_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+        if (ec) return false;
+    }
+
+    logger_.info("Accel FOC completed (offsets: X={}, Y={}, Z={})", off_x, off_y, off_z);
+    return true;
+  }
+
+  /// Perform gyroscope Fast Offset Compensation (FOC)
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool perform_gyro_foc(std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+
+    // Save status
+    ImuConfig saved_config = imu_config_;
+    bool saved_gyr_en = (read_u8_from_register(static_cast<uint8_t>(Register::PWR_CTRL), ec) & GYR_ENABLE) != 0;
+    if (ec) return false;
+    uint8_t pwr_conf = read_u8_from_register(static_cast<uint8_t>(Register::PWR_CONF), ec);
+    if (ec) return false;
+    bool saved_aps = (pwr_conf & 0x01) != 0;
+
+    // Prepare for FOC
+    if (saved_aps) {
+      clear_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+      if (ec) return false;
+    }
+    if (!saved_gyr_en) {
+      set_bits_in_register(static_cast<uint8_t>(Register::PWR_CTRL), GYR_ENABLE, ec);
+      if (ec) return false;
+      std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    }
+
+    // Configure FOC (25Hz, 2000dps)
+    write_u8_to_register(static_cast<uint8_t>(Register::GYR_CONF), 0xB6, ec);
+    if (ec) return false;
+    write_u8_to_register(static_cast<uint8_t>(Register::GYR_RANGE), 0x00, ec);
+    if (ec) return false;
+
+    // Accumulate 128 samples
+    int32_t x_sum = 0, y_sum = 0, z_sum = 0;
+    for (int i = 0; i < 128; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(45));
+        auto raw = get_gyroscope_raw(ec);
+        if (ec) return false;
+        x_sum += raw.x;
+        y_sum += raw.y;
+        z_sum += raw.z;
+    }
+
+    // Average and saturate (10-bit)
+    auto saturate = [](int16_t val) -> int16_t {
+        return std::clamp<int16_t>(val, -512, 511);
+    };
+    int16_t off_x = saturate(-(x_sum / 128));
+    int16_t off_y = saturate(-(y_sum / 128));
+    int16_t off_z = saturate(-(z_sum / 128));
+
+    uint8_t current_reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), ec);
+    if (ec) return false;
+    bool gain_enabled = (current_reg & 0x80) != 0;
+
+    // Write offset and enable compensation (Bit 6)
+    uint8_t data[4];
+    data[0] = static_cast<uint8_t>(off_x & 0xFF);
+    data[1] = static_cast<uint8_t>(off_y & 0xFF);
+    data[2] = static_cast<uint8_t>(off_z & 0xFF);
+    data[3] = static_cast<uint8_t>(((off_x >> 8) & 0x03) | ((off_y >> 6) & 0x0C) | ((off_z >> 4) & 0x30)) | 0x40;
+
+    if (gain_enabled) {
+        data[3] |= 0x80;
+    }
+
+    write_many_to_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_3), data, 4, ec);
+    if (ec) return false;
+
+    // Restore status
+    set_config(saved_config, ec);
+    if (ec) return false;
+    if (!saved_gyr_en) enable_gyroscope(false, ec);
+    if (saved_aps) set_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+
+    logger_.info("Gyro FOC completed (offsets: X={}, Y={}, Z={})", off_x, off_y, off_z);
+    return true;
+  }
+
+  /// Perform Component Re-Trim (CRT)
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool perform_crt(std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+
+    // Save status and disable APS
+    uint8_t pwr_conf = read_u8_from_register(static_cast<uint8_t>(Register::PWR_CONF), ec);
+    if (ec) return false;
+    bool saved_aps = (pwr_conf & 0x01) != 0;
+    if (saved_aps) {
+      clear_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+      if (ec) return false;
+    }
+
+    // Setup: Disable Gyro/FIFO, Enable Accel
+    enable_gyroscope(false, ec);
+    if (ec) return false;
+    clear_bits_in_register(static_cast<uint8_t>(Register::FIFO_CONFIG_1), 0xE0, ec);
+    if (ec) return false;
+    enable_accelerometer(true, ec);
+    if (ec) return false;
+
+    // Initialize CRT: Clear running bit first, then set both CRT mode and running
+    write_u8_to_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), 0x00, ec);
+    if (ec) return false;
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+
+    // Set CRT mode (bit 0)
+    set_bits_in_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), 0x01, ec);
+    if (ec) return false;
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+
+    // Trigger CRT by setting running bit (bit 2)
+    set_bits_in_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), 0x04, ec);
+    if (ec) return false;
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+
+    // Read initial state before CMD
+    uint8_t gyr_crt_conf = read_u8_from_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), ec);
+    if (ec) return false;
+    bool rdy_for_dl_initial = (gyr_crt_conf & 0x08) != 0;
+    logger_.debug("CRT: Setup complete, state: 0x{:02X}", gyr_crt_conf);
+
+    // Send command to trigger test (bit 1 of CMD register)
+    write_u8_to_register(static_cast<uint8_t>(Register::CMD), 0x02, ec);
+    if (ec) return false;
+
+    // Wait for bit 3 to toggle/change
+    bool rdy_toggled = false;
+    for (int i = 0; i < 200; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        uint8_t reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), ec);
+        if (ec) return false;
+
+        bool rdy_for_dl_current = (reg & 0x08) != 0;
+
+        // Check if bit 3 changed from initial state
+        if (rdy_for_dl_current != rdy_for_dl_initial) {
+            rdy_toggled = true;
+            logger_.debug("CRT: Ready bit transitioned at {}ms", i * 10);
+            break;
+        }
+    }
+
+    if (!rdy_toggled) {
+        logger_.debug("CRT: Ready bit transition not detected (device variant)");
+    }
+
+    // Upload CRT configuration image
+    uint8_t addr_data[2] = {static_cast<uint8_t>((0x1800 / 2) & 0x0F), static_cast<uint8_t>((0x1800 / 2) >> 4)};
+    write_many_to_register(static_cast<uint8_t>(Register::INIT_ADDR_0), addr_data, 2, ec);
+    if (ec) return false;
+
+    if (config_file_size < 0x1800 + 2048) {
+        logger_.error("Config file too small for CRT");
+        return false;
+    }
+    const uint8_t* crt_data = config_file + 0x1800;
+    for (size_t offset = 0; offset < 2048; offset += burst_write_size_) {
+        size_t to_write = std::min(static_cast<size_t>(burst_write_size_), 2048 - offset);
+        write_many_to_register(static_cast<uint8_t>(Register::INIT_DATA), crt_data + offset, to_write, ec);
+        if (ec) return false;
+    }
+
+    // Wait for completion: Running flag (bit 2) clears when CRT finishes
+    bool completed = false;
+    for (int i = 0; i < 250; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        uint8_t reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), ec);
+        if (ec) return false;
+        if ((reg & 0x04) == 0) {
+            completed = true;
+            break;
+        }
+    }
+    if (!completed) {
+        logger_.error("CRT: Timeout (2.5s)");
+        return false;
+    }
+
+    // Verify result
+    uint8_t status_reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_USR_GAIN_0), ec);
+    if (ec) return false;
+    uint8_t status = (status_reg & 0x38) >> 3;
+
+    if (status != 0x00) {
+        logger_.error("CRT: Failed");
+        return false;
+    }
+    logger_.info("CRT: Completed successfully");
+
+    // Restore state
+    set_config(imu_config_, ec);
+    if (ec) return false;
+    if (saved_aps) set_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+
+    return status == 0x00;
+  }
+
+  /// Abort CRT or Gyro Self Test
+  bool abort_crt_gyro_self_test(std::error_code &ec) {
+      std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+      set_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x02, ec);
+      return !ec;
+  }
+
+  /// Perform Gyro Self Test
+  bool perform_gyro_self_test(std::error_code &ec) {
+      std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+
+      uint8_t pwr_conf = read_u8_from_register(static_cast<uint8_t>(Register::PWR_CONF), ec);
+      if (ec) return false;
+      bool saved_aps = (pwr_conf & 0x01) != 0;
+      if (saved_aps) {
+        clear_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+        if (ec) return false;
+      }
+
+      enable_gyroscope(false, ec);
+      if (ec) return false;
+      clear_bits_in_register(static_cast<uint8_t>(Register::FIFO_CONFIG_1), 0xE0, ec);
+      if (ec) return false;
+      enable_accelerometer(true, ec);
+      if (ec) return false;
+
+      // Set Running (Bit 2), Clear Selection (Bit 0 for Self Test)
+      set_bits_in_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), 0x04, ec);
+      if (ec) return false;
+      clear_bits_in_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), 0x01, ec);
+      if (ec) return false;
+
+      write_u8_to_register(static_cast<uint8_t>(Register::CMD), 0x02, ec);
+      if (ec) return false;
+
+      bool completed = false;
+      for (int i = 0; i < 200; ++i) {
+          std::this_thread::sleep_for(std::chrono::milliseconds(10));
+          uint8_t reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_CRT_CONF), ec);
+          if (ec) return false;
+          if ((reg & 0x04) == 0) { completed = true; break; }
+      }
+
+      if (!completed) {
+          logger_.error("Gyro Self Test timeout");
+          return false;
+      }
+
+      uint8_t st_result = read_u8_from_register(static_cast<uint8_t>(Register::GYR_SELF_TEST_AXES), ec);
+      if (ec) return false;
+
+      bool success = (st_result & 0x01) && (st_result & 0x02) && (st_result & 0x04) && (st_result & 0x08);
+      if (success) {
+          logger_.info("Gyro Self Test passed");
+      } else {
+          logger_.error("Gyro Self Test failed: 0x{:02X}", st_result);
+      }
+
+      set_config(imu_config_, ec);
+      if (ec) return false;
+      if (saved_aps) set_bits_in_register(static_cast<uint8_t>(Register::PWR_CONF), 0x01, ec);
+
+      return success;
+  }
+
+  /// Get Accelerometer Offsets
+  /// @param x [out] X-axis offset (8-bit signed)
+  /// @param y [out] Y-axis offset (8-bit signed)
+  /// @param z [out] Z-axis offset (8-bit signed)
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool get_accel_offset(int8_t &x, int8_t &y, int8_t &z, std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+    uint8_t data[3];
+    // ACC_OFF_COMP_0 is 0x71. Reads 0x71 (X), 0x72 (Y), 0x73 (Z)
+    read_many_from_register(static_cast<uint8_t>(Register::ACC_OFF_COMP_0), data, 3, ec);
+    if (ec) return false;
+
+    x = static_cast<int8_t>(data[0]);
+    y = static_cast<int8_t>(data[1]);
+    z = static_cast<int8_t>(data[2]);
+    return true;
+  }
+
+  /// Set Accelerometer Offsets
+  /// @param x X-axis offset (8-bit signed)
+  /// @param y Y-axis offset (8-bit signed)
+  /// @param z Z-axis offset (8-bit signed)
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool set_accel_offset(int8_t x, int8_t y, int8_t z, std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+    uint8_t data[3] = {static_cast<uint8_t>(x), static_cast<uint8_t>(y), static_cast<uint8_t>(z)};
+    write_many_to_register(static_cast<uint8_t>(Register::ACC_OFF_COMP_0), data, 3, ec);
+    return !ec;
+  }
+
+  /// Enable/Disable Accelerometer Offset Compensation
+  /// @param enable True to enable, false to disable
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool set_accel_offset_enable(bool enable, std::error_code &ec) {
+      std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+      // NV_CONF (0x70) Bit 3 controls Accel Offset Enable
+      static constexpr uint8_t ACC_OFFSET_EN_MASK = 0x08;
+      if (enable) {
+          set_bits_in_register(static_cast<uint8_t>(Register::NV_CONF), ACC_OFFSET_EN_MASK, ec);
+      } else {
+          clear_bits_in_register(static_cast<uint8_t>(Register::NV_CONF), ACC_OFFSET_EN_MASK, ec);
+      }
+      return !ec;
+  }
+
+  /// Get Gyroscope Offsets
+  /// @param x [out] X-axis offset
+  /// @param y [out] Y-axis offset
+  /// @param z [out] Z-axis offset
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool get_gyro_offset(int16_t &x, int16_t &y, int16_t &z, std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+    uint8_t data[4];
+    // GYR_OFF_COMP_3 is 0x74. Reads 0x74...0x77
+    read_many_from_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_3), data, 4, ec);
+    if (ec) return false;
+
+    // LSBs
+    x = static_cast<int16_t>(data[0]);
+    y = static_cast<int16_t>(data[1]);
+    z = static_cast<int16_t>(data[2]);
+
+    // MSBs from data[3]
+    x |= static_cast<int16_t>((data[3] & 0x03) << 8);
+    y |= static_cast<int16_t>((data[3] & 0x0C) << 6);
+    z |= static_cast<int16_t>((data[3] & 0x30) << 4);
+
+    // Sign extension for 10-bit values
+    if (x & 0x0200) x |= 0xFC00;
+    if (y & 0x0200) y |= 0xFC00;
+    if (z & 0x0200) z |= 0xFC00;
+
+    return true;
+  }
+
+  /// Set Gyroscope Offsets
+  /// @param x X-axis offset
+  /// @param y Y-axis offset
+  /// @param z Z-axis offset
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool set_gyro_offset(int16_t x, int16_t y, int16_t z, std::error_code &ec) {
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+
+    // Read current register to preserve enable bit (bit 6 of data[3] / register 0x77)
+    uint8_t current_reg;
+    current_reg = read_u8_from_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), ec);
+    if (ec) return false;
+    uint8_t preserved_bits = current_reg & 0xC0;
+
+    uint8_t data[4];
+    data[0] = static_cast<uint8_t>(x & 0xFF);
+    data[1] = static_cast<uint8_t>(y & 0xFF);
+    data[2] = static_cast<uint8_t>(z & 0xFF);
+    data[3] = static_cast<uint8_t>(((x >> 8) & 0x03) | ((y >> 6) & 0x0C) | ((z >> 4) & 0x30));
+    
+    data[3] |= preserved_bits;
+
+    write_many_to_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_3), data, 4, ec);
+    return !ec;
+  }
+
+  /// Enable/Disable Gyroscope Offset Compensation
+  /// @param enable True to enable, false to disable
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool set_gyro_offset_enable(bool enable, std::error_code &ec) {
+      std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+      // GYR_OFF_COMP_6 (0x77) Bit 6
+      static constexpr uint8_t GYR_OFFSET_EN_MASK = 0x40;
+      if (enable) {
+          set_bits_in_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), GYR_OFFSET_EN_MASK, ec);
+      } else {
+          clear_bits_in_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), GYR_OFFSET_EN_MASK, ec);
+      }
+      return !ec;
+  }
+
+  /// Enable/Disable Gyroscope Gain Compensation
+  /// @param enable True to enable, false to disable
+  /// @param ec The error code to set if an error occurs
+  /// @return True if successful
+  bool set_gyro_gain_enable(bool enable, std::error_code &ec) {
+      std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+      static constexpr uint8_t GYR_GAIN_EN_MASK = 0x80;
+      if (enable) {
+          set_bits_in_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), GYR_GAIN_EN_MASK, ec);
+      } else {
+          clear_bits_in_register(static_cast<uint8_t>(Register::GYR_OFF_COMP_6), GYR_GAIN_EN_MASK, ec);
+      }
+      return !ec;
+  }
+
 protected:
   // BMI270 specific constants
   static constexpr uint8_t BMI270_CHIP_ID = 0x24; ///< BMI270 chip ID
@@ -539,11 +1056,12 @@ class Bmi270 : public espp::BasePeripheral<uint8_t, Interface == bmi270::Interfa
     INIT_ADDR_0 = 0x5B,        ///< Initialization address LSB
     INIT_ADDR_1 = 0x5C,        ///< Initialization address MSB
     INIT_DATA = 0x5E,          ///< Initialization data
+    GYR_CRT_CONF = 0x69,       ///< Gyroscope CRT configuration
     ACC_SELF_TEST = 0x6D,      ///< Accelerometer self-test
     GYR_SELF_TEST_AXES = 0x6E, ///< Gyroscope self-test axes
-    NV_CONF = 0x6A,            ///< Non-volatile memory configuration
     IF_CONF = 0x6B,            ///< Interface configuration
     DRV = 0x6C,                ///< Drive register
+    NV_CONF = 0x70,     ///< Non-volatile memory configuration offset
     ACC_OFF_COMP_0 = 0x71,     ///< Accelerometer offset compensation 0
     GYR_OFF_COMP_3 = 0x74,     ///< Gyroscope offset compensation 3
     GYR_OFF_COMP_6 = 0x77,     ///< Gyroscope offset compensation 6
diff --git a/components/bmi270/include/bmi270_detail.hpp b/components/bmi270/include/bmi270_detail.hpp
index 2100abacf..acdc19227 100644
--- a/components/bmi270/include/bmi270_detail.hpp
+++ b/components/bmi270/include/bmi270_detail.hpp
@@ -209,6 +209,38 @@ struct FifoConfig {
   uint16_t watermark_level = 1024;  ///< FIFO watermark level (bytes)
 };
 
+/// Structure to define accelerometer FOC target values
+struct AccelFocGValue {
+  uint8_t x;    ///< Target value for x axis: '0' for 0g, '1' for 1g
+  uint8_t y;    ///< Target value for y axis: '0' for 0g, '1' for 1g
+  uint8_t z;    ///< Target value for z axis: '0' for 0g, '1' for 1g
+  uint8_t sign; ///< Sign of 1g: '0' for positive (+), '1' for negative (-)
+};
+
+/// Structure to store the status of gyro self test result
+struct GyroSelfTestStatus {
+  bool gyr_st_axes_done; ///< gyro self test axes done
+  bool gyr_axis_x_ok;    ///< status of gyro X-axis self test
+  bool gyr_axis_y_ok;    ///< status of gyro Y-axis self test
+  bool gyr_axis_z_ok;    ///< status of gyro Z-axis self test
+};
+
+/// G trigger status
+enum class GTriggerStatus : uint8_t {
+  NO_ERROR = 0x00,     ///< No error
+  PRECON_ERROR = 0x01, ///< Pre-condition error
+  DL_ERROR = 0x02,     ///< Download error
+  ABORT_ERROR = 0x03,  ///< Abort error
+};
+
+/// Structure to define gyroscope saturation status of user gain
+struct GyrUserGainStatus {
+  uint8_t sat_x;                 ///< Status in x-axis
+  uint8_t sat_y;                 ///< Status in y-axis
+  uint8_t sat_z;                 ///< Status in z-axis
+  GTriggerStatus g_trigger_status; ///< G trigger status
+};
+
 } // namespace bmi270
 } // namespace espp
 
@@ -281,3 +313,22 @@ template <> struct fmt::formatter<espp::bmi270::Value> {
     return format_to(ctx.out(), "{{ x: {:.2f}, y: {:.2f}, z: {:.2f} }}", value.x, value.y, value.z);
   }
 };
+
+template <> struct fmt::formatter<espp::bmi270::GTriggerStatus> {
+  constexpr auto parse(format_parse_context &ctx) const { return ctx.begin(); }
+  template <typename FormatContext>
+  auto format(espp::bmi270::GTriggerStatus status, FormatContext &ctx) const {
+    switch (status) {
+    case espp::bmi270::GTriggerStatus::NO_ERROR:
+      return format_to(ctx.out(), "No Error");
+    case espp::bmi270::GTriggerStatus::PRECON_ERROR:
+      return format_to(ctx.out(), "Pre-condition Error");
+    case espp::bmi270::GTriggerStatus::DL_ERROR:
+      return format_to(ctx.out(), "Download Error");
+    case espp::bmi270::GTriggerStatus::ABORT_ERROR:
+      return format_to(ctx.out(), "Abort Error");
+    default:
+      return format_to(ctx.out(), "Unknown ({})", static_cast<uint8_t>(status));
+    }
+  }
+};