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

temporary working commit

Browse Source
This commit is contained in:
erki 2023-09-06 18:35:34 +03:00
parent a488ba66f3
commit 56c16b8984
1 changed files with 66 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
Peripherals/Inc/peripherals_imu_icm.hpp
View File

@ -115,11 +115,19 @@ public:
void calibrate(const std::uint32_t samples) override 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};
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++) for (std::uint32_t i = 0; i < samples; i++)
{ {
@ -138,11 +146,62 @@ public:
for (std::uint32_t i = 0; i < 3; i++) 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;
// -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
// 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());
setAccelerometerScale(prev_accel_scale);
setGyroscopeScale(prev_gyro_scale);
// TODO: remove manual bias calculations. These are now done in hardware.
} }
void setGyroscopeScale(const GyroScale scale) void setGyroscopeScale(const GyroScale scale)