erki/skullc-peripherals
1
0
Fork 0
Code Issues Pull Requests Actions Projects Releases Wiki Activity

Temporary IMU fixes
Some checks failed
continuous-integration/drone/push Build is failing
Details

Browse Source
This commit is contained in:
Erki 2022-09-06 22:27:41 +03:00
parent 59c2fc6f7f
commit 992c55b785
1 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
Peripherals/Inc/peripherals_imu_icm.hpp
View File

@ -72,18 +72,14 @@ public:
// rate = 1KHz, temp filter = 42 // rate = 1KHz, temp filter = 42
hal::delay(10); 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, registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
new_gyro_conf); 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, registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
new_accel_config); new_accel_config);
// setGyroscopeScale(scale_gyro_);
//
// setAccelerometerScale(scale_accel_);
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK, registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
0x03); 0x03);
@ -129,16 +125,16 @@ public:
for (std::uint32_t i = 0; i < samples; i++) for (std::uint32_t i = 0; i < samples; i++)
{ {
std::array<std::int16_t, 3> raw; 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()); 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()); 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++) for (std::uint32_t i = 0; i < 3; i++)
@ -248,9 +244,6 @@ private:
GyroScale scale_gyro_ = GyroScale::DPS_2000; GyroScale scale_gyro_ = GyroScale::DPS_2000;
AccelerometerScale scale_accel_ = AccelerometerScale::G16; 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] = { static constexpr float accel_fs_to_bit_constants_[4] = {
(2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f), (2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f),
(16.0f / 32768.0f)}; (16.0f / 32768.0f)};
@ -259,6 +252,9 @@ private:
(250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f), (250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
(2000.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_ struct Registers_
{ {
static constexpr std::uint32_t ICM20689_ID = 0x98; static constexpr std::uint32_t ICM20689_ID = 0x98;