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); }