QGC 自动调整 航区角度

V3.1.3

1，规划任务时，目前QGC 是将所有航线的方向默认设置为正北方向，但是实际使用的时候需要需要根据前两个航点自动调整。

效果如下。

2，方法

（1），求得  多边形 前两个点 与正北方向的夹角

（2），生成多边形时 设置航线角度为 第一步计算的夹角

3，

static double Rc=6378137;

static double Rj= 6356725;

static double PI = 3.1415926535897932384626;

class MyLatLng:public QObject {

    Q_OBJECT

public:

    MyLatLng(double longitude,double latitude)

    {

        m_LoDeg=(int)longitude;

        m_LoMin=(int)((longitude-m_LoDeg)*60);

        m_LoSec=(longitude-m_LoDeg-m_LoMin/60.)*3600;

        m_LaDeg=(int)latitude;

        m_LaMin=(int)((latitude-m_LaDeg)*60);

        m_LaSec=(latitude-m_LaDeg-m_LaMin/60.)*3600;

        m_Longitude=longitude;

        m_Latitude=latitude;

        m_RadLo=longitude*PI/180.;

        m_RadLa=latitude*PI/180.;

        Ec=Rj+(Rc-Rj)*(90.-m_Latitude)/90.;

        Ed=Ec*cos(m_RadLa);

    }

public:

    double m_LoDeg,m_LoMin,m_LoSec;

    double m_LaDeg,m_LaMin,m_LaSec;

    double m_Longitude,m_Latitude;

    double m_RadLo,m_RadLa;

    double Ec;

    double Ed;

};

/**

    * 获连线与正北方向的角度 ,用于 任务规划是 改变区域角度

    * @return  连线与正北方向的角度（0~360）

    */

    Q_INVOKABLE double getAngel(QGeoCoordinate coordinateStart,  QGeoCoordinate coordinateStop);

double VisualMissionItem::getAngel(QGeoCoordinate coordinateStart, QGeoCoordinate coordinateStop)

{

    MyLatLng A(coordinateStart.longitude(),coordinateStart.latitude());

    MyLatLng B(coordinateStop.longitude(),coordinateStop.latitude());

    double dx=(B.m_RadLo-A.m_RadLo)*A.Ed;

    double dy=(B.m_RadLa-A.m_RadLa)*A.Ec;

    double angle=0.0;

    angle=atan(abs(dx/dy))*180./PI;

    double dLo=B.m_Longitude-A.m_Longitude;

    double dLa=B.m_Latitude-A.m_Latitude;

    if(dLo>0&&dLa<=0){

        angle=(90.-angle)+90;

    }

    else if(dLo<=0&&dLa<0){

        angle=angle+180.;

    }else if(dLo<0&&dLa>=0){

        angle= (90.-angle)+270;

    }

    return angle;

}

//改变角度 设置航区角度 为 前两个点 与正北方向的夹角

    QGeoCoordinate coordinateOne = _polygonPath[0].value<QGeoCoordinate>();

    QGeoCoordinate coordinateTwo = _polygonPath[1].value<QGeoCoordinate>();

    double surveyAngle = getAngel(coordinateOne,coordinateTwo);

    qDebug()<<"surveyAngle"<<surveyAngle<<coordinateOne<<coordinateTwo;

    _gridAngleFact.setRawValue(surveyAngle);