无名创新开源飞控利用GPS数据航向对准开源代码

本文主要是介绍无名创新开源飞控利用GPS数据航向对准开源代码，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

无名创新开源飞控利用GPS数据航向对准开源代码：

#define acc_sync_cnt 5
void GPS_Azimuth_Update_Method1(uint8_t flag)
{
   if(flag==0) return;
   //下面利用到角公式求解误差方位角，先方位角误差正切值
   float x1=0,y1=0,x2=0,y2=0,molecule=0,denominator=0;
   WP_Sensor.azimuth_err_ang_his=WP_Sensor.azimuth_err_ang;//航向角误差历史值
   x1=GPS_Vel_Div.E;
   y1=GPS_Vel_Div.N;
   x2=NamelessQuad.Acce_History[_PITCH][acc_sync_cnt];//Origion_NamelessQuad.Acceleration[_PITCH];
   y2=NamelessQuad.Acce_History[_ROLL][acc_sync_cnt];//Origion_NamelessQuad.Acceleration[_ROLL];
   WP_Sensor.azimuth_k_ins=y2/x2;
   WP_Sensor.azimuth_k_gps=y1/x1;

   //ins加速度向量到gps加速度向量的夹角正切值
   molecule=WP_Sensor.azimuth_k_gps-WP_Sensor.azimuth_k_ins;//分子
   denominator=1+WP_Sensor.azimuth_k_gps*WP_Sensor.azimuth_k_ins;//分母
   if(denominator==0)//剔除分母为0，到角度为90度情况
   {
   //存在垂直的情况时，保持上次的航向角误差
   WP_Sensor.azimuth_err_ang=WP_Sensor.azimuth_err_ang_his;
       return ;
   }
   WP_Sensor.azimuth_err_tan=molecule/denominator;
   WP_Sensor.azimuth_err_ang=atan(WP_Sensor.azimuth_err_tan)*RAD2DEG;//角度范围为(-90,90)
   //到角的范围为0-180度，将经过反正切后求得的到角量化到0-180
   if(WP_Sensor.azimuth_err_ang<0) WP_Sensor.azimuth_err_ang+=180;//到角为顿角

   //1、到角锐角时，存在两种情况
   if(WP_Sensor.azimuth_err_ang>0&&WP_Sensor.azimuth_err_ang<90)//然后根据平面向量夹角为钝角时的条件，A*B<0，即x1*x2+y1+y2<0条件,将角度误差扩展到(-180,180)
   {
       if(x1*x2+y1*y2<0)//顿角
       WP_Sensor.azimuth_err_ang=-180+WP_Sensor.azimuth_err_ang;
   }
   //2、到角顿角时，存在两种情况
   if(WP_Sensor.azimuth_err_ang>90&&WP_Sensor.azimuth_err_ang<180)//然后根据平面向量夹角为钝角时的条件，A*B<0，即x1*x2+y1+y2<0条件,将角度误差扩展到(-180,180)
   {
       if(x1*x2+y1*y2>0)//锐角
       WP_Sensor.azimuth_err_ang=-180+WP_Sensor.azimuth_err_ang;
   }
   WP_Sensor.azimuth_err_ang=constrain_float(WP_Sensor.azimuth_err_ang,-90.0f,90.0f);

   Sensor_State.Azimuth_Health=TRUE;
}