change driver_gps_hcp2. add tmerc.

src/common/common/math/gnss_coordinate_convert.cpp

@@ -1,5 +1,11 @@
 #include <math/gnss_coordinate_convert.h>
 
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
 void gps2xy(double J4, double K4, double *x, double *y)
 {
     int L4 = (int)((K4 - 1.5) / 3) + 1;
@@ -25,6 +31,19 @@ void gps2xy(double J4, double K4, double *x, double *y)
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -55,6 +74,7 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
 
@@ -115,6 +135,19 @@ void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -150,4 +183,6 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
+
+#endif
 }

src/driver/driver_gps_hcp2/driver_gps_hcp2.pro

@@ -38,3 +38,14 @@ HEADERS += \
 INCLUDEPATH += $$PWD/../../common/common/math
 
 LIBS += -livprotoif
+
+
+
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}
+
+
+

src/driver/driver_gps_hcp2/hcp2.cpp

@@ -5,6 +5,126 @@
 extern iv::Ivlog * givlog;
 extern iv::Ivfault *gfault;
 
+
+
+#include <math.h>
+
+//定义一些常量
+double x_PI = 3.14159265358979324 * 3000.0 / 180.0;
+double PI = 3.1415926535897932384626;
+double a = 6378245.0;
+double ee = 0.00669342162296594323;
+
+bool out_of_china(double lng, double lat);
+double transformlat(double lng, double lat);
+double transformlng(double lng, double lat);
+ /**
+ * 百度坐标系 (BD-09) 与 火星坐标系 (GCJ-02)的转换
+ * 即 百度 转 谷歌、高德
+ * @param bd_lon
+ * @param bd_lat
+ * @returns {*[]}
+ */
+void bd09togcj02(double bd_lon, double bd_lat,double & gg_lng,double & gg_lat) {
+    double x_pi = 3.14159265358979324 * 3000.0 / 180.0;
+    double x = bd_lon - 0.0065;
+    double y = bd_lat - 0.006;
+    double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);
+    double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);
+    gg_lng = z * cos(theta);
+    gg_lat = z * sin(theta);
+}
+/**
+* 火星坐标系 (GCJ-02) 与百度坐标系 (BD-09) 的转换
+* 即谷歌、高德 转 百度
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void  gcj02tobd09(double lng, double lat,double & bd_lng, double & bd_lat) {
+    double z = sqrt(lng * lng + lat * lat) + 0.00002 * sin(lat * x_PI);
+    double theta = atan2(lat, lng) + 0.000003 * cos(lng * x_PI);
+    bd_lng = z * cos(theta) + 0.0065;
+    bd_lat = z * sin(theta) + 0.006;
+}
+/**
+* WGS84转GCj02
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void  wgs84togcj02(double lng, double lat,double & lng_gcj,double & lat_gcj) {
+if (out_of_china(lng, lat)) {
+lng_gcj = lng;
+lat_gcj = lat;
+}
+else {
+double dlat = transformlat(lng - 105.0, lat - 35.0);
+double dlng = transformlng(lng - 105.0, lat - 35.0);
+double radlat = 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);
+dlng = (dlng * 180.0) / (a / sqrtmagic * cos(radlat) * PI);
+double mglat = lat + dlat;
+double mglng = lng + dlng;
+lng_gcj = mglng;
+lat_gcj = mglat;
+
+}
+}
+/**
+* GCJ02 转换为 WGS84
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void gcj02towgs84(double lng, double lat,double & lng_wgs,double & lat_wgs) {
+if (out_of_china(lng, lat)) {
+    lng_wgs = lng;
+    lat_wgs = lat;
+    return ;
+}
+else {
+double dlat = transformlat(lng - 105.0, lat - 35.0);
+double dlng = transformlng(lng - 105.0, lat - 35.0);
+double radlat = 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);
+dlng = (dlng * 180.0) / (a / sqrtmagic * cos(radlat) * PI);
+double mglat = lat + dlat;
+double mglng = lng + dlng;
+lng_wgs = lng * 2 - mglng;
+lat_wgs = lat * 2 - mglat;
+}
+}
+double transformlat(double lng, double lat) {
+double ret = -100.0 + 2.0 * lng + 3.0 * lat + 0.2 * lat * lat + 0.1 * lng * lat + 0.2 * sqrt(abs(lng));
+ret += (20.0 * sin(6.0 * lng * PI) + 20.0 * sin(2.0 * lng * PI)) * 2.0 / 3.0;
+ret += (20.0 * sin(lat * PI) + 40.0 * sin(lat / 3.0 * PI)) * 2.0 / 3.0;
+ret += (160.0 * sin(lat / 12.0 * PI) + 320 * sin(lat * PI / 30.0)) * 2.0 / 3.0;
+return ret;
+}
+double transformlng(double lng, double lat) {
+double ret = 300.0 + lng + 2.0 * lat + 0.1 * lng * lng + 0.1 * lng * lat + 0.1 * sqrt(abs(lng));
+ret += (20.0 * sin(6.0 * lng * PI) + 20.0 * sin(2.0 * lng * PI)) * 2.0 / 3.0;
+ret += (20.0 * sin(lng * PI) + 40.0 * sin(lng / 3.0 * PI)) * 2.0 / 3.0;
+ret += (150.0 * sin(lng / 12.0 * PI) + 300.0 * sin(lng / 30.0 * PI)) * 2.0 / 3.0;
+return ret;
+}
+/**
+* 判断是否在国内，不在国内则不做偏移
+* @param lng
+* @param lat
+* @returns {boolean}
+*/
+bool out_of_china(double lng, double lat) {
+return (lng < 72.004 || lng > 137.8347) || ((lat < 0.8293 || lat > 55.8271) || false);
+}
+
 static bool checknmeasen(const char * strsen,const unsigned int nlen)
 {
     if(nlen< 4)return false;
@@ -44,8 +164,13 @@ static bool checknmeasen(const char * strsen,const unsigned int nlen)
 }
 
 hcp2::hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                         const char * strmsgimu,double fmovex,double fmovey)
+                         const char * strmsgimu,double fmovex,double fmovey,bool bOutGCJ02)
 {
+    mbOutGCJ02 = bOutGCJ02;
+    if(mbOutGCJ02 == true)
+    {
+        std::cout<<" Out GCJ02 Lon Lat. "<<std::endl;
+    }
     mfmovex = fmovex;
     mfmovey = fmovey;
     mTime.start();
@@ -605,8 +730,18 @@ void hcp2::SerialGPSDecodeSen(QString strsen)
    gpsimu.set_ve(ve);
    gpsimu.set_vn(vn);
    gpsimu.set_speed(fVel);
-   gpsimu.set_lat(fLat);
-   gpsimu.set_lon(fLon);
+   if(mbOutGCJ02 == false)
+   {
+    gpsimu.set_lat(fLat);
+    gpsimu.set_lon(fLon);
+   }
+   else
+   {
+       double lon_gcj,lat_gcj;
+       wgs84togcj02(fLon,fLat,lon_gcj,lat_gcj);
+       gpsimu.set_lat(lat_gcj);
+       gpsimu.set_lon(lon_gcj);
+   }
    gpsimu.set_heading(fheading);
    gpsimu.set_state(4);
    gpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());

src/driver/driver_gps_hcp2/hcp2.h

@@ -35,7 +35,7 @@ class hcp2 : public QThread
     Q_OBJECT
 public:
     hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                const char * strmsgimu,double fmovex,double fmovey);
+                const char * strmsgimu,double fmovex,double fmovey,bool bOutGCJ02 = false);
 
 private slots:
     void onTimer();
@@ -51,6 +51,7 @@ private:
     QSerialPort *m_serialPort_GPS;
     int mnNotRecvCount;
     qint64 mnLastOpenTime;
+    bool mbOutGCJ02;
     void ins550dDecode();
 
     QString mstrHCP2;

src/driver/driver_gps_hcp2/main.cpp

@@ -1,15 +1,21 @@
 #include <QCoreApplication>
 
 #include "ivversion.h"
-
 #include "hcp2.h"
 
 
 iv::Ivlog * givlog = nullptr;
 iv::Ivfault *gfault = nullptr;
 
+#include "gnss_coordinate_convert.h"
+
 int main(int argc, char *argv[])
 {
+
+//    double lat = 29.0;
+//    double lon = 139.0;
+//    double xv,yv;
+//    GaussProjCal(lon,lat,&xv,&yv);
     showversion("driver_gps_hcp2");
     QCoreApplication a(argc, argv);
 
@@ -30,9 +36,12 @@ int main(int argc, char *argv[])
     std::string strimumemname = xp.GetParam("msg_imu","hcp2_imu");
     std::string strmovex = xp.GetParam("movefront","0.0");
     std::string strmovey = xp.GetParam("moveleft","0");
+    std::string stroutgcj02 = xp.GetParam("outgcj02","false");
+    bool bOutGCJ02 = false;
+    if(stroutgcj02 == "true")bOutGCJ02 = true;
     double fmovex = atof(strmovex.data());
     double fmovey = atof(strmovey.data());
-    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data(),fmovex,fmovey);
+    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data(),fmovex,fmovey,bOutGCJ02);
     x.start();
     return a.exec();
 }