int ms_update() {
if (!dmpReady) {
printf("Error: DMP not ready!!\n");
return -1;
}
while (dmp_read_fifo(g,a,_q,&sensors,&fifoCount)!=0); //gyro and accel can be null because of being disabled in the efeatures
q = _q;
GetGravity(&gravity, &q);
GetYawPitchRoll(ypr, &q, &gravity);
mpu_get_temperature(&t);
temp=(float)t/65536L;
mpu_get_compass_reg(c);
//scaling for degrees output
for (int i=0;i<DIM;i++){
ypr[i]*=180/M_PI;
}
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of Pitch, MPU is attached upside down
ypr[1]*=-1.0;
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
accel[i] = (float)(a[DIM-i-1]);
compass[i] = (float)(c[DIM-i-1]);
}
return 0;
}
/**
* get yaw, pitch and roll information
*
* @param yprAttitude
* yaw, pitch and roll
*
* @param yprRate
* rate
*
* @param xyzAcc
* acceleration
*
* @param xyzGravity
* gravity
*
* @param xyzMagnet
* magnet
*
* @return
* void
*
*/
void getYawPitchRollInfo(float *yprAttitude, float *yprRate,
float *xyzAcc, float *xComponent, float *yComponent, float *zComponent, float *xyzMagnet) {
float q[4]; // [w, x, y, z] quaternion container
float mXComponent[3];
float mYComponent[3];
float mZComponent[3];
float ax = 0.f;
float ay = 0.f;
float az = 0.f;
float gx = 0.f;
float gy = 0.f;
float gz = 0.f;
#ifdef MPU6050_9AXIS
short s_mx=0;
short s_my=0;
short s_mz=0;
float f_mx=0.f;
float f_my=0.f;
float f_mz=0.f;
float f_x=0.f;
float f_y=0.f;
float f_z=0.f;
#endif
getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
#ifdef MPU6050_9AXIS
if(pollingMagnetDataBySingleMeasurementMode(&s_mx, &s_my, &s_mz)){
f_x = (float)s_mx - mag_hard_iron_cal[0];
f_y = (float)s_my - mag_hard_iron_cal[1];
f_z = (float)s_mz - mag_hard_iron_cal[2];
f_mx = f_x * mag_soft_iron_cal[0][0] + f_y * mag_soft_iron_cal[0][1] + f_z * mag_soft_iron_cal[0][2];
f_my = f_x * mag_soft_iron_cal[1][0] + f_y * mag_soft_iron_cal[1][1] + f_z * mag_soft_iron_cal[1][2];
f_mz = f_x * mag_soft_iron_cal[2][0] + f_y * mag_soft_iron_cal[2][1] + f_z * mag_soft_iron_cal[2][2];
pushSmaData(&x_magnetSmaFilterEntry,f_mx);
pushSmaData(&y_magnetSmaFilterEntry,f_my);
pushSmaData(&z_magnetSmaFilterEntry,f_mz);
f_mx = pullSmaData(&x_magnetSmaFilterEntry);
f_my = pullSmaData(&y_magnetSmaFilterEntry);
f_mz = pullSmaData(&z_magnetSmaFilterEntry);
IMUupdate9(gx, gy, gz, ax, ay, az, f_my, f_mx, f_mz, q);
}else{
IMUupdate6(gx, gy, gz, ax, ay, az, q);
}
#else
IMUupdate6(gx, gy, gz, ax, ay, az, q);
#endif
GetXComponent(mXComponent, q);
GetYComponent(mYComponent, q);
GetZComponent(mZComponent, q);
GetYawPitchRoll(yprAttitude, q, mZComponent);
yprAttitude[0] = yprAttitude[0] * RA_TO_DE;
yprAttitude[1] = yprAttitude[1] * RA_TO_DE;
yprAttitude[2] = yprAttitude[2] * RA_TO_DE;
xComponent[0] = mXComponent[0];
xComponent[1] = mXComponent[1];
xComponent[2] = mXComponent[2];
yComponent[0] = mYComponent[0];
yComponent[1] = mYComponent[1];
yComponent[2] = mYComponent[2];
zComponent[0] = mZComponent[0];
zComponent[1] = mZComponent[1];
zComponent[2] = mZComponent[2];
yprRate[0] = gx * RA_TO_DE;
yprRate[1] = gy * RA_TO_DE;
yprRate[2] = gz * RA_TO_DE;
xyzAcc[0] = ax;
xyzAcc[1] = ay;
xyzAcc[2] = az;
}
float Quaternion::GetRoll() {
FloatVectorStruct ypr;
GetYawPitchRoll(ypr);
return ypr.z;
}
void Quaternion::GetYawPitchRoll(FloatVectorStruct& ypr) {
FloatVectorStruct gravity;
GetGravity(ypr,*this);
GetYawPitchRoll(*this,gravity,ypr);
}
