超高速的反正切算法,纯整数运算

2019-12-10 18:25发布

本帖最后由 s1j2h3 于 2017-3-14 21:02 编辑

加入这个论坛,大部时间都在潜水,今天刚完成一个纯整数乘加运算的快速反正切算法,用于加速度传感器解析倾角。
大家评价下,看能否再优化

解析出的为放大了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;
}
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
该问题目前已经被作者或者管理员关闭, 无法添加新回复
99条回答
zzj2009a1
1楼-- · 2019-12-22 20:53
了解一下,谢谢分享
四川李工
2楼-- · 2019-12-23 01:59
看了文章都看懂了!!!!!!!!11
四川李工
3楼-- · 2019-12-23 05:03
   if(y>0)
        {
            x  += yt;
            y  -= xt;
            m  += ang;
        }
        else
        {
            x  -= yt;
            y  += xt;
            m  -= ang;
        }
楼主这段为何判断Y 决定是正转还是反转呢?  不是该判断角度大于目标还是小于目标吗?
hzpyl
4楼-- · 2019-12-23 10:57
感谢楼主分享
songyongpan
5楼-- · 2019-12-23 13:04
 精彩回答 2  元偷偷看……
wahaha
6楼-- · 2019-12-23 14:20
支持!下载收藏

一周热门 更多>