Using MPU9250 IMU with Arduino mega for getting Euler angles

Hello,

I am trying to get pitch, roll, and yaw from my MPU9250 IMU which I have connected to my Arduino Mega via the Serial port (SDA, SCL). I start by trying to run calibration on the accelerometer. Then I am basically extracting the raw data from the X Y Z accelerometers, using simple trig to calculate for pitch, roll:

pitch = tan^-1 (Z/Y)

roll = tan^-1(Z/X)

For the gyro, I'm integrating the data from the appropriate axes to get the same pitch and roll:

pitch = X*dt

then dividing that by 2pie and multiplying by 360 to get degrees.

Problems are, the calibration doesn't seem to work, my pitch from gyro is giving nonsensical results - anywhere from inf to random numbers (not even mentioning it matching the pitch from accelerometers). Here's the code:

#include "MPU9250.h"
#include "Math.h"
#include "Wire.h"

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
int cal_status_accel;

float pitch_raw_accel;
float pitch_raw_accel_deg;
float roll_raw_accel;
float roll_raw_accel_deg;
float flat_pitch_angle;
float dynamic_pitch_angle;
float dynamic_roll_angle;
float flat_roll_angle;
float pitch_raw_gyro;
unsigned long dt;
unsigned long newMillis;
unsigned long oldMillis;

float y_z_ratio;
float z_y_ratio;
float x_z_ratio;
float z_x_ratio;

void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {
      Serial.println("Communication with IMU established");
      }

  }

  // calibrate accelerometers
  cal_status_accel = IMU.calibrateAccel();
  if (cal_status_accel < 0) {
    Serial.println("Accelerometers are not calibrated");
    Serial.println("Try power cycling");
    Serial.print("Accel Cal Status: ");
    Serial.println(cal_status_accel);   
    if(cal_status_accel > 0) {
      Serial.println("Acelerometer X bias: ");
      Serial.print(IMU.getAccelBiasX_mss());
      Serial.println("Acelerometer Y bias: ");
      Serial.print(IMU.getAccelBiasY_mss());
      Serial.println("Acelerometer Z bias: ");
      Serial.println(IMU.getAccelBiasZ_mss());
      }
    }

   IMU.readSensor(); 

  y_z_ratio = (IMU.getAccelY_mss()/9.8)/(IMU.getAccelZ_mss()/9.8);
  z_y_ratio = (IMU.getAccelZ_mss()/9.8)/(IMU.getAccelY_mss()/9.8);
  z_x_ratio = (IMU.getAccelZ_mss()/9.8)/(IMU.getAccelX_mss()/9.8);
  x_z_ratio = (IMU.getAccelX_mss()/9.8)/(IMU.getAccelZ_mss()/9.8);
  
  pitch_raw_accel = atan(z_y_ratio);
  pitch_raw_accel_deg = (pitch_raw_accel/2/PI*360);
  flat_pitch_angle = pitch_raw_accel_deg;

  roll_raw_accel = atan(z_x_ratio);
  roll_raw_accel_deg = (roll_raw_accel/2/PI*360);
  flat_roll_angle = roll_raw_accel_deg;
  
oldMillis = millis();
}

void loop() {
  // read the sensor
  IMU.readSensor();

  y_z_ratio = (IMU.getAccelY_mss()/9.8)/(IMU.getAccelZ_mss()/9.8);
  z_y_ratio = (IMU.getAccelZ_mss()/9.8)/(IMU.getAccelY_mss()/9.8);
  z_x_ratio = (IMU.getAccelZ_mss()/9.8)/(IMU.getAccelX_mss()/9.8);
  x_z_ratio = (IMU.getAccelX_mss()/9.8)/(IMU.getAccelZ_mss()/9.8);

pitch_raw_accel = atan(z_y_ratio);

pitch_raw_accel_deg = (pitch_raw_accel/2/PI*360);

dynamic_pitch_angle = flat_pitch_angle - pitch_raw_accel_deg;

  Serial.println("Dynamic pitch: ");
  Serial.println(dynamic_pitch_angle);

  roll_raw_accel = atan(z_x_ratio);
  roll_raw_accel_deg = (roll_raw_accel/2/PI*360);
  
  dynamic_roll_angle = flat_roll_angle - roll_raw_accel_deg;
  Serial.println("Dynamic roll: ");
  Serial.println(dynamic_roll_angle);
 
  newMillis = millis();
  dt = (newMillis - oldMillis)/1000.0;

  Serial.println("dt: ");
  Serial.println(dt);
  Serial.println("Gyro raw reading: ");
  Serial.println(IMU.getGyroX_rads());

  pitch_raw_gyro = (IMU.getGyroX_rads()*dt)/2/PI*360;
  newMillis = oldMillis;

  Serial.println("Pitch angle from gyro: ");
  Serial.println(pitch_raw_gyro);

delay(100);
}

Here's the serial readout when the sensor is sitting flat:

01:33:11.928 -> Dynamic pitch: 
01:33:11.928 -> -0.11
01:33:11.928 -> Dynamic roll: 
01:33:11.968 -> -0.12
01:33:11.968 -> dt: 
01:33:11.968 -> 10
01:33:11.968 -> Gyro raw reading: 
01:33:11.968 -> 0.00
01:33:11.968 -> Pitch angle from gyro: 
01:33:11.968 -> 0.23
01:33:12.056 -> Dynamic pitch: 
01:33:12.056 -> -0.09
01:33:12.056 -> Dynamic roll: 
01:33:12.056 -> -0.01
01:33:12.056 -> dt: 
01:33:12.056 -> 10
01:33:12.056 -> Gyro raw reading: 
01:33:12.056 -> -0.00
01:33:12.056 -> Pitch angle from gyro: 
01:33:12.056 -> -0.38
01:33:12.187 -> Dynamic pitch: 
01:33:12.187 -> -0.00
01:33:12.187 -> Dynamic roll: 
01:33:12.187 -> -0.01
01:33:12.187 -> dt: 
01:33:12.187 -> 10
01:33:12.187 -> Gyro raw reading: 
01:33:12.187 -> -0.00
01:33:12.187 -> Pitch angle from gyro: 
01:33:12.187 -> -0.45
01:33:12.267 -> Dynamic pitch: 
01:33:12.267 -> -0.04
01:33:12.267 -> Dynamic roll: 
01:33:12.267 -> -0.03


Here's the serial readout when I move the sensor to create a pitch:

01:35:28.538 -> Gyro raw reading: 
01:35:28.538 -> -0.00
01:35:28.538 -> Pitch angle from gyro: 
01:35:28.538 -> -16.63
01:35:28.618 -> Dynamic pitch: 
01:35:28.658 -> -0.91
01:35:28.658 -> Dynamic roll: 
01:35:28.658 -> -178.77
01:35:28.658 -> dt: 
01:35:28.658 -> 147
01:35:28.658 -> Gyro raw reading: 
01:35:28.658 -> 0.17
01:35:28.658 -> Pitch angle from gyro: 
01:35:28.658 -> 1422.19
01:35:28.738 -> Dynamic pitch: 
01:35:28.738 -> -0.86
01:35:28.738 -> Dynamic roll: 
01:35:28.738 -> -3.57
01:35:28.738 -> dt: 
01:35:28.738 -> 147
01:35:28.738 -> Gyro raw reading: 
01:35:28.738 -> -0.53
01:35:28.738 -> Pitch angle from gyro: 
01:35:28.738 -> -4495.05
01:35:28.857 -> Dynamic pitch: 
01:35:28.857 -> 175.57
01:35:28.857 -> Dynamic roll: 
01:35:28.857 -> -2.90
01:35:28.857 -> dt: 
01:35:28.857 -> 147
01:35:28.857 -> Gyro raw reading: 
01:35:28.857 -> -0.80
01:35:28.857 -> Pitch angle from gyro: 
01:35:28.857 -> -6739.24
01:35:28.947 -> Dynamic pitch: 
01:35:28.947 -> 171.74
01:35:28.947 -> Dynamic roll: 
01:35:28.947 -> -0.07
01:35:28.947 -> dt: 
01:35:28.947 -> 147
01:35:28.947 -> Gyro raw reading: 
01:35:28.947 -> -0.76
01:35:28.947 -> Pitch angle from gyro: 
01:35:28.947 -> -6422.97

Also, is it possible to keep dt constant?

What exactly does this line of code do ?

takes accelerometer reading in Z and divides by accelerometer reading in Y consistent with the coordinate reference frame of the sensor body. Then takes inverse tangent of the ratio to give angle (pitch)

There are several libraries for the (obsolete) MPU-9250 that offer reasonable 3D orientations.

The Sparkfun and Kris Winer libraries have fatal bugs, but this one works, if you take the time to calibrate the sensor properly: GitHub - jremington/MPU-9250-AHRS: Arduino Mahony AHRS, includes magnetometer and accelerometer calibration code

For simple pitch and roll estimation using the accelerometer, follow this tutorial: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot