Peripherals/Inc/peripherals_imu_icm.hpp

@@ -43,11 +43,7 @@ public:
   registers_handle registers;
 
   ImuIcm() = delete;
-  ImuIcm(const registers_handle& registers) : registers(registers)
-  {
-    bias_accel_.fill(0);
-    bias_gyro_.fill(0);
-  }
+  ImuIcm(const registers_handle& registers) : registers(registers) {}
 
   void setup() override
   {
@@ -72,17 +68,9 @@ public:
                                   // rate = 1KHz, temp filter = 42
     hal::delay(10);
 
-    const std::uint8_t new_gyro_conf =  (std::uint32_t(scale_gyro_) << 3);
-    registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
-        new_gyro_conf);
+    setGyroscopeScale(scale_gyro_);
 
-    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_);
+    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,
@@ -108,24 +96,11 @@ public:
     hal::delay(10);
   }
 
-  std::array<std::int16_t, 3>& gyroBias()
-  {
-    return bias_gyro_;
-  }
-
-  std::array<std::int16_t, 3>& accelBias()
-  {
-    return bias_accel_;
-  }
-
   void calibrate(const std::uint32_t samples) override
   {
     std::array<std::int32_t, 3> avg_gyro;
     std::array<std::int32_t, 3> avg_accel;
 
-    avg_gyro.fill(0);
-    avg_accel.fill(0);
-
     for (std::uint32_t i = 0; i < samples; i++)
     {
       std::array<std::int16_t, 3> raw;
@@ -143,8 +118,11 @@ public:
 
     for (std::uint32_t i = 0; i < 3; i++)
     {
-      bias_gyro_[i] = avg_gyro[i] / std::int32_t(samples);
-      bias_accel_[i] = avg_accel[i] / std::int32_t(samples);
+      const std::int32_t max = std::numeric_limits<std::int16_t>::max();
+      const std::int32_t min = std::numeric_limits<std::int16_t>::min();
+
+      bias_gyro_[i] = clamp(avg_gyro[i], max, min);
+      bias_accel_[i] = clamp(avg_accel[i], max, min);
     }
 
     bias_accel_[2] -= accelerometerReadingToRaw(1);
@@ -180,9 +158,11 @@ public:
     registers.readRegisterMultibyte(
             Registers_::GYRO_XOUT_H | Registers_::READ_MASK, data, 6);
 
-    output[0] = gyroRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
-    output[1] = gyroRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
-    output[2] = gyroRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
+    for (std::uint32_t i = 0; i < 3; i++)
+    {
+      const std::int16_t bit = byteToTypeBE<std::int16_t, 2>(&data[i * 2]);
+      output[i] = gyroRawToReading(bit);
+    }
   }
 
   void readGyroRaw(std::int16_t* output) override
@@ -191,9 +171,10 @@ public:
     registers.readRegisterMultibyte(
             Registers_::GYRO_XOUT_H | Registers_::READ_MASK, data, 6);
 
-    output[0] = std::int16_t((data[0] << 8) | data[1]);
-    output[1] = std::int16_t((data[2] << 8) | data[3]);
-    output[2] = std::int16_t((data[4] << 8) | data[5]);
+    for (std::uint32_t i = 0; i < 3; i++)
+    {
+      output[i] = byteToTypeBE<std::int16_t, 2>(&data[i * 2]);
+    }
   }
 
   void readAccelerometer(float* output) override
@@ -202,9 +183,11 @@ public:
     registers.readRegisterMultibyte(
             Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);
 
-    output[0] = accelerometerRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
-    output[1] = accelerometerRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
-    output[2] = accelerometerRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
+    for (std::uint32_t i = 0; i < 3; i++)
+    {
+      const std::int16_t bit = byteToTypeBE<std::int16_t, 2>(&data[i * 2]);
+      output[i] = accelerometerRawToReading(bit);
+    }
   }
 
   void readAccelerometerRaw(std::int16_t* output) override
@@ -213,10 +196,10 @@ public:
     registers.readRegisterMultibyte(
             Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);
 
-
-    output[0] = std::int16_t((data[0] << 8) | data[1]);
-    output[1] = std::int16_t((data[2] << 8) | data[3]);
-    output[2] = std::int16_t((data[4] << 8) | data[5]);
+    for (std::uint32_t i = 0; i < 3; i++)
+    {
+      output[i] = byteToTypeBE<std::int16_t, 2>(&data[i * 2]);
+    }
   }
 
   std::int16_t accelerometerReadingToRaw(const float& fs) const
@@ -229,14 +212,14 @@ public:
     return fs / gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
   }
 
-  float accelerometerRawToReading(const std::int16_t bit, const std::size_t axis) const
+  float accelerometerRawToReading(const std::int16_t bit) const
   {
-    return (float(bit - bias_accel_[axis]) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f;
+    return (float(bit) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f;
   }
 
-  float gyroRawToReading(const std::int16_t bit, const std::size_t axis) const
+  float gyroRawToReading(const std::int16_t bit) const
   {
-    return float(bit - bias_gyro_[axis]) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
+    return float(bit) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
   }
 
   std::uint8_t readProductId()

Threads/Inc/threads_signal.hpp

@@ -8,10 +8,7 @@
 #ifndef THREADS_INC_THREADS_SIGNAL_HPP_
 #define THREADS_INC_THREADS_SIGNAL_HPP_
 
-#include <type_traits>
-
 #include <freertos_os2.h>
-#include <threads_primitivethread.hpp>
 
 namespace Threads
 {