Skip to content

Instantly share code, notes, and snippets.

@shoebahmedadeel
Created May 6, 2017 18:25
Show Gist options
  • Save shoebahmedadeel/0d8ca4eaa65664492cf1db2ab3a9e572 to your computer and use it in GitHub Desktop.
Save shoebahmedadeel/0d8ca4eaa65664492cf1db2ab3a9e572 to your computer and use it in GitHub Desktop.
MPU 9250 - roll, pitch, yaw from Accelerometer,Gyroscope and Magnetometer
#include <Wire.h>
#include <Servo.h>
#define SerialPort Serial
#include <SparkFunMPU9250-DMP.h>
MPU9250_DMP imu;
double roll , pitch, yaw;
long int pre_ts=0;
void setup()
{
{
SerialPort.begin(115200);
if (imu.begin() != INV_SUCCESS)
{
while (1)
{
SerialPort.println("Unable to communicate with MPU-9250");
SerialPort.println("Check connections, and try again.");
SerialPort.println();
delay(3000);
}
}
imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
imu.setGyroFSR(250); // Set gyro to 2000 dps
// Accel options are +/- 2, 4, 8, or 16 g
imu.setAccelFSR(2); // Set accel to +/-2g
imu.setLPF(10); // Set LPF corner frequency to 5Hz
imu.setSampleRate(10); // Set sample rate to 10Hz
imu.setCompassSampleRate(50); // Set mag rate to 10Hz
}
pre_ts=millis();
}
//}
void loop()
{
if ( imu.dataReady() )
{
imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
printIMUData(millis()-pre_ts);
pre_ts=millis();
}
}
void printIMUData(long int dt)
{
float accelX = imu.calcAccel(imu.ax);
float accelY = imu.calcAccel(imu.ay);
float accelZ = imu.calcAccel(imu.az);
float gyroX = imu.calcGyro(imu.gx)/57.3;
float gyroY = imu.calcGyro(imu.gy)/57.3;
float gyroZ = imu.calcGyro(imu.gz)/57.3;
float magX = imu.calcMag(imu.mx);
float magY = imu.calcMag(imu.my);
float magZ = imu.calcMag(imu.mz);
SerialPort.println("Accel: " + String(accelX) + ", " + String(accelY) + ", " + String(accelZ) + " g");
SerialPort.println("Gyro: " + String(gyroX) + ", " + String(gyroY) + ", " + String(gyroZ) + " dps");
SerialPort.println("Mag: " + String(magX) + ", " + String(magY) + ", " + String(magZ) + " uT");
SerialPort.println("Time: " + String(imu.time) + " ms");
//Euler angle from accel
pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ))));
roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ))));
// yaw from mag
float Yh = (magY * cos(roll)) - (magZ * sin(roll));
float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch));
yaw = atan2(Yh, Xh);
roll = roll*57.3;
pitch = pitch*57.3;
yaw = yaw*57.3;
Serial.println("pitch" + String( pitch) );
Serial.println("roll" + String( roll));
Serial.println("yaw" + String( yaw ));
}
@avsingh999
Copy link

what is the formula for calculating raw pitch yaw because

roll  =  (atan2(-Accel_Y, Accel_Z)*180.0)/M_PI;
pitch =  (atan2(Accel_X sqrt(Accel_Y*Accel_Y + Accel_Y*Accel_Y))*180.0)/M_PI;

https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/

   accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
   accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
   accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
   pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
   roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;
   yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

https://engineering.stackexchange.com/questions/3348/calculating-pitch-yaw-and-roll-from-mag-acc-and-gyro-data

  Accroll = atan2( accy / (accy^2 + accz^2) )
  Accpitch = atan2( accx / (accx^2 + accz^2) )

http://franciscoraulortega.com/pubs/Algo3DFusionsMems.pdf

@michel4731
Copy link

hi. I used this equation for solved "yaw" but i can't get a stable angle.
please help me

@shoebahmedadeel
Copy link
Author

Yaw is never stable. Because when u see a compass there is always a drift in it. So u have to either average it across time or use a PID to get a closest value. Hope this answers ur question.

@michel4731
Copy link

Thank you.
How do I use PID?

@michel4731
Copy link

I have very big errors for example first time i have yaw=-170 and second time yaw= -10 and etc.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment