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

Partial fixes to ICM reading
Some checks failed
continuous-integration/drone/push Build is failing
Details

Browse Source 
TODO: Fix the actual conversions
This commit is contained in:
Erki 2021-11-04 03:02:10 +02:00
parent 92b18cae83
commit 0762d5c9cd
1 changed files with 47 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
Peripherals/Inc/peripherals_imu_icm.hpp
View File

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