Temporary IMU fixes

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;

Threads/Inc/threads_primitivethread.hpp

@@ -13,6 +13,7 @@
 
 #include <tuple>
 #include <type_traits>
+#include <cstdint>
 
 namespace Threads
 {

Threads/Src/threads_primitivethread.cpp

@@ -9,6 +9,8 @@
 
 #include "peripherals_utility.hpp"
 
+#include <limits>
+
 #ifdef INCLUDE_xTaskGetCurrentTaskHandle
 #define ASSERT_IS_CURRENT() \
   assert(Threads::PrimitiveThread::getCurrentThread().taskHandle() == this->taskHandle())