stm32f4读取mpu9250姿态角?

2019-07-20 08:35发布

原子哥的代码,在读取mpu9250姿态角的时候,为什么一个是DMP处理过的,一个是MPL处理过的,这有什么区别吗?

//μÃμ½dmp′|àíoóμÄêy¾Y(×¢òa,±¾oˉêyDèòa±è½Ï¶à¶ÑÕ»,¾Ö2¿±äá¿óDμã¶à)
//pitch:&#184;&#169;&#209;&#246;&#189;&#199; &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-90.0&#161;&#227; <---> +90.0&#161;&#227;
//rollá1&#246;&#189;&#199;  &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-180.0&#161;&#227;<---> +180.0&#161;&#227;
//yaw&#189;&#207;ò&#189;&#199;   &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-180.0&#161;&#227;<---> +180.0&#161;&#227;
//·μ&#187;&#216;&#214;μ:0,&#213;y3£
//    &#198;&#228;&#203;&#251;,꧰ü
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&#184;&#241;ê&#189;×a&#187;&#187;&#206;a&#184;&#161;μ&#227;êy
                q1 = quat[1] / q30;
                q2 = quat[2] / q30;
                q3 = quat[3] / q30;
                //&#188;&#198;&#203;&#227;μ&#195;μ&#189;&#184;&#169;&#209;&#246;&#189;&#199;/oá1&#246;&#189;&#199;/o&#189;&#207;ò&#189;&#199;
                *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;
}

//μ&#195;μ&#189;mpl′|àíoóμ&#196;êy&#190;Y(×¢òa,±&#190;oˉêyDèòa±è&#189;&#207;&#182;à&#182;&#209;&#213;&#187;,&#190;&#214;2&#191;±&#228;á&#191;óDμ&#227;&#182;à)
//pitch:&#184;&#169;&#209;&#246;&#189;&#199; &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-90.0&#161;&#227; <---> +90.0&#161;&#227;
//rollá1&#246;&#189;&#199;  &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-180.0&#161;&#227;<---> +180.0&#161;&#227;
//yaw&#189;&#207;ò&#189;&#199;   &#190;&#171;&#182;è:0.1&#161;&#227;   ·&#182;&#206;§:-180.0&#161;&#227;<---> +180.0&#161;&#227;
//·μ&#187;&#216;&#214;μ:0,&#213;y3£
//    &#198;&#228;&#203;&#251;,꧰ü
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);          //°&#209;D&#194;êy&#190;Y·¢&#203;í&#184;&#248;MPL
        mpu_get_temperature(&temperature,&sensor_timestamp);
        inv_build_temp(temperature,sensor_timestamp);   //°&#209;&#206;&#194;&#182;è&#214;μ·¢&#184;&#248;MPL£&#172;&#214;&#187;óDíó&#194;Yò&#199;Dèòa&#206;&#194;&#182;è&#214;μ
    }

    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);      //°&#209;&#188;ó&#203;ù&#182;è&#214;μ·¢&#184;&#248;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); //°&#209;′&#197;á|&#188;&#198;&#214;μ·¢&#184;&#248;MPL
    }
    inv_execute_on_data();
    inv_get_sensor_type_euler(data,&accuracy,&timestamp);

    *roll  = (data[0]/q16);
    *pitch = -(data[1]/q16);
    *yaw   = -data[2] / q16;
        return 0;
}

友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。