倾角方位角算法融合思考

 

float calculateTilt(float ax,float ay,float az,char flag_x,char flag_y,char flag_z)

{

     float g=9.80665;

     float temp;

     float Tiltangle=0;

     temp=((sqrt(2)/2)*g/10);

     if(az>((sqrt(2)/2)*g/10))

     {

          Tiltangle=(1-ay*ay)-(1-ax*ax);

          if(Tiltangle<0)

          Tiltangle=-Tiltangle;

      

          Tiltangle=acos(sqrt(Tiltangle));

          Tiltangle=Tiltangle/PI*180;

          if(flag_x==1||flag_y==1)

          {

              Tiltangle+=90;

          }

          else

          {

              Tiltangle=90-Tiltangle;

          }

    }

    else

    {

       

       Tiltangle=asin(az);

       Tiltangle=Tiltangle/PI*180;

       if(flag_z==1)

       {

           Tiltangle+=90;

       }

       else

       {

           Tiltangle=90-Tiltangle;

       }

       

    }

   

    return  Tiltangle;

}