Skip to content

Fix accelerometer offset calculation in calibrateMPU9250 method #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Fix accelerometer offset calculation in calibrateMPU9250 method #6

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 7 additions & 30 deletions src/MPU9250.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -384,15 +384,15 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias)
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
// Divide by total readings to get average bias
accel_bias[0] /= (int32_t) packet_count;
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;

// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
// Correct for gravity
if (accel_bias[2] > 0L)
{
accel_bias[2] -= (int32_t) accelsensitivity;
Expand Down Expand Up @@ -445,47 +445,24 @@ void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias)
readBytes(_I2Caddr, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);

// Define mask for temperature compensation bit 0 of lower byte of
// accelerometer bias registers
uint32_t mask = 1uL;
// Define array to hold mask bit for each accelerometer bias axis
uint8_t mask_bit[3] = {0, 0, 0};

for (ii = 0; ii < 3; ii++)
{
// If temperature compensation bit is set, record that fact in mask_bit
if ((accel_bias_reg[ii] & mask))
{
mask_bit[ii] = 0x01;
}
}

// 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)
accel_bias_reg[0] -= (accel_bias[0]/8);
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);
// Measured bias is left-shifted to preserve LSB of register
// Accelerometer offset is upper 15 bits
accel_bias_reg[0] -= (accel_bias[0]/8) << 1;
accel_bias_reg[1] -= (accel_bias[1]/8) << 1;
accel_bias_reg[2] -= (accel_bias[2]/8) << 1;

data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
// preserve temperature compensation bit when writing back to accelerometer
// bias registers
data[1] = data[1] | mask_bit[0];
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
// Preserve temperature compensation bit when writing back to accelerometer
// bias registers
data[3] = data[3] | mask_bit[1];
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
// Preserve temperature compensation bit when writing back to accelerometer
// bias registers
data[5] = data[5] | mask_bit[2];

// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
writeByte(_I2Caddr, XA_OFFSET_H, data[0]);
writeByte(_I2Caddr, XA_OFFSET_L, data[1]);
Expand Down