上周发生了一个很低级的错误,找个很长时间,发现是开启中断的时候,只在gd32f1x0_it.c文件里边定义了中断函数,但是却没有在main.c文件里中断初始化时注册。
GCLIB_CalculateGbRollAngle_32bitTimer2At72MHz函数里,先是进行三角函数的逆变换,即pSysParam->gbAccel_Roll为三角函数值,经过逆变换得出AccelAngle角度值。同时因为逆变换函数是针对-90度到+90度之间的,所以对于不在这个范围内的角度要分别对融合后的角度和第三轴角度进行判断来修正象限差别。如是-90度到+90度之间,则不需要进行变换。
pSysParam->gbGyro_Roll = ((pSysParam->gbGyro_X_Error * pSysParam->gbCos_Tilt >> (SINCOS_1_SCALE - GYRO_DPS)) - (pSysParam->gbGyro_Z_Error * pSysParam->gbSin_Tilt >> (SINCOS_1_SCALE - GYRO_DPS)));这句话是通过陀螺仪的原始数据gbGyro_X_Error和gbGyro_Z_Error在有所倾斜时,乘上对应的三角函数,得出roll轴的加速度值。同时有对应的三角函数数量级变换。
之后再对得到的加速度值进行积分,然后再对陀螺仪和加速度计的值进行融合,融合的周期数是gbGA_Interval决定的,在上电时,这个值会稍微大,使加速度计的参与量更大。
在函数的最后,是对融合角度与加速度计角度做差,如果差值过大,则重启设备。