一个纯整数乘加运算的快速反正切算法,用于加速度传感器解析倾角,根据CORDIC算法改写的,适用于stm8s003。
是基于y = 3.5714 * x / (x * x + 3.5714)的近似算法。大家评价下,看能否再优化 ,适用于不便于浮点运算的单片机
解析出的为放大了1600倍的角度值,右移四位就是精度为0.01度的角度值,范围为0~360度
编译完成的整个代码不到500字节,大家可以用IAR FOR STM8验证一下。
const static INT16U __tanz[] = { 32767 , 16384 , 8192 , 4096 , 2048 , 1024 , 512 , 256 , 128 , 64 , 32 , 16 , 8 , 4 , 2 , 1 , }; const static INT32U __angle[] = { 72000 , 42504 , 22458 , 11400 , 5722 , 2864 , 1432 , 716 , 358 , 179 , 90 , 45 , 22 , 11 , 6 , 3 , }; static INT32U __atan_core(INT16U tan) { INT32S x,y; INT32S xt,yt,t; INT16U *ptan; INT32U *pang; INT32U m,ang; x = 65535; y = tan; ptan = (INT16U *)&__tanz; pang = (INT32U *)&__angle; m = 0; for(INT8U i=0; i<16; i++) { t = *ptan++; ang = *pang++; yt = y*t; xt = x*t; yt >>= 15; xt >>= 15; if(y>0) { x += yt; y -= xt; m += ang; } else { x -= yt; y += xt; m -= ang; } } return m; } INT32U __FastIntAtan2__(INT16S x, INT16S y) { INT8U flag = 0; INT32U angle; if(x<0) { x = -x; flag |= 0x01; } if(y<0) { y = -y; flag |= 0x02; } if(y>x) { INT16S t; t = y; y = x; x = t; flag |= 0x04; } { INT32U m = y; m *= 65535; m /= (INT16U)x; angle = __atan_core(m); } if(flag & 0x04) { angle = 144000 - angle; } if(flag & 0x01) { angle = 288000 - angle; } if(flag & 0x02) { angle = 576000 - angle; } return angle; }
参考文章
http://www.cnblogs.com/touchblue/p/3535968.html
最新回复 [0]