change apollolib.add hcp2.

src/apollo/apollolib/hcp2/hcp2.cpp

@@ -40,6 +40,18 @@ static bool checknmeasen(const char * strsen,const unsigned int nlen)
     return false;
 }
 
+static std::vector<std::string> splitByCommaManual(const std::string& input,char strsplit) {
+    std::vector<std::string> tokens;
+    size_t start = 0, end = 0;
+    while ((end = input.find(strsplit, start)) != std::string::npos) {
+        tokens.push_back(input.substr(start, end - start));
+        start = end + 1;  // 跳过 ',' 继续查找
+    }
+    if(start != input.size())
+        tokens.push_back(input.substr(start));  // 添加最后一个子串
+    return tokens;
+}
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
@@ -180,8 +192,7 @@ void hcp2::threadrecv()
         {
             strbuf[len] = 0;
             mstrgpsdata = mstrgpsdata + strbuf;
-//            mstrHCP2.append(strbuf);
-//            SerialGPSDecode();
+            SerialGPSDecode();
         }
         else
         {
@@ -190,3 +201,158 @@ void hcp2::threadrecv()
     }
 
 }
+
+void hcp2::SerialGPSDecode()
+{
+
+    size_t xpos = mstrgpsdata.find('\n');
+    if(xpos!= std::string::npos)
+    {
+
+        std::string xsen = mstrgpsdata.substr(0,xpos+1);
+        SerialGPSDecodeSen(xsen);
+        mstrgpsdata.erase(0,xpos+1);
+        SerialGPSDecode();
+    }
+}
+
+void hcp2::SerialGPSDecodeSen(std::string strsen)
+{
+    std::vector<std::string> strlistrmc;
+    strlistrmc = splitByCommaManual(strsen,'\n');
+
+   if(strlistrmc.size() < 23)return;
+   if(strlistrmc.at(0) != "$GPCHC")return;
+
+   if(!checknmeasen(strsen.data(),strsen.length()))
+   {
+       size_t nPos = strsen.find('$',10);
+       if(nPos != std::string::npos)
+       {
+           std::string strnewsen = strsen.substr(nPos,strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           SerialGPSDecodeSen(strnewsen);
+       }
+       return;
+   }
+
+   double fheading,fLat,fLon,fVel,fPitch,fRoll;
+   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
+   int nsv1,nsv2;
+   int gpsweek,gpstime;
+   int insstate,rtkstate;
+   std::string strx = strlistrmc.at(3);
+   fheading = std::atof(strx.data());
+
+   strx = strlistrmc.at(1);
+   gpsweek = std::atoi(strx.data());
+
+   strx = strlistrmc.at(2);
+   gpstime = std::atoi(strx.data());
+
+   strx = strlistrmc.at(12);
+   fLat = std::atof(strx.data());
+
+   strx  = strlistrmc.at(13);
+   fLon = std::atof(strx.data());
+
+   strx = strlistrmc.at(14);
+   fHgt = std::atof(strx.data());
+
+   double ve,vn,vu;
+   strx = strlistrmc.at(15);
+   ve = std::atof(strx.data());
+
+   strx = strlistrmc.at(16);
+   vn = std::atof(strx.data());
+
+   strx = strlistrmc.at(17);
+   vu = std::atof(strx.data());
+
+//   fVel = sqrt(ve*ve + vn*vn + vu*vu);
+   strx = strlistrmc.at(18);
+   fVel = std::atof(strx.data());
+
+   int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+   if(abs(nnow-mOldTime) >= 1e8)
+   {
+       if(mOldTime == 0)
+       {
+           mfCalc_acc = 0;
+           mfOldVel = fVel;
+           mOldTime = nnow;
+       }
+       else
+       {
+           int64_t timediff = nnow -mOldTime;
+           double ftimediff =  timediff;
+           ftimediff = ftimediff/1e9;
+           mfCalc_acc = (fVel - mfOldVel)/ftimediff;
+           mfOldVel = fVel;
+           mOldTime = nnow;
+       }
+   }
+
+   strx = strlistrmc.at(4);
+   fPitch = std::atof(strx.data());
+
+   strx = strlistrmc.at(5);
+   fRoll = std::atof(strx.data());
+
+   strx = strlistrmc.at(6);
+   gyro_x = std::atof(strx.data());
+
+   strx = strlistrmc.at(7);
+   gyro_y = std::atof(strx.data());
+
+   strx = strlistrmc.at(8);
+   gyro_z = std::atof(strx.data());
+
+   strx = strlistrmc.at(9);
+   acc_x = std::atof(strx.data());
+
+   strx = strlistrmc.at(10);
+   acc_y = std::atof(strx.data());
+
+   strx = strlistrmc.at(11);
+   acc_z = std::atof(strx.data());
+
+   strx = strlistrmc.at(19);
+   nsv1 = std::atoi(strx.data());
+
+   strx = strlistrmc.at(20);
+   nsv2 =std::atoi(strx.data());
+
+   strx = strlistrmc.at(21);
+   char strstate[3];
+   strstate[2] = 0;
+   if(strx.size() >= 2)
+   {
+
+       memcpy(strstate,strx.data(),2);
+  //     qDebug(strstate);
+       char xstate;
+       xstate = strstate[0];
+       rtkstate = 4;
+       int xs;
+       if(xstate == '0')xs = 0;
+       if(xstate == '1')xs = 3;
+       if(xstate == '2')xs = 4;
+       if(xstate == '3')xs = 8;
+       if(xstate == '4')xs = 6;
+       if(xstate == '5')xs = 5;
+       if(xstate == '6')xs = 3;
+       if(xstate == '7')xs = 4;
+       if(xstate == '8')xs = 5;
+       if(xstate == '9')xs = 5;
+
+       rtkstate = xs;
+//       if(mstate == 0)minsstate = 0;
+//       else minsstate = 4;
+
+       xstate = strstate[1];
+       if(xstate == '0')insstate = 3;
+       else insstate = 4;
+       if(rtkstate == 0)insstate = 3;
+   }
+}

src/apollo/apollolib/hcp2/hcp2.h

@@ -6,6 +6,7 @@
 #include <string>
 #include <cstring>
 #include <thread>
+#include <vector>
 
 class  hcp2
 {
@@ -20,8 +21,15 @@ private:
 
     std::string mstrgpsdata;
 
+    int64_t mOldTime = 0;
+    double mfCalc_acc = 0;
+    double mfOldVel = 0;
+
 private:
     void threadrecv();
+
+    void SerialGPSDecode();
+    void SerialGPSDecodeSen(std::string strsen);
 };
 
 #endif // HCP2_H