complete driver_gps_cookooo_f9k. need test.

src/driver/driver_gps_cookoo_f9k/driver_gps_cookoo_f9k.pro

@@ -8,6 +8,7 @@ CONFIG -= app_bundle
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        ../../common/common/math/gnss_coordinate_convert.cpp \
         main.cpp
 
 # Default rules for deployment.
@@ -18,4 +19,18 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 LIBS += -L$$PWD -lcookoosdk
 
 HEADERS += \
+    ../../common/common/math/gnss_coordinate_convert.h \
     cookoo_sdk.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/common/math
+
+LIBS += -livprotoif

src/driver/driver_gps_cookoo_f9k/main.cpp

@@ -20,6 +20,21 @@
 #include <iostream>
 #include "cookoo_sdk.h"
 
+#include <QString>
+#include <QStringList>
+#include "math.h"
+#include "modulecomm.h"
+#include "xmlparam.h"
+#include "gpsimu.pb.h"
+
+void * gpa;
+std::string gstrip = "116.63.46.168";
+int gnport = 8002;
+std::string gstruser = "63ae1792";
+std::string gstrpasswd = "97786";
+std::string gstrmountpoint = "RTCM32GGB";
+
+
 using namespace cookoo;
 
 static const char* str_imu_auto_sw[] =
@@ -377,19 +392,191 @@ static inline uint64_t GetTimeU64()
     return (tv.tv_sec * 1000 + tv.tv_usec / 1000);
 }
 
+QString gstrbuf;
+
+static bool checknmeasen(const char * strsen,const unsigned int nlen)
+{
+    if(nlen< 4)return false;
+
+    int i;
+    char check;
+    int nstarpos = -1;
+    check = strsen[1]^strsen[2];
+    for(i=3;i<static_cast<int>(nlen);i++)
+    {
+        if(strsen[i] == '*')
+        {
+            nstarpos = i;
+            break;
+        }
+        check = check^strsen[i];
+    }
+    if(nstarpos < 0)return false;
+    if(nstarpos >static_cast<int>((nlen -3)))return false;
+    char strcheck[3];
+    int ncheck = check;
+    snprintf(strcheck,3,"%02X",ncheck);
+    char strsencheck[3];
+    strsencheck[2] = 0;
+    strsencheck[0] = strsen[nstarpos + 1];
+    strsencheck[1] = strsen[nstarpos + 2];
+    if(strncmp(strcheck,strsencheck,2) == 0)
+    {
+   //     qDebug("  r sen is %s",strsen);
+        return true;
+    }
+
+    std::cout<<"cal check is "<<strcheck<<", sen check is "<<strsencheck<<std::endl;
+
+//    qDebug("  sen is %s",strsen);
+    return false;
+}
+
+void DecodeGGA(QStringList & strlistrmc)
+{
+    (void)strlistrmc;
+}
+
+
+static double rawtovalue(double & fLatraw)
+{
+    fLatraw = fLatraw/100.0;
+    int Lat_d = (int)(fLatraw);
+    double lat_m = fLatraw - static_cast<double>(Lat_d);
+    lat_m = lat_m *100.0/60.0;
+    return static_cast<double>(Lat_d) + lat_m;
+}
+
+void DecodeRMC(QStringList & strlistrmc)
+{
+   if(strlistrmc.size() < 13)return;
+   double fheading,fLat,fLon,fspeed;
+   double fcal_acc = 0;
+   static int64_t noldtime = std::chrono::system_clock::now().time_since_epoch().count();
+   (void)fheading;
+   (void)fLat;
+   (void)fLon;
+   (void)fspeed;
+   static bool bhaveoldspeed = false;
+   double fspeedold = 0;
+
+   double fLatraw = atof(strlistrmc[3].toLatin1().data());
+   fLat = rawtovalue(fLatraw);
+   double fLonraw = atof(strlistrmc[5].toLatin1().data());
+   fLon = rawtovalue(fLonraw);
+   double fspeedraw = atof(strlistrmc[7].toLatin1().data());
+   fspeed = rawtovalue(fspeedraw) * 0.5144; // m/s
+   double fheadingraw = atof(strlistrmc[8].toLatin1().data());
+   fheading = rawtovalue(fheadingraw);
+
+   static int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+   if(abs((nnow - noldtime)/1e6)>100)
+   {
+       int64_t ndiff = nnow - noldtime;
+       double fdiff = ndiff;
+       fdiff = fdiff/1e9;
+       if(bhaveoldspeed)
+       {
+           fcal_acc = (fspeed - fspeedold)/fdiff;
+       }
+
+       fspeedold = fspeed;
+       bhaveoldspeed = true;
+       noldtime  = nnow;
+   }
+
+   iv::gps::gpsimu gpsimu;
+   gpsimu.set_acc_calc(fcal_acc);
+   gpsimu.set_lat(fLat);
+   gpsimu.set_lon(fLon);
+   gpsimu.set_heading(fheading);
+   gpsimu.set_speed(fspeed);
+   gpsimu.set_vd(0);
+   double fhdg = (90 - fheading)*M_PI/180.0;
+   gpsimu.set_ve(fspeed * cos(fhdg));
+   gpsimu.set_vn(fspeed * sin(fhdg));
+
+   char * strser;
+   bool bser;
+   int nbytesize;
+   nbytesize = gpsimu.ByteSize();
+   strser = new char[nbytesize];
+   bser = gpsimu.SerializeToArray(strser,nbytesize);
+   if(bser)
+       iv::modulecomm::ModuleSendMsg(gpa,strser,nbytesize);
+   else
+   {
+       std::cout<<"gpsimu serialize error."<<std::endl;
+   }
+   delete strser;
+}
+
+void SerialGPSDecodeSen(QString strsen)
+{
+
+
+   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
+   {
+       int nPos = strsen.indexOf('$',10);
+       if(nPos > 0)
+       {
+           QString strnewsen = strsen.right(strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           SerialGPSDecodeSen(strnewsen);
+       }
+       return;
+   }
+
+   QStringList strlistrmc;
+   strlistrmc = strsen.split(",");
+
+   if(strlistrmc.at(0) == "$GNRMC" )DecodeRMC(strlistrmc);
+   if(strlistrmc.at(0) == "$GNGGA" )DecodeGGA(strlistrmc);
+
+
+
+}
+
+
+static void decode()
+{
+    int xpos = gstrbuf.indexOf('\n');
+    if(xpos>=0)
+    {
+
+        QString xsen = gstrbuf.left(xpos+1);
+        SerialGPSDecodeSen(xsen);
+        gstrbuf.remove(0,xpos+1);
+        decode();
+    }
+}
+
+void LoadXML(const char * strpath)
+{
+    iv::xmlparam::Xmlparam xp(strpath);
+    gstrip = xp.GetParam("ntrip_ip","116.63.46.168");
+    gnport = xp.GetParam("ntrip_port",8002);
+    gstruser = xp.GetParam("ntrip_user","adc");
+    gstrpasswd = xp.GetParam("ntrip_passwd","adc");
+    gstrmountpoint = xp.GetParam("ntrip_mountpoint","RTCM32GGB");
+}
+
 int main(int argc, char *argv[])
 {
 
 
 
+    if(argc>1)
+    {
+        LoadXML(argv[1]);
+    }
+    else
+    {
+        LoadXML("./driver_gps_cookoo_f9k.xml");
+    }
 
-    std::string ip = "116.63.46.168";
-    int port = 8002;
-    std::string user = "63ae1792";
-    std::string passwd = "97786";
-    std::string mountpoint = "RTCM32GGB";
 
-    cookoo::Ck_GPS::GetInstance()->Set_Ntrip(ip, port, user, passwd, mountpoint);
+    cookoo::Ck_GPS::GetInstance()->Set_Ntrip(gstrip, gnport, gstruser, gstrpasswd, gstrmountpoint);
 
 
     // IMU数据输出,0-不输出
@@ -412,6 +599,8 @@ int main(int argc, char *argv[])
     char* uds_send_data = new char[4096];
     // 保存的文件名
 
+   gpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",10000,1);
+
     uint64_t last_time = 0;
     uint64_t curr_time = 0;
     uint8_t  imu_stat = 0;
@@ -427,6 +616,8 @@ int main(int argc, char *argv[])
 
             printf("%s", gps_str.data());
         }
+        gstrbuf.append(gps_str.data());
+        decode();
 
 
         // 获取IMU状态, 1秒1次