nmea数据具体各字段的含义请参考标准定义，这里给出一个C++实现的例子，环境是Android，本文只解析了几个常用的字段；
本示例的关键方法有split、startswith方法和stringToNumber模版函数
bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
return (0 == src.compare(srcpos, sublen, str));
}
bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
srcpos = srclen - sublen;
return (0 == src.compare(srcpos, sublen, str));
}template <class T>
T stringToNumber(const std::string &sstr)
{
T number {};
std::istringstream iss {};
iss.str(sstr);
iss >> number; /* can auto remove leading 0 */
return number;
}size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
{
size_t pstart = 0;
size_t phit = 0;
std::string sstr;
size_t length = line.length();
vstr.clear();
for (;pstart <= length;)
{
phit = line.find(delim, pstart);
if (std::string::npos != phit)
{
/* find delim, get substr */
sstr = line.substr(pstart, phit-pstart);
vstr.push_back(sstr);
pstart = phit + delim.size();
} else {
/* not find delim, append remaining str and break */
vstr.push_back(line.substr(pstart));
break;
}
}
return vstr.size();
}
for Android Gnss V1.0
解析.h头文件
#ifndef HAL_GNSS_V1_0_GNSSNMEAPARSER_H
#define HAL_GNSS_V1_0_GNSSNMEAPARSER_H
#include <cstdio>
#include <iostream>
#include <sstream>
#include <bitset>
#include <vector>
#include <map>
#include <queue>
#include <hardware/gps.h>
#define GNSS_NMEA_LINE_SEP "\n"
#define GNSS_NMEA_ELEMENT_SEP ","
#define GNSS_NMEA_PARSER_VERSION "v1.0-20.0716"
#if __GNSS_HAL_DEBUG_ON__
/* Flags that indicate information about the satellite */
typedef uint8_t LocGnssSvFlags;
#define GNSS_SV_FLAGS_NONE 0
#define GNSS_SV_FLAGS_HAS_EPHEMERIS_DATA (1)
#define GNSS_SV_FLAGS_HAS_ALMANAC_DATA (2)
#define GNSS_SV_FLAGS_USED_IN_FIX (4)
//hardware/libhardware/include/hardware/gps.h
typedef int64_t GpsUtcTime;
/**
* Flags that indicate information about the satellite
*/
typedef uint8_t GnssSvFlags;
/**
* Constellation type of GnssSvInfo
*/
typedef uint8_t GnssConstellationType;
/** Represents a location. */
typedef struct {
/** set to sizeof(GpsLocation) */
size_t size;
/** Contains GpsLocationFlags bits. */
uint16_t flags;
/** Represents latitude in degrees. */
double latitude;
/** Represents longitude in degrees. */
double longitude;
/**
* Represents altitude in meters above the WGS 84 reference ellipsoid.
*/
double altitude;
/** Represents speed in meters per second. */
float speed;
/** Represents heading in degrees. */
float bearing;
/** Represents expected accuracy in meters. */
float accuracy;
/** Timestamp for the location fix. */
GpsUtcTime timestamp;
} GpsLocation;
typedef struct {
/** set to sizeof(GnssSvInfo) */
size_t size;
/**
* Pseudo-random number for the SV, or FCN/OSN number for Glonass. The
* distinction is made by looking at constellation field. Values should be
* in the range of:
*
* - GPS: 1-32
* - SBAS: 120-151, 183-192
* - GLONASS: 1-24, the orbital slot number (OSN), if known. Or, if not:
* 93-106, the frequency channel number (FCN) (-7 to +6) offset by + 100
* i.e. report an FCN of -7 as 93, FCN of 0 as 100, and FCN of +6 as 106.
* - QZSS: 193-200
* - Galileo: 1-36
* - Beidou: 1-37
*/
int16_t svid;
/**
* Defines the constellation of the given SV. Value should be one of those
* GNSS_CONSTELLATION_* constants
*/
GnssConstellationType constellation;
/**
* Carrier-to-noise density in dB-Hz, typically in the range [0, 63].
* It contains the measured C/N0 value for the signal at the antenna port.
*
* This is a mandatory value.
*/
float c_n0_dbhz;
/** Elevation of SV in degrees. */
float elevation;
/** Azimuth of SV in degrees. */
float azimuth;
/**
* Contains additional data about the given SV. Value should be one of those
* GNSS_SV_FLAGS_* constants
*/
GnssSvFlags flags;
} GnssSvInfo;
/**
* Legacy struct to represents SV information.
* Deprecated, to be removed in the next Android release.
* Use GnssSvInfo instead.
*/
typedef struct {
/** set to sizeof(GpsSvInfo) */
size_t size;
/** Pseudo-random number for the SV. */
int prn;
/** Signal to noise ratio. */
float snr;
/** Elevation of SV in degrees. */
float elevation;
/** Azimuth of SV in degrees. */
float azimuth;
} GpsSvInfo;
/**
* Legacy struct to represents SV status.
* Deprecated, to be removed in the next Android release.
* Use GnssSvStatus instead.
*/
typedef struct {
/** set to sizeof(GpsSvStatus) */
size_t size;
int num_svs;
GpsSvInfo sv_list[GPS_MAX_SVS];
uint32_t ephemeris_mask;
uint32_t almanac_mask;
uint32_t used_in_fix_mask;
} GpsSvStatus;
typedef struct {
/** set to sizeof(GnssSvStatus) */
size_t size;
/** Number of GPS SVs currently visible, refers to the SVs stored in sv_list */
int num_svs;
/**
* Pointer to an array of SVs information for all GNSS constellations,
* except GPS, which is reported using sv_list
*/
GnssSvInfo gnss_sv_list[GNSS_MAX_SVS];
} GnssSvStatus;
#endif
//GGA 定位信息
enum eGNSS_GGA_ITEM_T {
eGGA_Header = 0,
eGGA_UTCTime = 1,
eGGA_Latitude = 2,
eGGA_LatitudeHemi = 3,
eGGA_Longitude = 4,
eGGA_LongitudeHemi = 5,
eGGA_StatusIndicator = 6,
eGGA_SatellitesCount = 7,
eGGA_HDOP = 8,
eGGA_Altitude = 9,
eGGA_AltitudeUnit = 10,
eGGA_GeoidHeight = 11,
eGGA_GeoidHeightUnit = 12,
eGGA_DiffTemporal = 13,
eGGA_DiffStationId = 14,
eGGA_CheckSum = 15,
};
//GLL 定位地理信息
enum eGNSS_GLL_ITEM_T {
eGLL_Header = 0,
eGLL_Latitude = 1,
eGLL_LatitudeHemi = 2,
eGLL_Longitude = 3,
eGLL_LongitudeHemi = 4,
eGLL_UTCTime = 5,
eGLL_FixStatus = 6,
eGLL_PositioningMode = 7, //optional
eGLL_CheckSum = 8,
};
//GSA 当前卫星信息
enum eGNSS_GSA_ITEM_T {
eGSA_Header = 0,
eGSA_Mode = 1,
eGSA_Type = 2,
eGSA_PRN1 = 3,
eGSA_PRN2 = 4,
eGSA_PRN3 = 5,
eGSA_PRN4 = 6,
eGSA_PRN5 = 7,
eGSA_PRN6 = 8,
eGSA_PRN7 = 9,
eGSA_PRN8 = 10,
eGSA_PRN9 = 11,
eGSA_PRN10 = 12,
eGSA_PRN11 = 13,
eGSA_PRN12 = 14,
eGSA_PDOP = 15,
eGSA_HDOP = 16,
eGSA_VDOP = 17,
eGSA_CheckSum = 18,
};
//GSV 可见卫星信息
enum eGNSS_GSV_ITEM_T {
eGSV_Header = 0,
eGSV_ItemCount = 1,
eGSV_ItemSequence = 2,
eGSV_SatellitesCount = 3,
eGSV_PRNCode = 4,
eGSV_SatelliteElevation = 5,
eGSV_SatelliteAzimuth = 6,
eGSV_SignalNoiseRatio = 7,
eGSV_PRNCode2 = 8,
eGSV_SatelliteElevation2 = 9,
eGSV_SatelliteAzimuth2 = 10,
eGSV_SignalNoiseRatio2 = 11,
eGSV_PRNCode3 = 12,
eGSV_SatelliteElevation3 = 13,
eGSV_SatelliteAzimuth3 = 14,
eGSV_SignalNoiseRatio3 = 15,
eGSV_PRNCode4 = 16,
eGSV_SatelliteElevation4 = 17,
eGSV_SatelliteAzimuth4 = 18,
eGSV_SignalNoiseRatio4 = 19,
eGSV_CheckSum = 20,
};
// RMC
enum eGNSS_RMC_ITEM_T {
eRMC_Header = 0,
eRMC_UTCTime = 1,
eRMC_FixStatus = 2,
eRMC_Latitude = 3,
eRMC_LatitudeHemi = 4,
eRMC_Longitude = 5,
eRMC_LongitudeHemi = 6,
eRMC_SpeedKnots = 7,
eRMC_Azimuth = 8,
eRMC_UTCDate = 9,
eRMC_MagneticDeclination = 10,
eRMC_MagneticDeclinationDirection = 11,
eRMC_PositioningMode = 12, //optional
eRMC_CheckSum = 13,
};
//VTG 地面速度信息
enum eGNSS_VTG_ITEM_T {
eVTG_Header = 0,
eVTG_MovementAngle = 1,
eVTG_TrueNorthRef = 2,
eVTG_MovementAngle2 = 3,
eVTG_MagneticNorthRef = 4,
eVTG_HorizontalMoveSpeed = 5,
eVTG_SpeedKnots = 6,
eVTG_HorizontalMoveSpeed2 = 7,
eVTG_SpeedKmh = 8,
eVTG_PositioningMode = 9, //optional
eVTG_CheckSum = 10,
};
//(GGA)GPS定位信息 Global Positioning System Fix Data
//$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*7B
struct XXGGA_Info_T {
double UTCTime; //UTC时间,格式为hhmmss.sss
double Latitude; //纬度,格式为ddmm.mmmm
char LatitudeHemi; //纬度半球,N或S
double Longitude; //经度,格式为dddmm.mmmm
char LongitudeHemi;//经度半球,E或W
uint16_t StatusIndicator;//GPS状态:0初始化,1单点定位,2码差分,3无效PPS,4固定解,5浮点解,6正在估算,7人工输入固定值,8模拟模式,9WAAS差分
size_t SatellitesCount; //使用卫星数量,从00到12
float HDOP; //HDOP-水平精度因子,0.5到99.9,一般认为HDOP越小,质量越好
double Altitude; //椭球高,-9999.9到9999.9米
char AltitudeUnit; //M指单位米
float GeoidHeight; //大地水准面高度异常差值,-9999.9到9999.9米
char GeoidHeightUnit; //M指单位米
int64_t DiffTemporal; //差分GPS数据期限(RTCM SC-104),最后设立RTCM传送的秒数量,如不是差分定位则为空
int16_t DiffStationId;//差分参考基站标号,从0000到1023
//char CheckSum[4]; //从$开始到*之间的所有ASCII码的异或校验
std::bitset<16> BitFlags;
};
//(GLL)定位地理信息 Geographic Position
struct XXGLL_Info_T {
double Latitude; //纬度ddmm.mmmm(度分)格式
char LatitudeHemi; //纬度半球N或S
double Longitude; //经度dddmm.mmmm(度分)格式
char LongitudeHemi;//经度半球E或W
int UTCTimeInt; //UTC时间,hhmmss(时分秒)格式
char FixStatus; //定位状态,A=有效定位,V=无效定位
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
//GSA 当前卫星信息 GPS DOP and Active Satellites (GSA)当前卫星信息
#define GSA_INFO_PRN_CNT 12
struct XXGSA_Info_T {
char Mode; //定位模式,A=自动手动2D/3D,M=手动2D/3D
uint16_t Type; //定位类型,1=未定位,2=2D定位,3=3D定位
int PRNList[GSA_INFO_PRN_CNT]; //PRN码-伪随机噪声码,第x信道正在使用的卫星PRN码编号
float PDOP; //accuracy PDOP综合位置精度因子,0.5-99.9
float HDOP; //HDOP水平精度因子0.5-99.9
float VDOP; //VDOP垂直精度因子0.5-99.9
//char CheckSum[4];
std::bitset<32> BitFlags;
};
//(GSV)可见卫星信息 可见卫星信息 GPS Satellites in View
struct XXGSV_Info_T {
uint8_t Ext_constellation; //UNKNOWN==0/GPS==1/SBAS==2/GLONASS==3/QZSS==4/BEIDOU==5/GALILEO==6
size_t ItemCount; //GSV语句的总数
int ItemSequence; //本句GSV的编号
int SatellitesCount; //可见卫星的总数,00~12,前面的0也将被传输
int PRNCode; //PRN码-伪随机噪声码,前面的0也将被传输;GPS:1-32,Beidou:1-37,GLONASS:1-24,Galileo:1-36,...
float SatelliteElevation;//卫星仰角,00~90度,前面的0也将被传输
float SatelliteAzimuth; //卫星方位角,000~359度,前面的0也将被传输
float SignalNoiseRatio; //信噪比,00~99dB,没有跟踪到卫星时为空,前面的0也将被传输
int PRNCode2; //按照每颗卫星进行循环显示,每条GSV语句最多可以显示4颗卫星的信息
float SatelliteElevation2;
float SatelliteAzimuth2;
float SignalNoiseRatio2;
int PRNCode3;
float SatelliteElevation3;
float SatelliteAzimuth3;
float SignalNoiseRatio3;
int PRNCode4;
float SatelliteElevation4;
float SatelliteAzimuth4;
float SignalNoiseRatio4;
//char CheckSum[4];
std::bitset<32> BitFlags;
};
// (RMC)推荐定位信息 Recommended Minimum Specific GPS/TRANSIT Data
//$GPRMC,200808.000,A,3114.4591,N,12118.0993,E,0.82,282.15,191220,,,A*61
struct XXRMC_Info_T {
double UTCTime; //UTC时间,hhmmss.sss格式,格林尼治时间;
char FixStatus; //状态,A=定位,V=未定位;当给GPS复位时为V,不输出速度,角度,时间数据;
double Latitude; //纬度ddmm.mmmm,度分格式
char LatitudeHemi; //纬度N或S
double Longitude; //经度dddmm.mmmm,度分格式
char LongitudeHemi; //经度E或W
float SpeedKnots; //速度,节,Knots
float Azimuth; //方位角,度 Azimuth/bearing
int UTCDate; //UTC日期,DDMMYY格式
float MagneticDeclination; //磁偏角,000-180度
char MagneticDeclinationDirection;//磁偏角方向,E或W
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
//(VTG)地面速度信息 Track Made Good and Ground Speed
struct XXVTG_Info_T {
float MovementAngleTN; //以真北为参考基准的地面航向,000~359度,前面的0也将被传输
char TrueNorthRef; //T
float MovementAngleMN; //以磁北为参考基准的地面航向,000~359度,前面的0也将被传输
char MagneticNorthRef; //M
float HorizontalMoveSpeedKn; //地面速率,000.0~999.9节,前面的0也将被传输
char SpeedKnots; //N
float HorizontalMoveSpeedKm; //地面速率,0000.0~1851.8Km/h,前面的0也将被传输
char SpeedKmh; //K
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
template <class T>
T stringToNumber(const std::string &sstr)
{
T number {};
std::istringstream iss {};
iss.str(sstr);
iss >> number; /* can auto remove leading 0 */
return number;
}
template <class T>
std::string toString(T &value)
{
std::ostringstream oss {};
oss << value;
return oss.str();
}
template <int BS_MAX_SIZE>
void bitsFlagSet(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
{
bs.set(pos);
}
template <int BS_MAX_SIZE>
void bitsFlagReset(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
{
bs.reset(pos);
}
template <int BS_MAX_SIZE>
void bitsFlagClear(std::bitset<BS_MAX_SIZE> &bs)
{
bs.reset();
}
template <int BS_MAX_SIZE>
bool bitsFlagTest(std::bitset<BS_MAX_SIZE> bs, size_t pos)
{
return bs.test(pos);
}
template <int BS_MAX_SIZE>
std::string bitsFlagToString(std::bitset<BS_MAX_SIZE> &bs)
{
return bs.to_string();
}
class GnssNmeaParser
{
public:
GnssNmeaParser();
~GnssNmeaParser();
int parse(const std::string &nmea);
int getNmeaLines(std::vector<std::string> &lines);
bool getGnssLocation(GpsLocation &gnssLocation);
bool getGnssSvStatus(GnssSvStatus &gnssSvStatus);
GpsUtcTime getUtcTime();
//bool getGpsSvStatus(GpsSvStatus &gpsSvStatus);
//bool getGpsStatus(GpsStatus &gpsStatus);
private:
int procGGA(XXGGA_Info_T &gga);
int procGLL(XXGLL_Info_T &gll);
int procGSA(std::vector<XXGSA_Info_T> &gsaVect);
int procGSV(std::vector<XXGSV_Info_T> &gsvVect);
int procRMC(XXRMC_Info_T &rmc);
int procVTG(XXVTG_Info_T &vtg);
/* convert latitude(ddmm.mmmm) and longitude(dddmm.mmmm) to degrees */
double latLongToDegree(const double dddmm_mmmm);
int gnssSvFlagUsedInFix(const int svid);
void updateUtcTime(const int ddmmyy, const double hhmmss_sss);
void updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi);
void updateAltitude(const double altitude);
void updateBearing(const float bearing);
void updateMagDec(const float magDec, const char magDecDir);
void updateAccuracy(const float pdop, const float hdop, const float vdop);
void updateSpeed(const float speed);
void updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT);
void removeChecksum(std::string &str);
uint8_t getNConstellation(const std::string &nmeaHead);
bool startsWith(const std::string &src, const std::string &str);
bool endsWith(const std::string &src, const std::string &str);
std::string replace(const std::string &raw, const std::string &oldstr, const std::string &newstr);
size_t split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr);
void reset();
std::vector<std::string> m_nmeaLines;
std::string m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
std::string m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
std::vector<std::string> m_nmeaGGAvect;
std::vector<std::string> m_nmeaGLLvect;
std::vector<std::vector<std::string>> m_nmeaGSAvec2d;
std::vector<std::vector<std::string>> m_nmeaGSVvec2d;
std::vector<std::string> m_nmeaRMCvect;
std::vector<std::string> m_nmeaVTGvect;
XXGGA_Info_T m_ggaInfoT;
XXGLL_Info_T m_gllInfoT;
std::vector<XXGSA_Info_T> m_gsaVectInfoT;
std::vector<XXGSV_Info_T> m_gsvVectInfoT;
XXRMC_Info_T m_rmcInfoT;
XXVTG_Info_T m_vtgInfoT;
GpsUtcTime m_gnssUtcTime;
GpsLocation m_gnssLocation;
GnssSvStatus m_gnssSvStatus;
GpsStatus m_gpsStatus;
};
#endif // HAL_GNSS_V1_0_GNSSNMEAPARSER_H
解析.cpp实现文件
#include <log/log.h>
#include "GnssNmeaParser.h"
#ifdef LOG_TAG
#undef LOG_TAG
#endif
#define LOG_TAG "GnssNmeaParserV1.0"
GnssNmeaParser::GnssNmeaParser()
{
m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
ALOGI("GnssNmeaParser created. %s", GNSS_NMEA_PARSER_VERSION);
reset();
}
GnssNmeaParser::~GnssNmeaParser()
{
reset();
}
int GnssNmeaParser::parse(const std::string &nmea)
{
reset();
int gpCount = 0;
if (0 == nmea.size()) {
return 0;
}
(void)split(nmea, m_nmeaLineSep, m_nmeaLines);
std::vector<std::string>::iterator vsit;
std::vector<std::string> nmeaGSAvect;
std::vector<std::string> nmeaGSVvect;
std::string line;
for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
line = *vsit;
if (line.size() <= 6) {
//$GPxxx
continue;
}
removeChecksum(line);
gpCount += 1;
if (startsWith(line, "$GPGGA")) { //GGA
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$GLGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$BDGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$GNGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
}
else if (startsWith(line, "$GPGLL")) { //GLL
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$GLGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$BDGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$GNGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
}
else if (startsWith(line, "$GPGSA")) { //GSA
// may contain multi-line
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$GLGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$BDGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$GNGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
}
else if (startsWith(line, "$GPGSV")) { //GSV
// may contain multi-line
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
} else if (startsWith(line, "$GLGSV")) {
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
} else if (startsWith(line, "$BDGSV")) {
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
}
else if (startsWith(line, "$GPRMC")) { //RMC
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$GLRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$BDRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$GNRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
}
else if (startsWith(line, "$GPVTG")) { //VTG
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$GLVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$BDVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$GNVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
}
else {
ALOGD("unkown line:%s", line.c_str());
}
}
(void)procGGA(m_ggaInfoT);
(void)procGLL(m_gllInfoT);
(void)procGSA(m_gsaVectInfoT);
(void)procGSV(m_gsvVectInfoT);
(void)procRMC(m_rmcInfoT);
(void)procVTG(m_vtgInfoT);
m_gnssLocation.size = sizeof(m_gnssLocation);
m_gnssSvStatus.size = sizeof(m_gnssSvStatus);
return gpCount;
}
int GnssNmeaParser::getNmeaLines(std::vector<std::string> &lines)
{
std::vector<std::string>::iterator vsit;
for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
lines.push_back(*vsit+m_nmeaLineSep);
}
return lines.size();
}
int GnssNmeaParser::procGGA(XXGGA_Info_T &gga)
{
size_t vecSize = m_nmeaGGAvect.size();
if (vecSize != eGGA_CheckSum) {
ALOGD("%s invalid vector size:%zu, expected(%d)",
__func__, vecSize, eGGA_CheckSum);
return 0;
}
bitsFlagClear<16>(gga.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaGGAvect[i].length() != 0)
{
bitsFlagSet<16>(gga.BitFlags, i);
}
}
gga.UTCTime = stringToNumber<double>(m_nmeaGGAvect[eGGA_UTCTime]);
gga.Latitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Latitude]);
gga.LatitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LatitudeHemi]);
gga.Longitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Longitude]);
gga.LongitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LongitudeHemi]);
gga.StatusIndicator = stringToNumber<uint16_t>(m_nmeaGGAvect[eGGA_StatusIndicator]);
gga.SatellitesCount = stringToNumber<size_t>(m_nmeaGGAvect[eGGA_SatellitesCount]);
gga.HDOP = stringToNumber<float>(m_nmeaGGAvect[eGGA_HDOP]);
gga.Altitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Altitude]);
gga.AltitudeUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_AltitudeUnit]);
gga.GeoidHeight = stringToNumber<float>(m_nmeaGGAvect[eGGA_GeoidHeight]);
gga.GeoidHeightUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
gga.DiffTemporal = stringToNumber<long>(m_nmeaGGAvect[eGGA_DiffTemporal]);
gga.DiffStationId = stringToNumber<int>(m_nmeaGGAvect[eGGA_DiffStationId]);
m_gpsStatus.size = sizeof(m_gpsStatus);
if (gga.StatusIndicator != 0) {
updateLatLong(gga.Latitude, gga.LatitudeHemi, gga.Longitude, gga.LongitudeHemi);
updateAltitude(gga.Altitude);
updateAccuracy(0.0f, gga.HDOP, 0.0f);
}
return m_nmeaGGAvect.size();
}
int GnssNmeaParser::procGLL(XXGLL_Info_T &gll)
{
size_t vecSize = m_nmeaGLLvect.size();
if ((vecSize != eGLL_CheckSum)
&& (vecSize != eGLL_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eGLL_CheckSum, eGLL_CheckSum-1);
return 0;
}
bitsFlagClear<16>(gll.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaGLLvect[i].length() != 0)
{
bitsFlagSet<16>(gll.BitFlags, i);
}
}
gll.Latitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Latitude]);
gll.LatitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LatitudeHemi]);
gll.Longitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Longitude]);
gll.LongitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LongitudeHemi]);
gll.UTCTimeInt = stringToNumber<int>(m_nmeaGLLvect[eGLL_UTCTime]);
gll.FixStatus = stringToNumber<char>(m_nmeaGLLvect[eGLL_FixStatus]);
if (vecSize == eGLL_CheckSum) {
gll.PositioningMode = stringToNumber<char>(m_nmeaGLLvect[eGLL_PositioningMode]);//optional
}
if ((gll.FixStatus == 'A') && (gll.PositioningMode != 'N')) {
updateLatLong(gll.Latitude, gll.LatitudeHemi, gll.Longitude, gll.LongitudeHemi);
}
return m_nmeaGLLvect.size();
}
int GnssNmeaParser::procGSA(std::vector<XXGSA_Info_T> &gsaVect)
{
if (m_nmeaGSAvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
return 0;
}
std::vector<std::vector<std::string>>::iterator vvsit;
std::vector<std::string> nmeaGSAvect {};
XXGSA_Info_T gsa {};
int prnSn = 0;
size_t vecSize = 0;
std::string nmeaHeader;
for (vvsit = m_nmeaGSAvec2d.begin(); vvsit != m_nmeaGSAvec2d.end(); vvsit++) {
nmeaGSAvect = *vvsit;
(void)memset((void *)&gsa, 0, sizeof(gsa));
bitsFlagClear<32>(gsa.BitFlags);
vecSize = nmeaGSAvect.size();
if (vecSize != eGSA_CheckSum) {
ALOGI("%s invalid vector size:%zu, expected(%d)",
__func__, vecSize, eGSA_CheckSum);
continue;
}
for (int i = 1; i < (int)vecSize; i++)
{
if (nmeaGSAvect[i].length() != 0)
{
bitsFlagSet<32>(gsa.BitFlags, i);
}
}
nmeaHeader = (nmeaGSAvect[eGSA_Header]);
gsa.Mode = stringToNumber<char>(nmeaGSAvect[eGSA_Mode]);
gsa.Type = stringToNumber<uint16_t>(nmeaGSAvect[eGSA_Type]);
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
gsa.PRNList[prnSn] = stringToNumber<int>(nmeaGSAvect[eGSA_PRN1+prnSn]);;
}
gsa.PDOP = stringToNumber<int>(nmeaGSAvect[eGSA_PDOP]);
gsa.HDOP = stringToNumber<int>(nmeaGSAvect[eGSA_HDOP]);
gsa.VDOP = stringToNumber<int>(nmeaGSAvect[eGSA_VDOP]);
gsaVect.push_back(gsa);
if ((gsa.Type <= 1)) {
ALOGD("%s Positioning Mode is %hu(1-untargeted,2-2D,3-3D)", __func__, gsa.Type);
} else {
updateAccuracy(gsa.PDOP, gsa.HDOP, gsa.VDOP);
}
}
return m_nmeaGSAvec2d.size();
}
int GnssNmeaParser::procGSV(std::vector<XXGSV_Info_T> &gsvVect)
{
if (m_nmeaGSVvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
return 0;
}
std::vector<std::vector<std::string>>::iterator vvsit;
std::vector<std::string> nmeaGSVvect {};
XXGSV_Info_T gsv {};
std::string nmeaHeader;
size_t vecSize = 0;
for (vvsit = m_nmeaGSVvec2d.begin(); vvsit != m_nmeaGSVvec2d.end(); vvsit++) {
nmeaGSVvect = *vvsit;
(void)memset((void *)&gsv, 0, sizeof(gsv));
bitsFlagClear<32>(gsv.BitFlags);
vecSize = nmeaGSVvect.size();
if ((0 != (vecSize % 4)) || (vecSize < eGSV_PRNCode)) {
ALOGI("%s invalid vector size:%zu, expected(8/12/16/20)", __func__, vecSize);
continue;
}
for (int i = 1; i < (int)vecSize; i++)
{
if (nmeaGSVvect[i].length() != 0)
{
bitsFlagSet<32>(gsv.BitFlags, i);
}
}
nmeaHeader = nmeaGSVvect[eGSV_Header];
gsv.Ext_constellation = getNConstellation(nmeaHeader);
gsv.ItemCount = stringToNumber<size_t>(nmeaGSVvect[eGSV_ItemCount]);
gsv.ItemSequence = stringToNumber<int>(nmeaGSVvect[eGSV_ItemSequence]);
gsv.SatellitesCount = stringToNumber<int>(nmeaGSVvect[eGSV_SatellitesCount]);
/* gsv slices count maybe 8/12/16/20 */
if (vecSize >= eGSV_PRNCode2) {
gsv.PRNCode = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode]);
gsv.SatelliteElevation = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation]);
gsv.SatelliteAzimuth = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth]);
gsv.SignalNoiseRatio = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio]);
}
if (vecSize >= eGSV_PRNCode3) {
gsv.PRNCode2 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode2]);
gsv.SatelliteElevation2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation2]);
gsv.SatelliteAzimuth2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
gsv.SignalNoiseRatio2 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
}
if (vecSize >= eGSV_PRNCode4) {
gsv.PRNCode3 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode3]);
gsv.SatelliteElevation3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation3]);
gsv.SatelliteAzimuth3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
gsv.SignalNoiseRatio3 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
}
if (vecSize == eGSV_CheckSum) {
gsv.PRNCode4 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode4]);
gsv.SatelliteElevation4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation4]);
gsv.SatelliteAzimuth4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
gsv.SignalNoiseRatio4 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio4]);
}
gsvVect.push_back(gsv);
}
updateGnssSvStatus(gsvVect);
return m_nmeaGSVvec2d.size();
}
int GnssNmeaParser::procRMC(XXRMC_Info_T &rmc)
{
size_t vecSize = m_nmeaRMCvect.size();
if ((vecSize != eRMC_CheckSum)
&& (vecSize != eRMC_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eRMC_CheckSum, eRMC_CheckSum-1);
return 0;
}
bitsFlagClear<16>(rmc.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaRMCvect[i].length() != 0)
{
bitsFlagSet<16>(rmc.BitFlags, i);
}
}
rmc.UTCTime = stringToNumber<double>(m_nmeaRMCvect[eRMC_UTCTime]);
rmc.FixStatus = stringToNumber<char>(m_nmeaRMCvect[eRMC_FixStatus]);
rmc.Latitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Latitude]);
rmc.LatitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LatitudeHemi]);
rmc.Longitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Longitude]);
rmc.LongitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LongitudeHemi]);
rmc.SpeedKnots = stringToNumber<float>(m_nmeaRMCvect[eRMC_SpeedKnots]);
rmc.Azimuth = stringToNumber<float>(m_nmeaRMCvect[eRMC_Azimuth]);
rmc.UTCDate = stringToNumber<int>(m_nmeaRMCvect[eRMC_UTCDate]);
rmc.MagneticDeclination = stringToNumber<float>(m_nmeaRMCvect[eRMC_MagneticDeclination]);
rmc.MagneticDeclinationDirection = stringToNumber<char>(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
if (vecSize == eRMC_CheckSum) {
rmc.PositioningMode = stringToNumber<char>(m_nmeaRMCvect[eRMC_PositioningMode]);//optional
}
if (rmc.FixStatus == 'A') {
updateUtcTime(rmc.UTCDate, rmc.UTCTime);
updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
updateBearing(rmc.Azimuth);
updateSpeed((1.852f * rmc.SpeedKnots / 3.6f));
updateMagDec(rmc.MagneticDeclination, rmc.MagneticDeclinationDirection);
} else if (rmc.FixStatus == 'V') {
ALOGW("%s FixStatus is V(A-targeted,V-untargeted)", __func__);
// UTCDate and UTCTime may be empty
//updateUtcTime(rmc.UTCDate, rmc.UTCTime);
updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
//updateBearing(rmc.Azimuth);
} else {
//invalid data
ALOGD("%s invalid FixStatus(%c)", __func__, rmc.FixStatus);
}
return m_nmeaRMCvect.size();
}
int GnssNmeaParser::procVTG(XXVTG_Info_T &vtg)
{
size_t vecSize = m_nmeaVTGvect.size();
if ((vecSize != eVTG_CheckSum)
&& (vecSize != eVTG_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eVTG_CheckSum, eVTG_CheckSum-1);
return 0;
}
bitsFlagClear<16>(vtg.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaVTGvect[i].length() != 0)
{
bitsFlagSet<16>(vtg.BitFlags, i);
}
}
vtg.MovementAngleTN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle]);
vtg.TrueNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_TrueNorthRef]);
vtg.MovementAngleMN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle2]);
vtg.MagneticNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
vtg.HorizontalMoveSpeedKn = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
vtg.SpeedKnots = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKnots]);
vtg.HorizontalMoveSpeedKm = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
vtg.SpeedKmh = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKmh]);
if (vecSize == eVTG_CheckSum) {
vtg.PositioningMode = stringToNumber<char>(m_nmeaVTGvect[eVTG_PositioningMode]);//optional
}
if ((vtg.PositioningMode == 'A') || (vtg.PositioningMode == 'D')) {
updateSpeed(vtg.HorizontalMoveSpeedKm/3.6f);
}
return m_nmeaVTGvect.size();
}
void GnssNmeaParser::updateUtcTime(const int ddmmyy, const double hhmmss_sss)
{
//get utc diff
time_t time_now = time(NULL);
struct tm tm_local {};
struct tm tm_utc {};
long time_local_sec = 0;
long time_utc_sec = 0;
long utc_diff_sec = 0;
//get fixed time
struct tm tm_gnss {};
time_t time_fixed = 0;
int utc_year = 0;
int utc_month = 0;
int utc_day = 0;
int utc_hour = 0;
int utc_minute = 0;
int utc_seconds = 0;
int hhmmss = (int)hhmmss_sss;
int milliseconds = 0;
gmtime_r(&time_now, &tm_utc);
localtime_r(&time_now, &tm_local);
time_local_sec = tm_local.tm_sec +
60*(tm_local.tm_min +
60*(tm_local.tm_hour +
24*(tm_local.tm_yday +
365*tm_local.tm_year)));
time_utc_sec = tm_utc.tm_sec +
60*(tm_utc.tm_min +
60*(tm_utc.tm_hour +
24*(tm_utc.tm_yday +
365*tm_utc.tm_year)));
utc_diff_sec = time_local_sec - time_utc_sec;
utc_day = (ddmmyy / 100) / 100;
utc_month = (ddmmyy / 100) % 100;
utc_year = (ddmmyy % 100) + 2000;
utc_hour = ((hhmmss / 100) / 100);
utc_minute = ((hhmmss / 100) % 100);
utc_seconds = (hhmmss % 100);
//milliseconds = (int)((hhmmss_sss - hhmmss) * 1000); //will less precise
milliseconds = (int)((hhmmss_sss * 1000) - (hhmmss * 1000)); //Improve accuracy
tm_gnss.tm_hour = utc_hour;
tm_gnss.tm_min = utc_minute;
tm_gnss.tm_sec = utc_seconds;
tm_gnss.tm_year = utc_year - 1900;
tm_gnss.tm_mon = utc_month - 1;
tm_gnss.tm_mday = utc_day;
tm_gnss.tm_isdst = -1;
time_fixed = mktime(&tm_gnss) + utc_diff_sec;
m_gnssUtcTime = (long long)time_fixed * 1000 + milliseconds;
m_gnssLocation.timestamp = m_gnssUtcTime;
if ((0 == ddmmyy) || (0 == hhmmss)) {
ALOGW("%s invalid UTCDate=%d, UTCTime=%d", __func__, ddmmyy, hhmmss);
//use local stored utc time
time_fixed = mktime(&tm_utc) + utc_diff_sec;
m_gnssUtcTime = (long long)time_fixed * 1000;
m_gnssLocation.timestamp = m_gnssUtcTime;
}
}
void GnssNmeaParser::updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi)
{
double lat = latitude;
double lon = longtitude;
if (latHemi == 'S') {
lat = -lat;
}
if (longHemi == 'W') {
lon = -lon;
}
m_gnssLocation.flags |= GPS_LOCATION_HAS_LAT_LONG;
m_gnssLocation.latitude = latLongToDegree(lat);
m_gnssLocation.longitude = latLongToDegree(lon);
}
void GnssNmeaParser::updateAltitude(const double altitude)
{
double alt = altitude;
m_gnssLocation.flags |= GPS_LOCATION_HAS_ALTITUDE;
m_gnssLocation.altitude = alt;
}
void GnssNmeaParser::updateBearing(const float bearing)
{
float bea = bearing;
m_gnssLocation.flags |= GPS_LOCATION_HAS_BEARING;
m_gnssLocation.bearing = bea;
}
void GnssNmeaParser::updateMagDec(const float magDec, const char magDecDir)
{
(void)magDec;
(void)magDecDir;
}
void GnssNmeaParser::updateAccuracy(const float pdop, const float hdop, const float vdop)
{
/* GSA HDOP */
(void)pdop;
(void)vdop;
if ((0.5f <= hdop) && (hdop <= 99.9f)) {
m_gnssLocation.flags |= GPS_LOCATION_HAS_HORIZONTAL_ACCURACY;
m_gnssLocation.accuracy = hdop;
}
}
void GnssNmeaParser::updateSpeed(const float speed)
{
float velocity = speed;
m_gnssLocation.flags |= GPS_LOCATION_HAS_SPEED;
m_gnssLocation.speed = velocity;
}
void GnssNmeaParser::updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT)
{
std::vector<XXGSV_Info_T>::const_iterator vit;
int itemsCount = 0;
int itemSequence = 0;
int sateSeq = 0;
m_gnssSvStatus.size = sizeof(m_gnssSvStatus);
m_gnssSvStatus.num_svs = 0;
for (vit = gsvVectInfoT.begin(); vit != gsvVectInfoT.end(); vit++) {
itemsCount = vit->ItemCount;
itemSequence = vit->ItemSequence;
if ((sateSeq+3) > (GNSS_MAX_SVS-1)) {
/* preventing arrays from out of bounds */
ALOGW(" gnssSvStatus num more than GNSS_MAX_SVS:%d", GNSS_MAX_SVS);
break;
}
//m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode;
m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation;
m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth;
m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio;
m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
if (bitsFlagTest<32>((*vit).BitFlags, eGSV_SignalNoiseRatio)) {
//m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
if (vit->PRNCode > 0) {
/* check and set flag whether available satellite */
m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode);
/* increase visible satellites count */
sateSeq += 1;
}
//m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode2;
m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation2;
m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth2;
m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio2;
m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
if ( bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio2) ) {
//m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
if (vit->PRNCode2 > 0) {
m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode2);
sateSeq += 1;
}
//m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode3;
m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation3;
m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth3;
m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio3;
m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio3)) {
//m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
if (vit->PRNCode3 > 0) {
m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode3);
sateSeq += 1;
}
//m_gnssSvStatus.gnss_sv_list[sateSeq].size = sizeof(GnssSvInfo);
m_gnssSvStatus.gnss_sv_list[sateSeq].svid = vit->PRNCode4;
m_gnssSvStatus.gnss_sv_list[sateSeq].elevation = vit->SatelliteElevation4;
m_gnssSvStatus.gnss_sv_list[sateSeq].azimuth = vit->SatelliteAzimuth4;
m_gnssSvStatus.gnss_sv_list[sateSeq].c_n0_dbhz = vit->SignalNoiseRatio4;
m_gnssSvStatus.gnss_sv_list[sateSeq].flags = 0;
if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio4)) {
//m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= GNSS_SV_FLAGS_HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnss_sv_list[sateSeq].constellation = vit->Ext_constellation;
if (vit->PRNCode4 > 0) {
m_gnssSvStatus.gnss_sv_list[sateSeq].flags |= gnssSvFlagUsedInFix(vit->PRNCode4);
sateSeq += 1;
}
}
m_gnssSvStatus.num_svs = sateSeq;
}
double GnssNmeaParser::latLongToDegree(const double dddmm_mmmm)
{
// eg.: 12031.0902 -> 120.51817(120+(31.0902/60.0=0.51817))
int ddd = (int)(dddmm_mmmm/100);
double mm_mmmm = dddmm_mmmm - (ddd*100.0);
double ddd_xxx = ddd + (mm_mmmm / 60.0);
return ddd_xxx;
}
int GnssNmeaParser::gnssSvFlagUsedInFix(const int svid) {
int fixed = 0;
int prnSn = 0;
std::vector<XXGSA_Info_T>::iterator gsaIt;
for (gsaIt = m_gsaVectInfoT.begin(); gsaIt != m_gsaVectInfoT.end(); gsaIt++) {
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
if (svid == gsaIt->PRNList[prnSn]) {
fixed = GNSS_SV_FLAGS_USED_IN_FIX;
break;
}
}
}
return fixed;
}
bool GnssNmeaParser::getGnssLocation(GpsLocation &gnssLocation)
{
(void)memcpy(&gnssLocation, &m_gnssLocation, sizeof(m_gnssLocation));
return true;
}
bool GnssNmeaParser::getGnssSvStatus(GnssSvStatus &gnssSvStatus)
{
#if 1
int numSv = m_gnssSvStatus.num_svs;
ALOGD("getGnssSvStatus size:%zu, num_svs:%d", m_gnssSvStatus.size, m_gnssSvStatus.num_svs);
for(int tmp = 0; tmp < numSv; tmp++)
{
ALOGD("getGnssSvStatus (id=%d,elevation=%f,azimuth=%f,dbhz=%f,CX=%d,svFlag=0x%x)",
m_gnssSvStatus.gnss_sv_list[tmp].svid,
m_gnssSvStatus.gnss_sv_list[tmp].elevation,
m_gnssSvStatus.gnss_sv_list[tmp].azimuth,
m_gnssSvStatus.gnss_sv_list[tmp].c_n0_dbhz,
(int)m_gnssSvStatus.gnss_sv_list[tmp].constellation,
(int)m_gnssSvStatus.gnss_sv_list[tmp].flags);
}
#endif
(void)memcpy(&gnssSvStatus, &m_gnssSvStatus, sizeof(gnssSvStatus));
return true;
}
GpsUtcTime GnssNmeaParser::getUtcTime()
{
return m_gnssUtcTime;
}
void GnssNmeaParser::reset()
{
m_nmeaLines.clear();
m_nmeaGGAvect.clear();
m_nmeaGLLvect.clear();
m_nmeaGSAvec2d.clear();
m_nmeaGSVvec2d.clear();
m_nmeaRMCvect.clear();
m_nmeaVTGvect.clear();
m_gnssUtcTime = 0;
(void)memset(&m_ggaInfoT, 0, sizeof(m_ggaInfoT));
(void)memset(&m_gllInfoT, 0, sizeof(m_gllInfoT));
m_gsaVectInfoT.clear();
m_gsvVectInfoT.clear();
(void)memset(&m_rmcInfoT, 0, sizeof(m_rmcInfoT));
(void)memset(&m_vtgInfoT, 0, sizeof(m_vtgInfoT));
(void)memset(&m_gnssLocation, 0, sizeof(m_gnssLocation));
(void)memset(&m_gnssSvStatus, 0, sizeof(m_gnssSvStatus));
}
void GnssNmeaParser::removeChecksum(std::string &str)
{
// get rid of checksum at the end of the sentecne
// remove chars after *
size_t phit = 0;
phit = str.find("*");
if (std::string::npos != phit)
{
str.erase(phit);
}
}
uint8_t GnssNmeaParser::getNConstellation(const std::string &nmeaHead)
{
uint8_t constellation = GNSS_CONSTELLATION_UNKNOWN;
if (startsWith(nmeaHead, "$GP")) {
constellation = GNSS_CONSTELLATION_GPS;
} else if (startsWith(nmeaHead, "$GL")) {
constellation = GNSS_CONSTELLATION_GLONASS;
} else if (startsWith(nmeaHead, "$BD")) {
constellation = GNSS_CONSTELLATION_BEIDOU;
} else {
constellation = GNSS_CONSTELLATION_UNKNOWN;
}
return constellation;
}
bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
return (0 == src.compare(srcpos, sublen, str));
}
bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
srcpos = srclen - sublen;
return (0 == src.compare(srcpos, sublen, str));
}
std::string GnssNmeaParser::replace(const std::string &raw, const std::string &oldstr, const std::string &newstr) {
std::string res_string = raw;
size_t startpos = 0;
size_t retpos = 0;
while (std::string::npos != (retpos = res_string.find(oldstr, startpos)))
{
if (oldstr.size() == newstr.size()) {
(void)res_string.replace(retpos, oldstr.size(), newstr);
} else {
(void)res_string.erase(retpos, oldstr.size());
(void)res_string.insert(retpos, newstr);
}
startpos = retpos + oldstr.size();
}
return res_string;
}
size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
{
size_t pstart = 0;
size_t phit = 0;
std::string sstr;
size_t length = line.length();
vstr.clear();
for (;pstart <= length;)
{
phit = line.find(delim, pstart);
if (std::string::npos != phit)
{
/* find delim, get substr */
sstr = line.substr(pstart, phit-pstart);
vstr.push_back(sstr);
pstart = phit + delim.size();
} else {
/* not find delim, append remaining str and break */
vstr.push_back(line.substr(pstart));
break;
}
}
return vstr.size();
}
for Android Gnss V1.1
#ifndef HAL_GNSS_V1_0_GNSSNMEAPARSER_H
#define HAL_GNSS_V1_0_GNSSNMEAPARSER_H
#include <cstdio>
#include <iostream>
#include <sstream>
#include <bitset>
#include <vector>
#include <map>
#include <queue>
//#include <hardware/gps.h>
#include <android/hardware/gnss/1.1/IGnss.h>
#define GNSS_NMEA_LINE_SEP "\n"
#define GNSS_NMEA_ELEMENT_SEP ","
#define GNSS_NMEA_PARSER_VERSION "v1.0-20.0716"
using GnssSvFlags = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvFlags;
using GnssStatusValue = ::android::hardware::gnss::V1_0::IGnssCallback::GnssStatusValue;
using GnssLocationFlags = android::hardware::gnss::V1_0::GnssLocationFlags;
using GnssMax = ::android::hardware::gnss::V1_0::GnssMax;
using GnssLocation = ::android::hardware::gnss::V1_0::GnssLocation;
using GnssConstellationType = ::android::hardware::gnss::V1_0::GnssConstellationType;
using GnssSvInfo = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvInfo;
using GnssSvStatus = ::android::hardware::gnss::V1_0::IGnssCallback::GnssSvStatus;
using GnssUtcTime = ::android::hardware::gnss::V1_0::GnssUtcTime;
using GnssAidingData = ::android::hardware::gnss::V1_0::IGnss::GnssAidingData;
using GnssPositionMode = ::android::hardware::gnss::V1_0::IGnss::GnssPositionMode;
using GnssPositionRecurrence = ::android::hardware::gnss::V1_0::IGnss::GnssPositionRecurrence;
//GGA 定位信息
enum eGNSS_GGA_ITEM_T {
eGGA_Header = 0,
eGGA_UTCTime = 1,
eGGA_Latitude = 2,
eGGA_LatitudeHemi = 3,
eGGA_Longitude = 4,
eGGA_LongitudeHemi = 5,
eGGA_StatusIndicator = 6,
eGGA_SatellitesCount = 7,
eGGA_HDOP = 8,
eGGA_Altitude = 9,
eGGA_AltitudeUnit = 10,
eGGA_GeoidHeight = 11,
eGGA_GeoidHeightUnit = 12,
eGGA_DiffTemporal = 13,
eGGA_DiffStationId = 14,
eGGA_CheckSum = 15,
};
//GLL 定位地理信息
enum eGNSS_GLL_ITEM_T {
eGLL_Header = 0,
eGLL_Latitude = 1,
eGLL_LatitudeHemi = 2,
eGLL_Longitude = 3,
eGLL_LongitudeHemi = 4,
eGLL_UTCTime = 5,
eGLL_FixStatus = 6,
eGLL_PositioningMode = 7, //optional
eGLL_CheckSum = 8,
};
//GSA 当前卫星信息
enum eGNSS_GSA_ITEM_T {
eGSA_Header = 0,
eGSA_Mode = 1,
eGSA_Type = 2,
eGSA_PRN1 = 3,
eGSA_PRN2 = 4,
eGSA_PRN3 = 5,
eGSA_PRN4 = 6,
eGSA_PRN5 = 7,
eGSA_PRN6 = 8,
eGSA_PRN7 = 9,
eGSA_PRN8 = 10,
eGSA_PRN9 = 11,
eGSA_PRN10 = 12,
eGSA_PRN11 = 13,
eGSA_PRN12 = 14,
eGSA_PDOP = 15,
eGSA_HDOP = 16,
eGSA_VDOP = 17,
eGSA_CheckSum = 18,
};
//GSV 可见卫星信息
enum eGNSS_GSV_ITEM_T {
eGSV_Header = 0,
eGSV_ItemCount = 1,
eGSV_ItemSequence = 2,
eGSV_SatellitesCount = 3,
eGSV_PRNCode = 4,
eGSV_SatelliteElevation = 5,
eGSV_SatelliteAzimuth = 6,
eGSV_SignalNoiseRatio = 7,
eGSV_PRNCode2 = 8,
eGSV_SatelliteElevation2 = 9,
eGSV_SatelliteAzimuth2 = 10,
eGSV_SignalNoiseRatio2 = 11,
eGSV_PRNCode3 = 12,
eGSV_SatelliteElevation3 = 13,
eGSV_SatelliteAzimuth3 = 14,
eGSV_SignalNoiseRatio3 = 15,
eGSV_PRNCode4 = 16,
eGSV_SatelliteElevation4 = 17,
eGSV_SatelliteAzimuth4 = 18,
eGSV_SignalNoiseRatio4 = 19,
eGSV_CheckSum = 20,
};
// RMC
enum eGNSS_RMC_ITEM_T {
eRMC_Header = 0,
eRMC_UTCTime = 1,
eRMC_FixStatus = 2,
eRMC_Latitude = 3,
eRMC_LatitudeHemi = 4,
eRMC_Longitude = 5,
eRMC_LongitudeHemi = 6,
eRMC_SpeedKnots = 7,
eRMC_Azimuth = 8,
eRMC_UTCDate = 9,
eRMC_MagneticDeclination = 10,
eRMC_MagneticDeclinationDirection = 11,
eRMC_PositioningMode = 12, //optional
eRMC_CheckSum = 13,
};
//VTG 地面速度信息
enum eGNSS_VTG_ITEM_T {
eVTG_Header = 0,
eVTG_MovementAngle = 1,
eVTG_TrueNorthRef = 2,
eVTG_MovementAngle2 = 3,
eVTG_MagneticNorthRef = 4,
eVTG_HorizontalMoveSpeed = 5,
eVTG_SpeedKnots = 6,
eVTG_HorizontalMoveSpeed2 = 7,
eVTG_SpeedKmh = 8,
eVTG_PositioningMode = 9, //optional
eVTG_CheckSum = 10,
};
//(GGA)GPS定位信息 Global Positioning System Fix Data
//$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*7B
struct XXGGA_Info_T {
double UTCTime; //UTC时间,格式为hhmmss.sss
double Latitude; //纬度,格式为ddmm.mmmm
char LatitudeHemi; //纬度半球,N或S
double Longitude; //经度,格式为dddmm.mmmm
char LongitudeHemi;//经度半球,E或W
uint16_t StatusIndicator;//GPS状态:0初始化,1单点定位,2码差分,3无效PPS,4固定解,5浮点解,6正在估算,7人工输入固定值,8模拟模式,9WAAS差分
size_t SatellitesCount; //使用卫星数量,从00到12
float HDOP; //HDOP-水平精度因子,0.5到99.9,一般认为HDOP越小,质量越好
double Altitude; //椭球高,-9999.9到9999.9米
char AltitudeUnit; //M指单位米
float GeoidHeight; //大地水准面高度异常差值,-9999.9到9999.9米
char GeoidHeightUnit; //M指单位米
int64_t DiffTemporal; //差分GPS数据期限(RTCM SC-104),最后设立RTCM传送的秒数量,如不是差分定位则为空
int16_t DiffStationId;//差分参考基站标号,从0000到1023
//char CheckSum[4]; //从$开始到*之间的所有ASCII码的异或校验
std::bitset<16> BitFlags;
};
//(GLL)定位地理信息 Geographic Position
struct XXGLL_Info_T {
double Latitude; //纬度ddmm.mmmm(度分)格式
char LatitudeHemi; //纬度半球N或S
double Longitude; //经度dddmm.mmmm(度分)格式
char LongitudeHemi;//经度半球E或W
int UTCTimeInt; //UTC时间,hhmmss(时分秒)格式
char FixStatus; //定位状态,A=有效定位,V=无效定位
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
//GSA 当前卫星信息 GPS DOP and Active Satellites (GSA)当前卫星信息
#define GSA_INFO_PRN_CNT 12
struct XXGSA_Info_T {
char Mode; //定位模式,A=自动手动2D/3D,M=手动2D/3D
uint16_t Type; //定位类型,1=未定位,2=2D定位,3=3D定位
int PRNList[GSA_INFO_PRN_CNT]; //PRN码-伪随机噪声码,第x信道正在使用的卫星PRN码编号
float PDOP; //accuracy PDOP综合位置精度因子,0.5-99.9
float HDOP; //HDOP水平精度因子0.5-99.9
float VDOP; //VDOP垂直精度因子0.5-99.9
//char CheckSum[4];
std::bitset<32> BitFlags;
};
//(GSV)可见卫星信息 可见卫星信息 GPS Satellites in View
struct XXGSV_Info_T {
uint8_t Ext_constellation; //UNKNOWN==0/GPS==1/SBAS==2/GLONASS==3/QZSS==4/BEIDOU==5/GALILEO==6
size_t ItemCount; //GSV语句的总数
int ItemSequence; //本句GSV的编号
int SatellitesCount; //可见卫星的总数,00~12,前面的0也将被传输
int PRNCode; //PRN码-伪随机噪声码,前面的0也将被传输;GPS:1-32,Beidou:1-37,GLONASS:1-24,Galileo:1-36,...
float SatelliteElevation;//卫星仰角,00~90度,前面的0也将被传输
float SatelliteAzimuth; //卫星方位角,000~359度,前面的0也将被传输
float SignalNoiseRatio; //信噪比,00~99dB,没有跟踪到卫星时为空,前面的0也将被传输
int PRNCode2; //按照每颗卫星进行循环显示,每条GSV语句最多可以显示4颗卫星的信息
float SatelliteElevation2;
float SatelliteAzimuth2;
float SignalNoiseRatio2;
int PRNCode3;
float SatelliteElevation3;
float SatelliteAzimuth3;
float SignalNoiseRatio3;
int PRNCode4;
float SatelliteElevation4;
float SatelliteAzimuth4;
float SignalNoiseRatio4;
//char CheckSum[4];
std::bitset<32> BitFlags;
};
// (RMC)推荐定位信息 Recommended Minimum Specific GPS/TRANSIT Data
//$GPRMC,200808.000,A,3114.4591,N,12118.0993,E,0.82,282.15,191220,,,A*61
struct XXRMC_Info_T {
double UTCTime; //UTC时间,hhmmss.sss格式,格林尼治时间;
char FixStatus; //状态,A=定位,V=未定位;当给GPS复位时为V,不输出速度,角度,时间数据;
double Latitude; //纬度ddmm.mmmm,度分格式
char LatitudeHemi; //纬度N或S
double Longitude; //经度dddmm.mmmm,度分格式
char LongitudeHemi; //经度E或W
float SpeedKnots; //速度,节,Knots
float Azimuth; //方位角,度 Azimuth/bearing
int UTCDate; //UTC日期,DDMMYY格式
float MagneticDeclination; //磁偏角,000-180度
char MagneticDeclinationDirection;//磁偏角方向,E或W
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
//(VTG)地面速度信息 Track Made Good and Ground Speed
struct XXVTG_Info_T {
float MovementAngleTN; //以真北为参考基准的地面航向,000~359度,前面的0也将被传输
char TrueNorthRef; //T
float MovementAngleMN; //以磁北为参考基准的地面航向,000~359度,前面的0也将被传输
char MagneticNorthRef; //M
float HorizontalMoveSpeedKn; //地面速率,000.0~999.9节,前面的0也将被传输
char SpeedKnots; //N
float HorizontalMoveSpeedKm; //地面速率,0000.0~1851.8Km/h,前面的0也将被传输
char SpeedKmh; //K
char PositioningMode; //模式指示,仅NMEA0183 v3.00输出,A=自主定位,D=差分,E=估算,N=数据无效
//char CheckSum[4];
std::bitset<16> BitFlags;
};
template <class T>
T stringToNumber(const std::string &sstr)
{
T number {};
std::istringstream iss {};
iss.str(sstr);
iss >> number; /* can auto remove leading 0 */
return number;
}
template <class T>
std::string toString(T &value)
{
std::ostringstream oss {};
oss << value;
return oss.str();
}
template <int BS_MAX_SIZE>
void bitsFlagSet(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
{
bs.set(pos);
}
template <int BS_MAX_SIZE>
void bitsFlagReset(std::bitset<BS_MAX_SIZE> &bs, size_t pos)
{
bs.reset(pos);
}
template <int BS_MAX_SIZE>
void bitsFlagClear(std::bitset<BS_MAX_SIZE> &bs)
{
bs.reset();
}
template <int BS_MAX_SIZE>
bool bitsFlagTest(std::bitset<BS_MAX_SIZE> bs, size_t pos)
{
return bs.test(pos);
}
template <int BS_MAX_SIZE>
std::string bitsFlagToString(std::bitset<BS_MAX_SIZE> &bs)
{
return bs.to_string();
}
class GnssNmeaParser
{
public:
GnssNmeaParser();
~GnssNmeaParser();
int parse(const std::string &nmea);
int getNmeaLines(std::vector<std::string> &lines);
bool getGnssLocation(GnssLocation &gnssLocation);
bool getGnssSvStatus(GnssSvStatus &gnssSvStatus);
GnssUtcTime getUtcTime();
private:
int procGGA(XXGGA_Info_T &gga);
int procGLL(XXGLL_Info_T &gll);
int procGSA(std::vector<XXGSA_Info_T> &gsaVect);
int procGSV(std::vector<XXGSV_Info_T> &gsvVect);
int procRMC(XXRMC_Info_T &rmc);
int procVTG(XXVTG_Info_T &vtg);
/* convert latitude(ddmm.mmmm) and longitude(dddmm.mmmm) to degrees */
double latLongToDegree(const double dddmm_mmmm);
int gnssSvFlagUsedInFix(const int svid);
void updateUtcTime(const int ddmmyy, const double hhmmss_sss);
void updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi);
void updateAltitude(const double altitude);
void updateBearing(const float bearing);
void updateMagDec(const float magDec, const char magDecDir);
void updateAccuracy(const float pdop, const float hdop, const float vdop);
void updateSpeed(const float speed);
void updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT);
void removeChecksum(std::string &str);
uint8_t getNConstellation(const std::string &nmeaHead);
bool startsWith(const std::string &src, const std::string &str);
bool endsWith(const std::string &src, const std::string &str);
std::string replace(const std::string &raw, const std::string &oldstr, const std::string &newstr);
size_t split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr);
void reset();
std::vector<std::string> m_nmeaLines;
std::string m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
std::string m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
std::vector<std::string> m_nmeaGGAvect;
std::vector<std::string> m_nmeaGLLvect;
std::vector<std::vector<std::string>> m_nmeaGSAvec2d;
std::vector<std::vector<std::string>> m_nmeaGSVvec2d;
std::vector<std::string> m_nmeaRMCvect;
std::vector<std::string> m_nmeaVTGvect;
XXGGA_Info_T m_ggaInfoT;
XXGLL_Info_T m_gllInfoT;
std::vector<XXGSA_Info_T> m_gsaVectInfoT;
std::vector<XXGSV_Info_T> m_gsvVectInfoT;
XXRMC_Info_T m_rmcInfoT;
XXVTG_Info_T m_vtgInfoT;
GnssUtcTime m_gnssUtcTime;
GnssLocation m_gnssLocation;
GnssSvStatus m_gnssSvStatus;
//GnssStatusValue m_gnssStatus;
};
#endif // HAL_GNSS_V1_0_GNSSNMEAPARSER_H
#include <log/log.h>
#include "GnssNmeaParser.h"
#ifdef LOG_TAG
#undef LOG_TAG
#endif
#define LOG_TAG "GnssNmeaParserV1.1"
GnssNmeaParser::GnssNmeaParser()
{
m_nmeaLineSep = GNSS_NMEA_LINE_SEP;
m_nmeaElementSep = GNSS_NMEA_ELEMENT_SEP;
ALOGI("GnssNmeaParser created. %s", GNSS_NMEA_PARSER_VERSION);
reset();
}
GnssNmeaParser::~GnssNmeaParser()
{
reset();
}
int GnssNmeaParser::parse(const std::string &nmea)
{
reset();
int gpCount = 0;
if (0 == nmea.size()) {
return 0;
}
(void)split(nmea, m_nmeaLineSep, m_nmeaLines);
std::vector<std::string>::iterator vsit;
std::vector<std::string> nmeaGSAvect;
std::vector<std::string> nmeaGSVvect;
std::string line;
for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
line = *vsit;
if (line.size() <= 6) {
//$GPxxx
continue;
}
removeChecksum(line);
gpCount += 1;
if (startsWith(line, "$GPGGA")) { //GGA
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$GLGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$BDGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
} else if (startsWith(line, "$GNGGA")) {
(void)split(line, m_nmeaElementSep, m_nmeaGGAvect);
}
else if (startsWith(line, "$GPGLL")) { //GLL
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$GLGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$BDGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
} else if (startsWith(line, "$GNGLL")) {
(void)split(line, m_nmeaElementSep, m_nmeaGLLvect);
}
else if (startsWith(line, "$GPGSA")) { //GSA
// may contain multi-line
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$GLGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$BDGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
} else if (startsWith(line, "$GNGSA")) {
(void)split(line, m_nmeaElementSep, nmeaGSAvect);
m_nmeaGSAvec2d.push_back(nmeaGSAvect);
}
else if (startsWith(line, "$GPGSV")) { //GSV
// may contain multi-line
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
} else if (startsWith(line, "$GLGSV")) {
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
} else if (startsWith(line, "$BDGSV")) {
(void)split(line, m_nmeaElementSep, nmeaGSVvect);
m_nmeaGSVvec2d.push_back(nmeaGSVvect);
}
else if (startsWith(line, "$GPRMC")) { //RMC
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$GLRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$BDRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
} else if (startsWith(line, "$GNRMC")) {
(void)split(line, m_nmeaElementSep, m_nmeaRMCvect);
}
else if (startsWith(line, "$GPVTG")) { //VTG
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$GLVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$BDVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
} else if (startsWith(line, "$GNVTG")) {
(void)split(line, m_nmeaElementSep, m_nmeaVTGvect);
}
else {
ALOGD("unkown line:%s", line.c_str());
}
}
(void)procGGA(m_ggaInfoT);
(void)procGLL(m_gllInfoT);
(void)procGSA(m_gsaVectInfoT);
(void)procGSV(m_gsvVectInfoT);
(void)procRMC(m_rmcInfoT);
(void)procVTG(m_vtgInfoT);
return gpCount;
}
int GnssNmeaParser::getNmeaLines(std::vector<std::string> &lines)
{
std::vector<std::string>::iterator vsit;
for (vsit = m_nmeaLines.begin(); vsit != m_nmeaLines.end(); vsit++) {
lines.push_back(*vsit+m_nmeaLineSep);
}
return lines.size();
}
int GnssNmeaParser::procGGA(XXGGA_Info_T &gga)
{
size_t vecSize = m_nmeaGGAvect.size();
if (vecSize != eGGA_CheckSum) {
ALOGD("%s invalid vector size:%zu, expected(%d)",
__func__, vecSize, eGGA_CheckSum);
return 0;
}
bitsFlagClear<16>(gga.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaGGAvect[i].length() != 0)
{
bitsFlagSet<16>(gga.BitFlags, i);
}
}
gga.UTCTime = stringToNumber<double>(m_nmeaGGAvect[eGGA_UTCTime]);
gga.Latitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Latitude]);
gga.LatitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LatitudeHemi]);
gga.Longitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Longitude]);
gga.LongitudeHemi = stringToNumber<char>(m_nmeaGGAvect[eGGA_LongitudeHemi]);
gga.StatusIndicator = stringToNumber<uint16_t>(m_nmeaGGAvect[eGGA_StatusIndicator]);
gga.SatellitesCount = stringToNumber<size_t>(m_nmeaGGAvect[eGGA_SatellitesCount]);
gga.HDOP = stringToNumber<float>(m_nmeaGGAvect[eGGA_HDOP]);
gga.Altitude = stringToNumber<double>(m_nmeaGGAvect[eGGA_Altitude]);
gga.AltitudeUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_AltitudeUnit]);
gga.GeoidHeight = stringToNumber<float>(m_nmeaGGAvect[eGGA_GeoidHeight]);
gga.GeoidHeightUnit = stringToNumber<char>(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
gga.DiffTemporal = stringToNumber<long>(m_nmeaGGAvect[eGGA_DiffTemporal]);
gga.DiffStationId = stringToNumber<int>(m_nmeaGGAvect[eGGA_DiffStationId]);
if (gga.StatusIndicator != 0) {
updateLatLong(gga.Latitude, gga.LatitudeHemi, gga.Longitude, gga.LongitudeHemi);
updateAltitude(gga.Altitude);
updateAccuracy(0.0f, gga.HDOP, 0.0f);
}
return m_nmeaGGAvect.size();
}
int GnssNmeaParser::procGLL(XXGLL_Info_T &gll)
{
size_t vecSize = m_nmeaGLLvect.size();
if ((vecSize != eGLL_CheckSum)
&& (vecSize != eGLL_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eGLL_CheckSum, eGLL_CheckSum-1);
return 0;
}
bitsFlagClear<16>(gll.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaGLLvect[i].length() != 0)
{
bitsFlagSet<16>(gll.BitFlags, i);
}
}
gll.Latitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Latitude]);
gll.LatitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LatitudeHemi]);
gll.Longitude = stringToNumber<double>(m_nmeaGLLvect[eGLL_Longitude]);
gll.LongitudeHemi = stringToNumber<char>(m_nmeaGLLvect[eGLL_LongitudeHemi]);
gll.UTCTimeInt = stringToNumber<int>(m_nmeaGLLvect[eGLL_UTCTime]);
gll.FixStatus = stringToNumber<char>(m_nmeaGLLvect[eGLL_FixStatus]);
if (vecSize == eGLL_CheckSum) {
gll.PositioningMode = stringToNumber<char>(m_nmeaGLLvect[eGLL_PositioningMode]);//optional
}
if ((gll.FixStatus == 'A') && (gll.PositioningMode != 'N')) {
updateLatLong(gll.Latitude, gll.LatitudeHemi, gll.Longitude, gll.LongitudeHemi);
}
return m_nmeaGLLvect.size();
}
int GnssNmeaParser::procGSA(std::vector<XXGSA_Info_T> &gsaVect)
{
if (m_nmeaGSAvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
return 0;
}
std::vector<std::vector<std::string>>::iterator vvsit;
std::vector<std::string> nmeaGSAvect {};
XXGSA_Info_T gsa {};
int prnSn = 0;
size_t vecSize = 0;
std::string nmeaHeader;
for (vvsit = m_nmeaGSAvec2d.begin(); vvsit != m_nmeaGSAvec2d.end(); vvsit++) {
nmeaGSAvect = *vvsit;
(void)memset((void *)&gsa, 0, sizeof(gsa));
bitsFlagClear<32>(gsa.BitFlags);
vecSize = nmeaGSAvect.size();
if (vecSize != eGSA_CheckSum) {
ALOGI("%s invalid vector size:%zu, expected(%d)",
__func__, vecSize, eGSA_CheckSum);
continue;
}
for (int i = 1; i < (int)vecSize; i++)
{
if (nmeaGSAvect[i].length() != 0)
{
bitsFlagSet<32>(gsa.BitFlags, i);
}
}
nmeaHeader = (nmeaGSAvect[eGSA_Header]);
gsa.Mode = stringToNumber<char>(nmeaGSAvect[eGSA_Mode]);
gsa.Type = stringToNumber<uint16_t>(nmeaGSAvect[eGSA_Type]);
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
gsa.PRNList[prnSn] = stringToNumber<int>(nmeaGSAvect[eGSA_PRN1+prnSn]);;
}
gsa.PDOP = stringToNumber<int>(nmeaGSAvect[eGSA_PDOP]);
gsa.HDOP = stringToNumber<int>(nmeaGSAvect[eGSA_HDOP]);
gsa.VDOP = stringToNumber<int>(nmeaGSAvect[eGSA_VDOP]);
gsaVect.push_back(gsa);
if ((gsa.Type <= 1)) {
ALOGD("%s Positioning Mode is %hu(1-untargeted,2-2D,3-3D)", __func__, gsa.Type);
} else {
updateAccuracy(gsa.PDOP, gsa.HDOP, gsa.VDOP);
}
}
return m_nmeaGSAvec2d.size();
}
int GnssNmeaParser::procGSV(std::vector<XXGSV_Info_T> &gsvVect)
{
if (m_nmeaGSVvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
return 0;
}
std::vector<std::vector<std::string>>::iterator vvsit;
std::vector<std::string> nmeaGSVvect {};
XXGSV_Info_T gsv {};
std::string nmeaHeader;
size_t vecSize = 0;
for (vvsit = m_nmeaGSVvec2d.begin(); vvsit != m_nmeaGSVvec2d.end(); vvsit++) {
nmeaGSVvect = *vvsit;
(void)memset((void *)&gsv, 0, sizeof(gsv));
bitsFlagClear<32>(gsv.BitFlags);
vecSize = nmeaGSVvect.size();
if ((0 != (vecSize % 4)) || (vecSize < eGSV_PRNCode)) {
ALOGI("%s invalid vector size:%zu, expected(8/12/16/20)", __func__, vecSize);
continue;
}
for (int i = 1; i < (int)vecSize; i++)
{
if (nmeaGSVvect[i].length() != 0)
{
bitsFlagSet<32>(gsv.BitFlags, i);
}
}
nmeaHeader = nmeaGSVvect[eGSV_Header];
gsv.Ext_constellation = getNConstellation(nmeaHeader);
gsv.ItemCount = stringToNumber<size_t>(nmeaGSVvect[eGSV_ItemCount]);
gsv.ItemSequence = stringToNumber<int>(nmeaGSVvect[eGSV_ItemSequence]);
gsv.SatellitesCount = stringToNumber<int>(nmeaGSVvect[eGSV_SatellitesCount]);
/* gsv slices count maybe 8/12/16/20 */
if (vecSize >= eGSV_PRNCode2) {
gsv.PRNCode = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode]);
gsv.SatelliteElevation = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation]);
gsv.SatelliteAzimuth = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth]);
gsv.SignalNoiseRatio = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio]);
}
if (vecSize >= eGSV_PRNCode3) {
gsv.PRNCode2 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode2]);
gsv.SatelliteElevation2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation2]);
gsv.SatelliteAzimuth2 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
gsv.SignalNoiseRatio2 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
}
if (vecSize >= eGSV_PRNCode4) {
gsv.PRNCode3 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode3]);
gsv.SatelliteElevation3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation3]);
gsv.SatelliteAzimuth3 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
gsv.SignalNoiseRatio3 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
}
if (vecSize == eGSV_CheckSum) {
gsv.PRNCode4 = stringToNumber<int>(nmeaGSVvect[eGSV_PRNCode4]);
gsv.SatelliteElevation4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteElevation4]);
gsv.SatelliteAzimuth4 = stringToNumber<float>(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
gsv.SignalNoiseRatio4 = stringToNumber<float>(nmeaGSVvect[eGSV_SignalNoiseRatio4]);
}
gsvVect.push_back(gsv);
}
updateGnssSvStatus(gsvVect);
return m_nmeaGSVvec2d.size();
}
int GnssNmeaParser::procRMC(XXRMC_Info_T &rmc)
{
size_t vecSize = m_nmeaRMCvect.size();
if ((vecSize != eRMC_CheckSum)
&& (vecSize != eRMC_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eRMC_CheckSum, eRMC_CheckSum-1);
return 0;
}
bitsFlagClear<16>(rmc.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaRMCvect[i].length() != 0)
{
bitsFlagSet<16>(rmc.BitFlags, i);
}
}
rmc.UTCTime = stringToNumber<double>(m_nmeaRMCvect[eRMC_UTCTime]);
rmc.FixStatus = stringToNumber<char>(m_nmeaRMCvect[eRMC_FixStatus]);
rmc.Latitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Latitude]);
rmc.LatitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LatitudeHemi]);
rmc.Longitude = stringToNumber<double>(m_nmeaRMCvect[eRMC_Longitude]);
rmc.LongitudeHemi = stringToNumber<char>(m_nmeaRMCvect[eRMC_LongitudeHemi]);
rmc.SpeedKnots = stringToNumber<float>(m_nmeaRMCvect[eRMC_SpeedKnots]);
rmc.Azimuth = stringToNumber<float>(m_nmeaRMCvect[eRMC_Azimuth]);
rmc.UTCDate = stringToNumber<int>(m_nmeaRMCvect[eRMC_UTCDate]);
rmc.MagneticDeclination = stringToNumber<float>(m_nmeaRMCvect[eRMC_MagneticDeclination]);
rmc.MagneticDeclinationDirection = stringToNumber<char>(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
if (vecSize == eRMC_CheckSum) {
rmc.PositioningMode = stringToNumber<char>(m_nmeaRMCvect[eRMC_PositioningMode]);//optional
}
if (rmc.FixStatus == 'A') {
updateUtcTime(rmc.UTCDate, rmc.UTCTime);
updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
updateBearing(rmc.Azimuth);
updateSpeed((1.852f * rmc.SpeedKnots / 3.6f));
updateMagDec(rmc.MagneticDeclination, rmc.MagneticDeclinationDirection);
} else if (rmc.FixStatus == 'V') {
ALOGW("%s FixStatus is V(A-targeted,V-untargeted)", __func__);
// UTCDate and UTCTime may be empty
//updateUtcTime(rmc.UTCDate, rmc.UTCTime);
updateLatLong(rmc.Latitude, rmc.LatitudeHemi, rmc.Longitude, rmc.LongitudeHemi);
//updateBearing(rmc.Azimuth);
} else {
//invalid data
ALOGD("%s invalid FixStatus(%c)", __func__, rmc.FixStatus);
}
return m_nmeaRMCvect.size();
}
int GnssNmeaParser::procVTG(XXVTG_Info_T &vtg)
{
size_t vecSize = m_nmeaVTGvect.size();
if ((vecSize != eVTG_CheckSum)
&& (vecSize != eVTG_CheckSum-1)) {
//PositioningMode is optional, only NMEA0183 v3.00 can provide PositioningMode
ALOGD("%s invalid vector size:%zu, expected(%d/%d)",
__func__, vecSize, eVTG_CheckSum, eVTG_CheckSum-1);
return 0;
}
bitsFlagClear<16>(vtg.BitFlags);
for (int i = 1; i < (int)vecSize; i++)
{
if (m_nmeaVTGvect[i].length() != 0)
{
bitsFlagSet<16>(vtg.BitFlags, i);
}
}
vtg.MovementAngleTN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle]);
vtg.TrueNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_TrueNorthRef]);
vtg.MovementAngleMN = stringToNumber<float>(m_nmeaVTGvect[eVTG_MovementAngle2]);
vtg.MagneticNorthRef = stringToNumber<char>(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
vtg.HorizontalMoveSpeedKn = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
vtg.SpeedKnots = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKnots]);
vtg.HorizontalMoveSpeedKm = stringToNumber<float>(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
vtg.SpeedKmh = stringToNumber<char>(m_nmeaVTGvect[eVTG_SpeedKmh]);
if (vecSize == eVTG_CheckSum) {
vtg.PositioningMode = stringToNumber<char>(m_nmeaVTGvect[eVTG_PositioningMode]);//optional
}
if ((vtg.PositioningMode == 'A') || (vtg.PositioningMode == 'D')) {
updateSpeed(vtg.HorizontalMoveSpeedKm/3.6f);
}
return m_nmeaVTGvect.size();
}
void GnssNmeaParser::updateUtcTime(const int ddmmyy, const double hhmmss_sss)
{
//get utc diff
time_t time_now = time(NULL);
struct tm tm_local {};
struct tm tm_utc {};
long time_local_sec = 0;
long time_utc_sec = 0;
long utc_diff_sec = 0;
//get fixed time
struct tm tm_gnss {};
time_t time_fixed = 0;
int utc_year = 0;
int utc_month = 0;
int utc_day = 0;
int utc_hour = 0;
int utc_minute = 0;
int utc_seconds = 0;
int hhmmss = (int)hhmmss_sss;
int milliseconds = 0;
gmtime_r(&time_now, &tm_utc);
localtime_r(&time_now, &tm_local);
time_local_sec = tm_local.tm_sec +
60*(tm_local.tm_min +
60*(tm_local.tm_hour +
24*(tm_local.tm_yday +
365*tm_local.tm_year)));
time_utc_sec = tm_utc.tm_sec +
60*(tm_utc.tm_min +
60*(tm_utc.tm_hour +
24*(tm_utc.tm_yday +
365*tm_utc.tm_year)));
utc_diff_sec = time_local_sec - time_utc_sec;
utc_day = (ddmmyy / 100) / 100;
utc_month = (ddmmyy / 100) % 100;
utc_year = (ddmmyy % 100) + 2000;
utc_hour = ((hhmmss / 100) / 100);
utc_minute = ((hhmmss / 100) % 100);
utc_seconds = (hhmmss % 100);
//milliseconds = (int)((hhmmss_sss - hhmmss) * 1000); //will less precise
milliseconds = (int)((hhmmss_sss * 1000) - (hhmmss * 1000)); //Improve accuracy
tm_gnss.tm_hour = utc_hour;
tm_gnss.tm_min = utc_minute;
tm_gnss.tm_sec = utc_seconds;
tm_gnss.tm_year = utc_year - 1900;
tm_gnss.tm_mon = utc_month - 1;
tm_gnss.tm_mday = utc_day;
tm_gnss.tm_isdst = -1;
time_fixed = mktime(&tm_gnss) + utc_diff_sec;
m_gnssUtcTime = (long long)time_fixed * 1000 + milliseconds;
m_gnssLocation.timestamp = m_gnssUtcTime;
if ((0 == ddmmyy) || (0 == hhmmss)) {
ALOGW("%s invalid UTCDate=%d, UTCTime=%d", __func__, ddmmyy, hhmmss);
//use local stored utc time
time_fixed = mktime(&tm_utc) + utc_diff_sec;
m_gnssUtcTime = (long long)time_fixed * 1000;
m_gnssLocation.timestamp = m_gnssUtcTime;
}
}
void GnssNmeaParser::updateLatLong(const double latitude, const char latHemi, const double longtitude, const char longHemi)
{
double lat = latitude;
double lon = longtitude;
if (latHemi == 'S') {
lat = -lat;
}
if (longHemi == 'W') {
lon = -lon;
}
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_LAT_LONG;
m_gnssLocation.latitudeDegrees = latLongToDegree(lat);
m_gnssLocation.longitudeDegrees = latLongToDegree(lon);
}
void GnssNmeaParser::updateAltitude(const double altitude)
{
double alt = altitude;
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE;
m_gnssLocation.altitudeMeters = alt;
}
void GnssNmeaParser::updateBearing(const float bearing)
{
float bea = bearing;
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING;
m_gnssLocation.bearingDegrees = bea;
}
void GnssNmeaParser::updateMagDec(const float magDec, const char magDecDir)
{
(void)magDec;
(void)magDecDir;
}
void GnssNmeaParser::updateAccuracy(const float pdop, const float hdop, const float vdop)
{
/* GSA HDOP */
(void)pdop;
(void)vdop;
if ((0.5f <= hdop) && (hdop <= 99.9f)) {
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
m_gnssLocation.horizontalAccuracyMeters = hdop;
}
if ((0.5f <= vdop) && (vdop <= 99.9f)) {
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_VERTICAL_ACCURACY;
m_gnssLocation.verticalAccuracyMeters = vdop;
}
}
void GnssNmeaParser::updateSpeed(const float speed)
{
float velocity = speed;
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED;
m_gnssLocation.speedMetersPerSec = velocity;
}
void GnssNmeaParser::updateGnssSvStatus(const std::vector<XXGSV_Info_T> &gsvVectInfoT)
{
std::vector<XXGSV_Info_T>::const_iterator vit;
int itemsCount = 0;
int itemSequence = 0;
int sateSeq = 0;
m_gnssSvStatus.numSvs = 0;
for (vit = gsvVectInfoT.begin(); vit != gsvVectInfoT.end(); vit++) {
itemsCount = vit->ItemCount;
itemSequence = vit->ItemSequence;
if ((sateSeq+3) > ((int)GnssMax::SVS_COUNT)-1) {
/* preventing arrays from out of bounds */
ALOGW("gnssSvStatus num more than SVS_COUNT:%d", GnssMax::SVS_COUNT);
break;
}
m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode;
m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation;
m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth;
m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio;
m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
if (bitsFlagTest<32>((*vit).BitFlags, eGSV_SignalNoiseRatio)) {
//m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
if (vit->PRNCode > 0) {
/* check and set flag whether available satellite */
m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode);
/* increase visible satellites count */
sateSeq += 1;
}
m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode2;
m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation2;
m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth2;
m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio2;
m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
if ( bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio2) ) {
//m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
if (vit->PRNCode2 > 0) {
m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode2);
sateSeq += 1;
}
m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode3;
m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation3;
m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth3;
m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio3;
m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio3)) {
//m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
if (vit->PRNCode3 > 0) {
m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode3);
sateSeq += 1;
}
m_gnssSvStatus.gnssSvList[sateSeq].svid = vit->PRNCode4;
m_gnssSvStatus.gnssSvList[sateSeq].elevationDegrees = vit->SatelliteElevation4;
m_gnssSvStatus.gnssSvList[sateSeq].azimuthDegrees = vit->SatelliteAzimuth4;
m_gnssSvStatus.gnssSvList[sateSeq].cN0Dbhz = vit->SignalNoiseRatio4;
m_gnssSvStatus.gnssSvList[sateSeq].svFlag = 0;
if (bitsFlagTest<32>(vit->BitFlags, eGSV_SignalNoiseRatio4)) {
//m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= GnssSvFlags:HAS_CARRIER_FREQUENCY;
}
m_gnssSvStatus.gnssSvList[sateSeq].constellation = (GnssConstellationType)vit->Ext_constellation;
if (vit->PRNCode4 > 0) {
m_gnssSvStatus.gnssSvList[sateSeq].svFlag |= gnssSvFlagUsedInFix(vit->PRNCode4);
sateSeq += 1;
}
}
m_gnssSvStatus.numSvs = sateSeq;
}
double GnssNmeaParser::latLongToDegree(const double dddmm_mmmm)
{
// eg.: 12031.0902 -> 120.51817(120+(31.0902/60.0=0.51817))
int ddd = (int)(dddmm_mmmm/100);
double mm_mmmm = dddmm_mmmm - (ddd*100.0);
double ddd_xxx = ddd + (mm_mmmm / 60.0);
return ddd_xxx;
}
int GnssNmeaParser::gnssSvFlagUsedInFix(const int svid) {
int fixed = 0;
int prnSn = 0;
std::vector<XXGSA_Info_T>::iterator gsaIt;
for (gsaIt = m_gsaVectInfoT.begin(); gsaIt != m_gsaVectInfoT.end(); gsaIt++) {
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
if (svid == gsaIt->PRNList[prnSn]) {
fixed = (int)GnssSvFlags::USED_IN_FIX;
break;
}
}
}
return fixed;
}
bool GnssNmeaParser::getGnssLocation(GnssLocation &gnssLocation)
{
(void)memcpy(&gnssLocation, &m_gnssLocation, sizeof(m_gnssLocation));
return true;
}
bool GnssNmeaParser::getGnssSvStatus(GnssSvStatus &gnssSvStatus)
{
#if 1
int numSv = m_gnssSvStatus.numSvs;
ALOGD("getGnssSvStatus numSvs:%d", m_gnssSvStatus.numSvs);
for(int tmp = 0; tmp < numSv; tmp++)
{
ALOGD("getGnssSvStatus (svid=%d,elevation=%f,azimuth=%f,dbhz=%f,CX=%d,svFlag=0x%x)",
m_gnssSvStatus.gnssSvList[tmp].svid,
m_gnssSvStatus.gnssSvList[tmp].elevationDegrees,
m_gnssSvStatus.gnssSvList[tmp].azimuthDegrees,
m_gnssSvStatus.gnssSvList[tmp].cN0Dbhz,
(int)m_gnssSvStatus.gnssSvList[tmp].constellation,
(int)m_gnssSvStatus.gnssSvList[tmp].svFlag);
}
#endif
(void)memcpy(&gnssSvStatus, &m_gnssSvStatus, sizeof(gnssSvStatus));
return true;
}
GnssUtcTime GnssNmeaParser::getUtcTime()
{
return m_gnssUtcTime;
}
void GnssNmeaParser::reset()
{
m_nmeaLines.clear();
m_nmeaGGAvect.clear();
m_nmeaGLLvect.clear();
m_nmeaGSAvec2d.clear();
m_nmeaGSVvec2d.clear();
m_nmeaRMCvect.clear();
m_nmeaVTGvect.clear();
m_gnssUtcTime = 0;
(void)memset(&m_ggaInfoT, 0, sizeof(m_ggaInfoT));
(void)memset(&m_gllInfoT, 0, sizeof(m_gllInfoT));
m_gsaVectInfoT.clear();
m_gsvVectInfoT.clear();
(void)memset(&m_rmcInfoT, 0, sizeof(m_rmcInfoT));
(void)memset(&m_vtgInfoT, 0, sizeof(m_vtgInfoT));
(void)memset(&m_gnssLocation, 0, sizeof(m_gnssLocation));
(void)memset(&m_gnssSvStatus, 0, sizeof(m_gnssSvStatus));
}
void GnssNmeaParser::removeChecksum(std::string &str)
{
// get rid of checksum at the end of the sentecne
// remove chars after *
size_t phit = 0;
phit = str.find("*");
if (std::string::npos != phit)
{
str.erase(phit);
}
}
uint8_t GnssNmeaParser::getNConstellation(const std::string &nmeaHead)
{
uint8_t constellation = (uint8_t)GnssConstellationType::UNKNOWN;
if (startsWith(nmeaHead, "$GP")) {
constellation = (uint8_t)GnssConstellationType::GPS;
} else if (startsWith(nmeaHead, "$GL")) {
constellation = (uint8_t)GnssConstellationType::GLONASS;
} else if (startsWith(nmeaHead, "$BD")) {
constellation = (uint8_t)GnssConstellationType::BEIDOU;
} else {
constellation = (uint8_t)GnssConstellationType::UNKNOWN;
}
return constellation;
}
bool GnssNmeaParser::startsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
return (0 == src.compare(srcpos, sublen, str));
}
bool GnssNmeaParser::endsWith(const std::string &src, const std::string &str)
{
int srcpos = 0;
int srclen = src.length();
int sublen = str.length();
if (srclen < sublen) {
return false;
}
srcpos = srclen - sublen;
return (0 == src.compare(srcpos, sublen, str));
}
std::string GnssNmeaParser::replace(const std::string &raw, const std::string &oldstr, const std::string &newstr) {
std::string res_string = raw;
size_t startpos = 0;
size_t retpos = 0;
while (std::string::npos != (retpos = res_string.find(oldstr, startpos)))
{
if (oldstr.size() == newstr.size()) {
(void)res_string.replace(retpos, oldstr.size(), newstr);
} else {
(void)res_string.erase(retpos, oldstr.size());
(void)res_string.insert(retpos, newstr);
}
startpos = retpos + oldstr.size();
}
return res_string;
}
size_t GnssNmeaParser::split(const std::string &line, const std::string &delim, std::vector<std::string> &vstr)
{
size_t pstart = 0;
size_t phit = 0;
std::string sstr;
size_t length = line.length();
vstr.clear();
for (;pstart <= length;)
{
phit = line.find(delim, pstart);
if (std::string::npos != phit)
{
/* find delim, get substr */
sstr = line.substr(pstart, phit-pstart);
vstr.push_back(sstr);
pstart = phit + delim.size();
} else {
/* not find delim, append remaining str and break */
vstr.push_back(line.substr(pstart));
break;
}
}
return vstr.size();
}