Temporary working commit

Peripherals/Inc/peripherals_imu_icm.hpp

@@ -13,6 +13,8 @@
 
 #include "peripherals_imu.hpp"
 
+#include "utility_logging.hpp"
+
 namespace Peripherals
 {
 
@@ -43,10 +45,7 @@ public:
 
   ImuIcm() = delete;
   ImuIcm(const registers_handle& registers) : registers(registers)
-  {
+  { }
-    bias_accel_.fill(0);
-    bias_gyro_.fill(0);
-  }
 
   void setup() override
   {
@@ -71,13 +70,9 @@ public:
                                   // rate = 1KHz, temp filter = 42
     hal::delay(10);
 
-    const std::uint8_t new_gyro_conf = (std::uint32_t(scale_gyro_) << 3);
+    setAccelerometerScale(scale_accel_);
-    registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
-                            new_gyro_conf);
 
-    const std::uint8_t new_accel_config = (std::uint32_t(scale_accel_) << 3);
+    setGyroscopeScale(scale_gyro_);
-    registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
-                            new_accel_config);
 
     // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
     registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
@@ -99,27 +94,25 @@ public:
                             0);
     hal::delay(10);
 
-    registers.writeRegister(Registers_::INT_ENABLE & Registers_::WRITE_MASK, 1);
+    registers.writeRegister(Registers_::INT_ENABLE & Registers_::WRITE_MASK, 0);
     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;
+	const auto prev_accel_scale = scale_accel_;
-    std::array<std::int32_t, 3> avg_accel;
+	const auto prev_gyro_scale = scale_gyro_;
 
-    avg_gyro.fill(0);
+	setAccelerometerScale(AccelerometerScale::G2);
-    avg_accel.fill(0);
+	setGyroscopeScale(GyroScale::DPS_250);
+
+    std::array<std::int32_t, 3> avg_gyro = {0, 0, 0};
+    std::array<std::int32_t, 3> avg_accel = {0, 0, 0};
+    std::array<std::int32_t, 3> bias_gyro = {0, 0, 0};
+    std::array<std::int32_t, 3> bias_accel = {0, 0, 0};
+
+    [[maybe_unused]] const std::uint16_t gyro_sensitivity = 131;   // = 131 LSB/degrees/sec
+    const std::uint16_t accel_sensitivity = 16384; // = 16384 LSB/g
 
     for (std::uint32_t i = 0; i < samples; i++)
     {
@@ -138,11 +131,65 @@ public:
 
     for (std::uint32_t i = 0; i < 3; i++)
     {
-      bias_gyro_[i] = avg_gyro[i] / std::int32_t(samples);
+      bias_gyro[i] = avg_gyro[i] / std::int32_t(samples);
-      bias_accel_[i] = avg_accel[i] / std::int32_t(samples);
+      bias_accel[i] = avg_accel[i] / std::int32_t(samples);
     }
 
-    bias_accel_[2] -= accelerometerReadingToRaw(1);
+    if (bias_accel[2] > 0)
+    	bias_accel[2] -= accel_sensitivity;
+    else
+    	bias_accel[2] += accel_sensitivity;
+
+    std::array<std::uint8_t, 6> push_data;
+
+    // Construct the gyro biases for push to the hardware gyro bias registers,
+    // which are reset to zero upon device startup.
+    // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
+    // format.
+    push_data[0] = (-bias_gyro[0]/4 >> 8) & 0xFF;
+    push_data[1] = (-bias_gyro[0]/4) & 0xFF;
+    push_data[2] = (-bias_gyro[1]/4 >> 8) & 0xFF;
+    push_data[3] = (-bias_gyro[1]/4) & 0xFF;
+    push_data[4] = (-bias_gyro[2]/4 >> 8) & 0xFF;
+    push_data[5] = (-bias_gyro[2]/4) & 0xFF;
+
+    registers.writeRegisterMultibyte(Registers_::XG_OFFSET_H & Registers_::WRITE_MASK,
+    		push_data.data(), push_data.size());
+
+    std::array<std::int32_t, 3> factory_values = {0, 0, 0};
+//    registers.readRegisterMultibyte(Registers_::XA_OFFSET_H | Registers_::READ_MASK,
+//    		push_data.data(), push_data.size());
+//    factory_values[0] = std::int32_t((std::int16_t(push_data[0]) << 8) | push_data[1]);
+//    factory_values[1] = std::int32_t((std::int16_t(push_data[2]) << 8) | push_data[3]);
+//    factory_values[2] = std::int32_t((std::int16_t(push_data[4]) << 8) | push_data[5]);
+
+    // Construct total accelerometer bias, including calculated average
+    // accelerometer bias from above
+    // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
+    // (16 g full scale)
+    bias_accel[0] = factory_values[0] - (bias_accel[0] / 8);
+    bias_accel[1] = factory_values[1] - (bias_accel[1] / 8);
+    bias_accel[1] = factory_values[1] - (bias_accel[1] / 8);
+
+    push_data[0] = ((bias_accel[0] >> 8) & 0xFF);
+    push_data[1] = ((bias_accel[0]) & 0xFF) | (factory_values[0] & 0x1);
+    push_data[2] = ((bias_accel[1] >> 8) & 0xFF);
+    push_data[3] = ((bias_accel[1]) & 0xFF) | (factory_values[1] & 0x1);
+    push_data[4] = ((bias_accel[2] >> 8) & 0xFF);
+    push_data[5] = ((bias_accel[2]) & 0xFF) | (factory_values[2] & 0x1);
+
+    registers.writeRegisterMultibyte(Registers_::XA_OFFSET_H & Registers_::WRITE_MASK,
+			push_data.data(), push_data.size());
+
+   // __asm__("BKPT");
+
+	setAccelerometerScale(prev_accel_scale);
+	setGyroscopeScale(prev_gyro_scale);
+
+	SKULLC_LOG_DEBUG("ICM: Accelerometer bias: %d, %d, %d; accel total: %d, %d, %d.",
+			bias_accel[0], bias_accel[1], bias_accel[2],
+			avg_accel[0], avg_accel[1], avg_accel[2]);
+	// TODO: remove manual bias calculations. These are now done in hardware.
   }
 
   void setGyroscopeScale(const GyroScale scale)
@@ -226,12 +273,12 @@ public:
 
   float accelerometerRawToReading(const std::int16_t bit, const std::size_t axis) 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
   {
-    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()
@@ -251,9 +298,6 @@ 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;