@Kris Winer, I am new in this field and I don't know much about this, could you please tell me basic how can I calculate the yaw, pitch and roll from MPU 9250 raw data?

[shahid377]

include

include

define MPU9250_ADDRESS 0x68

define MAG_ADDRESS 0x0C

define GYRO_FULL_SCALE_250_DPS 0x00

define GYRO_FULL_SCALE_500_DPS 0x08

define GYRO_FULL_SCALE_1000_DPS 0x10

define GYRO_FULL_SCALE_2000_DPS 0x18

define ACC_FULL_SCALE_2_G 0x00

define ACC_FULL_SCALE_4_G 0x08

define ACC_FULL_SCALE_8_G 0x10

define ACC_FULL_SCALE_16_G 0x18

//Funcion auxiliar lectura void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data) { Wire.beginTransmission(Address); Wire.write(Register); Wire.endTransmission();

Wire.requestFrom(Address, Nbytes); uint8_t index = 0; while (Wire.available()) Data[index++] = Wire.read(); }

// Funcion auxiliar de escritura void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data) { Wire.beginTransmission(Address); Wire.write(Register); Wire.write(Data); Wire.endTransmission(); }

int a; void setup() {

a=0; Wire.begin(); Serial.begin(115200);

// Configurar acelerometro I2CwriteByte(MPU9250_ADDRESS, 28, ACC_FULL_SCALE_2_G); // Configurar giroscopio I2CwriteByte(MPU9250_ADDRESS, 27, GYRO_FULL_SCALE_2000_DPS); // Configurar magnetometro I2CwriteByte(MPU9250_ADDRESS, 0x37, 0x02); I2CwriteByte(MAG_ADDRESS, 0x0A, 0x01); }

int16_t average_accelometer_x=0; int16_t average_accelometer_y=0; int16_t average_accelometer_z=0;

int16_t average_gyroscope_x=0; int16_t average_gyroscope_y=0; int16_t average_gyroscope_z=0;

void loop()

{

// --- Lectura acelerometro y giroscopio --- uint8_t Buf[14]; I2Cread(MPU9250_ADDRESS, 0x3B, 14, Buf);

// Convertir registros acelerometro int16_t ax = -(Buf[0] << 8 | Buf[1]); int16_t ay = -(Buf[2] << 8 | Buf[3]); int16_t az = Buf[4] << 8 | Buf[5];

// Convertir registros giroscopio int16_t gx = -(Buf[8] << 8 | Buf[9]); int16_t gy = -(Buf[10] << 8 | Buf[11]); int16_t gz = Buf[12] << 8 | Buf[13];

// --- Lectura del magnetometro --- uint8_t ST1; I2CwriteByte(MAG_ADDRESS,0x0A,0x01);
do { I2Cread(MAG_ADDRESS, 0x02, 1, &ST1); } while (!(ST1 & 0x01));

uint8_t Mag[7]; I2Cread(MAG_ADDRESS, 0x03, 7, Mag);

// Convertir registros magnetometro int16_t mx = -(Mag[3] << 8 | Mag[2]); int16_t my = -(Mag[1] << 8 | Mag[0]); int16_t mz = -(Mag[5] << 8 | Mag[4]);

float accx,accy,accz; accx=(float)ax/16384.0f; accy=(float)ay/16384.0f; accz=(float)az/16384.0f;

float pitch,roll;

pitch=360atan2(accx,sqrt(accyaccy+acczaccz))/PI; roll=360atan2(accy,sqrt(accxaccx+acczaccz))/PI;

float mag_x,mag_y,yaw;

float magx,magy,magz;

magx=(float)mx; magy=(float)my; magz=(float)mz;

magx=magx/0.6f; magy=magy/0.6f; magz=magz/0.6f;

mag_x = magxcos(pitch) + magysin(roll)sin(pitch) + magzcos(roll)sin(pitch); mag_y = magycos(roll) - magx sin(roll); yaw = 180 atan2(-mag_y,mag_x)/M_PI;

// //Serial.println(yaw);

//Serial.print(magx); //Serial.print("\t"); //Serial.print(magy); //Serial.print("\t"); //Serial.println(magz);

//Serial.print(accx); //Serial.print("\t"); //Serial.print(accy); //Serial.print("\t"); //Serial.println(accz);

// //Serial.print(ax); //Serial.print("\t"); //Serial.print(ay); //Serial.print("\t"); //Serial.println(az);

Serial.print(pitch); Serial.print("\t"); Serial.print(roll); Serial.print("\t"); Serial.println(yaw);

delay(100);

[shahid377]

Please could you please check above code and verify that the way of calculating yaw, pitch, roll is correct ?

[kriswiner]

No, sorry, I cannot debug your code. There are plenty of examples you can use to compare reposited on GitHub.

On Tue, Sep 24, 2019 at 12:52 AM shahid377 notifications@github.com wrote:

Please could you please check above code and verify that the way of calculating yaw, pitch, roll is correct ?

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/389?email_source=notifications&email_token=ABTDLKW3ZXK4MOX5CL47CWDQLHBLPA5CNFSM4IZ4BHYKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD7NOB2Q#issuecomment-534438122, or mute the thread https://github.com/notifications/unsubscribe-auth/ABTDLKXMONY52HSWTNND35TQLHBLPANCNFSM4IZ4BHYA .