新手上路
- 积分
- 29
- 金钱
- 29
- 注册时间
- 2017-2-9
- 在线时间
- 18 小时
|
发表于 2017-2-9 07:47:45
|
显示全部楼层
本帖最后由 ringudk 于 2017-2-9 09:11 编辑
你好,我用你移植的代码在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;
}
|
|