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

Temporary IMU fixes for ICM IMUs

Browse Source 
# Conflicts:
#	Peripherals/Inc/peripherals_imu_icm.hpp
This commit is contained in:
Erki 2022-09-06 22:27:41 +03:00
parent 458fd9e7f2
commit 6e22b02e92
1 changed files with 9 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Peripherals/Inc/peripherals_imu_icm.hpp
View File

@ -80,10 +80,6 @@ public:
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;