From 481f1d2c8d75446a32c25c2db99112e240e3438c Mon Sep 17 00:00:00 2001 From: erki Date: Sat, 30 Sep 2023 19:01:57 +0300 Subject: [PATCH] Temporary working commit --- Peripherals/Inc/peripherals_imu_icm.hpp | 53 +++++++++---------------- 1 file changed, 19 insertions(+), 34 deletions(-) diff --git a/Peripherals/Inc/peripherals_imu_icm.hpp b/Peripherals/Inc/peripherals_imu_icm.hpp index 227c0d0..bff1e43 100644 --- a/Peripherals/Inc/peripherals_imu_icm.hpp +++ b/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); - registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK, - new_gyro_conf); + setAccelerometerScale(scale_accel_); - 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_); // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK, @@ -99,20 +94,10 @@ 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& gyroBias() - { - return bias_gyro_; - } - - std::array& accelBias() - { - return bias_accel_; - } - void calibrate(const std::uint32_t samples) override { const auto prev_accel_scale = scale_accel_; @@ -126,7 +111,7 @@ public: std::array bias_gyro = {0, 0, 0}; std::array bias_accel = {0, 0, 0}; - const std::uint16_t gyro_sensitivity = 131; // = 131 LSB/degrees/sec + [[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++) @@ -157,8 +142,6 @@ public: std::array push_data; - // -3888, -2972, 0033 - // 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 @@ -174,11 +157,11 @@ public: push_data.data(), push_data.size()); std::array 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]); +// 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 @@ -198,9 +181,14 @@ public: 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. } @@ -285,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() @@ -310,9 +298,6 @@ private: (250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f), (2000.0f / 32768.0f)}; - std::array bias_gyro_; - std::array bias_accel_; - struct Registers_ { static constexpr std::uint32_t ICM20689_ID = 0x98;