Can't get yaw/pitch/roll reading with a delay

[C-EkoEko]

Environment

IDE (Arduino IDE, PlatformIO, etc.) : Arduino IDE Board/MCU (Arduino Uno, ESP32, etc.): Arduino UNO, Teensy 3.6 Board Firmware Version (see board manager): MPU-9250 Library Version: 0.4.8

Problem Summary

When I add delay quaternions do not show accurate data.

Problem Details

Hello, I am trying to read a few values from MPU9250 however when I add a delay I cant get yaw, pitch and roll values. So I checked the quaternions and saw they dont get updated when I use no filter they get updated but values are not accurate. When I use MADGWICK they do not get updated. And when I use MAHONY they get updated but not accurate again. I also tested with a Teensy 3.6 but results are same.

Thank you in advance.

Reproducible & Minimal & Compilable Code

This is the code I used. The only different thing is filter type between outputs.

#include<MPU9250.h>
#include <math.h>
MPU9250 mpu;
MPU9250Setting setting;

float accel[3];
float gyro[3];
float magne[3];
float rpy[3];

void setup() {
    Serial.begin(115200);
    Wire.begin();
    smartDelay(2000);
    setMPU();
    if (!mpu.setup(0x68, setting,Wire)) {  // change to your own address
        while (1) {
            Serial.println("MPU connection failed. Please check your connection with connection_check example.");
            smartDelay(5000);
        }
    }
    mpu.setMagneticDeclination(5.93);
    mpu.selectFilter(QuatFilterSel::MADGWICK);
    mpu.setFilterIterations(10);
    mpu.verbose(1);
    mpu.selftest();
}

void loop() {
    if (mpu.update()) {
        getQuaternions();
        accel[0] = mpu.getAccX();
        accel[1] = mpu.getAccY();
        accel[2] = mpu.getAccZ();
        gyro [0] = mpu.getGyroX();
        gyro [1] = mpu.getGyroY();
        gyro [2] = mpu.getGyroZ();
        magne[0] = mpu.getMagX();
        magne[1] = mpu.getMagY();
        magne[2] = mpu.getMagZ();
        rpy  [0] = mpu.getYaw();
        rpy  [1] = mpu.getPitch();
        rpy  [2] = mpu.getRoll();
    }
    Serial.print("Accel X/Y/Z:  ");
    for(int i=0;i<3;i++){Serial.print(accel[i]);if(i<2){Serial.print(" , ");}}
    Serial.print("\nGyro X/Y/Z:  ");
    for(int i=0;i<3;i++){Serial.print(gyro[i]);if(i<2){Serial.print(" , ");}}
    Serial.print("\nYaw/Pitch/Roll:  ");
    for(int i=0;i<3;i++){Serial.print(rpy[i]);if(i<2){Serial.print(" , ");}}
    Serial.print("\n\n#####################\n\n");
    smartDelay(100);
}
void setMPU(){
    setting.accel_fs_sel = ACCEL_FS_SEL::A16G;
    setting.gyro_fs_sel = GYRO_FS_SEL::G2000DPS;
    setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
    setting.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_200HZ;
    setting.gyro_fchoice = 0x03;
    setting.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;
    setting.accel_fchoice = 0x01;
    setting.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_45HZ;
  return;
}
void getQuaternions(){
 Serial.print("Quaterion: ");
    Serial.print(mpu.getQuaternionX(), 2);
    Serial.print(", ");
    Serial.print(mpu.getQuaternionY(), 2);
    Serial.print(", ");
    Serial.print(mpu.getQuaternionZ(), 2);
    Serial.print(", ");
    Serial.print(mpu.getQuaternionW(), 2);
    smartDelay(100);
    Serial.println();
  return;
}
static void smartDelay(unsigned long ms) 
{                                        
  unsigned long beginning = millis();
  while (millis() - beginning < ms){}
  return;
}

Output (NONE):

x-axis self test: acceleration trim within : 0.0% of factory value
y-axis self test: acceleration trim within : -4.2% of factory value
z-axis self test: acceleration trim within : 4.1% of factory value
x-axis self test: gyration trim within : -1.9% of factory value
y-axis self test: gyration trim within : 1.0% of factory value
z-axis self test: gyration trim within : 0.6% of factory value
Quaterion X/Y/Z/W: 0.54, -0.32, 0.26, 0.73
Accel X/Y/Z:  0.43 , 0.06 , 7.08
Gyro X/Y/Z:  25.02 , 14.77 , -12.08
Yaw/Pitch/Roll:  8.89 , -48.95 , 72.07

#####################

Quaterion X/Y/Z/W: 0.58, -0.34, 0.28, 0.69
Accel X/Y/Z:  0.42 , 0.05 , 7.05
Gyro X/Y/Z:  24.60 , 12.27 , -11.72
Yaw/Pitch/Roll:  5.84 , -52.04 , 79.72

#####################

Quaterion X/Y/Z/W: 0.61, -0.35, 0.30, 0.65
Accel X/Y/Z:  0.44 , 0.03 , 7.10
Gyro X/Y/Z:  23.86 , 13.18 , -11.29
Yaw/Pitch/Roll:  1.52 , -54.76 , 88.35

#####################

Quaterion X/Y/Z/W: 0.63, -0.37, 0.31, 0.61
Accel X/Y/Z:  0.45 , 0.04 , 7.13
Gyro X/Y/Z:  24.41 , 12.15 , -11.23
Yaw/Pitch/Roll:  -3.38 , -57.04 , 97.67

Output (MADGWICK):

x-axis self test: acceleration trim within : -0.1% of factory value
y-axis self test: acceleration trim within : -4.1% of factory value
z-axis self test: acceleration trim within : 4.2% of factory value
x-axis self test: gyration trim within : -1.9% of factory value
y-axis self test: gyration trim within : 1.0% of factory value
z-axis self test: gyration trim within : 0.6% of factory value
Quaterion X/Y/Z/W: 0.00, 0.00, 0.00, 1.00
Accel X/Y/Z:  0.44 , 0.07 , 7.11
Gyro X/Y/Z:  25.57 , 13.24 , -11.90
Yaw/Pitch/Roll:  5.93 , 0.00 , 0.00

#####################

Quaterion X/Y/Z/W: 0.00, 0.00, 0.00, 1.00
Accel X/Y/Z:  0.44 , 0.04 , 7.11
Gyro X/Y/Z:  25.57 , 13.98 , -10.93
Yaw/Pitch/Roll:  5.93 , 0.00 , 0.00

#####################

Quaterion X/Y/Z/W: 0.00, 0.00, 0.00, 1.00
Accel X/Y/Z:  0.44 , 0.07 , 7.13
Gyro X/Y/Z:  24.23 , 13.85 , -11.90
Yaw/Pitch/Roll:  5.93 , 0.00 , 0.00

#####################

Quaterion X/Y/Z/W: 0.00, 0.00, 0.00, 1.00
Accel X/Y/Z:  0.43 , 0.06 , 7.09
Gyro X/Y/Z:  23.62 , 13.31 , -10.62
Yaw/Pitch/Roll:  5.93 , 0.00 , 0.00

#####################

Quaterion X/Y/Z/W: 0.00, 0.00, 0.00, 1.00
Accel X/Y/Z:  0.44 , 0.07 , 7.11
Gyro X/Y/Z:  25.70 , 13.92 , -10.50
Yaw/Pitch/Roll:  5.93 , 0.00 , 0.00

#####################

Output (MAHONY):

x-axis self test: acceleration trim within : -0.3% of factory value
y-axis self test: acceleration trim within : -4.2% of factory value
z-axis self test: acceleration trim within : 4.0% of factory value
x-axis self test: gyration trim within : -1.9% of factory value
y-axis self test: gyration trim within : 1.0% of factory value
z-axis self test: gyration trim within : 0.7% of factory value
Quaterion X/Y/Z/W: 0.46, 0.65, 0.18, 0.58
Accel X/Y/Z:  0.45 , 0.04 , 7.09
Gyro X/Y/Z:  24.17 , 12.51 , -10.68
Yaw/Pitch/Roll:  89.93 , 36.20 , 109.14

#####################

Quaterion X/Y/Z/W: -0.04, -0.05, 0.26, 0.96
Accel X/Y/Z:  0.46 , 0.05 , 7.10
Gyro X/Y/Z:  24.60 , 12.51 , -12.76
Yaw/Pitch/Roll:  35.77 , -4.51 , -5.38

#####################

Quaterion X/Y/Z/W: 0.10, 0.17, 0.28, 0.94
Accel X/Y/Z:  0.46 , 0.07 , 7.09
Gyro X/Y/Z:  23.13 , 12.70 , -12.15
Yaw/Pitch/Roll:  41.21 , 15.91 , 16.80

#####################

Quaterion X/Y/Z/W: -0.13, -0.21, 0.27, 0.93
Accel X/Y/Z:  0.46 , 0.06 , 7.09
Gyro X/Y/Z:  25.76 , 12.45 , -11.54
Yaw/Pitch/Roll:  42.66 , -19.19 , -22.13

#####################

[kevinjacb]

Has there been any update on this? I too am facing this issue. A delay upto 8 ms or so doesn't drastically affect the readings but if you go above it then the output drops to 0.

[C-EkoEko]

Unfortunately, I couldn't solve it. The only possible explanation I could reach from previous issues is that it may not be an original chip.

[kevinjacb]

Fairly certain I have the original variant since I bought it from a legit store. Does your calibration function properly? That is set the initial values of the getPitch and whatnot to 0? Mine has an offset of 9 degrees for some odd reason after calibration 🥲

[C-EkoEko]

Sorry, I don't remember and don't have the sensor with me anymore. However, as far as I can remember, the sample outputs I provided above were right after calibration.

[chrisbloomfieldcollie]

I have this same problem. The rest of my program runs for more than 40ms which means I can't get any readings. When I comment out the rest of my program and have a delay less than 5ms it works fine.

[youssiefkhaled]

Did you try mills function?

[hideakitai]

You should call mpu.update() frequently to estimate a pose. Too long delay() causes incorrect estimation