首页 » 技术分享 » QGC 谷歌中国地图 火星坐标系 转换

QGC 谷歌中国地图 火星坐标系 转换

　
评论关闭　982次浏览

1，算法

 

static double pi = 3.1415926535897932384626;
static double a = 6378245.0;
static double ee = 0.00669342162296594323;

 

//坐标系转换

 

    static double lonTrans(double x,double y);
    static double latTrans(double x,double y);
    static bool outOfChina(double lat,double lng);
    static void wgs84ToGcj02(double wgs_lat, double wgs_lon,double &gcjLat,double &gcjLon);

        static void gcj02ToWgs84(double gcjLat,double gcjLon,double &wgs_lat, double &wgs_lon);

 

 

double Vehicle::lonTrans(double x, double y)
{
    double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
    ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
    ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;
    return ret;
}
 
double Vehicle::latTrans(double x, double y)
{
    double ret =-100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
    ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
    ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;
    return ret;
}
 
bool Vehicle::outOfChina(double lat,double lng)
{
    if (lng < 72.004 || lng > 137.8347) {
        return true;
    } else if (lat < 0.8293 || lat > 55.8271) {
        return true;
    }
    return false;
}
 
void Vehicle::wgs84ToGcj02(double wgs_lat, double wgs_lon,double &gcjLat,double &gcjLon)
{
    if(!outOfChina(wgs_lat,wgs_lon))
    {
        double dLat = latTrans(wgs_lon - 105.0, wgs_lat - 35.0);
        double dLon = lonTrans(wgs_lon - 105.0, wgs_lat - 35.0);
        double radLat = wgs_lat / 180.0 * pi;
        double magic = sin(radLat);
        magic = 1 - ee * magic * magic;
        double sqrtMagic = sqrt(magic);
        dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
        dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
        gcjLat = wgs_lat + dLat;
        gcjLon = wgs_lon + dLon;
    }else{
        gcjLat=wgs_lat;
        gcjLon = wgs_lon;
    }
}
 
void Vehicle::gcj02ToWgs84(double gcjLat, double gcjLon, double &wgs_lat, double &wgs_lon)
{
    if(!outOfChina(gcjLat,gcjLon))
    {
        double dLat = latTrans(gcjLon - 105.0, gcjLat - 35.0);
        double dLon = lonTrans(gcjLon - 105.0, gcjLat - 35.0);
        double radLat = gcjLat / 180.0 * pi;
        double magic = sin(radLat);
        magic = 1 - ee * magic * magic;
        double sqrtMagic = sqrt(magic);
        dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
        dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
        double tempLat = gcjLat + dLat;
        double tempLon = gcjLon + dLon;
 
        wgs_lat = gcjLat*2-tempLat;
        wgs_lon = gcjLon*2-tempLon;
    }else{
        wgs_lat = gcjLat;
        wgs_lon = gcjLon;
    }
}

 

2，四个 地方需要添加转换

（1），飞机位置

 

//设置飞机的 位置   wgs84->火星坐标系
void Vehicle::setLatitude(double latitude)
{   
    _coordinate.setLatitude(latitude);
 
        //坐标转换
        double gcjLat=0,gcjLon=0;
        wgs84ToGcj02(_coordinate.latitude(),_coordinate.longitude(),gcjLat,gcjLon);
        _coordinate.setLatitude(gcjLat);
 
    emit coordinateChanged(_coordinate);
}
 
void Vehicle::setLongitude(double longitude){
    _coordinate.setLongitude(longitude);
 
        //坐标转换
        double gcjLat=0,gcjLon=0;
        wgs84ToGcj02(_coordinate.latitude(),_coordinate.longitude(),gcjLat,gcjLon);
        _coordinate.setLongitude(gcjLon);
 
    emit coordinateChanged(_coordinate);
}

 

（2），上传任务时

 

void MissionManager::_handleMissionItem(const mavlink_message_t& message)
{
    mavlink_mission_item_t missionItem;
    
    if (!_checkForExpectedAck(AckMissionItem)) {
        return;
    }
    
    mavlink_msg_mission_item_decode(&message, &missionItem);
    
    qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq<<"->> "<<missionItem.x<<" "<<missionItem.y;
    
    if (_itemIndicesToRead.contains(missionItem.seq)) {
        _itemIndicesToRead.removeOne(missionItem.seq);
        //wgs84->火星坐标系
        double gcjLat=0,gcjLon=0;
        Vehicle::wgs84ToGcj02(missionItem.x,missionItem.y,gcjLat,gcjLon);
        qDebug()<<"wgs84->gcj"<<missionItem.x<<missionItem.y<<"  "<<gcjLat<<gcjLon;
        /********************************************/
 
 
        MissionItem* item = new MissionItem(missionItem.seq,
                                            (MAV_CMD)missionItem.command,
                                            (MAV_FRAME)missionItem.frame,
                                            missionItem.param1,
                                            missionItem.param2,
                                            missionItem.param3,
                                            missionItem.param4,
                                            gcjLat/*missionItem.x*/,
                                            gcjLon/*missionItem.y*/,
                                            missionItem.z,
                                            missionItem.autocontinue,
                                            missionItem.current,
                                            this);
 
 
 
        if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
            // Home is in position 0
            item->setParam1((int)item->param1() + 1);
        }
 
        _missionItems.append(item);
    } else {
        qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq;
        // We have to put the ack timeout back since it was removed above
        _startAckTimeout(AckMissionItem);
        return;
    }
    
    _retryCount = 0;
    if (_itemIndicesToRead.count() == 0) {
        _readTransactionComplete();
    } else {
        _requestNextMissionItem();
    }
}

 

（3），下载任务时

 

void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
{
    mavlink_mission_request_t missionRequest;
    
    if (!_checkForExpectedAck(AckMissionRequest)) {
        return;
    }
    
    mavlink_msg_mission_request_decode(&message, &missionRequest);
    
    qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq;
    
    if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
        if (missionRequest.seq > _missionItems.count()) {
            _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
            _finishTransaction(false);
            return;
        } else {
            qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
        }
    } else {
        _itemIndicesToWrite.removeOne(missionRequest.seq);
    }
    
    mavlink_message_t       messageOut;
    mavlink_mission_item_t  missionItem;
    
    MissionItem* item = _missionItems[missionRequest.seq];
    //火星 坐标 ->WGS84
    double gcjLat= item->param5();
    double gcjLon = item->param6();
    double wgsLat=0;
    double wgsLon=0;
 
    Vehicle::gcj02ToWgs84(gcjLat,gcjLon,wgsLat,wgsLon);
    qDebug()<<"gcj->wgs84"<<gcjLat<<gcjLon<<"  "<<wgsLat<<wgsLon;
    /***********************************/
 
    
    missionItem.target_system =     _vehicle->id();
    missionItem.target_component =  MAV_COMP_ID_MISSIONPLANNER;
    missionItem.seq =               missionRequest.seq;
    missionItem.command =           item->command();
    missionItem.param1 =            item->param1();
    missionItem.param2 =            item->param2();
    missionItem.param3 =            item->param3();
    missionItem.param4 =            item->param4();
    missionItem.x =                 wgsLat;//item->param5();
    missionItem.y =                 wgsLon;//item->param6();
    missionItem.z =                 item->param7();
    missionItem.frame =             item->frame();
    missionItem.current =           missionRequest.seq == 0;
    missionItem.autocontinue =      item->autoContinue();
    
    mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                         qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                         _dedicatedLink->mavlinkChannel(),
                                         &messageOut,
                                         &missionItem);
 
    
    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
    _startAckTimeout(AckMissionRequest);
}

 

（4），飞行界面 home点

void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
    bool emitHomePositionChanged =          false;
    bool emitHomePositionAvailableChanged = false;

    mavlink_home_position_t homePos;

    mavlink_msg_home_position_decode(&message, &homePos);

    //坐标转换
    double gcjLat=0,gcjLon=0;
    wgs84ToGcj02(homePos.latitude / 10000000.0,  homePos.longitude / 10000000.0,gcjLat,gcjLon);
    QGeoCoordinate newHomePosition (gcjLat, gcjLon, homePos.altitude / 1000.0);
    /*********************************************************************************/

//    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
//                                    homePos.longitude / 10000000.0,
//                                    homePos.altitude / 1000.0);
    if (!_homePositionAvailable || newHomePosition != _homePosition) {
        emitHomePositionChanged = true;
        _homePosition = newHomePosition;
    }

    if (!_homePositionAvailable) {
        emitHomePositionAvailableChanged = true;
        _homePositionAvailable = true;
    }

    if (emitHomePositionChanged) {
        qCDebug(VehicleLog) << "New home position" << newHomePosition;
        qgcApp()->setLastKnownHomePosition(_homePosition);
        emit homePositionChanged(_homePosition);
    }
    if (emitHomePositionAvailableChanged) {
        emit homePositionAvailableChanged(true);
    }
}

 

 

关注微信，以免错过更多精彩内容：

转载自原文链接, 如需删除请联系管理员。

原文链接：QGC 谷歌中国地图 火星坐标系 转换，转载请注明来源！

赞 0