From 992c55b7852439c356a20f87b1009ff8ec51368d Mon Sep 17 00:00:00 2001
From: Erki <erki@skullnet.me>
Date: Tue, 6 Sep 2022 22:27:41 +0300
Subject: [PATCH] Temporary IMU fixes

---
 Peripherals/Inc/peripherals_imu_icm.hpp | 26 +++++++++++--------------
 1 file changed, 11 insertions(+), 15 deletions(-)

diff --git a/Peripherals/Inc/peripherals_imu_icm.hpp b/Peripherals/Inc/peripherals_imu_icm.hpp
index 76f71f8..9c02218 100644
--- a/Peripherals/Inc/peripherals_imu_icm.hpp
+++ b/Peripherals/Inc/peripherals_imu_icm.hpp
@@ -72,18 +72,14 @@ public:
                                   // rate = 1KHz, temp filter = 42
     hal::delay(10);
 
-    const std::uint8_t new_gyro_conf =  (std::uint32_t(scale_gyro_) << 3);
+    const std::uint8_t new_gyro_conf = (std::uint32_t(scale_gyro_) << 3);
     registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
         new_gyro_conf);
 
-    const std::uint8_t new_accel_config =  (std::uint32_t(scale_accel_) << 3);
+    const std::uint8_t new_accel_config = (std::uint32_t(scale_accel_) << 3);
     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;