mpu9250静止时yaw角度很准,一快速运动然后回到原位置,yaw角度就差了很远!

2019-08-20 17:07发布

问题如上。用的mpl库。stm32f030R8T6
说说几个我怀疑的问题。
一个是自检的时候无法通过。这里我修改了const struct test_s test里面两个成员变量的值。为什么要修改,因为不修改就通不过自检。(1).packet_thresh  = 100,    /* 200 samples。原来是200 */  (2).sample_wait_ms = 5  //10ms sample time wait。原来是10.
初始化部分代码[mw_shl_code=c,true]u8 mpu_dmp_init(void)
{
        u8 res=0;
    struct int_param_s int_param;
    unsigned char accel_fsr;
    unsigned short gyro_rate, gyro_fsr;
    unsigned short compass_fsr;
   
        IIC_Init();
        res = mpu_init(&int_param);
        if(res ==0)        //3õê¼»ˉMPU9250
        {         
        res=inv_init_mpl();     //3õê¼»ˉMPL
        if(res)return 1;
        inv_enable_quaternion();
        inv_enable_9x_sensor_fusion();
        inv_enable_fast_nomot();
        inv_enable_gyro_tc();
        inv_enable_vector_compass_cal();
        inv_enable_magnetic_disturbance();
        inv_enable_eMPL_outputs();
        res=inv_start_mpl();    //¿aÆôMPL
        if(res)return 1;
                res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);//éèÖÃËùDèòaμÄ′«¸DÆ÷
                if(res)return 2;
                res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);   //éèÖÃFIFO
                if(res)return 3;
                res=mpu_set_sample_rate(DEFAULT_MPU_HZ);                    //éèÖÃ2éÑùÂê
                if(res)return 4;
        res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS);  //éèÖÃ′Åá|¼Æ2éÑùÂê
        if(res)return 5;
        mpu_get_sample_rate(&gyro_rate);
        mpu_get_gyro_fsr(&gyro_fsr);
        mpu_get_accel_fsr(&accel_fsr);
        mpu_get_compass_fsr(&compass_fsr);
        inv_set_gyro_sample_rate(1000000L/gyro_rate);
        inv_set_accel_sample_rate(1000000L/gyro_rate);
        inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);
        inv_set_gyro_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);
        inv_set_accel_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);
        inv_set_compass_orientation_and_scale(
            inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);
            
            
                res=dmp_load_motion_driver_firmware();                             //&#188;ó&#212;&#216;dmp1ì&#188;t
                if(res)return 6;
                res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//éè&#214;&#195;íó&#194;Yò&#199;·&#189;&#207;ò
                if(res)return 7;
               
                dmp_register_tap_cb(tap_cb);
                dmp_register_android_orient_cb(android_orient_cb);
               
                res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|                    //éè&#214;&#195;dmp1|&#196;ü
                    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
                    DMP_FEATURE_GYRO_CAL);
                if(res)return 8;
                res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);        //éè&#214;&#195;DMPê&#228;3&#246;&#203;ù&#194;ê(×&#238;′ó2&#187;3&#172;1y200Hz)
                if(res)return 9;   
                res=run_self_test();                //×&#212;&#188;ì
                if(res)return 10;
                res=mpu_set_dmp_state(1);        //ê1&#196;üDMP
                if(res)return 11;            
        }
        else
        {
                printf("mpu_init(&int_param) error =%d ",res);
                return 12;
        }
               
        return 0;
}[/mw_shl_code]
读取数据的时候一直循环读取的,中间没有加任何延时。
[mw_shl_code=c,true]u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw, unsigned char more)
{
        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;
        int8_t newData = 0;
   
        if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;         
//        while(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more) == 0)
//                ;
    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;μ
                                newData = 1;
    }
   
    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
                                newData = 1;
    }
   
    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
                                newData = 1;
    }
                if(newData)
                {
                        inv_execute_on_data();
                        if(inv_get_sensor_type_euler(data,&accuracy,×tamp))
                        {
                                *roll  = (data[0]/q16);
                                *pitch = -(data[1]/q16);
                                *yaw   = data[2] / q16;
                        }
                        return 0;
                }
    return 1;
}[/mw_shl_code]
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。