MPU9250 Accelerometer offset registers fixed

[cutlas62]

Hi Kris,

I was using your MPU9250BasicAHRS_t3.ino sketch when I saw your comment about the accelerometer offset registers not working properly. (Line 908) I was able to fix this problem and both Gyroscope and Accelerometer are correctly calibrated:

int16_t accel_bias_reg[3] = { 0, 0, 0 }; // A place to hold the factory accelerometer trim biases
    int16_t mask_bit[3] = { 1, 1, 1 };// Define array to hold mask bit for each accelerometer bias axis

    I2C_Read_N_Bytes(MPU9250_ADDR, MPU9250_XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
    accel_bias_reg[0] = ((int16_t) data[0] << 8) | data[1];
    I2C_Read_N_Bytes(MPU9250_ADDR, MPU9250_YA_OFFSET_H, 2, &data[0]);
    accel_bias_reg[1] = ((int16_t) data[0] << 8) | data[1];
    I2C_Read_N_Bytes(MPU9250_ADDR, MPU9250_ZA_OFFSET_H, 2, &data[0]);
    accel_bias_reg[2] = ((int16_t) data[0] << 8) | data[1];

    for (int i = 0; i < 3; i++) {
        if (accel_bias_reg[i] % 2) {
            mask_bit[i] = 0;
        }
        accel_bias_reg[i] -= accel_bias[i] >> 3; // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
        if (mask_bit[i]) {
            accel_bias_reg[i] = accel_bias_reg[i] & ~mask_bit[i]; // Preserve temperature compensation bit
        } else {
            accel_bias_reg[i] = accel_bias_reg[i] | 0x0001; // Preserve temperature compensation bit
        }
    }

    data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
    data[1] = (accel_bias_reg[0]) & 0xFF;
    data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
    data[3] = (accel_bias_reg[1]) & 0xFF;
    data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
    data[5] = (accel_bias_reg[2]) & 0xFF;

    // Push accelerometer biases to hardware registers

    I2C_Write_Byte(MPU9250_ADDR, MPU9250_XA_OFFSET_H, data[0]);
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_XA_OFFSET_L, data[1]);
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_YA_OFFSET_H, data[2]);
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_YA_OFFSET_L, data[3]);
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_ZA_OFFSET_H, data[4]);
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_ZA_OFFSET_L, data[5]);

Please check it and give me your opinion. It's working on my ESP32.

Thank you for your awesome work!

[kbloxsom]

Hey cutlas62, that's good news. I am currently working on a leveling project also with a ESP NodeMCU. I have been trying to figure out how to implement the accel/gyro offsets so that it boots up to a known level position each time by saving the offsets from a "good" level and restoring them on boot. Do you have the .ino file posted that works with the above code? What modifications did you have to make from Kris's code to make it work in an ESP?

[cutlas62]

Hi kbloxsom,

I don't have a .ino file since I'm writing all my code in C for the ESP32 without the Arduino IDE. You can find the latest version of my code here (Note that it's not by far finished). The only difference from Kris' calibrateMPU9250 is how to update the accelerometer offset (the for loop I added above).

[kbloxsom]

Thanks Carlos! I’ll have a look at your code. Thanks again for sharing.

Regards, Kim

On Sep 10, 2018, at 5:20 PM, Carlos notifications@github.com wrote:

Hi kbloxsom,

I don't have a .ino file since I'm writing all my code in C for the ESP32 without the Arduino IDE. You can find the latest version of my code here (Note that it's not by far finished). The only difference from Kris' calibrateMPU9250 is how to update the accelerometer offset (the for loop I added above).

— You are receiving this because you commented. Reply to this email directly, view it on GitHub, or mute the thread.