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?