纯整数乘加运算的快速反正切算法,用于加速度传感器解析倾角

admin 2017-6-24 3190

一个纯整数乘加运算的快速反正切算法,用于加速度传感器解析倾角,根据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]
返回