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
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 &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
#include
#include
#include
#include
#include
解析.cpp实现文件
#include
#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::iterator vsit;
std::vector nmeaGSAvect;
std::vector 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 &lines)
{
std::vector::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(m_nmeaGGAvect[eGGA_UTCTime]);
gga.Latitude = stringToNumber(m_nmeaGGAvect[eGGA_Latitude]);
gga.LatitudeHemi = stringToNumber(m_nmeaGGAvect[eGGA_LatitudeHemi]);
gga.Longitude = stringToNumber(m_nmeaGGAvect[eGGA_Longitude]);
gga.LongitudeHemi = stringToNumber(m_nmeaGGAvect[eGGA_LongitudeHemi]);
gga.StatusIndicator = stringToNumber(m_nmeaGGAvect[eGGA_StatusIndicator]);
gga.SatellitesCount = stringToNumber(m_nmeaGGAvect[eGGA_SatellitesCount]);
gga.HDOP = stringToNumber(m_nmeaGGAvect[eGGA_HDOP]);
gga.Altitude = stringToNumber(m_nmeaGGAvect[eGGA_Altitude]);
gga.AltitudeUnit = stringToNumber(m_nmeaGGAvect[eGGA_AltitudeUnit]);
gga.GeoidHeight = stringToNumber(m_nmeaGGAvect[eGGA_GeoidHeight]);
gga.GeoidHeightUnit = stringToNumber(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
gga.DiffTemporal = stringToNumber(m_nmeaGGAvect[eGGA_DiffTemporal]);
gga.DiffStationId = stringToNumber(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(m_nmeaGLLvect[eGLL_Latitude]);
gll.LatitudeHemi = stringToNumber(m_nmeaGLLvect[eGLL_LatitudeHemi]);
gll.Longitude = stringToNumber(m_nmeaGLLvect[eGLL_Longitude]);
gll.LongitudeHemi = stringToNumber(m_nmeaGLLvect[eGLL_LongitudeHemi]);
gll.UTCTimeInt = stringToNumber(m_nmeaGLLvect[eGLL_UTCTime]);
gll.FixStatus = stringToNumber(m_nmeaGLLvect[eGLL_FixStatus]);
if (vecSize == eGLL_CheckSum) {
gll.PositioningMode = stringToNumber(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 &gsaVect)
{
if (m_nmeaGSAvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
return 0;
}
std::vector>::iterator vvsit;
std::vector 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(nmeaGSAvect[eGSA_Mode]);
gsa.Type = stringToNumber(nmeaGSAvect[eGSA_Type]);
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
gsa.PRNList[prnSn] = stringToNumber(nmeaGSAvect[eGSA_PRN1+prnSn]);;
}
gsa.PDOP = stringToNumber(nmeaGSAvect[eGSA_PDOP]);
gsa.HDOP = stringToNumber(nmeaGSAvect[eGSA_HDOP]);
gsa.VDOP = stringToNumber(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 &gsvVect)
{
if (m_nmeaGSVvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
return 0;
}
std::vector>::iterator vvsit;
std::vector 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(nmeaGSVvect[eGSV_ItemCount]);
gsv.ItemSequence = stringToNumber(nmeaGSVvect[eGSV_ItemSequence]);
gsv.SatellitesCount = stringToNumber(nmeaGSVvect[eGSV_SatellitesCount]);
/* gsv slices count maybe 8/12/16/20 */
if (vecSize >= eGSV_PRNCode2) {
gsv.PRNCode = stringToNumber(nmeaGSVvect[eGSV_PRNCode]);
gsv.SatelliteElevation = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation]);
gsv.SatelliteAzimuth = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth]);
gsv.SignalNoiseRatio = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio]);
}
if (vecSize >= eGSV_PRNCode3) {
gsv.PRNCode2 = stringToNumber(nmeaGSVvect[eGSV_PRNCode2]);
gsv.SatelliteElevation2 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation2]);
gsv.SatelliteAzimuth2 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
gsv.SignalNoiseRatio2 = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
}
if (vecSize >= eGSV_PRNCode4) {
gsv.PRNCode3 = stringToNumber(nmeaGSVvect[eGSV_PRNCode3]);
gsv.SatelliteElevation3 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation3]);
gsv.SatelliteAzimuth3 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
gsv.SignalNoiseRatio3 = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
}
if (vecSize == eGSV_CheckSum) {
gsv.PRNCode4 = stringToNumber(nmeaGSVvect[eGSV_PRNCode4]);
gsv.SatelliteElevation4 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation4]);
gsv.SatelliteAzimuth4 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
gsv.SignalNoiseRatio4 = stringToNumber(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(m_nmeaRMCvect[eRMC_UTCTime]);
rmc.FixStatus = stringToNumber(m_nmeaRMCvect[eRMC_FixStatus]);
rmc.Latitude = stringToNumber(m_nmeaRMCvect[eRMC_Latitude]);
rmc.LatitudeHemi = stringToNumber(m_nmeaRMCvect[eRMC_LatitudeHemi]);
rmc.Longitude = stringToNumber(m_nmeaRMCvect[eRMC_Longitude]);
rmc.LongitudeHemi = stringToNumber(m_nmeaRMCvect[eRMC_LongitudeHemi]);
rmc.SpeedKnots = stringToNumber(m_nmeaRMCvect[eRMC_SpeedKnots]);
rmc.Azimuth = stringToNumber(m_nmeaRMCvect[eRMC_Azimuth]);
rmc.UTCDate = stringToNumber(m_nmeaRMCvect[eRMC_UTCDate]);
rmc.MagneticDeclination = stringToNumber(m_nmeaRMCvect[eRMC_MagneticDeclination]);
rmc.MagneticDeclinationDirection = stringToNumber(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
if (vecSize == eRMC_CheckSum) {
rmc.PositioningMode = stringToNumber(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(m_nmeaVTGvect[eVTG_MovementAngle]);
vtg.TrueNorthRef = stringToNumber(m_nmeaVTGvect[eVTG_TrueNorthRef]);
vtg.MovementAngleMN = stringToNumber(m_nmeaVTGvect[eVTG_MovementAngle2]);
vtg.MagneticNorthRef = stringToNumber(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
vtg.HorizontalMoveSpeedKn = stringToNumber(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
vtg.SpeedKnots = stringToNumber(m_nmeaVTGvect[eVTG_SpeedKnots]);
vtg.HorizontalMoveSpeedKm = stringToNumber(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
vtg.SpeedKmh = stringToNumber(m_nmeaVTGvect[eVTG_SpeedKmh]);
if (vecSize == eVTG_CheckSum) {
vtg.PositioningMode = stringToNumber(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.1f <= hdop)) {
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 &gsvVectInfoT)
{
std::vector::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::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 &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
#include
#include
#include
#include
#include
#include
#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::iterator vsit;
std::vector nmeaGSAvect;
std::vector 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 &lines)
{
std::vector::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(m_nmeaGGAvect[eGGA_UTCTime]);
gga.Latitude = stringToNumber(m_nmeaGGAvect[eGGA_Latitude]);
gga.LatitudeHemi = stringToNumber(m_nmeaGGAvect[eGGA_LatitudeHemi]);
gga.Longitude = stringToNumber(m_nmeaGGAvect[eGGA_Longitude]);
gga.LongitudeHemi = stringToNumber(m_nmeaGGAvect[eGGA_LongitudeHemi]);
gga.StatusIndicator = stringToNumber(m_nmeaGGAvect[eGGA_StatusIndicator]);
gga.SatellitesCount = stringToNumber(m_nmeaGGAvect[eGGA_SatellitesCount]);
gga.HDOP = stringToNumber(m_nmeaGGAvect[eGGA_HDOP]);
gga.Altitude = stringToNumber(m_nmeaGGAvect[eGGA_Altitude]);
gga.AltitudeUnit = stringToNumber(m_nmeaGGAvect[eGGA_AltitudeUnit]);
gga.GeoidHeight = stringToNumber(m_nmeaGGAvect[eGGA_GeoidHeight]);
gga.GeoidHeightUnit = stringToNumber(m_nmeaGGAvect[eGGA_GeoidHeightUnit]);
gga.DiffTemporal = stringToNumber(m_nmeaGGAvect[eGGA_DiffTemporal]);
gga.DiffStationId = stringToNumber(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(m_nmeaGLLvect[eGLL_Latitude]);
gll.LatitudeHemi = stringToNumber(m_nmeaGLLvect[eGLL_LatitudeHemi]);
gll.Longitude = stringToNumber(m_nmeaGLLvect[eGLL_Longitude]);
gll.LongitudeHemi = stringToNumber(m_nmeaGLLvect[eGLL_LongitudeHemi]);
gll.UTCTimeInt = stringToNumber(m_nmeaGLLvect[eGLL_UTCTime]);
gll.FixStatus = stringToNumber(m_nmeaGLLvect[eGLL_FixStatus]);
if (vecSize == eGLL_CheckSum) {
gll.PositioningMode = stringToNumber(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 &gsaVect)
{
if (m_nmeaGSAvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSAvec2d.size());
return 0;
}
std::vector>::iterator vvsit;
std::vector 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(nmeaGSAvect[eGSA_Mode]);
gsa.Type = stringToNumber(nmeaGSAvect[eGSA_Type]);
for (prnSn = 0; prnSn < GSA_INFO_PRN_CNT; prnSn++) {
gsa.PRNList[prnSn] = stringToNumber(nmeaGSAvect[eGSA_PRN1+prnSn]);;
}
gsa.PDOP = stringToNumber(nmeaGSAvect[eGSA_PDOP]);
gsa.HDOP = stringToNumber(nmeaGSAvect[eGSA_HDOP]);
gsa.VDOP = stringToNumber(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 &gsvVect)
{
if (m_nmeaGSVvec2d.size() == 0) {
ALOGD("%s invalid vector size:%zu", __func__, m_nmeaGSVvec2d.size());
return 0;
}
std::vector>::iterator vvsit;
std::vector 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(nmeaGSVvect[eGSV_ItemCount]);
gsv.ItemSequence = stringToNumber(nmeaGSVvect[eGSV_ItemSequence]);
gsv.SatellitesCount = stringToNumber(nmeaGSVvect[eGSV_SatellitesCount]);
/* gsv slices count maybe 8/12/16/20 */
if (vecSize >= eGSV_PRNCode2) {
gsv.PRNCode = stringToNumber(nmeaGSVvect[eGSV_PRNCode]);
gsv.SatelliteElevation = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation]);
gsv.SatelliteAzimuth = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth]);
gsv.SignalNoiseRatio = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio]);
}
if (vecSize >= eGSV_PRNCode3) {
gsv.PRNCode2 = stringToNumber(nmeaGSVvect[eGSV_PRNCode2]);
gsv.SatelliteElevation2 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation2]);
gsv.SatelliteAzimuth2 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth2]);
gsv.SignalNoiseRatio2 = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio2]);
}
if (vecSize >= eGSV_PRNCode4) {
gsv.PRNCode3 = stringToNumber(nmeaGSVvect[eGSV_PRNCode3]);
gsv.SatelliteElevation3 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation3]);
gsv.SatelliteAzimuth3 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth3]);
gsv.SignalNoiseRatio3 = stringToNumber(nmeaGSVvect[eGSV_SignalNoiseRatio3]);
}
if (vecSize == eGSV_CheckSum) {
gsv.PRNCode4 = stringToNumber(nmeaGSVvect[eGSV_PRNCode4]);
gsv.SatelliteElevation4 = stringToNumber(nmeaGSVvect[eGSV_SatelliteElevation4]);
gsv.SatelliteAzimuth4 = stringToNumber(nmeaGSVvect[eGSV_SatelliteAzimuth4]);
gsv.SignalNoiseRatio4 = stringToNumber(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(m_nmeaRMCvect[eRMC_UTCTime]);
rmc.FixStatus = stringToNumber(m_nmeaRMCvect[eRMC_FixStatus]);
rmc.Latitude = stringToNumber(m_nmeaRMCvect[eRMC_Latitude]);
rmc.LatitudeHemi = stringToNumber(m_nmeaRMCvect[eRMC_LatitudeHemi]);
rmc.Longitude = stringToNumber(m_nmeaRMCvect[eRMC_Longitude]);
rmc.LongitudeHemi = stringToNumber(m_nmeaRMCvect[eRMC_LongitudeHemi]);
rmc.SpeedKnots = stringToNumber(m_nmeaRMCvect[eRMC_SpeedKnots]);
rmc.Azimuth = stringToNumber(m_nmeaRMCvect[eRMC_Azimuth]);
rmc.UTCDate = stringToNumber(m_nmeaRMCvect[eRMC_UTCDate]);
rmc.MagneticDeclination = stringToNumber(m_nmeaRMCvect[eRMC_MagneticDeclination]);
rmc.MagneticDeclinationDirection = stringToNumber(m_nmeaRMCvect[eRMC_MagneticDeclinationDirection]);
if (vecSize == eRMC_CheckSum) {
rmc.PositioningMode = stringToNumber(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(m_nmeaVTGvect[eVTG_MovementAngle]);
vtg.TrueNorthRef = stringToNumber(m_nmeaVTGvect[eVTG_TrueNorthRef]);
vtg.MovementAngleMN = stringToNumber(m_nmeaVTGvect[eVTG_MovementAngle2]);
vtg.MagneticNorthRef = stringToNumber(m_nmeaVTGvect[eVTG_MagneticNorthRef]);
vtg.HorizontalMoveSpeedKn = stringToNumber(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed]);
vtg.SpeedKnots = stringToNumber(m_nmeaVTGvect[eVTG_SpeedKnots]);
vtg.HorizontalMoveSpeedKm = stringToNumber(m_nmeaVTGvect[eVTG_HorizontalMoveSpeed2]);
vtg.SpeedKmh = stringToNumber(m_nmeaVTGvect[eVTG_SpeedKmh]);
if (vecSize == eVTG_CheckSum) {
vtg.PositioningMode = stringToNumber(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.1f <= hdop)) {
m_gnssLocation.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
m_gnssLocation.horizontalAccuracyMeters = hdop;
}
if ((0.1f <= vdop)) {
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 &gsvVectInfoT)
{
std::vector::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::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 &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();
}