修改ＧＮＳＳ数据解析容错处理

src/localization/localization.cpp

@@ -9,6 +9,7 @@
 #include "coordsconvertion.h"
 #include <localization/localMsg.h>
 #include "../common/message.h"
+#include <iostream>
 
 
 
@@ -20,7 +21,7 @@ std::string Localization::Name() const { return "localization"; }
 int Localization::Init() {
     try
     {
-        LOG(INFO)<<Name()<<" init start ...";
+        LOG(INFO)<<Name()<<" init start ...";       
         serial_port = "/dev/ttyUSB0";
         baudrate = 115200;
         timeout = 1000;
@@ -49,7 +50,7 @@ int Localization::Start() {
      if (IsOpen())
      {
          //指定循环的频率
-         ros::Rate loop_rate(50);
+         ros::Rate loop_rate(2000);
          while(ros::ok())
          {
 
@@ -104,12 +105,14 @@ int Localization::Analyse(const std::string &strSerialResult,  NMEA0183 &nmea){
     LOG(INFO)<<Name()<<" Analyse start.";
     try
     {
-        std::string delim("$");
+        std::string delim("\r\n");
         std::vector<std::string> v_nmeaData = Split(strSerialResult, delim);
+        if (v_nmeaData.size() !=3)
+            return ROBOT_FAILTURE;
         std::vector<std::string>::iterator iter_start = v_nmeaData.begin();
         std::vector<std::string>::iterator iter_end = v_nmeaData.end();
         std::vector<std::string>::iterator iter;
-        for ( iter = iter_start; iter != iter_end; iter++)
+        for (iter =iter_start; iter != iter_end; iter++)
         {
             std::string type = (*iter).substr(1, 5);
             LOG(INFO)<<Name()<<" type:"<<type;
@@ -165,11 +168,6 @@ int Localization::ParaGPRMC(std::string &strGPRMC, GPRMC &gprmc){
         gprmc.Deviated = atof(nmea_gprmc[8].c_str());
         memset(&gprmc.Date, 0x00, 5);
         memcpy(&gprmc.Date, nmea_gprmc[9].c_str(), 4);
-        gprmc.Magnetic = atof(nmea_gprmc[10].c_str());
-        memset(&gprmc.MagneticDirection, 0x00, 2);
-        memcpy(&gprmc.MagneticDirection, nmea_gprmc[11].c_str(), 1);
-        memset(&gprmc.Mode, 0x00, 2);
-        memcpy(&gprmc.Mode, nmea_gprmc[12].c_str(), 1);
     }
     catch (ros::Exception ex)
     {
@@ -197,11 +195,8 @@ int Localization::ParaGPGGA(std::string &strGPGGA, GPGGA &gpgga){
         gpgga.SatelliteNumber = atoi(nmea_gpgga[7].c_str());
         gpgga.Hdop = atof(nmea_gpgga[8].c_str());
         gpgga.Height = atof(nmea_gpgga[9].c_str());
-        gpgga.ReleaveHeight = atoi(nmea_gpgga[10].c_str());
-        gpgga.CTKTime = atof(nmea_gpgga[11].c_str());
-        gpgga.CTKID = atoi(nmea_gpgga[12].c_str());
-        memset(&gpgga.Check, 0x00, 5);
-        memcpy(&gpgga.Check, nmea_gpgga[13].c_str(), 4);
+        memset(&gpgga.HeightUint, 0x00, 2);
+        memcpy(&gpgga.HeightUint, nmea_gpgga[10].c_str(), 1);
     }
     catch(ros::Exception ex)
     {

src/localization/localization.h

@@ -15,10 +15,13 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <sstream>
 #include <serial/serial.h>  //ROS已经内置了的串口包
 #include "../common/robotapp.h"
 #include "ros/ros.h"
 #include <stdlib.h>
+#include <math.h>
+
 #include "localizationdata.h"
 
 /**
@@ -80,7 +83,7 @@ class Localization : public robot::common::RobotApp{
    */
   inline double StrToDouble(const std::string &strResult, int pos)
   {
-      return atof(strResult.substr(0, pos).c_str()) + atof(strResult.substr(pos, strResult.length() - pos).c_str())/60;
+      return atof(strResult.substr(0, pos).c_str()) + atof(strResult.substr(pos, strResult.length() - pos).c_str())/60.0;
   }
 
   //void Nmea0138Analyse( std::string &serial_data, nmea_msgs::Gpgga &gpgga_msg, nmea_msgs::Gprmc);
@@ -100,7 +103,7 @@ private:
 
   uint32_t baudrate = 0;
 
-  uint32_t timeout = 0;
+  uint32_t timeout = 10;
 
   serial::Serial serial_gnss; //声明串口对象
 

src/localization/localizationdata.h

@@ -14,6 +14,7 @@
 
 #include <math.h>
 #include <string>
+#include <iomanip>
 #include "../common/gloghelper.h"
 
 /**
@@ -72,25 +73,25 @@ struct GPRMC
     // <9> UTC日期，ddmmyy（日月年）格式
     char Date[5];
     // <10> 磁偏角（000.0~180.0度，前面的0也将被传输）
-    double Magnetic;
+    //double Magnetic;
     // <11> 磁偏角方向，E（东）或W（西）
-    char MagneticDirection[2];
+    //char MagneticDirection[2];
     // <12> 模式指示（仅NMEA0183 3.00版本输出，A=自主定位，D=差分，E=估算，N=数据无效）
-    char Mode[2];
+    //char Mode[2];
     void print()
     {
         LOG(INFO)<< "utcTime:"<<utcTime;
         LOG(INFO)<< "locationStatus:"<<std::string(locationStatus);
-        LOG(INFO)<< "Latitude:"<<Latitude;
+        LOG(INFO)<< "Latitude:"<<std::setprecision(18)<<Latitude;
         LOG(INFO)<< "LatType:"<<std::string(LatType);
-        LOG(INFO)<< "Longitude:"<<Longitude;
+        LOG(INFO)<< "Longitude:"<<std::setprecision(18)<<Longitude;
         LOG(INFO)<< "LonType:"<<std::string(LonType);
         LOG(INFO)<< "Speed:"<<Speed;
         LOG(INFO)<< "Deviated:"<<Deviated;
         LOG(INFO)<< "Date:"<<std::string(Date);
-        LOG(INFO)<< "Magnetic:"<<Magnetic;
-        LOG(INFO)<< "MagneticDirection:"<<std::string(MagneticDirection);
-        LOG(INFO)<< "Mode:"<<std::string(Mode);
+//        LOG(INFO)<< "Magnetic:"<<Magnetic;
+//        LOG(INFO)<< "MagneticDirection:"<<std::string(MagneticDirection);
+//        LOG(INFO)<< "Mode:"<<std::string(Mode);
     }
 };
 
@@ -113,33 +114,24 @@ struct GPGGA
     char GPSStatus[2];
     // 字段7：正在使用的卫星数量（00 - 12）（前导位数不足则补0）
     int SatelliteNumber;
-    // 字段8：HDOP水平精度因子（0.5 - 99.9）
+    // 字段8：HDOP水平精度因子HDOP=0.0~9.9
     double Hdop;
     // 字段9：海拔高度（-9999.9 - 99999.9）
     double Height;
-    // 字段10：地球椭球面相对大地水准面的高度
-    double  ReleaveHeight;
-    // 字段11：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空）
-    double CTKTime;
-    // 字段12：差分站ID号0000 - 1023（前导位数不足则补0，如果不是差分定位将为空）
-    int  CTKID;
-    // 字段13：校验值
-    char Check[5];
+    // 字段10：海拔高度单位（米）
+    char  HeightUint[2];
     void print()
     {
         LOG(INFO)<< "utcTime:"<<utcTime;
-        LOG(INFO)<< "Latitude:"<<Latitude;
+        LOG(INFO)<< "Latitude:"<<std::setprecision(18)<<Latitude;
         LOG(INFO)<< "LatType:"<<std::string(LatType);
-        LOG(INFO)<< "Longitude:"<<Longitude;
+        LOG(INFO)<< "Longitude:"<<std::setprecision(18)<<Longitude;
         LOG(INFO)<< "LonType:"<<std::string(LonType);
         LOG(INFO)<< "GPSStatus:"<<std::string(GPSStatus);
         LOG(INFO)<< "SatelliteNumber:"<<SatelliteNumber;
         LOG(INFO)<< "Hdop:"<<Hdop;
         LOG(INFO)<< "Height:"<<Height;
-        LOG(INFO)<< "ReleaveHeight:"<<ReleaveHeight;
-        LOG(INFO)<< "CTKTime:"<<CTKTime;
-        LOG(INFO)<< "CTKID:"<<CTKID;
-        LOG(INFO)<< "Check:"<<std::string(Check);
+        LOG(INFO)<< "HeightUint:"<<std::string(HeightUint);
     }
 };