C++实现GNSS/GPS的NMEA数据格式解析类示例

nmea数据具体各字段的含义请参考标准定义，这里给出一个C++实现的例子，环境是Android，本文只解析了几个常用的字段；

本示例的关键方法有split、startswith方法和stringToNumber模版函数

  1. bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
  2. {
  3.     int srcpos = 0;
  4.     int srclen = src.length();
  5.     int sublen = str.length();
  6.     if (srclen < sublen) {
  7.         return false;
  8.     }
  9.     return (0 == src.compare(srcpos, sublen, str));
  10. }
  11.  
  12. bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
  13. {
  14.     int srcpos = 0;
  15.     int srclen = src.length();
  16.     int sublen = str.length();
  17.     if (srclen < sublen) {
  18.         return false;
  19.     }
  20.     srcpos = srclen - sublen;
  21.     return (0 == src.compare(srcpos, sublen, str));
  22. }
  1. template <class T>
  2. T stringToNumber(const std::string &sstr)
  3. {
  4.     T number {};
  5.     std::istringstream iss {};
  6.     iss.str(sstr);
  7.     iss >> number; /* can auto remove leading 0 */
  8.     return number;
  9. }
  1. size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
  2. {
  3.     size_t pstart = 0;
  4.     size_t phit = 0;
  5.     std::string sstr;
  6.     size_t length = line.length();
  7.  
  8.     vstr.clear();
  9.     for (;pstart <= length;)
  10.     {
  11.         phit = line.find(delim, pstart);
  12.         if (std::string::npos != phit)
  13.         {
  14.             /* find delim, get substr */
  15.             sstr = line.substr(pstart, phit-pstart);
  16.             vstr.push_back(sstr);
  17.             pstart = phit + delim.size();
  18.         } else {
  19.             /* not find delim, append remaining str and break */
  20.             vstr.push_back(line.substr(pstart));
  21.             break;
  22.         }
  23.     }
  24.     return vstr.size();
  25. }

for Android Gnss V1.0

解析.h头文件

  1. #ifndef HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  2. #define HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  3.  
  4. #include <cstdio>
  5. #include <iostream>
  6. #include <sstream>
  7. #include <bitset>
  8. #include <vector>
  9. #include <map>
  10. #include <queue>
  11.  
  12. #include <hardware/gps.h>
  13.  
  14. #define GNSS_NMEA_LINE_SEP "\n"
  15. #define GNSS_NMEA_ELEMENT_SEP ","
  16. #define GNSS_NMEA_PARSER_VERSION "v1.0-20.0716"
  17.  
  18.  
  19. #if __GNSS_HAL_DEBUG_ON__
  20. /* Flags that indicate information about the satellite */
  21. typedef uint8_t                                 LocGnssSvFlags;
  22. #define GNSS_SV_FLAGS_NONE                      0
  23. #define GNSS_SV_FLAGS_HAS_EPHEMERIS_DATA        (1)
  24. #define GNSS_SV_FLAGS_HAS_ALMANAC_DATA          (2)
  25. #define GNSS_SV_FLAGS_USED_IN_FIX               (4)
  26.  
  27. //hardware/libhardware/include/hardware/gps.h
  28. typedef int64_t GpsUtcTime;
  29. /**
  30.  * Flags that indicate information about the satellite
  31.  */
  32. typedef uint8_t                                 GnssSvFlags;
  33.  
  34. /**
  35.  * Constellation type of GnssSvInfo
  36.  */
  37. typedef uint8_t                         GnssConstellationType;
  38.  
  39. /** Represents a location. */
  40. typedef struct {
  41.     /** set to sizeof(GpsLocation) */
  42.     size_t          size;
  43.     /** Contains GpsLocationFlags bits. */
  44.     uint16_t        flags;
  45.     /** Represents latitude in degrees. */
  46.     double          latitude;
  47.     /** Represents longitude in degrees. */
  48.     double          longitude;
  49.     /**
  50.      * Represents altitude in meters above the WGS 84 reference ellipsoid.
  51.      */
  52.     double          altitude;
  53.     /** Represents speed in meters per second. */
  54.     float           speed;
  55.     /** Represents heading in degrees. */
  56.     float           bearing;
  57.     /** Represents expected accuracy in meters. */
  58.     float           accuracy;
  59.     /** Timestamp for the location fix. */
  60.     GpsUtcTime      timestamp;
  61. } GpsLocation;
  62.  
  63. typedef struct {
  64.     /** set to sizeof(GnssSvInfo) */
  65.     size_t size;
  66.  
  67.     /**
  68.      * Pseudo-random number for the SV, or FCN/OSN number for Glonass. The
  69.      * distinction is made by looking at constellation field. Values should be
  70.      * in the range of:
  71.      *
  72.      * - GPS:     1-32
  73.      * - SBAS:    120-151, 183-192
  74.      * - GLONASS: 1-24, the orbital slot number (OSN), if known.  Or, if not:
  75.      *            93-106, the frequency channel number (FCN) (-7 to +6) offset by + 100
  76.      *            i.e. report an FCN of -7 as 93, FCN of 0 as 100, and FCN of +6 as 106.
  77.      * - QZSS:    193-200
  78.      * - Galileo: 1-36
  79.      * - Beidou:  1-37
  80.      */
  81.     int16_t svid;
  82.  
  83.     /**
  84.      * Defines the constellation of the given SV. Value should be one of those
  85.      * GNSS_CONSTELLATION_* constants
  86.      */
  87.     GnssConstellationType constellation;
  88.  
  89.     /**
  90.      * Carrier-to-noise density in dB-Hz, typically in the range [0, 63].
  91.      * It contains the measured C/N0 value for the signal at the antenna port.
  92.      *
  93.      * This is a mandatory value.
  94.      */
  95.     float c_n0_dbhz;
  96.  
  97.     /** Elevation of SV in degrees. */
  98.     float elevation;
  99.  
  100.     /** Azimuth of SV in degrees. */
  101.     float azimuth;
  102.  
  103.     /**
  104.      * Contains additional data about the given SV. Value should be one of those
  105.      * GNSS_SV_FLAGS_* constants
  106.      */
  107.     GnssSvFlags flags;
  108.  
  109. } GnssSvInfo;
  110.  
  111. /**
  112.  * Legacy struct to represents SV information.
  113.  * Deprecated, to be removed in the next Android release.
  114.  * Use GnssSvInfo instead.
  115.  */
  116. typedef struct {
  117.     /** set to sizeof(GpsSvInfo) */
  118.     size_t          size;
  119.     /** Pseudo-random number for the SV. */
  120.     int     prn;
  121.     /** Signal to noise ratio. */
  122.     float   snr;
  123.     /** Elevation of SV in degrees. */
  124.     float   elevation;
  125.     /** Azimuth of SV in degrees. */
  126.     float   azimuth;
  127. } GpsSvInfo;
  128.  
  129. /**
  130.  * Legacy struct to represents SV status.
  131.  * Deprecated, to be removed in the next Android release.
  132.  * Use GnssSvStatus instead.
  133.  */
  134. typedef struct {
  135.     /** set to sizeof(GpsSvStatus) */
  136.     size_t size;
  137.     int num_svs;
  138.     GpsSvInfo sv_list[GPS_MAX_SVS];
  139.     uint32_t ephemeris_mask;
  140.     uint32_t almanac_mask;
  141.     uint32_t used_in_fix_mask;
  142. } GpsSvStatus;
  143.  
  144. typedef struct {
  145.     /** set to sizeof(GnssSvStatus) */
  146.     size_t size;
  147.     /** Number of GPS SVs currently visible, refers to the SVs stored in sv_list */
  148.     int num_svs;
  149.     /**
  150.      * Pointer to an array of SVs information for all GNSS constellations,
  151.      * except GPS, which is reported using sv_list
  152.      */
  153.     GnssSvInfo gnss_sv_list[GNSS_MAX_SVS];
  154. } GnssSvStatus;
  155. #endif
  156.  
  157.  
  158. //GGA 定位信息
  159. enum eGNSS_GGA_ITEM_T {
  160.     eGGA_Header = 0,
  161.     eGGA_UTCTime = 1,
  162.     eGGA_Latitude = 2,
  163.     eGGA_LatitudeHemi = 3,
  164.     eGGA_Longitude = 4,
  165.     eGGA_LongitudeHemi = 5,
  166.     eGGA_StatusIndicator = 6,
  167.     eGGA_SatellitesCount = 7,
  168.     eGGA_HDOP = 8,
  169.     eGGA_Altitude = 9,
  170.     eGGA_AltitudeUnit = 10,
  171.     eGGA_GeoidHeight = 11,
  172.     eGGA_GeoidHeightUnit = 12,
  173.     eGGA_DiffTemporal = 13,
  174.     eGGA_DiffStationId = 14,
  175.     eGGA_CheckSum = 15,
  176. };
  177.  
  178. //GLL 定位地理信息
  179. enum eGNSS_GLL_ITEM_T {
  180.     eGLL_Header = 0,
  181.     eGLL_Latitude = 1,
  182.     eGLL_LatitudeHemi = 2,
  183.     eGLL_Longitude = 3,
  184.     eGLL_LongitudeHemi = 4,
  185.     eGLL_UTCTime = 5,
  186.     eGLL_FixStatus = 6,
  187.     eGLL_PositioningMode = 7, //optional
  188.     eGLL_CheckSum = 8,
  189. };
  190.  
  191. //GSA 当前卫星信息
  192. enum eGNSS_GSA_ITEM_T {
  193.     eGSA_Header = 0,
  194.     eGSA_Mode = 1,
  195.     eGSA_Type = 2,
  196.     eGSA_PRN1 = 3,
  197.     eGSA_PRN2 = 4,
  198.     eGSA_PRN3 = 5,
  199.     eGSA_PRN4 = 6,
  200.     eGSA_PRN5 = 7,
  201.     eGSA_PRN6 = 8,
  202.     eGSA_PRN7 = 9,
  203.     eGSA_PRN8 = 10,
  204.     eGSA_PRN9 = 11,
  205.     eGSA_PRN10 = 12,
  206.     eGSA_PRN11 = 13,
  207.     eGSA_PRN12 = 14,
  208.     eGSA_PDOP = 15,
  209.     eGSA_HDOP = 16,
  210.     eGSA_VDOP = 17,
  211.     eGSA_CheckSum = 18,
  212. };
  213.  
  214. //GSV 可见卫星信息
  215. enum eGNSS_GSV_ITEM_T {
  216.     eGSV_Header = 0,
  217.     eGSV_ItemCount = 1,
  218.     eGSV_ItemSequence = 2,
  219.     eGSV_SatellitesCount = 3,
  220.     eGSV_PRNCode = 4,
  221.     eGSV_SatelliteElevation = 5,
  222.     eGSV_SatelliteAzimuth = 6,
  223.     eGSV_SignalNoiseRatio = 7,
  224.     eGSV_PRNCode2 = 8,
  225.     eGSV_SatelliteElevation2 = 9,
  226.     eGSV_SatelliteAzimuth2 = 10,
  227.     eGSV_SignalNoiseRatio2 = 11,
  228.     eGSV_PRNCode3 = 12,
  229.     eGSV_SatelliteElevation3 = 13,
  230.     eGSV_SatelliteAzimuth3 = 14,
  231.     eGSV_SignalNoiseRatio3 = 15,
  232.     eGSV_PRNCode4 = 16,
  233.     eGSV_SatelliteElevation4 = 17,
  234.     eGSV_SatelliteAzimuth4 = 18,
  235.     eGSV_SignalNoiseRatio4 = 19,
  236.     eGSV_CheckSum = 20,
  237. };
  238.  
  239. // RMC
  240. enum eGNSS_RMC_ITEM_T {
  241.     eRMC_Header = 0,
  242.     eRMC_UTCTime = 1,
  243.     eRMC_FixStatus = 2,
  244.     eRMC_Latitude = 3,
  245.     eRMC_LatitudeHemi = 4,
  246.     eRMC_Longitude = 5,
  247.     eRMC_LongitudeHemi = 6,
  248.     eRMC_SpeedKnots = 7,
  249.     eRMC_Azimuth = 8,
  250.     eRMC_UTCDate = 9,
  251.     eRMC_MagneticDeclination = 10,
  252.     eRMC_MagneticDeclinationDirection = 11,
  253.     eRMC_PositioningMode = 12, //optional
  254.     eRMC_CheckSum = 13,
  255. };
  256.  
  257. //VTG 地面速度信息
  258. enum eGNSS_VTG_ITEM_T {
  259.     eVTG_Header = 0,
  260.     eVTG_MovementAngle = 1,
  261.     eVTG_TrueNorthRef = 2,
  262.     eVTG_MovementAngle2 = 3,
  263.     eVTG_MagneticNorthRef = 4,
  264.     eVTG_HorizontalMoveSpeed = 5,
  265.     eVTG_SpeedKnots = 6,
  266.     eVTG_HorizontalMoveSpeed2 = 7,
  267.     eVTG_SpeedKmh = 8,
  268.     eVTG_PositioningMode = 9, //optional
  269.     eVTG_CheckSum = 10,
  270. };
  271.  
  272.  
  273. //(GGA)GPS定位信息 Global Positioning System Fix Data
  274. //$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*7B
  275. struct XXGGA_Info_T {
  276.     double UTCTime;    //UTC时间,格式为hhmmss.sss
  277.     double Latitude;   //纬度,格式为ddmm.mmmm
  278.     char LatitudeHemi; //纬度半球,N或S
  279.     double Longitude;  //经度,格式为dddmm.mmmm
  280.     char LongitudeHemi;//经度半球,E或W
  281.     uint16_t StatusIndicator;//GPS状态:0初始化,1单点定位,2码差分,3无效PPS,4固定解,5浮点解,6正在估算,7人工输入固定值,8模拟模式,9WAAS差分
  282.     size_t SatellitesCount;  //使用卫星数量,从00到12
  283.     float HDOP;        //HDOP-水平精度因子,0.5到99.9,一般认为HDOP越小,质量越好
  284.     double Altitude;   //椭球高,-9999.9到9999.9米
  285.     char AltitudeUnit; //M指单位米
  286.     float GeoidHeight; //大地水准面高度异常差值,-9999.9到9999.9米
  287.     char GeoidHeightUnit; //M指单位米
  288.     int64_t DiffTemporal; //差分GPS数据期限(RTCM SC-104),最后设立RTCM传送的秒数量,如不是差分定位则为空
  289.     int16_t DiffStationId;//差分参考基站标号,从0000到1023
  290.     //char CheckSum[4]; //从$开始到*之间的所有ASCII码的异或校验
  291.     std::bitset<16> BitFlags;
  292. };
  293.  
  294. //(GLL)定位地理信息 Geographic Position
  295. struct XXGLL_Info_T {
  296.     double Latitude;   //纬度ddmm.mmmm(度分)格式
  297.     char LatitudeHemi; //纬度半球N或S
  298.     double Longitude;  //经度dddmm.mmmm(度分)格式
  299.     char LongitudeHemi;//经度半球E或W
  300.     int UTCTimeInt;       //UTC时间,hhmmss(时分秒)格式
  301.     char FixStatus;    //定位状态,A=有效定位,V=无效定位
  302.     char PositioningMode;  //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  303.     //char CheckSum[4];
  304.     std::bitset<16> BitFlags;
  305. };
  306.  
  307. //GSA 当前卫星信息 GPS DOP and Active Satellites (GSA)当前卫星信息
  308. #define GSA_INFO_PRN_CNT 12
  309. struct XXGSA_Info_T {
  310.     char Mode;     //定位模式,A=自动手动2D/3D,M=手动2D/3D
  311.     uint16_t Type; //定位类型,1=未定位,2=2D定位,3=3D定位
  312.     int PRNList[GSA_INFO_PRN_CNT]; //PRN码-伪随机噪声码,第x信道正在使用的卫星PRN码编号
  313.     float PDOP; //accuracy PDOP综合位置精度因子,0.5-99.9
  314.     float HDOP; //HDOP水平精度因子0.5-99.9
  315.     float VDOP; //VDOP垂直精度因子0.5-99.9
  316.     //char CheckSum[4];
  317.     std::bitset<32> BitFlags;
  318. };
  319.  
  320. //(GSV)可见卫星信息 可见卫星信息 GPS Satellites in View
  321. struct XXGSV_Info_T {
  322.     uint8_t Ext_constellation;  //UNKNOWN==0/GPS==1/SBAS==2/GLONASS==3/QZSS==4/BEIDOU==5/GALILEO==6
  323.     size_t ItemCount;    //GSV语句的总数
  324.     int ItemSequence;    //本句GSV的编号
  325.     int SatellitesCount; //可见卫星的总数,00~12,前面的0也将被传输
  326.     int PRNCode;             //PRN码-伪随机噪声码,前面的0也将被传输;GPS:1-32,Beidou:1-37,GLONASS:1-24,Galileo:1-36,...
  327.     float SatelliteElevation;//卫星仰角,00~90度,前面的0也将被传输
  328.     float SatelliteAzimuth;  //卫星方位角,000~359度,前面的0也将被传输
  329.     float SignalNoiseRatio;  //信噪比,00~99dB,没有跟踪到卫星时为空,前面的0也将被传输
  330.     int PRNCode2; //按照每颗卫星进行循环显示,每条GSV语句最多可以显示4颗卫星的信息
  331.     float SatelliteElevation2;
  332.     float SatelliteAzimuth2;
  333.     float SignalNoiseRatio2;
  334.     int PRNCode3;
  335.     float SatelliteElevation3;
  336.     float SatelliteAzimuth3;
  337.     float SignalNoiseRatio3;
  338.     int PRNCode4;
  339.     float SatelliteElevation4;
  340.     float SatelliteAzimuth4;
  341.     float SignalNoiseRatio4;
  342.     //char CheckSum[4];
  343.     std::bitset<32> BitFlags;
  344. };
  345.  
  346. // (RMC)推荐定位信息 Recommended Minimum Specific GPS/TRANSIT Data
  347. //$GPRMC,200808.000,A,3114.4591,N,12118.0993,E,0.82,282.15,191220,,,A*61
  348. struct XXRMC_Info_T {
  349.     double UTCTime;             //UTC时间,hhmmss.sss格式,格林尼治时间;
  350.     char FixStatus;             //状态,A=定位,V=未定位;当给GPS复位时为V,不输出速度,角度,时间数据;
  351.     double Latitude;            //纬度ddmm.mmmm,度分格式
  352.     char LatitudeHemi;          //纬度N或S
  353.     double Longitude;           //经度dddmm.mmmm,度分格式
  354.     char LongitudeHemi;         //经度E或W
  355.     float SpeedKnots;           //速度,节,Knots
  356.     float Azimuth;              //方位角,度 Azimuth/bearing
  357.     int UTCDate;                //UTC日期,DDMMYY格式
  358.     float MagneticDeclination;  //磁偏角,000-180度
  359.     char MagneticDeclinationDirection;//磁偏角方向,E或W
  360.     char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  361.     //char CheckSum[4];
  362.     std::bitset<16> BitFlags;
  363. };
  364.  
  365. //(VTG)地面速度信息 Track Made Good and Ground Speed
  366. struct XXVTG_Info_T {
  367.     float MovementAngleTN;    //以真北为参考基准的地面航向,000~359度,前面的0也将被传输
  368.     char TrueNorthRef;      //T
  369.     float MovementAngleMN;   //以磁北为参考基准的地面航向,000~359度,前面的0也将被传输
  370.     char MagneticNorthRef;      //M
  371.     float HorizontalMoveSpeedKn;  //地面速率,000.0~999.9节,前面的0也将被传输
  372.     char SpeedKnots;            //N
  373.     float HorizontalMoveSpeedKm; //地面速率,0000.0~1851.8Km/h,前面的0也将被传输
  374.     char SpeedKmh;              //K
  375.     char PositioningMode;  //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  376.     //char CheckSum[4];
  377.     std::bitset<16> BitFlags;
  378. };
  379.  
  380. template <class T>
  381. T stringToNumber(const std::string &sstr)
  382. {
  383.     T number {};
  384.     std::istringstream iss {};
  385.     iss.str(sstr);
  386.     iss >> number; /* can auto remove leading 0 */
  387.     return number;
  388. }
  389.  
  390. template <class T>
  391. std::string toString(T &value)
  392. {
  393.     std::ostringstream oss {};
  394.     oss << value;
  395.     return oss.str();
  396. }
  397.  
  398. template <int BS_MAX_SIZE>
  399. void bitsFlagSet(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
  400. {
  401.     bs.set(pos);
  402. }
  403.  
  404. template <int BS_MAX_SIZE>
  405. void bitsFlagReset(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
  406. {
  407.     bs.reset(pos);
  408. }
  409.  
  410. template <int BS_MAX_SIZE>
  411. void bitsFlagClear(std::bitset<BS_MAX_SIZE> &bs)
  412. {
  413.     bs.reset();
  414. }
  415.  
  416. template <int BS_MAX_SIZE>
  417. bool bitsFlagTest(std::bitset<BS_MAX_SIZE> bs, size_t pos)
  418. {
  419.     return bs.test(pos);
  420. }
  421.  
  422. template <int BS_MAX_SIZE>
  423. std::string bitsFlagToString(std::bitset<BS_MAX_SIZE> &bs)
  424. {
  425.     return bs.to_string();
  426. }
  427.  
  428. class GnssNmeaParser
  429. {
  430. public:
  431.     GnssNmeaParser();
  432.     ~GnssNmeaParser();
  433.     int parse(const std::string &nmea);
  434.     int getNmeaLines(std::vector<std::string> &lines);
  435.     bool getGnssLocation(GpsLocation &gnssLocation);
  436.     bool getGnssSvStatus(GnssSvStatus &gnssSvStatus);
  437.     GpsUtcTime getUtcTime();
  438.     //bool getGpsSvStatus(GpsSvStatus &gpsSvStatus);
  439.     //bool getGpsStatus(GpsStatus &gpsStatus);
  440.  
  441. private:
  442.     int procGGA(XXGGA_Info_T &gga);
  443.     int procGLL(XXGLL_Info_T &gll);
  444.     int procGSA(std::vector<XXGSA_Info_T> &gsaVect);
  445.     int procGSV(std::vector<XXGSV_Info_T> &gsvVect);
  446.     int procRMC(XXRMC_Info_T &rmc);
  447.     int procVTG(XXVTG_Info_T &vtg);
  448.  
  449.     /* convert latitude(ddmm.mmmm) and longitude(dddmm.mmmm) to degrees */
  450.     double latLongToDegree(const double dddmm_mmmm);
  451.     int gnssSvFlagUsedInFix(const int svid);
  452.     void updateUtcTime(const int ddmmyy, const double hhmmss_sss);
  453.     void updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi);
  454.     void updateAltitude(const double altitude);
  455.     void updateBearing(const float bearing);
  456.     void updateMagDec(const float magDec, const char magDecDir);
  457.     void updateAccuracy(const float pdop, const float hdop, const float vdop);
  458.     void updateSpeed(const float speed);
  459.     void updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT);
  460.     void removeChecksum(std::string &str);
  461.     uint8_t getNConstellation(const std::string &nmeaHead);
  462.     bool startsWith(const std::string &src, const std::string &str);
  463.     bool endsWith(const std::string &src, const std::string &str);
  464.     std::string replace(const std::string &raw, const std::string &oldstr, const std::string &newstr);
  465.     size_t split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr);
  466.     void reset();
  467.  
  468.     std::vector<std::string> m_nmeaLines;
  469.     std::string m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
  470.     std::string m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
  471.  
  472.     std::vector<std::string> m_nmeaGGAvect;
  473.     std::vector<std::string> m_nmeaGLLvect;
  474.     std::vector<std::vector<std::string>> m_nmeaGSAvec2d;
  475.     std::vector<std::vector<std::string>> m_nmeaGSVvec2d;
  476.     std::vector<std::string> m_nmeaRMCvect;
  477.     std::vector<std::string> m_nmeaVTGvect;
  478.  
  479.     XXGGA_Info_T m_ggaInfoT;
  480.     XXGLL_Info_T m_gllInfoT;
  481.     std::vector<XXGSA_Info_T> m_gsaVectInfoT;
  482.     std::vector<XXGSV_Info_T> m_gsvVectInfoT;
  483.     XXRMC_Info_T m_rmcInfoT;
  484.     XXVTG_Info_T m_vtgInfoT;
  485.  
  486.     GpsUtcTime m_gnssUtcTime;
  487.     GpsLocation m_gnssLocation;
  488.     GnssSvStatus m_gnssSvStatus;
  489.  
  490.     GpsStatus m_gpsStatus;
  491. };
  492.  
  493. #endif  // HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  494.

解析.cpp实现文件

  1. #include <log/log.h>
  2. #include "GnssNmeaParser.h"
  3.  
  4. #ifdef LOG_TAG
  5. #undef LOG_TAG
  6. #endif
  7. #define LOG_TAG  "GnssNmeaParserV1.0"
  8.  
  9. GnssNmeaParser::GnssNmeaParser()
  10. {
  11.     m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
  12.     m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
  13.     ALOGI("GnssNmeaParser created. %s", GNSS_NMEA_PARSER_VERSION);
  14.     reset();
  15. }
  16.  
  17. GnssNmeaParser::~GnssNmeaParser()
  18. {
  19.     reset();
  20. }
  21.  
  22. int GnssNmeaParser::parse(const std::string &nmea)
  23. {
  24.     reset();
  25.     int gpCount = 0;
  26.     if (0 == nmea.size()) {
  27.         return 0;
  28.     }
  29.     (void)split(nmea, m_nmeaLineSep, m_nmeaLines);
  30.  
  31.     std::vector<std::string>::iterator vsit;
  32.     std::vector<std::string> nmeaGSAvect;
  33.     std::vector<std::string> nmeaGSVvect;
  34.     std::string line;
  35.     for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
  36.         line = *vsit;
  37.         if (line.size() <= 6) {
  38.             //$GPxxx
  39.             continue;
  40.         }
  41.         removeChecksum(line);
  42.         gpCount += 1;
  43.  
  44.         if (startsWith(line, "$GPGGA")) { //GGA
  45.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  46.         } else if (startsWith(line, "$GLGGA")) {
  47.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  48.         } else if (startsWith(line, "$BDGGA")) {
  49.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  50.         } else if (startsWith(line, "$GNGGA")) {
  51.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  52.         }
  53.  
  54.         else if (startsWith(line, "$GPGLL")) { //GLL
  55.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  56.         } else if (startsWith(line, "$GLGLL")) {
  57.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  58.         } else if (startsWith(line, "$BDGLL")) {
  59.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  60.         } else if (startsWith(line, "$GNGLL")) {
  61.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  62.         }
  63.  
  64.         else if (startsWith(line, "$GPGSA")) { //GSA
  65.             // may contain multi-line
  66.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  67.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  68.         } else if (startsWith(line, "$GLGSA")) {
  69.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  70.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  71.         } else if (startsWith(line, "$BDGSA")) {
  72.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  73.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  74.         } else if (startsWith(line, "$GNGSA")) {
  75.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  76.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  77.         }
  78.  
  79.         else if (startsWith(line, "$GPGSV")) { //GSV
  80.             // may contain multi-line
  81.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  82.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  83.         } else if (startsWith(line, "$GLGSV")) {
  84.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  85.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  86.         } else if (startsWith(line, "$BDGSV")) {
  87.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  88.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  89.         }
  90.  
  91.         else if (startsWith(line, "$GPRMC")) { //RMC
  92.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  93.         } else if (startsWith(line, "$GLRMC")) {
  94.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  95.         } else if (startsWith(line, "$BDRMC")) {
  96.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  97.         } else if (startsWith(line, "$GNRMC")) {
  98.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  99.         }
  100.  
  101.         else if (startsWith(line, "$GPVTG")) { //VTG
  102.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  103.         } else if (startsWith(line, "$GLVTG")) {
  104.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  105.         } else if (startsWith(line, "$BDVTG")) {
  106.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  107.         } else if (startsWith(line, "$GNVTG")) {
  108.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  109.         }
  110.  
  111.         else {
  112.             ALOGD("unkown line:%s", line.c_str());
  113.         }
  114.     }
  115.  
  116.     (void)procGGA(m_ggaInfoT);
  117.     (void)procGLL(m_gllInfoT);
  118.     (void)procGSA(m_gsaVectInfoT);
  119.     (void)procGSV(m_gsvVectInfoT);
  120.     (void)procRMC(m_rmcInfoT);
  121.     (void)procVTG(m_vtgInfoT);
  122.     m_gnssLocation.size = sizeof(m_gnssLocation);
  123.     m_gnssSvStatus.size = sizeof(m_gnssSvStatus);
  124.     return gpCount;
  125. }
  126.  
  127. int GnssNmeaParser::getNmeaLines(std::vector<std::string> &lines)
  128. {
  129.     std::vector<std::string>::iterator vsit;
  130.     for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
  131.         lines.push_back(*vsit+m_nmeaLineSep);
  132.     }
  133.     return lines.size();
  134. }
  135.  
  136. int GnssNmeaParser::procGGA(XXGGA_Info_T &gga)
  137. {
  138.     size_t vecSize = m_nmeaGGAvect.size();
  139.     if (vecSize != eGGA_CheckSum) {
  140.         ALOGD("%s invalid vector size:%zu, expected(%d)",
  141.                 __func__, vecSize, eGGA_CheckSum);
  142.         return 0;
  143.     }
  144.     bitsFlagClear<16>(gga.BitFlags);
  145.  
  146.     for (int i = 1; i < (int)vecSize; i++)
  147.     {
  148.         if (m_nmeaGGAvect[i].length() != 0)
  149.         {
  150.             bitsFlagSet<16>(gga.BitFlags, i);
  151.         }
  152.     }
  153.     gga.UTCTime = stringToNumber<double>(m_nmeaGGAvect[eGGA_UTCTime]);
  154.     gga.Latitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Latitude]);
  155.     gga.LatitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LatitudeHemi]);
  156.     gga.Longitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Longitude]);
  157.     gga.LongitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LongitudeHemi]);
  158.     gga.StatusIndicator = stringToNumber<uint16_t>(m_nmeaGGAvect[eGGA_StatusIndicator]);
  159.     gga.SatellitesCount = stringToNumber<size_t>(m_nmeaGGAvect[eGGA_SatellitesCount]);
  160.     gga.HDOP = stringToNumber<float>(m_nmeaGGAvect[eGGA_HDOP]);
  161.     gga.Altitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Altitude]);
  162.     gga.AltitudeUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_AltitudeUnit]);
  163.     gga.GeoidHeight = stringToNumber<float>(m_nmeaGGAvect[eGGA_GeoidHeight]);
  164.     gga.GeoidHeightUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
  165.     gga.DiffTemporal = stringToNumber<long>(m_nmeaGGAvect[eGGA_DiffTemporal]);
  166.     gga.DiffStationId = stringToNumber<int>(m_nmeaGGAvect[eGGA_DiffStationId]);
  167.  
  168.     m_gpsStatus.size = sizeof(m_gpsStatus);
  169.     if (gga.StatusIndicator != 0) {
  170.         updateLatLong(gga.Latitude, gga.LatitudeHemi, gga.Longitude, gga.LongitudeHemi);
  171.         updateAltitude(gga.Altitude);
  172.         updateAccuracy(0.0f, gga.HDOP, 0.0f);
  173.     }
  174.     return m_nmeaGGAvect.size();
  175. }
  176.  
  177. int GnssNmeaParser::procGLL(XXGLL_Info_T &gll)
  178. {
  179.     size_t vecSize = m_nmeaGLLvect.size();
  180.     if ((vecSize != eGLL_CheckSum)
  181.         && (vecSize != eGLL_CheckSum-1)) {
  182.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  183.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  184.                 __func__, vecSize, eGLL_CheckSum, eGLL_CheckSum-1);
  185.         return 0;
  186.     }
  187.     bitsFlagClear<16>(gll.BitFlags);
  188.     for (int i = 1; i < (int)vecSize; i++)
  189.     {
  190.         if (m_nmeaGLLvect[i].length() != 0)
  191.         {
  192.             bitsFlagSet<16>(gll.BitFlags, i);
  193.         }
  194.     }
  195.     gll.Latitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Latitude]);
  196.     gll.LatitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LatitudeHemi]);
  197.     gll.Longitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Longitude]);
  198.     gll.LongitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LongitudeHemi]);
  199.     gll.UTCTimeInt = stringToNumber<int>(m_nmeaGLLvect[eGLL_UTCTime]);
  200.     gll.FixStatus = stringToNumber<char>(m_nmeaGLLvect[eGLL_FixStatus]);
  201.     if (vecSize == eGLL_CheckSum) {
  202.         gll.PositioningMode = stringToNumber<char>(m_nmeaGLLvect[eGLL_PositioningMode]);//optional
  203.     }
  204.  
  205.     if ((gll.FixStatus == 'A') && (gll.PositioningMode != 'N')) {
  206.         updateLatLong(gll.Latitude, gll.LatitudeHemi, gll.Longitude, gll.LongitudeHemi);
  207.     }
  208.     return m_nmeaGLLvect.size();
  209. }
  210.  
  211. int GnssNmeaParser::procGSA(std::vector<XXGSA_Info_T> &gsaVect)
  212. {
  213.     if (m_nmeaGSAvec2d.size() == 0) {
  214.         ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
  215.         return 0;
  216.     }
  217.     std::vector<std::vector<std::string>>::iterator vvsit;
  218.     std::vector<std::string> nmeaGSAvect {};
  219.     XXGSA_Info_T gsa {};
  220.     int prnSn = 0;
  221.     size_t vecSize = 0;
  222.     std::string nmeaHeader;
  223.     for (vvsit = m_nmeaGSAvec2d.begin(); vvsit != m_nmeaGSAvec2d.end(); vvsit++) {
  224.         nmeaGSAvect = *vvsit;
  225.         (void)memset((void *)&gsa, 0, sizeof(gsa));
  226.         bitsFlagClear<32>(gsa.BitFlags);
  227.         vecSize = nmeaGSAvect.size();
  228.  
  229.         if (vecSize != eGSA_CheckSum) {
  230.             ALOGI("%s invalid vector size:%zu, expected(%d)",
  231.                     __func__, vecSize, eGSA_CheckSum);
  232.             continue;
  233.         }
  234.         for (int i = 1; i < (int)vecSize; i++)
  235.         {
  236.             if (nmeaGSAvect[i].length() != 0)
  237.             {
  238.                 bitsFlagSet<32>(gsa.BitFlags, i);
  239.             }
  240.         }
  241.  
  242.         nmeaHeader = (nmeaGSAvect[eGSA_Header]);
  243.  
  244.         gsa.Mode = stringToNumber<char>(nmeaGSAvect[eGSA_Mode]);
  245.         gsa.Type = stringToNumber<uint16_t>(nmeaGSAvect[eGSA_Type]);
  246.         for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
  247.             gsa.PRNList[prnSn] = stringToNumber<int>(nmeaGSAvect[eGSA_PRN1+prnSn]);;
  248.         }
  249.         gsa.PDOP = stringToNumber<int>(nmeaGSAvect[eGSA_PDOP]);
  250.         gsa.HDOP = stringToNumber<int>(nmeaGSAvect[eGSA_HDOP]);
  251.         gsa.VDOP = stringToNumber<int>(nmeaGSAvect[eGSA_VDOP]);
  252.         gsaVect.push_back(gsa);
  253.  
  254.         if ((gsa.Type <= 1)) {
  255.             ALOGD("%s Positioning Mode is %hu(1-untargeted,2-2D,3-3D)", __func__, gsa.Type);
  256.         } else {
  257.             updateAccuracy(gsa.PDOP, gsa.HDOP, gsa.VDOP);
  258.         }
  259.     }
  260.     return m_nmeaGSAvec2d.size();
  261. }
  262.  
  263. int GnssNmeaParser::procGSV(std::vector<XXGSV_Info_T> &gsvVect)
  264. {
  265.     if (m_nmeaGSVvec2d.size() == 0) {
  266.         ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
  267.         return 0;
  268.     }
  269.     std::vector<std::vector<std::string>>::iterator vvsit;
  270.     std::vector<std::string> nmeaGSVvect {};
  271.     XXGSV_Info_T gsv {};
  272.     std::string nmeaHeader;
  273.     size_t vecSize = 0;
  274.     for (vvsit = m_nmeaGSVvec2d.begin(); vvsit != m_nmeaGSVvec2d.end(); vvsit++) {
  275.         nmeaGSVvect = *vvsit;
  276.         (void)memset((void *)&gsv, 0, sizeof(gsv));
  277.         bitsFlagClear<32>(gsv.BitFlags);
  278.         vecSize = nmeaGSVvect.size();
  279.  
  280.         if ((0 != (vecSize % 4)) || (vecSize < eGSV_PRNCode)) {
  281.             ALOGI("%s invalid vector size:%zu, expected(8/12/16/20)", __func__, vecSize);
  282.             continue;
  283.         }
  284.         for (int i = 1; i < (int)vecSize; i++)
  285.         {
  286.             if (nmeaGSVvect[i].length() != 0)
  287.             {
  288.                 bitsFlagSet<32>(gsv.BitFlags, i);
  289.             }
  290.         }
  291.         nmeaHeader = nmeaGSVvect[eGSV_Header];
  292.         gsv.Ext_constellation = getNConstellation(nmeaHeader);
  293.  
  294.         gsv.ItemCount = stringToNumber<size_t>(nmeaGSVvect[eGSV_ItemCount]);
  295.         gsv.ItemSequence = stringToNumber<int>(nmeaGSVvect[eGSV_ItemSequence]);
  296.         gsv.SatellitesCount = stringToNumber<int>(nmeaGSVvect[eGSV_SatellitesCount]);
  297.  
  298.         /* gsv slices count maybe 8/12/16/20 */
  299.         if (vecSize >= eGSV_PRNCode2) {
  300.             gsv.PRNCode = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode]);
  301.             gsv.SatelliteElevation = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation]);
  302.             gsv.SatelliteAzimuth = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth]);
  303.             gsv.SignalNoiseRatio = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio]);
  304.         }
  305.         if (vecSize >= eGSV_PRNCode3) {
  306.             gsv.PRNCode2 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode2]);
  307.             gsv.SatelliteElevation2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation2]);
  308.             gsv.SatelliteAzimuth2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
  309.             gsv.SignalNoiseRatio2 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
  310.         }
  311.         if (vecSize >= eGSV_PRNCode4) {
  312.             gsv.PRNCode3 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode3]);
  313.             gsv.SatelliteElevation3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation3]);
  314.             gsv.SatelliteAzimuth3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
  315.             gsv.SignalNoiseRatio3 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
  316.         }
  317.         if (vecSize == eGSV_CheckSum) {
  318.             gsv.PRNCode4 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode4]);
  319.             gsv.SatelliteElevation4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation4]);
  320.             gsv.SatelliteAzimuth4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
  321.             gsv.SignalNoiseRatio4 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio4]);
  322.         }
  323.         gsvVect.push_back(gsv);
  324.     }
  325.  
  326.     updateGnssSvStatus(gsvVect);
  327.     return m_nmeaGSVvec2d.size();
  328. }
  329.  
  330. int GnssNmeaParser::procRMC(XXRMC_Info_T &rmc)
  331. {
  332.     size_t vecSize = m_nmeaRMCvect.size();
  333.     if ((vecSize != eRMC_CheckSum)
  334.         && (vecSize != eRMC_CheckSum-1)) {
  335.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  336.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  337.                 __func__, vecSize, eRMC_CheckSum, eRMC_CheckSum-1);
  338.         return 0;
  339.     }
  340.     bitsFlagClear<16>(rmc.BitFlags);
  341.     for (int i = 1; i < (int)vecSize; i++)
  342.     {
  343.         if (m_nmeaRMCvect[i].length() != 0)
  344.         {
  345.             bitsFlagSet<16>(rmc.BitFlags, i);
  346.         }
  347.     }
  348.     rmc.UTCTime = stringToNumber<double>(m_nmeaRMCvect[eRMC_UTCTime]);
  349.     rmc.FixStatus = stringToNumber<char>(m_nmeaRMCvect[eRMC_FixStatus]);
  350.     rmc.Latitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Latitude]);
  351.     rmc.LatitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LatitudeHemi]);
  352.     rmc.Longitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Longitude]);
  353.     rmc.LongitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LongitudeHemi]);
  354.     rmc.SpeedKnots = stringToNumber<float>(m_nmeaRMCvect[eRMC_SpeedKnots]);
  355.     rmc.Azimuth = stringToNumber<float>(m_nmeaRMCvect[eRMC_Azimuth]);
  356.     rmc.UTCDate = stringToNumber<int>(m_nmeaRMCvect[eRMC_UTCDate]);
  357.     rmc.MagneticDeclination = stringToNumber<float>(m_nmeaRMCvect[eRMC_MagneticDeclination]);
  358.     rmc.MagneticDeclinationDirection = stringToNumber<char>(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
  359.     if (vecSize == eRMC_CheckSum) {
  360.         rmc.PositioningMode = stringToNumber<char>(m_nmeaRMCvect[eRMC_PositioningMode]);//optional
  361.     }
  362.  
  363.     if (rmc.FixStatus == 'A') {
  364.         updateUtcTime(rmc.UTCDate, rmc.UTCTime);
  365.         updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
  366.         updateBearing(rmc.Azimuth);
  367.         updateSpeed((1.852f * rmc.SpeedKnots / 3.6f));
  368.         updateMagDec(rmc.MagneticDeclination, rmc.MagneticDeclinationDirection);
  369.     } else if (rmc.FixStatus == 'V') {
  370.         ALOGW("%s FixStatus is V(A-targeted,V-untargeted)", __func__);
  371.         // UTCDate and UTCTime may be empty
  372.         //updateUtcTime(rmc.UTCDate, rmc.UTCTime);
  373.         updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
  374.         //updateBearing(rmc.Azimuth);
  375.     } else {
  376.         //invalid data
  377.         ALOGD("%s invalid FixStatus(%c)", __func__, rmc.FixStatus);
  378.     }
  379.     return m_nmeaRMCvect.size();
  380. }
  381.  
  382. int GnssNmeaParser::procVTG(XXVTG_Info_T &vtg)
  383. {
  384.     size_t vecSize = m_nmeaVTGvect.size();
  385.     if ((vecSize != eVTG_CheckSum)
  386.         && (vecSize != eVTG_CheckSum-1)) {
  387.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  388.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  389.                 __func__, vecSize, eVTG_CheckSum, eVTG_CheckSum-1);
  390.         return 0;
  391.     }
  392.     bitsFlagClear<16>(vtg.BitFlags);
  393.     for (int i = 1; i < (int)vecSize; i++)
  394.     {
  395.         if (m_nmeaVTGvect[i].length() != 0)
  396.         {
  397.             bitsFlagSet<16>(vtg.BitFlags, i);
  398.         }
  399.     }
  400.     vtg.MovementAngleTN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle]);
  401.     vtg.TrueNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_TrueNorthRef]);
  402.     vtg.MovementAngleMN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle2]);
  403.     vtg.MagneticNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
  404.     vtg.HorizontalMoveSpeedKn = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
  405.     vtg.SpeedKnots = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKnots]);
  406.     vtg.HorizontalMoveSpeedKm = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
  407.     vtg.SpeedKmh = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKmh]);
  408.     if (vecSize == eVTG_CheckSum) {
  409.         vtg.PositioningMode = stringToNumber<char>(m_nmeaVTGvect[eVTG_PositioningMode]);//optional
  410.     }
  411.  
  412.     if ((vtg.PositioningMode == 'A') || (vtg.PositioningMode == 'D')) {
  413.         updateSpeed(vtg.HorizontalMoveSpeedKm/3.6f);
  414.     }
  415.     return m_nmeaVTGvect.size();
  416. }
  417.  
  418. void GnssNmeaParser::updateUtcTime(const int ddmmyy, const double hhmmss_sss)
  419. {
  420.     //get utc diff
  421.     time_t time_now = time(NULL);
  422.     struct tm tm_local {};
  423.     struct tm tm_utc {};
  424.     long time_local_sec = 0;
  425.     long time_utc_sec = 0;
  426.     long utc_diff_sec = 0;
  427.  
  428.     //get fixed time
  429.     struct tm tm_gnss {};
  430.     time_t time_fixed = 0;
  431.     int utc_year = 0;
  432.     int utc_month = 0;
  433.     int utc_day = 0;
  434.     int utc_hour = 0;
  435.     int utc_minute = 0;
  436.     int utc_seconds = 0;
  437.     int hhmmss = (int)hhmmss_sss;
  438.     int milliseconds = 0;
  439.  
  440.     gmtime_r(&time_now, &tm_utc);
  441.     localtime_r(&time_now, &tm_local);
  442.  
  443.     time_local_sec = tm_local.tm_sec +
  444.                     60*(tm_local.tm_min +
  445.                         60*(tm_local.tm_hour +
  446.                             24*(tm_local.tm_yday +
  447.                                 365*tm_local.tm_year)));
  448.  
  449.     time_utc_sec = tm_utc.tm_sec +
  450.                     60*(tm_utc.tm_min +
  451.                         60*(tm_utc.tm_hour +
  452.                             24*(tm_utc.tm_yday +
  453.                                 365*tm_utc.tm_year)));
  454.  
  455.     utc_diff_sec = time_local_sec - time_utc_sec;
  456.  
  457.     utc_day = (ddmmyy / 100) / 100;
  458.     utc_month = (ddmmyy / 100) % 100;
  459.     utc_year = (ddmmyy % 100) + 2000;
  460.     utc_hour = ((hhmmss / 100) / 100);
  461.     utc_minute = ((hhmmss / 100) % 100);
  462.     utc_seconds = (hhmmss % 100);
  463.     //milliseconds = (int)((hhmmss_sss - hhmmss) * 1000); //will less precise
  464.     milliseconds = (int)((hhmmss_sss * 1000) - (hhmmss * 1000)); //Improve accuracy
  465.  
  466.     tm_gnss.tm_hour  = utc_hour;
  467.     tm_gnss.tm_min   = utc_minute;
  468.     tm_gnss.tm_sec   = utc_seconds;
  469.     tm_gnss.tm_year  = utc_year - 1900;
  470.     tm_gnss.tm_mon   = utc_month - 1;
  471.     tm_gnss.tm_mday  = utc_day;
  472.     tm_gnss.tm_isdst = -1;
  473.  
  474.     time_fixed = mktime(&tm_gnss) + utc_diff_sec;
  475.     m_gnssUtcTime = (long long)time_fixed * 1000 + milliseconds;
  476.     m_gnssLocation.timestamp = m_gnssUtcTime;
  477.  
  478.     if ((0 == ddmmyy) || (0 == hhmmss)) {
  479.         ALOGW("%s invalid UTCDate=%d, UTCTime=%d", __func__, ddmmyy, hhmmss);
  480.         //use local stored utc time
  481.         time_fixed = mktime(&tm_utc) + utc_diff_sec;
  482.         m_gnssUtcTime = (long long)time_fixed * 1000;
  483.         m_gnssLocation.timestamp = m_gnssUtcTime;
  484.     }
  485. }
  486.  
  487. void GnssNmeaParser::updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi)
  488. {
  489.     double lat = latitude;
  490.     double lon = longtitude;
  491.     if (latHemi == 'S') {
  492.         lat = -lat;
  493.     }
  494.     if (longHemi == 'W') {
  495.         lon = -lon;
  496.     }
  497.     m_gnssLocation.flags    |= GPS_LOCATION_HAS_LAT_LONG;
  498.     m_gnssLocation.latitude  = latLongToDegree(lat);
  499.     m_gnssLocation.longitude = latLongToDegree(lon);
  500. }
  501.  
  502. void GnssNmeaParser::updateAltitude(const double altitude)
  503. {
  504.     double alt = altitude;
  505.     m_gnssLocation.flags   |= GPS_LOCATION_HAS_ALTITUDE;
  506.     m_gnssLocation.altitude = alt;
  507. }
  508.  
  509. void GnssNmeaParser::updateBearing(const float bearing)
  510. {
  511.     float bea = bearing;
  512.     m_gnssLocation.flags   |= GPS_LOCATION_HAS_BEARING;
  513.     m_gnssLocation.bearing  = bea;
  514. }
  515.  
  516. void GnssNmeaParser::updateMagDec(const float magDec, const char magDecDir)
  517. {
  518.     (void)magDec;
  519.     (void)magDecDir;
  520. }
  521.  
  522. void GnssNmeaParser::updateAccuracy(const float pdop, const float hdop, const float vdop)
  523. {
  524.     /* GSA HDOP */
  525.     (void)pdop;
  526.     (void)vdop;
  527.     if ((0.1f <= hdop)) {
  528.         m_gnssLocation.flags   |= GPS_LOCATION_HAS_HORIZONTAL_ACCURACY;
  529.         m_gnssLocation.accuracy = hdop;
  530.     }
  531. }
  532.  
  533. void GnssNmeaParser::updateSpeed(const float speed)
  534. {
  535.     float velocity = speed;
  536.     m_gnssLocation.flags   |= GPS_LOCATION_HAS_SPEED;
  537.     m_gnssLocation.speed    = velocity;
  538. }
  539.  
  540. void GnssNmeaParser::updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT)
  541. {
  542.     std::vector<XXGSV_Info_T>::const_iterator vit;
  543.     int itemsCount = 0;
  544.     int itemSequence = 0;
  545.     int sateSeq = 0;
  546.  
  547.     m_gnssSvStatus.size = sizeof(m_gnssSvStatus);
  548.     m_gnssSvStatus.num_svs = 0;
  549.     for (vit = gsvVectInfoT.begin(); vit != gsvVectInfoT.end(); vit++) {
  550.         itemsCount = vit->ItemCount;
  551.         itemSequence = vit->ItemSequence;
  552.  
  553.         if ((sateSeq+3) > (GNSS_MAX_SVS-1)) {
  554.             /* preventing arrays from out of bounds */
  555.             ALOGW(" gnssSvStatus num more than GNSS_MAX_SVS:%d", GNSS_MAX_SVS);
  556.             break;
  557.         }
  558.  
  559.         //m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
  560.         m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode;
  561.         m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation;
  562.         m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth;
  563.         m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio;
  564.         m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
  565.         if (bitsFlagTest<32>((*vit).BitFlags, eGSV_SignalNoiseRatio)) {
  566.             //m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
  567.         }
  568.         m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
  569.         if (vit->PRNCode > 0) {
  570.             /* check and set flag whether available satellite */
  571.             m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode);
  572.             /* increase visible satellites count */
  573.             sateSeq += 1;
  574.         }
  575.  
  576.         //m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
  577.         m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode2;
  578.         m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation2;
  579.         m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth2;
  580.         m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio2;
  581.         m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
  582.         if ( bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio2) ) {
  583.             //m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
  584.         }
  585.         m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
  586.         if (vit->PRNCode2 > 0) {
  587.             m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode2);
  588.             sateSeq += 1;
  589.         }
  590.  
  591.         //m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
  592.         m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode3;
  593.         m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation3;
  594.         m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth3;
  595.         m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio3;
  596.         m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
  597.         if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio3)) {
  598.             //m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
  599.         }
  600.         m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
  601.         if (vit->PRNCode3 > 0) {
  602.             m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode3);
  603.             sateSeq += 1;
  604.         }
  605.  
  606.         //m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
  607.         m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode4;
  608.         m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation4;
  609.         m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth4;
  610.         m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio4;
  611.         m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
  612.         if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio4)) {
  613.             //m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
  614.         }
  615.         m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
  616.         if (vit->PRNCode4 > 0) {
  617.             m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode4);
  618.             sateSeq += 1;
  619.         }
  620.     }
  621.     m_gnssSvStatus.num_svs = sateSeq;
  622. }
  623.  
  624. double GnssNmeaParser::latLongToDegree(const double dddmm_mmmm)
  625. {
  626.     // eg.: 12031.0902 -> 120.51817(120+(31.0902/60.0=0.51817))
  627.     int    ddd = (int)(dddmm_mmmm/100);
  628.     double mm_mmmm = dddmm_mmmm - (ddd*100.0);
  629.     double ddd_xxx = ddd + (mm_mmmm / 60.0);
  630.     return ddd_xxx;
  631. }
  632.  
  633. int GnssNmeaParser::gnssSvFlagUsedInFix(const int svid) {
  634.     int fixed = 0;
  635.     int prnSn = 0;
  636.     std::vector<XXGSA_Info_T>::iterator gsaIt;
  637.     for (gsaIt = m_gsaVectInfoT.begin(); gsaIt != m_gsaVectInfoT.end(); gsaIt++) {
  638.         for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
  639.             if (svid == gsaIt->PRNList[prnSn]) {
  640.                 fixed = GNSS_SV_FLAGS_USED_IN_FIX;
  641.                 break;
  642.             }
  643.         }
  644.     }
  645.     return fixed;
  646. }
  647.  
  648. bool GnssNmeaParser::getGnssLocation(GpsLocation &gnssLocation)
  649. {
  650.     (void)memcpy(&gnssLocation, &m_gnssLocation, sizeof(m_gnssLocation));
  651.     return true;
  652. }
  653.  
  654. bool GnssNmeaParser::getGnssSvStatus(GnssSvStatus &gnssSvStatus)
  655. {
  656. #if 1
  657.     int numSv = m_gnssSvStatus.num_svs;
  658.     ALOGD("getGnssSvStatus size:%zu, num_svs:%d", m_gnssSvStatus.size, m_gnssSvStatus.num_svs);
  659.     for(int tmp = 0; tmp < numSv; tmp++)
  660.     {
  661.         ALOGD("getGnssSvStatus (id=%d,elevation=%f,azimuth=%f,dbhz=%f,CX=%d,svFlag=0x%x)",
  662.                 m_gnssSvStatus.gnss_sv_list[tmp].svid,
  663.                 m_gnssSvStatus.gnss_sv_list[tmp].elevation,
  664.                 m_gnssSvStatus.gnss_sv_list[tmp].azimuth,
  665.                 m_gnssSvStatus.gnss_sv_list[tmp].c_n0_dbhz,
  666.                 (int)m_gnssSvStatus.gnss_sv_list[tmp].constellation,
  667.                 (int)m_gnssSvStatus.gnss_sv_list[tmp].flags);
  668.     }
  669. #endif
  670.     (void)memcpy(&gnssSvStatus, &m_gnssSvStatus, sizeof(gnssSvStatus));
  671.     return true;
  672. }
  673.  
  674. GpsUtcTime GnssNmeaParser::getUtcTime()
  675. {
  676.     return m_gnssUtcTime;
  677. }
  678.  
  679. void GnssNmeaParser::reset()
  680. {
  681.     m_nmeaLines.clear();
  682.     m_nmeaGGAvect.clear();
  683.     m_nmeaGLLvect.clear();
  684.     m_nmeaGSAvec2d.clear();
  685.     m_nmeaGSVvec2d.clear();
  686.     m_nmeaRMCvect.clear();
  687.     m_nmeaVTGvect.clear();
  688.  
  689.     m_gnssUtcTime = 0;
  690.     (void)memset(&m_ggaInfoT, 0, sizeof(m_ggaInfoT));
  691.     (void)memset(&m_gllInfoT, 0, sizeof(m_gllInfoT));
  692.  
  693.     m_gsaVectInfoT.clear();
  694.     m_gsvVectInfoT.clear();
  695.     (void)memset(&m_rmcInfoT, 0, sizeof(m_rmcInfoT));
  696.     (void)memset(&m_vtgInfoT, 0, sizeof(m_vtgInfoT));
  697.  
  698.     (void)memset(&m_gnssLocation, 0, sizeof(m_gnssLocation));
  699.     (void)memset(&m_gnssSvStatus, 0, sizeof(m_gnssSvStatus));
  700. }
  701.  
  702. void GnssNmeaParser::removeChecksum(std::string &str)
  703. {
  704.     // get rid of checksum at the end of the sentecne
  705.     // remove chars after *
  706.     size_t phit = 0;
  707.     phit = str.find("*");
  708.     if (std::string::npos != phit)
  709.     {
  710.         str.erase(phit);
  711.     }
  712. }
  713.  
  714. uint8_t GnssNmeaParser::getNConstellation(const std::string &nmeaHead)
  715. {
  716.     uint8_t constellation = GNSS_CONSTELLATION_UNKNOWN;
  717.     if (startsWith(nmeaHead, "$GP")) {
  718.         constellation = GNSS_CONSTELLATION_GPS;
  719.     } else if (startsWith(nmeaHead, "$GL")) {
  720.         constellation = GNSS_CONSTELLATION_GLONASS;
  721.     } else if (startsWith(nmeaHead, "$BD")) {
  722.         constellation = GNSS_CONSTELLATION_BEIDOU;
  723.     } else {
  724.         constellation = GNSS_CONSTELLATION_UNKNOWN;
  725.     }
  726.     return constellation;
  727. }
  728.  
  729. bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
  730. {
  731.     int srcpos = 0;
  732.     int srclen = src.length();
  733.     int sublen = str.length();
  734.     if (srclen < sublen) {
  735.         return false;
  736.     }
  737.     return (0 == src.compare(srcpos, sublen, str));
  738. }
  739.  
  740. bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
  741. {
  742.     int srcpos = 0;
  743.     int srclen = src.length();
  744.     int sublen = str.length();
  745.     if (srclen < sublen) {
  746.         return false;
  747.     }
  748.     srcpos = srclen - sublen;
  749.     return (0 == src.compare(srcpos, sublen, str));
  750. }
  751.  
  752. std::string GnssNmeaParser::replace(const std::string &raw, const std::string &oldstr, const std::string &newstr) {
  753.     std::string res_string = raw;
  754.     size_t startpos = 0;
  755.     size_t retpos = 0;
  756.     while (std::string::npos != (retpos = res_string.find(oldstr, startpos)))
  757.     {
  758.         if (oldstr.size() == newstr.size()) {
  759.             (void)res_string.replace(retpos, oldstr.size(), newstr);
  760.         } else {
  761.             (void)res_string.erase(retpos, oldstr.size());
  762.             (void)res_string.insert(retpos, newstr);
  763.         }
  764.         startpos = retpos + oldstr.size();
  765.     }
  766.     return res_string;
  767. }
  768.  
  769. size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
  770. {
  771.     size_t pstart = 0;
  772.     size_t phit = 0;
  773.     std::string sstr;
  774.     size_t length = line.length();
  775.  
  776.     vstr.clear();
  777.     for (;pstart <= length;)
  778.     {
  779.         phit = line.find(delim, pstart);
  780.         if (std::string::npos != phit)
  781.         {
  782.             /* find delim, get substr */
  783.             sstr = line.substr(pstart, phit-pstart);
  784.             vstr.push_back(sstr);
  785.             pstart = phit + delim.size();
  786.         } else {
  787.             /* not find delim, append remaining str and break */
  788.             vstr.push_back(line.substr(pstart));
  789.             break;
  790.         }
  791.     }
  792.     return vstr.size();
  793. }
  794.  
  795.

for Android Gnss V1.1

  1. #ifndef HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  2. #define HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  3.  
  4. #include <cstdio>
  5. #include <iostream>
  6. #include <sstream>
  7. #include <bitset>
  8. #include <vector>
  9. #include <map>
  10. #include <queue>
  11.  
  12. //#include <hardware/gps.h>
  13. #include <android/hardware/gnss/1.1/IGnss.h>
  14.  
  15. #define GNSS_NMEA_LINE_SEP "\n"
  16. #define GNSS_NMEA_ELEMENT_SEP ","
  17. #define GNSS_NMEA_PARSER_VERSION "v1.0-20.0716"
  18.  
  19. using GnssSvFlags = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvFlags;
  20. using GnssStatusValue = ::android::hardware::gnss::V1_0::IGnssCallback::GnssStatusValue;
  21. using GnssLocationFlags = android::hardware::gnss::V1_0::GnssLocationFlags;
  22. using GnssMax = ::android::hardware::gnss::V1_0::GnssMax;
  23.  
  24. using GnssLocation = ::android::hardware::gnss::V1_0::GnssLocation;
  25. using GnssConstellationType = ::android::hardware::gnss::V1_0::GnssConstellationType;
  26. using GnssSvInfo = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvInfo;
  27. using GnssSvStatus = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvStatus;
  28.  
  29. using GnssUtcTime = ::android::hardware::gnss::V1_0::GnssUtcTime;
  30. using GnssAidingData = ::android::hardware::gnss::V1_0::IGnss::GnssAidingData;
  31. using GnssPositionMode = ::android::hardware::gnss::V1_0::IGnss::GnssPositionMode;
  32. using GnssPositionRecurrence = ::android::hardware::gnss::V1_0::IGnss::GnssPositionRecurrence;
  33.  
  34.  
  35. //GGA 定位信息
  36. enum eGNSS_GGA_ITEM_T {
  37.     eGGA_Header = 0,
  38.     eGGA_UTCTime = 1,
  39.     eGGA_Latitude = 2,
  40.     eGGA_LatitudeHemi = 3,
  41.     eGGA_Longitude = 4,
  42.     eGGA_LongitudeHemi = 5,
  43.     eGGA_StatusIndicator = 6,
  44.     eGGA_SatellitesCount = 7,
  45.     eGGA_HDOP = 8,
  46.     eGGA_Altitude = 9,
  47.     eGGA_AltitudeUnit = 10,
  48.     eGGA_GeoidHeight = 11,
  49.     eGGA_GeoidHeightUnit = 12,
  50.     eGGA_DiffTemporal = 13,
  51.     eGGA_DiffStationId = 14,
  52.     eGGA_CheckSum = 15,
  53. };
  54.  
  55. //GLL 定位地理信息
  56. enum eGNSS_GLL_ITEM_T {
  57.     eGLL_Header = 0,
  58.     eGLL_Latitude = 1,
  59.     eGLL_LatitudeHemi = 2,
  60.     eGLL_Longitude = 3,
  61.     eGLL_LongitudeHemi = 4,
  62.     eGLL_UTCTime = 5,
  63.     eGLL_FixStatus = 6,
  64.     eGLL_PositioningMode = 7, //optional
  65.     eGLL_CheckSum = 8,
  66. };
  67.  
  68. //GSA 当前卫星信息
  69. enum eGNSS_GSA_ITEM_T {
  70.     eGSA_Header = 0,
  71.     eGSA_Mode = 1,
  72.     eGSA_Type = 2,
  73.     eGSA_PRN1 = 3,
  74.     eGSA_PRN2 = 4,
  75.     eGSA_PRN3 = 5,
  76.     eGSA_PRN4 = 6,
  77.     eGSA_PRN5 = 7,
  78.     eGSA_PRN6 = 8,
  79.     eGSA_PRN7 = 9,
  80.     eGSA_PRN8 = 10,
  81.     eGSA_PRN9 = 11,
  82.     eGSA_PRN10 = 12,
  83.     eGSA_PRN11 = 13,
  84.     eGSA_PRN12 = 14,
  85.     eGSA_PDOP = 15,
  86.     eGSA_HDOP = 16,
  87.     eGSA_VDOP = 17,
  88.     eGSA_CheckSum = 18,
  89. };
  90.  
  91. //GSV 可见卫星信息
  92. enum eGNSS_GSV_ITEM_T {
  93.     eGSV_Header = 0,
  94.     eGSV_ItemCount = 1,
  95.     eGSV_ItemSequence = 2,
  96.     eGSV_SatellitesCount = 3,
  97.     eGSV_PRNCode = 4,
  98.     eGSV_SatelliteElevation = 5,
  99.     eGSV_SatelliteAzimuth = 6,
  100.     eGSV_SignalNoiseRatio = 7,
  101.     eGSV_PRNCode2 = 8,
  102.     eGSV_SatelliteElevation2 = 9,
  103.     eGSV_SatelliteAzimuth2 = 10,
  104.     eGSV_SignalNoiseRatio2 = 11,
  105.     eGSV_PRNCode3 = 12,
  106.     eGSV_SatelliteElevation3 = 13,
  107.     eGSV_SatelliteAzimuth3 = 14,
  108.     eGSV_SignalNoiseRatio3 = 15,
  109.     eGSV_PRNCode4 = 16,
  110.     eGSV_SatelliteElevation4 = 17,
  111.     eGSV_SatelliteAzimuth4 = 18,
  112.     eGSV_SignalNoiseRatio4 = 19,
  113.     eGSV_CheckSum = 20,
  114. };
  115.  
  116. // RMC
  117. enum eGNSS_RMC_ITEM_T {
  118.     eRMC_Header = 0,
  119.     eRMC_UTCTime = 1,
  120.     eRMC_FixStatus = 2,
  121.     eRMC_Latitude = 3,
  122.     eRMC_LatitudeHemi = 4,
  123.     eRMC_Longitude = 5,
  124.     eRMC_LongitudeHemi = 6,
  125.     eRMC_SpeedKnots = 7,
  126.     eRMC_Azimuth = 8,
  127.     eRMC_UTCDate = 9,
  128.     eRMC_MagneticDeclination = 10,
  129.     eRMC_MagneticDeclinationDirection = 11,
  130.     eRMC_PositioningMode = 12, //optional
  131.     eRMC_CheckSum = 13,
  132. };
  133.  
  134. //VTG 地面速度信息
  135. enum eGNSS_VTG_ITEM_T {
  136.     eVTG_Header = 0,
  137.     eVTG_MovementAngle = 1,
  138.     eVTG_TrueNorthRef = 2,
  139.     eVTG_MovementAngle2 = 3,
  140.     eVTG_MagneticNorthRef = 4,
  141.     eVTG_HorizontalMoveSpeed = 5,
  142.     eVTG_SpeedKnots = 6,
  143.     eVTG_HorizontalMoveSpeed2 = 7,
  144.     eVTG_SpeedKmh = 8,
  145.     eVTG_PositioningMode = 9, //optional
  146.     eVTG_CheckSum = 10,
  147. };
  148.  
  149.  
  150. //(GGA)GPS定位信息 Global Positioning System Fix Data
  151. //$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*7B
  152. struct XXGGA_Info_T {
  153.     double UTCTime;    //UTC时间,格式为hhmmss.sss
  154.     double Latitude;   //纬度,格式为ddmm.mmmm
  155.     char LatitudeHemi; //纬度半球,N或S
  156.     double Longitude;  //经度,格式为dddmm.mmmm
  157.     char LongitudeHemi;//经度半球,E或W
  158.     uint16_t StatusIndicator;//GPS状态:0初始化,1单点定位,2码差分,3无效PPS,4固定解,5浮点解,6正在估算,7人工输入固定值,8模拟模式,9WAAS差分
  159.     size_t SatellitesCount;  //使用卫星数量,从00到12
  160.     float HDOP;        //HDOP-水平精度因子,0.5到99.9,一般认为HDOP越小,质量越好
  161.     double Altitude;   //椭球高,-9999.9到9999.9米
  162.     char AltitudeUnit; //M指单位米
  163.     float GeoidHeight; //大地水准面高度异常差值,-9999.9到9999.9米
  164.     char GeoidHeightUnit; //M指单位米
  165.     int64_t DiffTemporal; //差分GPS数据期限(RTCM SC-104),最后设立RTCM传送的秒数量,如不是差分定位则为空
  166.     int16_t DiffStationId;//差分参考基站标号,从0000到1023
  167.     //char CheckSum[4]; //从$开始到*之间的所有ASCII码的异或校验
  168.     std::bitset<16> BitFlags;
  169. };
  170.  
  171. //(GLL)定位地理信息 Geographic Position
  172. struct XXGLL_Info_T {
  173.     double Latitude;   //纬度ddmm.mmmm(度分)格式
  174.     char LatitudeHemi; //纬度半球N或S
  175.     double Longitude;  //经度dddmm.mmmm(度分)格式
  176.     char LongitudeHemi;//经度半球E或W
  177.     int UTCTimeInt;       //UTC时间,hhmmss(时分秒)格式
  178.     char FixStatus;    //定位状态,A=有效定位,V=无效定位
  179.     char PositioningMode;  //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  180.     //char CheckSum[4];
  181.     std::bitset<16> BitFlags;
  182. };
  183.  
  184. //GSA 当前卫星信息 GPS DOP and Active Satellites (GSA)当前卫星信息
  185. #define GSA_INFO_PRN_CNT 12
  186. struct XXGSA_Info_T {
  187.     char Mode;     //定位模式,A=自动手动2D/3D,M=手动2D/3D
  188.     uint16_t Type; //定位类型,1=未定位,2=2D定位,3=3D定位
  189.     int PRNList[GSA_INFO_PRN_CNT]; //PRN码-伪随机噪声码,第x信道正在使用的卫星PRN码编号
  190.     float PDOP; //accuracy PDOP综合位置精度因子,0.5-99.9
  191.     float HDOP; //HDOP水平精度因子0.5-99.9
  192.     float VDOP; //VDOP垂直精度因子0.5-99.9
  193.     //char CheckSum[4];
  194.     std::bitset<32> BitFlags;
  195. };
  196.  
  197. //(GSV)可见卫星信息 可见卫星信息 GPS Satellites in View
  198. struct XXGSV_Info_T {
  199.     uint8_t Ext_constellation;  //UNKNOWN==0/GPS==1/SBAS==2/GLONASS==3/QZSS==4/BEIDOU==5/GALILEO==6
  200.     size_t ItemCount;    //GSV语句的总数
  201.     int ItemSequence;    //本句GSV的编号
  202.     int SatellitesCount; //可见卫星的总数,00~12,前面的0也将被传输
  203.     int PRNCode;             //PRN码-伪随机噪声码,前面的0也将被传输;GPS:1-32,Beidou:1-37,GLONASS:1-24,Galileo:1-36,...
  204.     float SatelliteElevation;//卫星仰角,00~90度,前面的0也将被传输
  205.     float SatelliteAzimuth;  //卫星方位角,000~359度,前面的0也将被传输
  206.     float SignalNoiseRatio;  //信噪比,00~99dB,没有跟踪到卫星时为空,前面的0也将被传输
  207.     int PRNCode2; //按照每颗卫星进行循环显示,每条GSV语句最多可以显示4颗卫星的信息
  208.     float SatelliteElevation2;
  209.     float SatelliteAzimuth2;
  210.     float SignalNoiseRatio2;
  211.     int PRNCode3;
  212.     float SatelliteElevation3;
  213.     float SatelliteAzimuth3;
  214.     float SignalNoiseRatio3;
  215.     int PRNCode4;
  216.     float SatelliteElevation4;
  217.     float SatelliteAzimuth4;
  218.     float SignalNoiseRatio4;
  219.     //char CheckSum[4];
  220.     std::bitset<32> BitFlags;
  221. };
  222.  
  223. // (RMC)推荐定位信息 Recommended Minimum Specific GPS/TRANSIT Data
  224. //$GPRMC,200808.000,A,3114.4591,N,12118.0993,E,0.82,282.15,191220,,,A*61
  225. struct XXRMC_Info_T {
  226.     double UTCTime;             //UTC时间,hhmmss.sss格式,格林尼治时间;
  227.     char FixStatus;             //状态,A=定位,V=未定位;当给GPS复位时为V,不输出速度,角度,时间数据;
  228.     double Latitude;            //纬度ddmm.mmmm,度分格式
  229.     char LatitudeHemi;          //纬度N或S
  230.     double Longitude;           //经度dddmm.mmmm,度分格式
  231.     char LongitudeHemi;         //经度E或W
  232.     float SpeedKnots;           //速度,节,Knots
  233.     float Azimuth;              //方位角,度 Azimuth/bearing
  234.     int UTCDate;                //UTC日期,DDMMYY格式
  235.     float MagneticDeclination;  //磁偏角,000-180度
  236.     char MagneticDeclinationDirection;//磁偏角方向,E或W
  237.     char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  238.     //char CheckSum[4];
  239.     std::bitset<16> BitFlags;
  240. };
  241.  
  242. //(VTG)地面速度信息 Track Made Good and Ground Speed
  243. struct XXVTG_Info_T {
  244.     float MovementAngleTN;    //以真北为参考基准的地面航向,000~359度,前面的0也将被传输
  245.     char TrueNorthRef;      //T
  246.     float MovementAngleMN;   //以磁北为参考基准的地面航向,000~359度,前面的0也将被传输
  247.     char MagneticNorthRef;      //M
  248.     float HorizontalMoveSpeedKn;  //地面速率,000.0~999.9节,前面的0也将被传输
  249.     char SpeedKnots;            //N
  250.     float HorizontalMoveSpeedKm; //地面速率,0000.0~1851.8Km/h,前面的0也将被传输
  251.     char SpeedKmh;              //K
  252.     char PositioningMode;  //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
  253.     //char CheckSum[4];
  254.     std::bitset<16> BitFlags;
  255. };
  256.  
  257. template <class T>
  258. T stringToNumber(const std::string &sstr)
  259. {
  260.     T number {};
  261.     std::istringstream iss {};
  262.     iss.str(sstr);
  263.     iss >> number; /* can auto remove leading 0 */
  264.     return number;
  265. }
  266.  
  267. template <class T>
  268. std::string toString(T &value)
  269. {
  270.     std::ostringstream oss {};
  271.     oss << value;
  272.     return oss.str();
  273. }
  274.  
  275. template <int BS_MAX_SIZE>
  276. void bitsFlagSet(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
  277. {
  278.     bs.set(pos);
  279. }
  280.  
  281. template <int BS_MAX_SIZE>
  282. void bitsFlagReset(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
  283. {
  284.     bs.reset(pos);
  285. }
  286.  
  287. template <int BS_MAX_SIZE>
  288. void bitsFlagClear(std::bitset<BS_MAX_SIZE> &bs)
  289. {
  290.     bs.reset();
  291. }
  292.  
  293. template <int BS_MAX_SIZE>
  294. bool bitsFlagTest(std::bitset<BS_MAX_SIZE> bs, size_t pos)
  295. {
  296.     return bs.test(pos);
  297. }
  298.  
  299. template <int BS_MAX_SIZE>
  300. std::string bitsFlagToString(std::bitset<BS_MAX_SIZE> &bs)
  301. {
  302.     return bs.to_string();
  303. }
  304.  
  305. class GnssNmeaParser
  306. {
  307. public:
  308.     GnssNmeaParser();
  309.     ~GnssNmeaParser();
  310.     int parse(const std::string &nmea);
  311.     int getNmeaLines(std::vector<std::string> &lines);
  312.     bool getGnssLocation(GnssLocation &gnssLocation);
  313.     bool getGnssSvStatus(GnssSvStatus &gnssSvStatus);
  314.     GnssUtcTime getUtcTime();
  315.  
  316. private:
  317.     int procGGA(XXGGA_Info_T &gga);
  318.     int procGLL(XXGLL_Info_T &gll);
  319.     int procGSA(std::vector<XXGSA_Info_T> &gsaVect);
  320.     int procGSV(std::vector<XXGSV_Info_T> &gsvVect);
  321.     int procRMC(XXRMC_Info_T &rmc);
  322.     int procVTG(XXVTG_Info_T &vtg);
  323.  
  324.     /* convert latitude(ddmm.mmmm) and longitude(dddmm.mmmm) to degrees */
  325.     double latLongToDegree(const double dddmm_mmmm);
  326.     int gnssSvFlagUsedInFix(const int svid);
  327.     void updateUtcTime(const int ddmmyy, const double hhmmss_sss);
  328.     void updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi);
  329.     void updateAltitude(const double altitude);
  330.     void updateBearing(const float bearing);
  331.     void updateMagDec(const float magDec, const char magDecDir);
  332.     void updateAccuracy(const float pdop, const float hdop, const float vdop);
  333.     void updateSpeed(const float speed);
  334.     void updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT);
  335.     void removeChecksum(std::string &str);
  336.     uint8_t getNConstellation(const std::string &nmeaHead);
  337.     bool startsWith(const std::string &src, const std::string &str);
  338.     bool endsWith(const std::string &src, const std::string &str);
  339.     std::string replace(const std::string &raw, const std::string &oldstr, const std::string &newstr);
  340.     size_t split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr);
  341.     void reset();
  342.  
  343.     std::vector<std::string> m_nmeaLines;
  344.     std::string m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
  345.     std::string m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
  346.  
  347.     std::vector<std::string> m_nmeaGGAvect;
  348.     std::vector<std::string> m_nmeaGLLvect;
  349.     std::vector<std::vector<std::string>> m_nmeaGSAvec2d;
  350.     std::vector<std::vector<std::string>> m_nmeaGSVvec2d;
  351.     std::vector<std::string> m_nmeaRMCvect;
  352.     std::vector<std::string> m_nmeaVTGvect;
  353.  
  354.     XXGGA_Info_T m_ggaInfoT;
  355.     XXGLL_Info_T m_gllInfoT;
  356.     std::vector<XXGSA_Info_T> m_gsaVectInfoT;
  357.     std::vector<XXGSV_Info_T> m_gsvVectInfoT;
  358.     XXRMC_Info_T m_rmcInfoT;
  359.     XXVTG_Info_T m_vtgInfoT;
  360.  
  361.     GnssUtcTime m_gnssUtcTime;
  362.     GnssLocation m_gnssLocation;
  363.     GnssSvStatus m_gnssSvStatus;
  364.     //GnssStatusValue m_gnssStatus;
  365. };
  366.  
  367. #endif  // HAL_GNSS_V1_0_GNSSNMEAPARSER_H
  368.  
  1. #include <log/log.h>
  2. #include "GnssNmeaParser.h"
  3.  
  4. #ifdef LOG_TAG
  5. #undef LOG_TAG
  6. #endif
  7. #define LOG_TAG  "GnssNmeaParserV1.1"
  8.  
  9. GnssNmeaParser::GnssNmeaParser()
  10. {
  11.     m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
  12.     m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
  13.     ALOGI("GnssNmeaParser created. %s", GNSS_NMEA_PARSER_VERSION);
  14.     reset();
  15. }
  16.  
  17. GnssNmeaParser::~GnssNmeaParser()
  18. {
  19.     reset();
  20. }
  21.  
  22. int GnssNmeaParser::parse(const std::string &nmea)
  23. {
  24.     reset();
  25.     int gpCount = 0;
  26.     if (0 == nmea.size()) {
  27.         return 0;
  28.     }
  29.     (void)split(nmea, m_nmeaLineSep, m_nmeaLines);
  30.  
  31.     std::vector<std::string>::iterator vsit;
  32.     std::vector<std::string> nmeaGSAvect;
  33.     std::vector<std::string> nmeaGSVvect;
  34.     std::string line;
  35.     for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
  36.         line = *vsit;
  37.         if (line.size() <= 6) {
  38.             //$GPxxx
  39.             continue;
  40.         }
  41.         removeChecksum(line);
  42.         gpCount += 1;
  43.  
  44.         if (startsWith(line, "$GPGGA")) { //GGA
  45.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  46.         } else if (startsWith(line, "$GLGGA")) {
  47.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  48.         } else if (startsWith(line, "$BDGGA")) {
  49.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  50.         } else if (startsWith(line, "$GNGGA")) {
  51.             (void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
  52.         }
  53.  
  54.         else if (startsWith(line, "$GPGLL")) { //GLL
  55.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  56.         } else if (startsWith(line, "$GLGLL")) {
  57.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  58.         } else if (startsWith(line, "$BDGLL")) {
  59.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  60.         } else if (startsWith(line, "$GNGLL")) {
  61.             (void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
  62.         }
  63.  
  64.         else if (startsWith(line, "$GPGSA")) { //GSA
  65.             // may contain multi-line
  66.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  67.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  68.         } else if (startsWith(line, "$GLGSA")) {
  69.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  70.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  71.         } else if (startsWith(line, "$BDGSA")) {
  72.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  73.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  74.         } else if (startsWith(line, "$GNGSA")) {
  75.             (void)split(line, m_nmeaElementSep, nmeaGSAvect);
  76.             m_nmeaGSAvec2d.push_back(nmeaGSAvect);
  77.         }
  78.  
  79.         else if (startsWith(line, "$GPGSV")) { //GSV
  80.             // may contain multi-line
  81.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  82.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  83.         } else if (startsWith(line, "$GLGSV")) {
  84.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  85.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  86.         } else if (startsWith(line, "$BDGSV")) {
  87.             (void)split(line, m_nmeaElementSep, nmeaGSVvect);
  88.             m_nmeaGSVvec2d.push_back(nmeaGSVvect);
  89.         }
  90.  
  91.         else if (startsWith(line, "$GPRMC")) { //RMC
  92.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  93.         } else if (startsWith(line, "$GLRMC")) {
  94.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  95.         } else if (startsWith(line, "$BDRMC")) {
  96.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  97.         } else if (startsWith(line, "$GNRMC")) {
  98.             (void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
  99.         }
  100.  
  101.         else if (startsWith(line, "$GPVTG")) { //VTG
  102.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  103.         } else if (startsWith(line, "$GLVTG")) {
  104.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  105.         } else if (startsWith(line, "$BDVTG")) {
  106.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  107.         } else if (startsWith(line, "$GNVTG")) {
  108.             (void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
  109.         }
  110.  
  111.         else {
  112.             ALOGD("unkown line:%s", line.c_str());
  113.         }
  114.     }
  115.  
  116.     (void)procGGA(m_ggaInfoT);
  117.     (void)procGLL(m_gllInfoT);
  118.     (void)procGSA(m_gsaVectInfoT);
  119.     (void)procGSV(m_gsvVectInfoT);
  120.     (void)procRMC(m_rmcInfoT);
  121.     (void)procVTG(m_vtgInfoT);
  122.     return gpCount;
  123. }
  124.  
  125. int GnssNmeaParser::getNmeaLines(std::vector<std::string> &lines)
  126. {
  127.     std::vector<std::string>::iterator vsit;
  128.     for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
  129.         lines.push_back(*vsit+m_nmeaLineSep);
  130.     }
  131.     return lines.size();
  132. }
  133.  
  134. int GnssNmeaParser::procGGA(XXGGA_Info_T &gga)
  135. {
  136.     size_t vecSize = m_nmeaGGAvect.size();
  137.     if (vecSize != eGGA_CheckSum) {
  138.         ALOGD("%s invalid vector size:%zu, expected(%d)",
  139.                 __func__, vecSize, eGGA_CheckSum);
  140.         return 0;
  141.     }
  142.     bitsFlagClear<16>(gga.BitFlags);
  143.  
  144.     for (int i = 1; i < (int)vecSize; i++)
  145.     {
  146.         if (m_nmeaGGAvect[i].length() != 0)
  147.         {
  148.             bitsFlagSet<16>(gga.BitFlags, i);
  149.         }
  150.     }
  151.     gga.UTCTime = stringToNumber<double>(m_nmeaGGAvect[eGGA_UTCTime]);
  152.     gga.Latitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Latitude]);
  153.     gga.LatitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LatitudeHemi]);
  154.     gga.Longitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Longitude]);
  155.     gga.LongitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LongitudeHemi]);
  156.     gga.StatusIndicator = stringToNumber<uint16_t>(m_nmeaGGAvect[eGGA_StatusIndicator]);
  157.     gga.SatellitesCount = stringToNumber<size_t>(m_nmeaGGAvect[eGGA_SatellitesCount]);
  158.     gga.HDOP = stringToNumber<float>(m_nmeaGGAvect[eGGA_HDOP]);
  159.     gga.Altitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Altitude]);
  160.     gga.AltitudeUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_AltitudeUnit]);
  161.     gga.GeoidHeight = stringToNumber<float>(m_nmeaGGAvect[eGGA_GeoidHeight]);
  162.     gga.GeoidHeightUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
  163.     gga.DiffTemporal = stringToNumber<long>(m_nmeaGGAvect[eGGA_DiffTemporal]);
  164.     gga.DiffStationId = stringToNumber<int>(m_nmeaGGAvect[eGGA_DiffStationId]);
  165.  
  166.     if (gga.StatusIndicator != 0) {
  167.         updateLatLong(gga.Latitude, gga.LatitudeHemi, gga.Longitude, gga.LongitudeHemi);
  168.         updateAltitude(gga.Altitude);
  169.         updateAccuracy(0.0f, gga.HDOP, 0.0f);
  170.     }
  171.     return m_nmeaGGAvect.size();
  172. }
  173.  
  174. int GnssNmeaParser::procGLL(XXGLL_Info_T &gll)
  175. {
  176.     size_t vecSize = m_nmeaGLLvect.size();
  177.     if ((vecSize != eGLL_CheckSum)
  178.         && (vecSize != eGLL_CheckSum-1)) {
  179.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  180.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  181.                 __func__, vecSize, eGLL_CheckSum, eGLL_CheckSum-1);
  182.         return 0;
  183.     }
  184.     bitsFlagClear<16>(gll.BitFlags);
  185.     for (int i = 1; i < (int)vecSize; i++)
  186.     {
  187.         if (m_nmeaGLLvect[i].length() != 0)
  188.         {
  189.             bitsFlagSet<16>(gll.BitFlags, i);
  190.         }
  191.     }
  192.     gll.Latitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Latitude]);
  193.     gll.LatitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LatitudeHemi]);
  194.     gll.Longitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Longitude]);
  195.     gll.LongitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LongitudeHemi]);
  196.     gll.UTCTimeInt = stringToNumber<int>(m_nmeaGLLvect[eGLL_UTCTime]);
  197.     gll.FixStatus = stringToNumber<char>(m_nmeaGLLvect[eGLL_FixStatus]);
  198.     if (vecSize == eGLL_CheckSum) {
  199.         gll.PositioningMode = stringToNumber<char>(m_nmeaGLLvect[eGLL_PositioningMode]);//optional
  200.     }
  201.  
  202.     if ((gll.FixStatus == 'A') && (gll.PositioningMode != 'N')) {
  203.         updateLatLong(gll.Latitude, gll.LatitudeHemi, gll.Longitude, gll.LongitudeHemi);
  204.     }
  205.     return m_nmeaGLLvect.size();
  206. }
  207.  
  208. int GnssNmeaParser::procGSA(std::vector<XXGSA_Info_T> &gsaVect)
  209. {
  210.     if (m_nmeaGSAvec2d.size() == 0) {
  211.         ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
  212.         return 0;
  213.     }
  214.     std::vector<std::vector<std::string>>::iterator vvsit;
  215.     std::vector<std::string> nmeaGSAvect {};
  216.     XXGSA_Info_T gsa {};
  217.     int prnSn = 0;
  218.     size_t vecSize = 0;
  219.     std::string nmeaHeader;
  220.     for (vvsit = m_nmeaGSAvec2d.begin(); vvsit != m_nmeaGSAvec2d.end(); vvsit++) {
  221.         nmeaGSAvect = *vvsit;
  222.         (void)memset((void *)&gsa, 0, sizeof(gsa));
  223.         bitsFlagClear<32>(gsa.BitFlags);
  224.         vecSize = nmeaGSAvect.size();
  225.  
  226.         if (vecSize != eGSA_CheckSum) {
  227.             ALOGI("%s invalid vector size:%zu, expected(%d)",
  228.                     __func__, vecSize, eGSA_CheckSum);
  229.             continue;
  230.         }
  231.         for (int i = 1; i < (int)vecSize; i++)
  232.         {
  233.             if (nmeaGSAvect[i].length() != 0)
  234.             {
  235.                 bitsFlagSet<32>(gsa.BitFlags, i);
  236.             }
  237.         }
  238.  
  239.         nmeaHeader = (nmeaGSAvect[eGSA_Header]);
  240.  
  241.         gsa.Mode = stringToNumber<char>(nmeaGSAvect[eGSA_Mode]);
  242.         gsa.Type = stringToNumber<uint16_t>(nmeaGSAvect[eGSA_Type]);
  243.         for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
  244.             gsa.PRNList[prnSn] = stringToNumber<int>(nmeaGSAvect[eGSA_PRN1+prnSn]);;
  245.         }
  246.         gsa.PDOP = stringToNumber<int>(nmeaGSAvect[eGSA_PDOP]);
  247.         gsa.HDOP = stringToNumber<int>(nmeaGSAvect[eGSA_HDOP]);
  248.         gsa.VDOP = stringToNumber<int>(nmeaGSAvect[eGSA_VDOP]);
  249.         gsaVect.push_back(gsa);
  250.  
  251.         if ((gsa.Type <= 1)) {
  252.             ALOGD("%s Positioning Mode is %hu(1-untargeted,2-2D,3-3D)", __func__, gsa.Type);
  253.         } else {
  254.             updateAccuracy(gsa.PDOP, gsa.HDOP, gsa.VDOP);
  255.         }
  256.     }
  257.     return m_nmeaGSAvec2d.size();
  258. }
  259.  
  260. int GnssNmeaParser::procGSV(std::vector<XXGSV_Info_T> &gsvVect)
  261. {
  262.     if (m_nmeaGSVvec2d.size() == 0) {
  263.         ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
  264.         return 0;
  265.     }
  266.     std::vector<std::vector<std::string>>::iterator vvsit;
  267.     std::vector<std::string> nmeaGSVvect {};
  268.     XXGSV_Info_T gsv {};
  269.     std::string nmeaHeader;
  270.     size_t vecSize = 0;
  271.     for (vvsit = m_nmeaGSVvec2d.begin(); vvsit != m_nmeaGSVvec2d.end(); vvsit++) {
  272.         nmeaGSVvect = *vvsit;
  273.         (void)memset((void *)&gsv, 0, sizeof(gsv));
  274.         bitsFlagClear<32>(gsv.BitFlags);
  275.         vecSize = nmeaGSVvect.size();
  276.  
  277.         if ((0 != (vecSize % 4)) || (vecSize < eGSV_PRNCode)) {
  278.             ALOGI("%s invalid vector size:%zu, expected(8/12/16/20)", __func__, vecSize);
  279.             continue;
  280.         }
  281.         for (int i = 1; i < (int)vecSize; i++)
  282.         {
  283.             if (nmeaGSVvect[i].length() != 0)
  284.             {
  285.                 bitsFlagSet<32>(gsv.BitFlags, i);
  286.             }
  287.         }
  288.         nmeaHeader = nmeaGSVvect[eGSV_Header];
  289.         gsv.Ext_constellation = getNConstellation(nmeaHeader);
  290.  
  291.         gsv.ItemCount = stringToNumber<size_t>(nmeaGSVvect[eGSV_ItemCount]);
  292.         gsv.ItemSequence = stringToNumber<int>(nmeaGSVvect[eGSV_ItemSequence]);
  293.         gsv.SatellitesCount = stringToNumber<int>(nmeaGSVvect[eGSV_SatellitesCount]);
  294.  
  295.         /* gsv slices count maybe 8/12/16/20 */
  296.         if (vecSize >= eGSV_PRNCode2) {
  297.             gsv.PRNCode = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode]);
  298.             gsv.SatelliteElevation = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation]);
  299.             gsv.SatelliteAzimuth = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth]);
  300.             gsv.SignalNoiseRatio = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio]);
  301.         }
  302.         if (vecSize >= eGSV_PRNCode3) {
  303.             gsv.PRNCode2 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode2]);
  304.             gsv.SatelliteElevation2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation2]);
  305.             gsv.SatelliteAzimuth2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
  306.             gsv.SignalNoiseRatio2 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
  307.         }
  308.         if (vecSize >= eGSV_PRNCode4) {
  309.             gsv.PRNCode3 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode3]);
  310.             gsv.SatelliteElevation3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation3]);
  311.             gsv.SatelliteAzimuth3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
  312.             gsv.SignalNoiseRatio3 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
  313.         }
  314.         if (vecSize == eGSV_CheckSum) {
  315.             gsv.PRNCode4 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode4]);
  316.             gsv.SatelliteElevation4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation4]);
  317.             gsv.SatelliteAzimuth4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
  318.             gsv.SignalNoiseRatio4 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio4]);
  319.         }
  320.         gsvVect.push_back(gsv);
  321.     }
  322.  
  323.     updateGnssSvStatus(gsvVect);
  324.     return m_nmeaGSVvec2d.size();
  325. }
  326.  
  327. int GnssNmeaParser::procRMC(XXRMC_Info_T &rmc)
  328. {
  329.     size_t vecSize = m_nmeaRMCvect.size();
  330.     if ((vecSize != eRMC_CheckSum)
  331.         && (vecSize != eRMC_CheckSum-1)) {
  332.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  333.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  334.                 __func__, vecSize, eRMC_CheckSum, eRMC_CheckSum-1);
  335.         return 0;
  336.     }
  337.     bitsFlagClear<16>(rmc.BitFlags);
  338.     for (int i = 1; i < (int)vecSize; i++)
  339.     {
  340.         if (m_nmeaRMCvect[i].length() != 0)
  341.         {
  342.             bitsFlagSet<16>(rmc.BitFlags, i);
  343.         }
  344.     }
  345.     rmc.UTCTime = stringToNumber<double>(m_nmeaRMCvect[eRMC_UTCTime]);
  346.     rmc.FixStatus = stringToNumber<char>(m_nmeaRMCvect[eRMC_FixStatus]);
  347.     rmc.Latitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Latitude]);
  348.     rmc.LatitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LatitudeHemi]);
  349.     rmc.Longitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Longitude]);
  350.     rmc.LongitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LongitudeHemi]);
  351.     rmc.SpeedKnots = stringToNumber<float>(m_nmeaRMCvect[eRMC_SpeedKnots]);
  352.     rmc.Azimuth = stringToNumber<float>(m_nmeaRMCvect[eRMC_Azimuth]);
  353.     rmc.UTCDate = stringToNumber<int>(m_nmeaRMCvect[eRMC_UTCDate]);
  354.     rmc.MagneticDeclination = stringToNumber<float>(m_nmeaRMCvect[eRMC_MagneticDeclination]);
  355.     rmc.MagneticDeclinationDirection = stringToNumber<char>(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
  356.     if (vecSize == eRMC_CheckSum) {
  357.         rmc.PositioningMode = stringToNumber<char>(m_nmeaRMCvect[eRMC_PositioningMode]);//optional
  358.     }
  359.  
  360.     if (rmc.FixStatus == 'A') {
  361.         updateUtcTime(rmc.UTCDate, rmc.UTCTime);
  362.         updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
  363.         updateBearing(rmc.Azimuth);
  364.         updateSpeed((1.852f * rmc.SpeedKnots / 3.6f));
  365.         updateMagDec(rmc.MagneticDeclination, rmc.MagneticDeclinationDirection);
  366.     } else if (rmc.FixStatus == 'V') {
  367.         ALOGW("%s FixStatus is V(A-targeted,V-untargeted)", __func__);
  368.         // UTCDate and UTCTime may be empty
  369.         //updateUtcTime(rmc.UTCDate, rmc.UTCTime);
  370.         updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
  371.         //updateBearing(rmc.Azimuth);
  372.     } else {
  373.         //invalid data
  374.         ALOGD("%s invalid FixStatus(%c)", __func__, rmc.FixStatus);
  375.     }
  376.     return m_nmeaRMCvect.size();
  377. }
  378.  
  379. int GnssNmeaParser::procVTG(XXVTG_Info_T &vtg)
  380. {
  381.     size_t vecSize = m_nmeaVTGvect.size();
  382.     if ((vecSize != eVTG_CheckSum)
  383.         && (vecSize != eVTG_CheckSum-1)) {
  384.         //PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
  385.         ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
  386.                 __func__, vecSize, eVTG_CheckSum, eVTG_CheckSum-1);
  387.         return 0;
  388.     }
  389.     bitsFlagClear<16>(vtg.BitFlags);
  390.     for (int i = 1; i < (int)vecSize; i++)
  391.     {
  392.         if (m_nmeaVTGvect[i].length() != 0)
  393.         {
  394.             bitsFlagSet<16>(vtg.BitFlags, i);
  395.         }
  396.     }
  397.     vtg.MovementAngleTN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle]);
  398.     vtg.TrueNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_TrueNorthRef]);
  399.     vtg.MovementAngleMN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle2]);
  400.     vtg.MagneticNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
  401.     vtg.HorizontalMoveSpeedKn = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
  402.     vtg.SpeedKnots = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKnots]);
  403.     vtg.HorizontalMoveSpeedKm = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
  404.     vtg.SpeedKmh = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKmh]);
  405.     if (vecSize == eVTG_CheckSum) {
  406.         vtg.PositioningMode = stringToNumber<char>(m_nmeaVTGvect[eVTG_PositioningMode]);//optional
  407.     }
  408.  
  409.     if ((vtg.PositioningMode == 'A') || (vtg.PositioningMode == 'D')) {
  410.         updateSpeed(vtg.HorizontalMoveSpeedKm/3.6f);
  411.     }
  412.     return m_nmeaVTGvect.size();
  413. }
  414.  
  415. void GnssNmeaParser::updateUtcTime(const int ddmmyy, const double hhmmss_sss)
  416. {
  417.     //get utc diff
  418.     time_t time_now = time(NULL);
  419.     struct tm tm_local {};
  420.     struct tm tm_utc {};
  421.     long time_local_sec = 0;
  422.     long time_utc_sec = 0;
  423.     long utc_diff_sec = 0;
  424.  
  425.     //get fixed time
  426.     struct tm tm_gnss {};
  427.     time_t time_fixed = 0;
  428.     int utc_year = 0;
  429.     int utc_month = 0;
  430.     int utc_day = 0;
  431.     int utc_hour = 0;
  432.     int utc_minute = 0;
  433.     int utc_seconds = 0;
  434.     int hhmmss = (int)hhmmss_sss;
  435.     int milliseconds = 0;
  436.  
  437.     gmtime_r(&time_now, &tm_utc);
  438.     localtime_r(&time_now, &tm_local);
  439.  
  440.     time_local_sec = tm_local.tm_sec +
  441.                     60*(tm_local.tm_min +
  442.                         60*(tm_local.tm_hour +
  443.                             24*(tm_local.tm_yday +
  444.                                 365*tm_local.tm_year)));
  445.  
  446.     time_utc_sec = tm_utc.tm_sec +
  447.                     60*(tm_utc.tm_min +
  448.                         60*(tm_utc.tm_hour +
  449.                             24*(tm_utc.tm_yday +
  450.                                 365*tm_utc.tm_year)));
  451.  
  452.     utc_diff_sec = time_local_sec - time_utc_sec;
  453.  
  454.     utc_day = (ddmmyy / 100) / 100;
  455.     utc_month = (ddmmyy / 100) % 100;
  456.     utc_year = (ddmmyy % 100) + 2000;
  457.     utc_hour = ((hhmmss / 100) / 100);
  458.     utc_minute = ((hhmmss / 100) % 100);
  459.     utc_seconds = (hhmmss % 100);
  460.     //milliseconds = (int)((hhmmss_sss - hhmmss) * 1000); //will less precise
  461.     milliseconds = (int)((hhmmss_sss * 1000) - (hhmmss * 1000)); //Improve accuracy
  462.  
  463.     tm_gnss.tm_hour  = utc_hour;
  464.     tm_gnss.tm_min   = utc_minute;
  465.     tm_gnss.tm_sec   = utc_seconds;
  466.     tm_gnss.tm_year  = utc_year - 1900;
  467.     tm_gnss.tm_mon   = utc_month - 1;
  468.     tm_gnss.tm_mday  = utc_day;
  469.     tm_gnss.tm_isdst = -1;
  470.  
  471.     time_fixed = mktime(&tm_gnss) + utc_diff_sec;
  472.     m_gnssUtcTime = (long long)time_fixed * 1000 + milliseconds;
  473.     m_gnssLocation.timestamp = m_gnssUtcTime;
  474.  
  475.     if ((0 == ddmmyy) || (0 == hhmmss)) {
  476.         ALOGW("%s invalid UTCDate=%d, UTCTime=%d", __func__, ddmmyy, hhmmss);
  477.         //use local stored utc time
  478.         time_fixed = mktime(&tm_utc) + utc_diff_sec;
  479.         m_gnssUtcTime = (long long)time_fixed * 1000;
  480.         m_gnssLocation.timestamp = m_gnssUtcTime;
  481.     }
  482. }
  483.  
  484. void GnssNmeaParser::updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi)
  485. {
  486.     double lat = latitude;
  487.     double lon = longtitude;
  488.     if (latHemi == 'S') {
  489.         lat = -lat;
  490.     }
  491.     if (longHemi == 'W') {
  492.         lon = -lon;
  493.     }
  494.     m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_LAT_LONG;
  495.     m_gnssLocation.latitudeDegrees  = latLongToDegree(lat);
  496.     m_gnssLocation.longitudeDegrees = latLongToDegree(lon);
  497. }
  498.  
  499. void GnssNmeaParser::updateAltitude(const double altitude)
  500. {
  501.     double alt = altitude;
  502.     m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_ALTITUDE;
  503.     m_gnssLocation.altitudeMeters = alt;
  504. }
  505.  
  506. void GnssNmeaParser::updateBearing(const float bearing)
  507. {
  508.     float bea = bearing;
  509.     m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_BEARING;
  510.     m_gnssLocation.bearingDegrees  = bea;
  511. }
  512.  
  513. void GnssNmeaParser::updateMagDec(const float magDec, const char magDecDir)
  514. {
  515.     (void)magDec;
  516.     (void)magDecDir;
  517. }
  518.  
  519. void GnssNmeaParser::updateAccuracy(const float pdop, const float hdop, const float vdop)
  520. {
  521.     /* GSA HDOP */
  522.     (void)pdop;
  523.     (void)vdop;
  524.     if ((0.1f <= hdop)) {
  525.         m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
  526.         m_gnssLocation.horizontalAccuracyMeters = hdop;
  527.     }
  528.     if ((0.1f <= vdop)) {
  529.         m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_VERTICAL_ACCURACY;
  530.         m_gnssLocation.verticalAccuracyMeters = vdop;
  531.     }
  532. }
  533.  
  534. void GnssNmeaParser::updateSpeed(const float speed)
  535. {
  536.     float velocity = speed;
  537.     m_gnssLocation.gnssLocationFlags   |= GnssLocationFlags::HAS_SPEED;
  538.     m_gnssLocation.speedMetersPerSec    = velocity;
  539. }
  540.  
  541. void GnssNmeaParser::updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT)
  542. {
  543.     std::vector<XXGSV_Info_T>::const_iterator vit;
  544.     int itemsCount = 0;
  545.     int itemSequence = 0;
  546.     int sateSeq = 0;
  547.  
  548.     m_gnssSvStatus.numSvs = 0;
  549.     for (vit = gsvVectInfoT.begin(); vit != gsvVectInfoT.end(); vit++) {
  550.         itemsCount = vit->ItemCount;
  551.         itemSequence = vit->ItemSequence;
  552.  
  553.         if ((sateSeq+3) > ((int)GnssMax::SVS_COUNT)-1) {
  554.             /* preventing arrays from out of bounds */
  555.             ALOGW("gnssSvStatus num more than SVS_COUNT:%d", GnssMax::SVS_COUNT);
  556.             break;
  557.         }
  558.  
  559.         m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode;
  560.         m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation;
  561.         m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth;
  562.         m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio;
  563.         m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
  564.         if (bitsFlagTest<32>((*vit).BitFlags, eGSV_SignalNoiseRatio)) {
  565.             //m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
  566.         }
  567.         m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
  568.         if (vit->PRNCode > 0) {
  569.             /* check and set flag whether available satellite */
  570.             m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode);
  571.             /* increase visible satellites count */
  572.             sateSeq += 1;
  573.         }
  574.  
  575.         m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode2;
  576.         m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation2;
  577.         m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth2;
  578.         m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio2;
  579.         m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
  580.         if ( bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio2) ) {
  581.             //m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
  582.         }
  583.         m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
  584.         if (vit->PRNCode2 > 0) {
  585.             m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode2);
  586.             sateSeq += 1;
  587.         }
  588.  
  589.         m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode3;
  590.         m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation3;
  591.         m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth3;
  592.         m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio3;
  593.         m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
  594.         if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio3)) {
  595.             //m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
  596.         }
  597.         m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
  598.         if (vit->PRNCode3 > 0) {
  599.             m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode3);
  600.             sateSeq += 1;
  601.         }
  602.  
  603.         m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode4;
  604.         m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation4;
  605.         m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth4;
  606.         m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio4;
  607.         m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
  608.         if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio4)) {
  609.             //m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
  610.         }
  611.         m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
  612.         if (vit->PRNCode4 > 0) {
  613.             m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode4);
  614.             sateSeq += 1;
  615.         }
  616.     }
  617.     m_gnssSvStatus.numSvs = sateSeq;
  618. }
  619.  
  620. double GnssNmeaParser::latLongToDegree(const double dddmm_mmmm)
  621. {
  622.     // eg.: 12031.0902 -> 120.51817(120+(31.0902/60.0=0.51817))
  623.     int    ddd = (int)(dddmm_mmmm/100);
  624.     double mm_mmmm = dddmm_mmmm - (ddd*100.0);
  625.     double ddd_xxx = ddd + (mm_mmmm / 60.0);
  626.     return ddd_xxx;
  627. }
  628.  
  629. int GnssNmeaParser::gnssSvFlagUsedInFix(const int svid) {
  630.     int fixed = 0;
  631.     int prnSn = 0;
  632.     std::vector<XXGSA_Info_T>::iterator gsaIt;
  633.     for (gsaIt = m_gsaVectInfoT.begin(); gsaIt != m_gsaVectInfoT.end(); gsaIt++) {
  634.         for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
  635.             if (svid == gsaIt->PRNList[prnSn]) {
  636.                 fixed = (int)GnssSvFlags::USED_IN_FIX;
  637.                 break;
  638.             }
  639.         }
  640.     }
  641.     return fixed;
  642. }
  643.  
  644. bool GnssNmeaParser::getGnssLocation(GnssLocation &gnssLocation)
  645. {
  646.     (void)memcpy(&gnssLocation, &m_gnssLocation, sizeof(m_gnssLocation));
  647.     return true;
  648. }
  649.  
  650. bool GnssNmeaParser::getGnssSvStatus(GnssSvStatus &gnssSvStatus)
  651. {
  652. #if 1
  653.     int numSv = m_gnssSvStatus.numSvs;
  654.     ALOGD("getGnssSvStatus numSvs:%d", m_gnssSvStatus.numSvs);
  655.     for(int tmp = 0; tmp < numSv; tmp++)
  656.     {
  657.         ALOGD("getGnssSvStatus (svid=%d,elevation=%f,azimuth=%f,dbhz=%f,CX=%d,svFlag=0x%x)",
  658.                 m_gnssSvStatus.gnssSvList[tmp].svid,
  659.                 m_gnssSvStatus.gnssSvList[tmp].elevationDegrees,
  660.                 m_gnssSvStatus.gnssSvList[tmp].azimuthDegrees,
  661.                 m_gnssSvStatus.gnssSvList[tmp].cN0Dbhz,
  662.                 (int)m_gnssSvStatus.gnssSvList[tmp].constellation,
  663.                 (int)m_gnssSvStatus.gnssSvList[tmp].svFlag);
  664.     }
  665. #endif
  666.     (void)memcpy(&gnssSvStatus, &m_gnssSvStatus, sizeof(gnssSvStatus));
  667.     return true;
  668. }
  669.  
  670. GnssUtcTime GnssNmeaParser::getUtcTime()
  671. {
  672.     return m_gnssUtcTime;
  673. }
  674.  
  675. void GnssNmeaParser::reset()
  676. {
  677.     m_nmeaLines.clear();
  678.     m_nmeaGGAvect.clear();
  679.     m_nmeaGLLvect.clear();
  680.     m_nmeaGSAvec2d.clear();
  681.     m_nmeaGSVvec2d.clear();
  682.     m_nmeaRMCvect.clear();
  683.     m_nmeaVTGvect.clear();
  684.  
  685.     m_gnssUtcTime = 0;
  686.     (void)memset(&m_ggaInfoT, 0, sizeof(m_ggaInfoT));
  687.     (void)memset(&m_gllInfoT, 0, sizeof(m_gllInfoT));
  688.  
  689.     m_gsaVectInfoT.clear();
  690.     m_gsvVectInfoT.clear();
  691.     (void)memset(&m_rmcInfoT, 0, sizeof(m_rmcInfoT));
  692.     (void)memset(&m_vtgInfoT, 0, sizeof(m_vtgInfoT));
  693.  
  694.     (void)memset(&m_gnssLocation, 0, sizeof(m_gnssLocation));
  695.     (void)memset(&m_gnssSvStatus, 0, sizeof(m_gnssSvStatus));
  696. }
  697.  
  698. void GnssNmeaParser::removeChecksum(std::string &str)
  699. {
  700.     // get rid of checksum at the end of the sentecne
  701.     // remove chars after *
  702.     size_t phit = 0;
  703.     phit = str.find("*");
  704.     if (std::string::npos != phit)
  705.     {
  706.         str.erase(phit);
  707.     }
  708. }
  709.  
  710. uint8_t GnssNmeaParser::getNConstellation(const std::string &nmeaHead)
  711. {
  712.     uint8_t constellation = (uint8_t)GnssConstellationType::UNKNOWN;
  713.     if (startsWith(nmeaHead, "$GP")) {
  714.         constellation = (uint8_t)GnssConstellationType::GPS;
  715.     } else if (startsWith(nmeaHead, "$GL")) {
  716.         constellation = (uint8_t)GnssConstellationType::GLONASS;
  717.     } else if (startsWith(nmeaHead, "$BD")) {
  718.         constellation = (uint8_t)GnssConstellationType::BEIDOU;
  719.     } else {
  720.         constellation = (uint8_t)GnssConstellationType::UNKNOWN;
  721.     }
  722.     return constellation;
  723. }
  724.  
  725. bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
  726. {
  727.     int srcpos = 0;
  728.     int srclen = src.length();
  729.     int sublen = str.length();
  730.     if (srclen < sublen) {
  731.         return false;
  732.     }
  733.     return (0 == src.compare(srcpos, sublen, str));
  734. }
  735.  
  736. bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
  737. {
  738.     int srcpos = 0;
  739.     int srclen = src.length();
  740.     int sublen = str.length();
  741.     if (srclen < sublen) {
  742.         return false;
  743.     }
  744.     srcpos = srclen - sublen;
  745.     return (0 == src.compare(srcpos, sublen, str));
  746. }
  747.  
  748. std::string GnssNmeaParser::replace(const std::string &raw, const std::string &oldstr, const std::string &newstr) {
  749.     std::string res_string = raw;
  750.     size_t startpos = 0;
  751.     size_t retpos = 0;
  752.     while (std::string::npos != (retpos = res_string.find(oldstr, startpos)))
  753.     {
  754.         if (oldstr.size() == newstr.size()) {
  755.             (void)res_string.replace(retpos, oldstr.size(), newstr);
  756.         } else {
  757.             (void)res_string.erase(retpos, oldstr.size());
  758.             (void)res_string.insert(retpos, newstr);
  759.         }
  760.         startpos = retpos + oldstr.size();
  761.     }
  762.     return res_string;
  763. }
  764.  
  765. size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
  766. {
  767.     size_t pstart = 0;
  768.     size_t phit = 0;
  769.     std::string sstr;
  770.     size_t length = line.length();
  771.  
  772.     vstr.clear();
  773.     for (;pstart <= length;)
  774.     {
  775.         phit = line.find(delim, pstart);
  776.         if (std::string::npos != phit)
  777.         {
  778.             /* find delim, get substr */
  779.             sstr = line.substr(pstart, phit-pstart);
  780.             vstr.push_back(sstr);
  781.             pstart = phit + delim.size();
  782.         } else {
  783.             /* not find delim, append remaining str and break */
  784.             vstr.push_back(line.substr(pstart));
  785.             break;
  786.         }
  787.     }
  788.     return vstr.size();
  789. }
  790.  
  791.

 

posted @ 2023-07-20 18:13  SymPny  阅读(543)  评论(0)  编辑  收藏  举报