原子哥的代码,在读取mpu9250姿态角的时候,为什么一个是DMP处理过的,一个是MPL处理过的,这有什么区别吗?
//μÃμ½dmp′|àíoóμÄêy¾Y(×¢òa,±¾oˉêyDèòa±è½Ï¶à¶ÑÕ»,¾Ö2¿±äá¿óDμã¶à)
//pitch:¸©Ñö½Ç ¾«¶è:0.1¡ã ·¶Î§:-90.0¡ã <---> +90.0¡ã
//roll
á1ö½Ç ¾«¶è:0.1¡ã ·¶Î§:-180.0¡ã<---> +180.0¡ã
//yaw
½Ïò½Ç ¾«¶è:0.1¡ã ·¶Î§:-180.0¡ã<---> +180.0¡ã
//·μ»ØÖμ:0,Õy3£
// ÆäËû,꧰ü
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30¸ñê½×a»»Îa¸¡μãêy
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//¼ÆËãμÃμ½¸©Ñö½Ç/oá1ö½Ç/o½Ïò½Ç
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}
//μÃμ½mpl′|àíoóμÄêy¾Y(×¢òa,±¾oˉêyDèòa±è½Ï¶à¶ÑÕ»,¾Ö2¿±äá¿óDμã¶à)
//pitch:¸©Ñö½Ç ¾«¶è:0.1¡ã ·¶Î§:-90.0¡ã <---> +90.0¡ã
//roll
á1ö½Ç ¾«¶è:0.1¡ã ·¶Î§:-180.0¡ã<---> +180.0¡ã
//yaw
½Ïò½Ç ¾«¶è:0.1¡ã ·¶Î§:-180.0¡ã<---> +180.0¡ã
//·μ»ØÖμ:0,Õy3£
// ÆäËû,꧰ü
u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
{
unsigned long sensor_timestamp,timestamp;
short gyro[3], accel_short[3],compass_short[3],sensors;
unsigned char more;
long compass[3],accel[3],quat[4],temperature;
long data[9];
int8_t accuracy;
if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;
if(sensors&INV_XYZ_GYRO)
{
inv_build_gyro(gyro,sensor_timestamp); //°ÑDÂêy¾Y·¢Ëí¸øMPL
mpu_get_temperature(&temperature,&sensor_timestamp);
inv_build_temp(temperature,sensor_timestamp); //°ÑζèÖμ·¢¸øMPL£¬Ö»óDíóÂYòÇDèòaζèÖμ
}
if(sensors&INV_XYZ_ACCEL)
{
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel,0,sensor_timestamp); //°Ñ¼óËù¶èÖμ·¢¸øMPL
}
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))
{
compass[0]=(long)compass_short[0];
compass[1]=(long)compass_short[1];
compass[2]=(long)compass_short[2];
inv_build_compass(compass,0,sensor_timestamp); //°Ñ′Åá|¼ÆÖμ·¢¸øMPL
}
inv_execute_on_data();
inv_get_sensor_type_euler(data,&accuracy,×tamp);
*roll = (data[0]/q16);
*pitch = -(data[1]/q16);
*yaw = -data[2] / q16;
return 0;
}
一周热门 更多>