teensy4.0 teensy4.0_lock GY521 calibrage

jean · Jun 8, 2024

Hello everyone,
I'm trying to run the basic GY521 calibration script on the Teensy 4.0 LOCK, but every time I get the error “MPU6050 connection failed”.

The sensor works well because when I run my final script, it reacts well even if the zeros are not good.

The same calibration program on the standard Teensy 4.0 with the same GY521 gives me a “connect with success”.

I don't understand why the Teensy 4.0 LOCK doesn't work.
Do you have any idea?

Below is the code used .

CPU Speed 450Mhz

C++:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps612.h"

#include "Wire.h"

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone=8;  //5   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;1

MPU6050 accelgyro(0x68); // <-- use for AD0 high

int16_t ax, ay, az,gx, gy, gz;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  //Wire.setClock(50000);
  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

  // initialize serial communication
  Serial.begin(115200);

  // initialize device
  accelgyro.initialize();

  // wait for ready
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available()){
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }
  while (Serial.available() && Serial.read()); // empty buffer again

  // start message
  Serial.println("\nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
  // verify connection
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // reset offsets
  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state==0){
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax);
    Serial.print("\t");
    Serial.print(mean_ay);
    Serial.print("\t");
    Serial.print(mean_az);
    Serial.print("\t");
    Serial.print(mean_gx);
    Serial.print("\t");
    Serial.print(mean_gy);
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset);
    Serial.print("\t");
    Serial.print(ay_offset);
    Serial.print("\t");
    Serial.print(az_offset);
    Serial.print("\t");
    Serial.print(gx_offset);
    Serial.print("\t");
    Serial.print(gy_offset);
    Serial.print("\t");
    Serial.println(gz_offset);
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

kd5rxt-mark · Jun 9, 2024

Have you tried the I2C scanner on the T4.0 LOCK to see if your MPU6050 is properly detected ??

Mark J Culross
KD5RXT

mjs513 · Jun 9, 2024

The lib you are using is based off of Jeff Rowberg's I2Cdev lib. Not sure if it was ever updated to work with the Teensy. Running the scanner may give a hint.

jean · Jun 10, 2024

@marque kd5rxt
@mjs513

H

Yes, the scanner works well and I have the address displayed in the 0x68 console.

No problem on Teensy 4.0
Only on Teensy 4.0 LOCK ( but only the calibration part doesn't work)

I get around the problem for the moment by running calibration on the Teensy 4.0 and then modifying my code to load it into the Teensy 4.0 LOCK.

kd5rxt-mark · Jun 11, 2024

@jean:

Just to be sure that I'm correctly interpreting what you are saying, your T4.0 LOCK, when running the I2C scanner sketch, does detect the MPU6050 successfully ??

I entered the sketch that you provided in post #1 (I did change the Serial.println("..."); line to Serial.print(ready); Serial.println(" of 6 offsets calibrated..."); so that each update gives some indication of how far along the offset calibration has progressed).

When running this modified sketch on a lockable T4.0 (which has not yet been locked), I get the following output in the Serial Monitor:

Code:

Send any character to start sketch.


MPU6050 Calibration Sketch

Your MPU6050 should be placed in horizontal position, with package letters facing up.
Don't touch it until you see a finish message.

MPU6050 connection successful

Reading sensors for first time...

Calculating offsets...
0 of 6 offsets calibrated...

This appears to show that my lockable T4.0 is successfully detecting the MPU6050. However, with my test hardware setup, the calibration never shows any progress (the 0 of 6 offsets calibrated... status line repeats indefinitely).

I will investigate removing the dependency on the I2Cdev (as @mjs513 hinted at) & see if that is somehow involved in keeping things from working as expected.

Mark J Culross
KD5RXT

defragster · Jun 11, 2024

jean said:
Only on Teensy 4.0 LOCK ( but only the calibration part doesn't work)

Odd for it to be LOCK'able related. Would be the first report of something acting differently AFAIK?

mjs513 · Jun 12, 2024

kd5rxt-mark said:
I will investigate removing the dependency on the I2Cdev (as @mjs513 hinted at) & see if that is somehow involved in keeping things from working as expected.

before doing that try changing the commented out i2c clock speed in the sketch:

Code:

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  //Wire.setClock(50000);
  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

to this

Code:

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  Wire.setClock(400000);
  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
  //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

jean · Jun 12, 2024

@kd5rxt-mark

Just to be sure that I'm correctly interpreting what you are saying, your T4.0 LOCK, when running the I2C scanner sketch, does detect the MPU6050 successfully ??=> YES
@defragster
before doing that try changing the commented out i2c clock speed in the sketch:
I don't know if there is a link.

@mjs513

I replaced with

C++:

[CODE=cpp]  Wire.begin();

  //Wire.setClock(50000);

  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE

  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

[/CODE]

by

Code:

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

and

C++:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps612.h"

#include "Wire.h"

by

C++:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps612.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

and they all work now.
I don't have your skills but I'd like to understand why?

kd5rxt-mark · Jun 12, 2024

@jean:

Can you please post your final sketch . . . I'd like to see what my hardware test setup does with your mods.

Thanks,

Mark J Culross
KD5RXT

jean · Jun 12, 2024

@kd5rxt-mark
yes, this script works for me

C++:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps612.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone=8;  //5   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;1

//MPU6050 accelgyro(0x68); // <-- use for AD0 high
MPU6050 accelgyro;
int16_t ax, ay, az,gx, gy, gz;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  //Wire.begin();
  //Wire.setClock(50000);
  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
  //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

  // initialize serial communication
  Serial.begin(38400);

   // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // wait for ready
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available()){
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }
  while (Serial.available() && Serial.read()); // empty buffer again

  // start message
  Serial.println("\nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
  // verify connection
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // reset offsets
  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state==0){
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax);
    Serial.print("\t");
    Serial.print(mean_ay);
    Serial.print("\t");
    Serial.print(mean_az);
    Serial.print("\t");
    Serial.print(mean_gx);
    Serial.print("\t");
    Serial.print(mean_gy);
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset);
    Serial.print("\t");
    Serial.print(ay_offset);
    Serial.print("\t");
    Serial.print(az_offset);
    Serial.print("\t");
    Serial.print(gx_offset);
    Serial.print("\t");
    Serial.print(gy_offset);
    Serial.print("\t");
    Serial.println(gz_offset);
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

mjs513 · Jun 12, 2024

jean said:
and they all work now.
I don't have your skills but I'd like to understand why?

Not really a matter of skill. Its from experience working with the I2CDev lib. The lib actually supports a few different methods for I2C, one was standard wire, another was a fast wire type and a couple of others from what I remember. Its been a while.

By doing your setup:

Code:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps612.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

You are telling the lib not to use Invensense MotionApps(this is DMP if I remember) and to use standard Wire by specifying:

Code:

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

Now why one was working with the non-lock T4 and not with T4-Lock - no clue on that one unless I dig a lot deeper.

jean · Jun 12, 2024

@mjs513

this may be due to the poor quality GY521.

kd5rxt-mark · Jun 12, 2024

My particular <HiLeto MPU6050> did not seem to be able to get the calibration of all six of the values to converge at the same time, so I modified the test program to allow the calibration status of each to be independently tracked for completion.

Here's the test sketch that I ended up with:

Code:

// I2Cdev and MPU6050 must be installed as libraries
//#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps612.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize = 1000;   //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone = 8;   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone = 1;   //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;1

//MPU6050 accelgyro(0x68); // <-- use for AD0 high
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;

boolean ax_cal_done, ay_cal_done, az_cal_done, gx_cal_done, gy_cal_done, gz_cal_done = false;

int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
   // join I2C bus (I2Cdev library doesn't do this automatically)
   //Wire.begin();
   //Wire.setClock(50000);
   // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
   //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
   // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
   Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
   Fastwire::setup(400, true);
#endif

   // initialize serial communication
   Serial.begin(38400);

   // initialize device
   Serial.println("Initializing I2C devices...");
   accelgyro.initialize();

   // verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

   //   // wait for ready
   //   while (Serial.available() && Serial.read()); // empty buffer
   //   while (!Serial.available()) {
   //      Serial.println(F("Send any character to start sketch.\n"));
   //      delay(1500);
   //   }
   //   while (Serial.available() && Serial.read()); // empty buffer again

   // start message
   Serial.println("\nMPU6050 Calibration Sketch");
   delay(2000);
   Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
   delay(3000);
   // verify connection
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
   delay(1000);
   // reset offsets
   accelgyro.setXAccelOffset(0);
   accelgyro.setYAccelOffset(0);
   accelgyro.setZAccelOffset(0);
   accelgyro.setXGyroOffset(0);
   accelgyro.setYGyroOffset(0);
   accelgyro.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
   if (state == 0) {
      Serial.println("\nReading sensors for first time...");
      meansensors();
      state++;
      delay(1000);
   }

   if (state == 1) {
      Serial.println("\nCalculating offsets...");
      calibration();
      state++;
      delay(1000);
   }

   if (state == 2) {
      meansensors();
      Serial.println("\nFINISHED!");
      Serial.print("\nSensor readings with offsets:\t");
      Serial.print(mean_ax);
      Serial.print("\t");
      Serial.print(mean_ay);
      Serial.print("\t");
      Serial.print(mean_az);
      Serial.print("\t");
      Serial.print(mean_gx);
      Serial.print("\t");
      Serial.print(mean_gy);
      Serial.print("\t");
      Serial.println(mean_gz);
      Serial.print("Your offsets:\t");
      Serial.print(ax_offset);
      Serial.print("\t");
      Serial.print(ay_offset);
      Serial.print("\t");
      Serial.print(az_offset);
      Serial.print("\t");
      Serial.print(gx_offset);
      Serial.print("\t");
      Serial.print(gy_offset);
      Serial.print("\t");
      Serial.println(gz_offset);
      Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
      Serial.println("Check that your sensor readings are close to 0 0 32767 0 0 0");
      Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
      while (1);
   }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors() {
   long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;

   //First 100 measures are discarded
   while (i++ < 100)
   {
      // read raw accel/gyro measurements from device
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
   }

   i = 0;
   while (i++ < buffersize)
   {
      // read raw accel/gyro measurements from device
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

      buff_ax = buff_ax + ax;
      buff_ay = buff_ay + ay;
      buff_az = buff_az + az;
      buff_gx = buff_gx + gx;
      buff_gy = buff_gy + gy;
      buff_gz = buff_gz + gz;

      //      delay(2);
   }

   mean_ax = buff_ax / buffersize;
   mean_ay = buff_ay / buffersize;
   mean_az = buff_az / buffersize;
   mean_gx = buff_gx / buffersize;
   mean_gy = buff_gy / buffersize;
   mean_gz = buff_gz / buffersize;
}

void calibration() {
   ax_offset = -mean_ax / 8;
   ay_offset = -mean_ay / 8;
   az_offset = (32767 - mean_az) / 8;

   gx_offset = -mean_gx / 4;
   gy_offset = -mean_gy / 4;
   gz_offset = -mean_gz / 4;

   while (1) {
      int ready = 0;

      accelgyro.setXAccelOffset(ax_offset);
      accelgyro.setYAccelOffset(ay_offset);
      accelgyro.setZAccelOffset(az_offset);

      accelgyro.setXGyroOffset(gx_offset);
      accelgyro.setYGyroOffset(gy_offset);
      accelgyro.setZGyroOffset(gz_offset);

      meansensors();

      Serial.println("");
      Serial.println("");

      if ((abs(mean_ax) <= acel_deadzone) || ax_cal_done)
      {
         ax_cal_done = true;

         ready++;

         Serial.println("mean_ax calibrated");
      } else {
         ax_offset = ax_offset - mean_ax / acel_deadzone;

         Serial.print("mean_ax (");
         Serial.print(mean_ax);
         Serial.print(", target = ");
         Serial.print(acel_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("ax_offset = ");
         Serial.print(ax_offset);
         Serial.println("...");
      }

      if ((abs(mean_ay) <= acel_deadzone) || ay_cal_done)
      {
         ay_cal_done = true;

         ready++;

         Serial.println("mean_ay calibrated");
      } else {
         ay_offset = ay_offset - mean_ay / acel_deadzone;

         Serial.print("mean_ay (");
         Serial.print(mean_ay);
         Serial.print(", target = ");
         Serial.print(acel_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("ay_offset = ");
         Serial.print(ay_offset);
         Serial.println("...");
      }

      if ((abs(32767 - mean_az) <= acel_deadzone) || az_cal_done)
      {
         az_cal_done = true;

         ready++;

         Serial.println("mean_az calibrated");
      } else {
         az_offset = az_offset + (32767 - mean_az) / acel_deadzone;

         Serial.print("mean_az (");
         //         Serial.print(16384 - mean_az);
         Serial.print(32767 - mean_az);
         Serial.print(", target = ");
         Serial.print(acel_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("az_offset = ");
         Serial.print(az_offset);
         Serial.println("...");
      }

      if ((abs(mean_gx) <= giro_deadzone) || gx_cal_done)
      {
         gx_cal_done = true;

         ready++;

         Serial.println("mean_gx calibrated");
      } else {
         gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);

         Serial.print("mean_gx (");
         Serial.print(mean_gx);
         Serial.print(", target = ");
         Serial.print(giro_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("gx_offset = ");
         Serial.print(gx_offset);
         Serial.println(")...");
      }

      if ((abs(mean_gy) <= giro_deadzone) || gy_cal_done)
      {
         gy_cal_done = true;

         ready++;

         Serial.println("mean_gy calibrated");
      } else {
         gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);

         Serial.print("mean_gy (");
         Serial.print(mean_gy);
         Serial.print(", target = ");
         Serial.print(giro_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("gy_offset = ");
         Serial.print(gy_offset);
         Serial.println(")...");
      }

      if ((abs(mean_gz) <= giro_deadzone) || gz_cal_done)
      {
         gz_cal_done = true;

         ready++;

         Serial.println("mean_gz calibrated");
      } else {
         gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);

         Serial.print("mean_gz (");
         Serial.print(mean_gz);
         Serial.print(", target = ");
         Serial.print(giro_deadzone);
         Serial.print(") not calibrated: ");
         Serial.print("gz_offset = ");
         Serial.print(gz_offset);
         Serial.println(")...");
      }

      Serial.print(ready);
      Serial.println(" of 6 offsets calibrated...");

      if (ready == 6) break;
   }
}

Mark J Culross
KD5RXT

teensy4.0 teensy4.0_lock GY521 calibrage

jean

Well-known member

kd5rxt-mark

Well-known member

mjs513

Senior Member+

jean

Well-known member

kd5rxt-mark

Well-known member

defragster

Senior Member+

mjs513

Senior Member+

jean

Well-known member

@kd5rxt-mark

kd5rxt-mark

Well-known member

jean

Well-known member

mjs513

Senior Member+

jean

Well-known member

kd5rxt-mark

Well-known member

teensy4.0 teensy4.0_lock GY521 calibrage

Well-known member

Well-known member

Senior Member+

Well-known member

Well-known member

Senior Member+

Senior Member+

Well-known member

@kd5rxt-mark​

Well-known member

Well-known member

Senior Member+

Well-known member

Well-known member

@kd5rxt-mark