|
发表于 2013-11-26 18:18:17
|
显示全部楼层
楼主想请教一个关于getEstimatedAttitude()函数的问题:EstG32.V.Z 和EstG.V.Z的关系?
在getEstimatedAttitude()函数中调用了rotateV(&EstG.V,deltaGyroAngle); 对EstG.V向量更新,只用到了角速度参量(deltaGyroAngle[axis] = gyroADC[axis] * scale;)。。。。(在后面的代码中 EstG.V不见了 ,很想知道它传递向哪里了?,一下我我的寻找过程)
代码然后就是互补滤波:EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; 只用到了加速度参量accSmooth[axis]
在之后就是浮点转换成整数 EstG32.A[axis] = EstG.A[axis]; // 还是没见EstG.V
最后几行代码了:
// Attitude of the estimated vector
int32_t sqGZ = sq(EstG32.V.Z);。。。。。。EstG32.V.Z 从哪里冒出来的 和EstG.V.Z有什么关系呢?
int32_t sqGX = sq(EstG32.V.X);
int32_t sqGY = sq(EstG32.V.Y);
int32_t sqGX_sqGZ = sqGX + sqGZ;
float invmagXZ = InvSqrt(sqGX_sqGZ);
invG = InvSqrt(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z);。。。。。。计算角度也是用的EstG32.V.Z 不见EstG.V.Z
angle[PITCH] = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);
#if MAG
heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
EstM32.V.Y * invG * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
#endif
|
|