无名创新开源飞控利用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;
}