diff --git a/Peripherals/Inc/peripherals_imu_icm.hpp b/Peripherals/Inc/peripherals_imu_icm.hpp
index f8afa95..c8418a7 100644
--- a/Peripherals/Inc/peripherals_imu_icm.hpp
+++ b/Peripherals/Inc/peripherals_imu_icm.hpp
@@ -80,10 +80,6 @@ public:
     registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
                             new_accel_config);
 
-    //    setGyroscopeScale(scale_gyro_);
-    //
-    //    setAccelerometerScale(scale_accel_);
-
     // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
     registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
                             0x03);
@@ -129,16 +125,16 @@ public:
     for (std::uint32_t i = 0; i < samples; i++)
     {
       std::array<std::int16_t, 3> raw;
-      auto add_to_avg = [&raw](std::array<std::int32_t, 3>& out) {
-        for (std::uint32_t j = 0; j < 3; j++)
-          out[j] += raw[j];
-      };
 
       readGyroRaw(raw.data());
-      add_to_avg(avg_gyro);
+      for (std::uint32_t j = 0; j < 3; j++)
+        avg_gyro[j] += raw[j];
 
       readAccelerometerRaw(raw.data());
-      add_to_avg(avg_accel);
+      for (std::uint32_t j = 0; j < 3; j++)
+        avg_accel[j] += raw[j];
+
+      hal::delay(2);
     }
 
     for (std::uint32_t i = 0; i < 3; i++)
@@ -248,9 +244,6 @@ private:
   GyroScale scale_gyro_ = GyroScale::DPS_2000;
   AccelerometerScale scale_accel_ = AccelerometerScale::G16;
 
-  std::array<std::int16_t, 3> bias_gyro_;
-  std::array<std::int16_t, 3> bias_accel_;
-
   static constexpr float accel_fs_to_bit_constants_[4] = {
           (2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f),
           (16.0f / 32768.0f)};
@@ -259,6 +252,9 @@ private:
           (250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
           (2000.0f / 32768.0f)};
 
+  std::array<std::int16_t, 3> bias_gyro_;
+  std::array<std::int16_t, 3> bias_accel_;
+
   struct Registers_
   {
     static constexpr std::uint32_t ICM20689_ID = 0x98;