本帖最后由 ringudk 于 2017-2-9 09:11 编辑

Boris 发表于 2016-11-18 17:14
代码和硬件连接都在这里了，其中大量引用了原子哥的代码，如有侵权，请及时告知。

你好，我用你移植的代码在STM32F103C8是做了测试，发现如果按上图顺序旋转的在回到原位置的话会有问题，10次有5次以上会出问题。
用STM32F407VE测试motion_driver_6.12的代码没有出现这个问题。
初始状态四元数输出
quat:  0.9986  0.0040  0.0527  0.0072
quat:  0.9986  0.0039  0.0527  0.0072
quat:  0.9986  0.0039  0.0527  0.0072
quat:  0.9986  0.0040  0.0527  0.0072
quat:  0.9986  0.0040  0.0527  0.0072
quat:  0.9986  0.0040  0.0526  0.0072
quat:  0.9986  0.0039  0.0526  0.0072


按我上图顺序旋转后回原位置四元数输出
quat:  0.4227 -0.0466  0.0264  0.9047
quat:  0.4227 -0.0466  0.0264  0.9047
quat:  0.4227 -0.0466  0.0263  0.9047
quat:  0.4227 -0.0466  0.0263  0.9047
quat:  0.4227 -0.0465  0.0263  0.9047
quat:  0.4227 -0.0465  0.0263  0.9047
quat:  0.4227 -0.0465  0.0263  0.9047


麻烦你帮忙测试一下是否会出现和我一样的问题，我找不出原因为什么会这样。


刚刚尝试在inv_error_t inv_get_quaternion函数，把罗盘数据强制设置为
        rh.compass_correction[0]=        0x40000000;
        rh.compass_correction[1]=        0x0;
        rh.compass_correction[2]=        0x0;
        rh.compass_correction[3]=        0x0;
这样就不会出现上面的错误了，由此估计是罗盘数据有问题。



inv_error_t inv_get_quaternion(long *data)
{
       
        rh.compass_correction[0]=        0x40000000;
        rh.compass_correction[1]=        0x0;
        rh.compass_correction[2]=        0x0;
        rh.compass_correction[3]=        0x0;

    if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) {
        inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat);
        rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET);
    }
    memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));
    return INV_SUCCESS;
}