bolderflight / invensense-imu

Arduino and CMake library for communicating with the InvenSense MPU-6500, MPU-9250 and MPU-9255 nine-axis IMUs.
MIT License
497 stars 210 forks source link

calibrateAccel - how does it work? #80

Closed sharkyenergy closed 3 years ago

sharkyenergy commented 3 years ago

Hello!

First off thank you for the amazing library...

Could you please explain how to calibrate the acceleration? I read teh documentation, but cannot understand how to procede.. specifically, if I do it 6 times, once per every orientation, I will get 6 different value pairs for each direction, what do I do with those then? could you please explain how I shoudl procede to calibrate it?

Thank you!

GiovanniCmpaner commented 3 years ago

You have to call calibrateAccel() in a loop and move the hardware physically before each call. Think of the hardware as a plain cube with 6 faces, each position would be one face to the touching the ground / table.

Here is an example:

#include <Arduino.h>
#include <Wire.h>
#include <MPU9250.h>

MPU9250 mpu(Wire, 0x68);

void setup()
{
    Serial.begin(115200);
    Serial.println("Doing calibration");

    mpu.begin();

    for(int n = 0; n < 6; n++)
    {
        Serial.print("Move the car into position ");
        Serial.print(n);
        Serial.println();

        delay(7000);

        mpu.calibrateAccel();
    }

    Serial.println("Calibration done");

    Serial.print("bias x = ");
    Serial.print(mpu.getAccelBiasX_mss(), 5);
    Serial.println();

    Serial.print("bias y = ");
    Serial.print(mpu.getAccelBiasY_mss(), 5);
    Serial.println();

    Serial.print("bias z = ");
    Serial.print(mpu.getAccelBiasZ_mss(), 5);
    Serial.println();

    Serial.print("factor x = ");
    Serial.print(mpu.getAccelScaleFactorX(), 5);
    Serial.println();

    Serial.print("factor y = ");
    Serial.print(mpu.getAccelScaleFactorY(), 5);
    Serial.println();

    Serial.print("factor z = ");
    Serial.print(mpu.getAccelScaleFactorZ(), 5);
    Serial.println();
}

void loop()
{

}

Then you use those numbers in the real code:

#include <Arduino.h>
#include <Wire.h>
#include <MPU9250.h>

MPU9250 mpu(Wire, 0x68);

const float accelBias[3] = { 0.11111, 0.22222, 0.33333 }; // Obtained through calibration scketch
const float accelFactor[3] = { 0.11111, 0.22222, 0.33333 }; // Obtained through calibration scketch

void setup()
{
    Serial.begin(115200);
    Serial.println("Doing calibrated reading");

    mpu.begin();

    mpu.setAccelCalX(accelBias[0], accelFactor[0]);
    mpu.setAccelCalY(accelBias[1], accelFactor[1]);
    mpu.setAccelCalZ(accelBias[2], accelFactor[2]);
}

void loop()
{
    mpu.readSensor();

    Serial.print("accel x = ");
    Serial.print(mpu.getAccelX_mss(), 5);
    Serial.println();

    Serial.print("accel y = ");
    Serial.print(mpu.getAccelY_mss(), 5);
    Serial.println();

    Serial.print("accel z = ");
    Serial.print(mpu.getAccelZ_mss(), 5);
    Serial.println();

    Serial.println();
    delay(100);
}
flybrianfly commented 3 years ago

Calibration functions are now moved external to this library and no longer included.