导航算法：

    有了位置、高度、地速、空速、姿态，我们就可以根据这些参数来进行计算，控制飞机给出相应舵面，实现导航、定高的目的。
    最简单的导航算法只考虑当前飞机所在点和目标点。设想一下，飞机在空中飞行时，当前点和目标点的连线指定了飞机应该飞行的航向（即目标航向），如果要使飞机朝向目标点飞行，必须控制飞机转弯，使飞机航向与目标航向一致。

为了准确描述航向，我们定义以正北为0度，顺时针起航向与正北的夹角为正，逆时针起航向与正北的夹角为负，这样将航向定义在-180度到+180度之间。这样，上例中，飞机的航向为90度，目标航向为120度左右。

好了，当前飞机的航向我们可以从GPS航向里提取，我们记其为Hangxiang。下面我们怎么根据目标点和当前点的坐标来计算目标航向呢？我们把目标航向记为beta，于是航向偏差为beta-Hangxiang。

记当前的经纬度为CurLongi、CurLatti，单位为度；目标点经纬度为tgtLongi、tgtLatti，单位为度。由平面几何的知识，我们计算反正切atan((tgtLongi-CurLongi)/(tgtLatti-CurLatti))，再经过象限处理，就可得到目标航向。

在实际程序中，我们是这样做的：

        vectorLatti = tgtLatti - CurLatti;

        vectorLongi = tgtLongi - CurLongi;

        beta = CalAtanVector();          //计算vectorLongi、vectorLatti向量与正北的夹角

   而CalAtanVector（）函数如下：

float CalAtanVector(void) //计算vectorLongi/vectorLatti的反正切函数，返回范围在-PI~PI之间

{
        float beta;

        float lTemp;

        if(vectorLatti > 0)
                bNorth = 1;
        else
                bNorth = 0;

        if(vectorLongi > 0)
                bEast = 1;
        else
                bEast = 0;

        vectorLatti = fabsf(vectorLatti);

        vectorLongi = fabsf(vectorLongi);

        vectorLongi = vectorLongi * COS_FACTOR;

        if(vectorLongi > vectorLatti)

        {
                lTemp = vectorLongi;

                vectorLongi = vectorLatti;

                vectorLatti = lTemp;

                bGT45 = 1;
        }
        else
        {
                bGT45 = 0;
        }
        if (vectorLatti!=0)
                beta= atan(vectorLongi/vectorLatti);           //0~PI/4范围
        else
                beta = PI/2;
        if(bGT45)
                beta = PI/2 - beta;                                        //PI/4~PI/2范围
        if(bNorth && bEast)
                beta = beta;                                                //0~PI/4范围
        if(bNorth && !bEast)
                beta = 0 - beta;                                        //-PI/4~0范围
        if(!bNorth && !bEast)
               beta = beta - PI;                                        //-PI~-3PI/4范围
        if(!bNorth && bEast)
                beta = PI - beta;                                        //3PI/4~PI范围
        return beta;
}

    其中出现了一个因数COS_FACTOR，这是当前纬度的余弦值。这是因为两点经度之差要乘以当前纬度的余弦值，才代表了两点水平距离之差。也就是说，同样的经度差，在赤道附近表征很大的距离，而在北极附近表征的距离就相对较小，这一点大家应该能理解。   

     好了，现在已经得到了alpha = beta – Hangxiang，即为当前航向与目标航向的偏差角，当不需要压航线进行控制的时候，比如进行所点即所到、临时改变航点的时候，就可以用alpha作为PID控制的误差输入量，去计算应该给出的方向舵控制量（方向舵转弯时）或横滚坡度量（副翼转弯时）了。

    当需要压航线进行飞行的时候呢？同样，我们可以根据航线的上一个点（坐标记为tgtLongi_s，tgtLatti_s）和航线的当前点，按照同上的算法计算出当前航线与正北的夹角，记为beta2，如下：

                vectorLatti = tgtLatti - tgtLatti_s;

                vectorLongi = tgtLongi - tgtLongi_s;

                beta2 = CalAtanVector();  //航线与正北的夹角

    然后我们需要再计算一个重要的量——偏航距，即飞机所在点到航线的距离。需要压航线时，飞机需要尽快的回到航线上，而不仅仅是朝着目标点。

    我们用一个newdistance（）函数来计算偏航距。大家可以回忆一下高中数学里的点到直线的距离公式：

float newdistance(void)  //计算当前点与当前航线的距离
{
        float dis,sin_t,cos_t;
        dis = sqrt( (tgtLatti - tgtLatti_s) * (tgtLatti - tgtLatti_s) + (tgtLongi - tgtLongi_s) * (tgtLongi - tgtLongi_s) * COS_FACTOR * COS_FACTOR );
        if (dis!=0)
        {
                 sin_t = (tgtLatti - tgtLatti_s) / dis;
                 cos_t = (tgtLongi - tgtLongi_s) * COS_FACTOR / dis;
        }
        else 
        {
                sin_t = 0;
                cos_t = 0;
        }
        dis = (CurLongi - tgtLongi) * sin_t * COS_FACTOR + (tgtLatti - CurLatti) * cos