利用li3dh算出角度

//角度算法
void get_angel(int x,int y,int z)
{
    int16_t tmp;
    double q,f_t,f_z,angle;

    if(true==is_motor_on()) return;
    
    f_t= sqrt(x*x+y*y+z*z);
    f_z=abs(z);


    q=acos(f_z/f_t);
    if(y<0)
        angle=q*180/3.14;
    else
        angle=-q*180/3.14;

    if(angle>90 || angle<-90)
    return;
    tmp=angle*100;

    
    angle_valu = tmp;
    
    

}

//将3dh数据计算出角度
void Get_lis3dh_buff(void)
{
    uint8_t times,i;

    int x_sum=0,y_sum=0,z_sum=0 ,n=0;

    if (g_3dh_ok == false)
        return;
//  AxesRaw_t lis3dh_data;
    LIS3DH_ReadReg(LIS3DH_FIFO_SRC_REG, &times);
    times = times&0x1f;

    for(i=0; i<times; i++)
    {
        LIS3DH_GetAccAxesRaw(&axes[i%25]);
    }

    n=times%25;
    for(uint8_t k= 0; k<n; k++)
    {
        x_sum+= axes[k].AXIS_X;
        y_sum+= axes[k].AXIS_Y;
        z_sum+= axes[k].AXIS_Z;
    }

    x_sum = x_sum/n;
    y_sum = y_sum/n;
    z_sum = z_sum/n;

    get_angel(x_sum,y_sum,z_sum);

    LI3DH_LOGRTT("x:%d y:%d z:%d  angle:%d.%d\r\n",x_sum,y_sum,z_sum,Retuen_angel()/100, abs(Retuen_angel()%100));

}

//应用层获取数据
int16_t Retuen_angel(void)
{
    if (g_3dh_ok == false)
    {
        return 0xffff;
    }
    return angle_valu;
}