Please i NEED HELP!


#1

Hello,

Here there are two weeks that I’m stuck and I need your help, I’m looking how to find YAW using a MPU9150 but I could not, I actually have the compass the accelerometer and gyroscope data’s, I managed to calculate the pitch and roll but not the Yaw, however I could figure out the yaw when pitch and roll zeros

this is my code, can someone help me to find the yaw even when pitch and rol != 0 ???

void rool_pitch (int k)
{

MPU_Read( ACC_TEMP_GYRO, (uint8_t *) &ACC_TEMP_GYRO_value, sizeof(ACC_TEMP_GYRO_value));

Xacc[k] = ((ACC_TEMP_GYRO_value[0]<<8) | ACC_TEMP_GYRO_value[1] ) ;
Yacc[k] = ((ACC_TEMP_GYRO_value[2]<<8) | ACC_TEMP_GYRO_value[3] ) ;
Zacc[k] = ( (ACC_TEMP_GYRO_value[4]<<8) | ACC_TEMP_GYRO_value[5] );

Xg[k] = ( ( ( ACC_TEMP_GYRO_value[8]<<8 ) | ACC_TEMP_GYRO_value[9] )- Xoff[k] );
Yg[k] = ( ( ( ACC_TEMP_GYRO_value[10]<<8) | ACC_TEMP_GYRO_value[11] ) - Yoff[k] ) ;
Zg[k] = ( ( ( ACC_TEMP_GYRO_value[12]<<8) | ACC_TEMP_GYRO_value[13] ) - Zoff[k] ) ;

digitalWrite(led, LOW);// turn the LED off by making the voltage LOW

float currentT = millis();

dt[k] = ((currentT - previousT[k]) * GYRO_SCALE) /1000000.0f ; // GYRO_SCALE unit: degre/millisecond

previousT[k] = currentT;

Xg_tp[k] = Xg[k] * dt[k]; // degre
Yg_tp[k] = Yg[k] * dt[k];
Zg_tp[k] = Zg[k] * dt[k];

//Low Pass Filter
fXacc[k] = Xacc[k] * alpha + (fXacc[k] * (1.0 - alpha));
fYacc[k] = Yacc[k] * alpha + (fYacc[k] * (1.0 - alpha));
fZacc[k] = Zacc[k] * alpha + (fZacc[k] * (1.0 - alpha));

//Roll & pitch Equations acc

rollacc[k] = (atan2( - fXacc[k], fZacc[k])*180.0)/M_PI;
pitchacc[k] = (atan2( fYacc[k], sqrt (fZacc[k]*fZacc[k] + fXacc[k]*fXacc[k] ) )*180.0)/M_PI;

pitch[k] = (pitch[k] + Xg_tp[k]) * (1-coef_gyro_acc) + pitchacc[k] * coef_gyro_acc ;
roll[k] = (roll[k] + Yg_tp[k]) * (1-coef_gyro_acc) + rollacc[k] * coef_gyro_acc;

Serial.print(roll[k]);
Serial.print(" “);
Serial.print(pitch[k]);
Serial.print(” ");

}

int calcul_yaw(int k)
{
MPU9150_writeSensor(0x37, 0x02); //Set Bypass to Magnetometer
MPU_address = 0x0C; //change Adress to Compass
MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
MPU9150_writeSensor(0x0A, 0x0F); //SelfTest
MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
delay(8);
MPU9150_writeSensor(0x0A,0x01); // Enable Magnetometer
delay(8);

MPU_Read( MPU9150_CMPS_DATA, (uint8_t *) & MPU9150_COMPS_value, sizeof( MPU9150_COMPS_value));

cmps[0] = (( MPU9150_COMPS_value[1]<<8) | MPU9150_COMPS_value[0] ) ;
cmps[1] = ((MPU9150_COMPS_value[3]<<8) | MPU9150_COMPS_value[2] ) ;
cmps[2] = ((MPU9150_COMPS_value[5]<<8) | MPU9150_COMPS_value[4] );

MPU_address = 0x69;

pitchm = (pitch[k]*M_PI)/180.0;
rollm = (roll[k]*M_PI)/180.0;

x = cmps[0]*cos(pitchm) + cmps[1]*sin(rollm)*sin(pitchm) + cmps[2]*cos(rollm)*sin(pitchm);
y = cmps[1]*cos(rollm) - cmps[2]*sin(rollm);

yaw = (atan2(y,x)*180.0)/M_PI ;

// if (yaw<0){ yaw = yaw +360;}
//
// yaw += mag_declination; // Set from GUI

Serial.print(yaw);

}