MiPad4/android_device_xiaomi_sdm660-common
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

move nmea generation logic inside utils

Browse source 
NMEA generation logic needs to be reused across the
interface boundaries. Moved this logic inside utils.

Change-Id: Icb5c6fbc38b178c5edea468d26286e21749bfbfb
CRs-Fixed: 1098734
This commit is contained in:
Naresh Munagala 2016-12-23 10:42:44 +05:30 committed by Baili Feng
parent a760fcb28c
commit 06e6d68aee
17 changed files with 1157 additions and 720 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Makefile.am
View file

@ -3,7 +3,7 @@
ACLOCAL_AMFLAGS = -I m4
SUBDIRS = utils core loc_api
SUBDIRS = core loc_api
pkgconfigdir = $(libdir)/pkgconfig
pkgconfig_DATA = loc-hal.pc

4
configure.ac
View file

@ -45,6 +45,10 @@ PKG_CHECK_MODULES([LOCPLA], [loc-pla])
AC_SUBST([LOCPLA_CFLAGS])
AC_SUBST([LOCPLA_LIBS])
PKG_CHECK_MODULES([GPSUTILS], [gps-utils])
AC_SUBST([GPSUTILS_CFLAGS])
AC_SUBST([GPSUTILS_LIBS])
AC_ARG_WITH([core_includes],
AC_HELP_STRING([--with-core-includes=@<:@dir@:>@],
[Specify the location of the core headers]),

3
core/Android.mk
View file

@ -50,9 +50,6 @@ LOCAL_COPY_HEADERS:= \
LocDualContext.h \
LBSProxyBase.h \
UlpProxyBase.h \
loc_gps.h \
gps_extended_c.h \
gps_extended.h \
loc_core_log.h \
LocAdapterProxyBase.h \
SystemStatus.h

8
core/Makefile.am
View file

@ -1,7 +1,8 @@
AM_CFLAGS = -I./ \
-I../utils \
$(LOCPLA_CFLAGS) \
-I$(WORKSPACE)/gps-noship/flp \
$(GPSUTILS_CFLAGS) \
-I$(WORKSPACE)/gps-noship/flp \
-D__func__=__PRETTY_FUNCTION__ \
-fno-short-enums
@ -12,9 +13,6 @@ libloc_core_la_h_sources = \
LocDualContext.h \
LBSProxyBase.h \
UlpProxyBase.h \
loc_gps.h \
gps_extended_c.h \
gps_extended.h \
loc_core_log.h \
LocAdapterProxyBase.h \
SystemStatus.h
@ -43,7 +41,7 @@ libloc_core_la_LDFLAGS = -Wl,-z,defs -lpthread -shared -version-info 1:0:0
libloc_core_la_CPPFLAGS = $(AM_CFLAGS) $(AM_CPPFLAGS)
endif
libloc_core_la_LIBADD = -lstdc++ -ldl $(LOCPLA_LIBS) ../utils/libgps_utils_so.la
libloc_core_la_LIBADD = -lstdc++ -ldl $(LOCPLA_LIBS) $(GPSUTILS_LIBS)
#Create and Install libraries
lib_LTLIBRARIES = libloc_core.la

687
gnss/GnssAdapter.cpp
View file

@ -42,6 +42,10 @@
#include <Agps.h>
#include <SystemStatus.h>
#include <loc_nmea.h>
#include <vector>
#include <string>
using namespace loc_core;
GnssAdapter::GnssAdapter() :
@ -51,7 +55,6 @@ GnssAdapter::GnssAdapter() :
LocDualContext::mLocationHalName,
false)),
mUlpProxy(new UlpProxyBase()),
mSuplMode(GNSS_SUPL_MODE_STANDALONE),
mUlpPositionMode(),
mGnssSvIdUsedInPosition(),
mGnssSvIdUsedInPosAvail(false),
@ -786,7 +789,7 @@ uint32_t
GnssAdapter::gnssDeleteAidingDataCommand(GnssAidingData& data)
{
uint32_t sessionId = generateSessionId();
LOC_LOGD("%s]: client %p id %u", __func__, sessionId);
LOC_LOGD("%s]: id %u", __func__, sessionId);
struct MsgDeleteAidingData : public LocMsg {
GnssAdapter& mAdapter;
@ -855,7 +858,7 @@ void
GnssAdapter::injectTimeCommand(int64_t time, int64_t timeReference, int32_t uncertainty)
{
LOC_LOGD("%s]: time %lld timeReference %lld uncertainty %d",
__func__, time, timeReference, uncertainty);
__func__, (long long)time, (long long)timeReference, uncertainty);
struct MsgInjectTime : public LocMsg {
LocApiBase& mApi;
@ -1328,8 +1331,6 @@ GnssAdapter::startTracking(const LocationOptions& options)
loc_api_adapter_err apiErr = mLocApi->startFix(locPosMode);
if (LOC_API_ADAPTER_ERR_SUCCESS == apiErr) {
err = LOCATION_ERROR_SUCCESS;
// save supl mode, which is used for NMEA generation
setSuplMode(options.mode);
} else {
err = LOCATION_ERROR_GENERAL_FAILURE;
}
@ -1383,8 +1384,6 @@ GnssAdapter::startTrackingCommand()
// ulp would be doing the multiplexing for us if it is present
LocPosMode& ulpPositionMode = mAdapter.getUlpPositionMode();
mApi.startFix(ulpPositionMode);
// save supl mode, which is used for NMEA generation
mAdapter.setSuplMode((GnssSuplMode)ulpPositionMode.mode);
}
};
@ -1884,10 +1883,11 @@ GnssAdapter::reportPosition(const UlpLocation& ulpLocation,
}
if (NMEA_PROVIDER_AP == ContextBase::mGps_conf.NMEA_PROVIDER && !mTrackingSessions.empty()) {
if (reported && status != LOC_SESS_FAILURE) {
generateNmea(ulpLocation, locationExtended);
} else {
generateNmeaBlank();
uint8_t generate_nmea = (reported && status != LOC_SESS_FAILURE);
std::vector<std::string> nmeaArraystr;
loc_nmea_generate_pos(ulpLocation, locationExtended, generate_nmea, nmeaArraystr);
for (auto sentence : nmeaArraystr) {
reportNmea(sentence.c_str(), sentence.length());
}
}
@ -1974,7 +1974,11 @@ GnssAdapter::reportSv(GnssSvNotification& svNotify)
}
if (NMEA_PROVIDER_AP == ContextBase::mGps_conf.NMEA_PROVIDER && !mTrackingSessions.empty()) {
generateNmea(svNotify);
std::vector<std::string> nmeaArraystr;
loc_nmea_generate_sv(svNotify, nmeaArraystr);
for (auto sentence : nmeaArraystr) {
reportNmea(sentence.c_str(), sentence.length());
}
}
mGnssSvIdUsedInPosAvail = false;
@ -2257,665 +2261,6 @@ GnssAdapter::reportSvPolynomialEvent(GnssSvPolynomial &svPolynomial)
mUlpProxy->reportSvPolynomial(svPolynomial);
}
int
GnssAdapter::nmeaPutChecksum(char *nmea, size_t maxSize)
{
uint8_t checksum = 0;
int length = 0;
nmea++; //skip the $
while (*nmea != '\0') {
checksum ^= *nmea++;
length++;
}
// length now contains nmea sentence string length not including $ sign.
int checksumLength = snprintf(nmea,(maxSize-length-1),"*%02X\r\n", checksum);
// total length of nmea sentence is length of nmea sentence inc $ sign plus
// length of checksum (+1 is to cover the $ character in the length).
return (length + checksumLength + 1);
}
void
GnssAdapter::generateNmea(const GnssSvNotification& svNotify)
{
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
// ------$GPGSV------
NmeaSvMeta gpsSvMeta =
{GNSS_SV_TYPE_GPS, "GP", 0, 0};
generateNmeaGSV(svNotify, gpsSvMeta, sentence, sizeof(sentence));
// ------$GLGSV------
NmeaSvMeta gloSvMeta =
{GNSS_SV_TYPE_GLONASS, "GL", 0, GLONASS_SV_ID_OFFSET};
generateNmeaGSV(svNotify, gloSvMeta, sentence, sizeof(sentence));
// ------$GAGSV------
NmeaSvMeta galSvMeta =
{GNSS_SV_TYPE_GALILEO, "GA", 0, 0};
generateNmeaGSV(svNotify, galSvMeta, sentence, sizeof(sentence));
}
void
GnssAdapter::generateNmea(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended)
{
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
time_t utcTime(ulpLocation.gpsLocation.timestamp/1000);
tm * pTm = gmtime(&utcTime);
if (NULL == pTm) {
LOC_LOGE("%s]: gmtime failed", __func__);
return;
}
uint32_t svUsedCount = 0;
uint32_t count = 0;
bool isCombinedFix = (mGnssSvIdUsedInPosition.gps_sv_used_ids_mask ? 1 : 0) +
(mGnssSvIdUsedInPosition.glo_sv_used_ids_mask ? 1 : 0) +
(mGnssSvIdUsedInPosition.gal_sv_used_ids_mask ? 1 : 0) > 1;
NmeaSvMeta gnssSvMeta =
{GNSS_SV_TYPE_GPS, isCombinedFix ? "GN" : "GP",
mGnssSvIdUsedInPosition.gps_sv_used_ids_mask, 0};
// ---$GPGSA/$GNGSA---
NmeaSvMeta gpsSvMeta =
{GNSS_SV_TYPE_GPS, isCombinedFix ? "GN" : "GP",
mGnssSvIdUsedInPosition.gps_sv_used_ids_mask, 0};
count = generateNmeaGSA(locationExtended, gpsSvMeta, sentence, sizeof(sentence));
if (count > 0) {
svUsedCount += count;
gnssSvMeta = gpsSvMeta;
}
// ---$GLGSA/$GNGSA---
NmeaSvMeta gloSvMeta =
{GNSS_SV_TYPE_GLONASS, isCombinedFix ? "GN" : "GL",
mGnssSvIdUsedInPosition.glo_sv_used_ids_mask, GLONASS_SV_ID_OFFSET};
count = generateNmeaGSA(locationExtended, gloSvMeta, sentence, sizeof(sentence));
if (count > 0) {
svUsedCount += count;
gnssSvMeta = gloSvMeta;
}
// ---$GAGSA/$GNGSA---
NmeaSvMeta galSvMeta =
{GNSS_SV_TYPE_GALILEO, isCombinedFix ? "GN" : "GA",
mGnssSvIdUsedInPosition.gal_sv_used_ids_mask, 0};
count = generateNmeaGSA(locationExtended, galSvMeta, sentence, sizeof(sentence));
if (count > 0) {
svUsedCount += count;
gnssSvMeta = galSvMeta;
}
// ---$GPVTG/$GLVTG/$GAVTG/$GNVTG---
generateNmeaVTG(ulpLocation, locationExtended, gnssSvMeta,
sentence, sizeof(sentence));
// ---$GPRMC/$GLRMC/$GARMC/$GNRMC---
generateNmeaRMC(ulpLocation, locationExtended, gnssSvMeta,
*pTm, sentence, sizeof(sentence));
// ---$GPGGA/$GLGGA/$GAGGA/$GNGGA---
generateNmeaGGA(ulpLocation, locationExtended, gnssSvMeta,
*pTm, svUsedCount, sentence, sizeof(sentence));
}
void
GnssAdapter::generateNmeaBlank()
{
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
int length = 0;
strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
length = nmeaPutChecksum(sentence, sizeof(sentence));
reportNmea(sentence, length);
strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
length = nmeaPutChecksum(sentence, sizeof(sentence));
reportNmea(sentence, length);
strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
length = nmeaPutChecksum(sentence, sizeof(sentence));
reportNmea(sentence, length);
strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
length = nmeaPutChecksum(sentence, sizeof(sentence));
reportNmea(sentence, length);
strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
length = nmeaPutChecksum(sentence, sizeof(sentence));
reportNmea(sentence, length);
}
void
GnssAdapter::generateNmeaGSV(const GnssSvNotification& svNotify, NmeaSvMeta& svMeta,
char* sentence, size_t size)
{
if (!sentence || size == 0) {
LOC_LOGE("%s]: NMEA Error invalid argument.", __func__);
return;
}
char* pMarker = sentence;
int lengthRemaining = size;
int length = 0;
int sentenceCount = 0;
int sentenceNumber = 1;
int svCount = 0;
int sv = 1;
for (sv=1; sv <= svNotify.count; sv++) {
if (svMeta.svType == svNotify.gnssSvs[sv - 1].type) {
// cache the used in fix mask, as it will be needed to send $GPGSA
// during the position report
if (LOC_GNSS_SV_FLAGS_USED_IN_FIX ==
(svNotify.gnssSvs[sv - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) {
svCount++;
}
}
}
if (svCount == 0) {
// no svs in view, so just send a blank $--GSV sentence
snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,", svMeta.talker);
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
return;
}
sv = 1;
sentenceNumber = 1;
sentenceCount = svCount / 4 + (svCount % 4 != 0);
while (sentenceNumber <= sentenceCount) {
pMarker = sentence;
lengthRemaining = size;
length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d",
svMeta.talker, sentenceCount, sentenceNumber, svCount);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
for (int i=0; (sv <= svNotify.count) && (i < 4); sv++) {
if (svMeta.svType == svNotify.gnssSvs[sv - 1].type) {
length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
svNotify.gnssSvs[sv - 1].svId,
(int)(0.5 + svNotify.gnssSvs[sv - 1].elevation), //float to int
(int)(0.5 + svNotify.gnssSvs[sv - 1].azimuth)); //float to int
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (svNotify.gnssSvs[sv - 1].cN0Dbhz > 0) {
length = snprintf(pMarker, lengthRemaining,"%02d",
(int)(0.5 + svNotify.gnssSvs[sv - 1].cN0Dbhz)); //float to int
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
}
i++;
}
}
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
sentenceNumber++;
} //while
}
uint8_t
GnssAdapter::generateNmeaGSA(const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, char* sentence, size_t size)
{
if (!sentence || size == 0) {
LOC_LOGE("%s]: NMEA Error invalid arguments.", __func__);
return 0;
}
char* pMarker = sentence;
int lengthRemaining = size;
int length = 0;
uint8_t svUsedCount = 0;
uint32_t svUsedList[32] = {0};
char fixType = '\0';
uint32_t svIdOffset = svMeta.svIdOffset;
uint32_t mask = svMeta.mask;
for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) {
if (mask & 1) {
svUsedList[svUsedCount++] = i + svIdOffset;
}
mask = mask >> 1;
}
if (svUsedCount == 0 && LOC_GNSS_CONSTELLATION_GPS != svMeta.svType) {
return 0;
}
if (svUsedCount == 0) {
fixType = '1'; // no fix
} else if (svUsedCount <= 3) {
fixType = '2'; // 2D fix
} else {
fixType = '3'; // 3D fix
}
// Start printing the sentence
// Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v*cc
// a : Mode : A : Automatic, allowed to automatically switch 2D/3D
// x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix)
// xx : 12 SV ID
// p.p : Position DOP (Dilution of Precision)
// h.h : Horizontal DOP
// v.v : Vertical DOP
// cc : Checksum value
length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", svMeta.talker, fixType);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);;
return 0;
}
pMarker += length;
lengthRemaining -= length;
// Add first 12 satellite IDs
for (uint8_t i = 0; i < 12; i++) {
if (i < svUsedCount) {
length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]);
} else {
length = snprintf(pMarker, lengthRemaining, ",");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return 0;
}
pMarker += length;
lengthRemaining -= length;
}
// Add the position/horizontal/vertical DOP values
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
locationExtended.pdop,
locationExtended.hdop,
locationExtended.vdop);
} else {
length = snprintf(pMarker, lengthRemaining, ",,");
}
/* Sentence is ready, add checksum and broadcast */
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
return svUsedCount;
}
void
GnssAdapter::generateNmeaVTG(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, char* sentence, size_t size)
{
if (!sentence || size == 0) {
LOC_LOGE("%s]: NMEA Error invalid arguments.", __func__);
return;
}
char* pMarker = sentence;
int lengthRemaining = size;
int length = 0;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
{
float magTrack = ulpLocation.gpsLocation.bearing;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
{
float magTrack = ulpLocation.gpsLocation.bearing - locationExtended.magneticDeviation;
if (magTrack < 0.0)
magTrack += 360.0;
else if (magTrack > 360.0)
magTrack -= 360.0;
}
length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,",
svMeta.talker, ulpLocation.gpsLocation.bearing, magTrack);
}
else
{
length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", svMeta.talker);
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
{
float speedKnots = ulpLocation.gpsLocation.speed * (3600.0/1852.0);
float speedKmPerHour = ulpLocation.gpsLocation.speed * 3.6;
length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour);
}
else
{
length = snprintf(pMarker, lengthRemaining, ",N,,K,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (!(ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
else if (GNSS_SUPL_MODE_STANDALONE == mSuplMode)
length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
else
length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
}
void
GnssAdapter::generateNmeaRMC(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, tm& utcTime,
char* sentence, size_t size)
{
if (!sentence || size == 0) {
LOC_LOGE("NMEA Error invalid arguments.");
return;
}
int utcYear = utcTime.tm_year % 100; // 2 digit year
int utcMonth = utcTime.tm_mon + 1; // tm_mon starts at zero
int utcDay = utcTime.tm_mday;
int utcHours = utcTime.tm_hour;
int utcMinutes = utcTime.tm_min;
int utcSeconds = utcTime.tm_sec;
int utcMSeconds = (ulpLocation.gpsLocation.timestamp)%1000;
char* pMarker = sentence;
int lengthRemaining = size;
int length = 0;
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
svMeta.talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) {
double latitude = ulpLocation.gpsLocation.latitude;
double longitude = ulpLocation.gpsLocation.longitude;
char latHemisphere;
char lonHemisphere;
double latMinutes;
double lonMinutes;
if (latitude > 0) {
latHemisphere = 'N';
} else {
latHemisphere = 'S';
latitude *= -1.0;
}
if (longitude < 0) {
lonHemisphere = 'W';
longitude *= -1.0;
} else {
lonHemisphere = 'E';
}
latMinutes = fmod(latitude * 60.0 , 60.0);
lonMinutes = fmod(longitude * 60.0 , 60.0);
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
(uint8_t)floor(latitude), latMinutes, latHemisphere,
(uint8_t)floor(longitude),lonMinutes, lonHemisphere);
} else {
length = snprintf(pMarker, lengthRemaining,",,,,");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) {
float speedKnots = ulpLocation.gpsLocation.speed * (3600.0/1852.0);
length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
} else {
length = snprintf(pMarker, lengthRemaining, ",");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING) {
length = snprintf(pMarker, lengthRemaining, "%.1lf,", ulpLocation.gpsLocation.bearing);
} else {
length = snprintf(pMarker, lengthRemaining, ",");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
utcDay, utcMonth, utcYear);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) {
float magneticVariation = locationExtended.magneticDeviation;
char direction;
if (magneticVariation < 0.0) {
direction = 'W';
magneticVariation *= -1.0;
} else {
direction = 'E';
}
length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
magneticVariation, direction);
} else {
length = snprintf(pMarker, lengthRemaining, ",,");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (!(ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) {
length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
} else if (GNSS_SUPL_MODE_STANDALONE == mSuplMode) {
length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
} else {
length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
}
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
}
void
GnssAdapter::generateNmeaGGA(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, tm& utcTime, uint32_t svUsedCount,
char* sentence, size_t size)
{
if (!sentence || size == 0) {
LOC_LOGE("NMEA Error invalid arguments.");
return;
}
int utcYear = utcTime.tm_year % 100; // 2 digit year
int utcMonth = utcTime.tm_mon + 1; // tm_mon starts at zero
int utcDay = utcTime.tm_mday;
int utcHours = utcTime.tm_hour;
int utcMinutes = utcTime.tm_min;
int utcSeconds = utcTime.tm_sec;
int utcMSeconds = (ulpLocation.gpsLocation.timestamp)%1000;
char* pMarker = sentence;
int lengthRemaining = size;
int length = 0;
length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
svMeta.talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) {
double latitude = ulpLocation.gpsLocation.latitude;
double longitude = ulpLocation.gpsLocation.longitude;
char latHemisphere;
char lonHemisphere;
double latMinutes;
double lonMinutes;
if (latitude > 0) {
latHemisphere = 'N';
} else {
latHemisphere = 'S';
latitude *= -1.0;
}
if (longitude < 0) {
lonHemisphere = 'W';
longitude *= -1.0;
} else {
lonHemisphere = 'E';
}
latMinutes = fmod(latitude * 60.0 , 60.0);
lonMinutes = fmod(longitude * 60.0 , 60.0);
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
(uint8_t)floor(latitude), latMinutes, latHemisphere,
(uint8_t)floor(longitude),lonMinutes, lonHemisphere);
} else {
length = snprintf(pMarker, lengthRemaining,",,,,");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
char gpsQuality;
if (!(ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) {
gpsQuality = '0'; // 0 means no fix
} else if (GNSS_SUPL_MODE_STANDALONE == mSuplMode) {
gpsQuality = '1'; // 1 means GPS fix
} else {
gpsQuality = '2'; // 2 means DGPS fix
}
// Number of satellites in use, 00-12
if (svUsedCount > MAX_SATELLITES_IN_USE) {
svUsedCount = MAX_SATELLITES_IN_USE;
}
// Add the position/horizontal/vertical DOP values
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
gpsQuality, svUsedCount, locationExtended.hdop);
} else {
length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
gpsQuality, svUsedCount);
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) {
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
locationExtended.altitudeMeanSeaLevel);
} else {
length = snprintf(pMarker, lengthRemaining,",,");
}
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("%s:%d]: NMEA Error in string formatting", __func__, __LINE__);
return;
}
pMarker += length;
lengthRemaining -= length;
if ((ulpLocation.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
(locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) {
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
ulpLocation.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
} else {
length = snprintf(pMarker, lengthRemaining,",,,");
}
length = nmeaPutChecksum(sentence, size);
reportNmea(sentence, length);
}
/* INIT LOC AGPS MANAGER */
void GnssAdapter::initAgpsCommand(void* statusV4Cb){

26
gnss/GnssAdapter.h
View file

@ -85,7 +85,6 @@ class GnssAdapter : public LocAdapterBase {
/* ==== TRACKING ======================================================================= */
LocationSessionMap mTrackingSessions;
GnssSuplMode mSuplMode;
LocPosMode mUlpPositionMode;
GnssSvUsedInPosition mGnssSvIdUsedInPosition;
bool mGnssSvIdUsedInPosAvail;
@ -160,7 +159,6 @@ public:
void eraseTrackingSession(LocationAPI* client, uint32_t sessionId);
void setUlpPositionMode(const LocPosMode& mode) { mUlpPositionMode = mode; }
LocPosMode& getUlpPositionMode() { return mUlpPositionMode; }
void setSuplMode(GnssSuplMode mode) { mSuplMode = mode; }
LocationError startTrackingMultiplex(const LocationOptions& options);
LocationError startTracking(const LocationOptions& options);
LocationError stopTrackingMultiplex(LocationAPI* client, uint32_t id);
@ -233,30 +231,6 @@ public:
bool requestNiNotify(const GnssNiNotification& notify, const void* data);
void reportGnssMeasurementData(const GnssMeasurementsNotification& measurementsNotify);
/*==== NMEA Generation =============================================================== */
/*======== SVS ======================================================================= */
void generateNmea(const GnssSvNotification& svNotify);
void generateNmeaGSV(const GnssSvNotification& svNotify,
NmeaSvMeta& svMeta, char* sentence, size_t size);
/*======== POSITION ================================================================== */
void generateNmea(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended);
void generateNmeaBlank();
uint8_t generateNmeaGSA(const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, char* sentence, size_t size);
void generateNmeaVTG(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, char* sentence, size_t size);
void generateNmeaRMC(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, tm& utcTime, char* sentence, size_t size);
void generateNmeaGGA(const UlpLocation& ulpLocation,
const GpsLocationExtended& locationExtended,
NmeaSvMeta& svMeta, tm& utcTime, uint32_t svUsedCount,
char* sentence, size_t size);
/*======== UTILITIES ================================================================*/
int nmeaPutChecksum(char *nmea, size_t maxSize);
/*======== GNSSDEBUG ================================================================*/
bool getDebugReport(GnssDebugReport& report);

2
gnss/location_gnss.cpp
View file

@ -197,7 +197,7 @@ static uint32_t gnssDeleteAidingData(GnssAidingData& data)
if (NULL != gGnssAdapter) {
return gGnssAdapter->gnssDeleteAidingDataCommand(data);
} else {
return NULL;
return 0;
}
}

2
loc-hal.pc.in
View file

@ -6,5 +6,5 @@ includedir=@includedir@
Name: loc-hal
Description: QTI GPS Loc HAL
Version: @VERSION
Libs: -L${libdir} -lgps_utils_so -lloc_core -lloc_eng_so -lloc_ds_api -lloc_api_v02
Libs: -L${libdir} -lloc_core -lloc_eng_so -lloc_ds_api -lloc_api_v02
Cflags: -I${includedir} -I${includedir}/loc-hal/utils -I${includedir}/loc-hal/core -I${includedir}/loc-hal

12
utils/Android.mk
View file

@ -25,7 +25,8 @@ LOCAL_SRC_FILES += \
LocTimer.cpp \
LocThread.cpp \
MsgTask.cpp \
loc_misc_utils.cpp
loc_misc_utils.cpp \
loc_nmea.cpp
# Flag -std=c++11 is not accepted by compiler when LOCAL_CLANG is set to true
LOCAL_CFLAGS += \
@ -40,7 +41,8 @@ LOCAL_LDFLAGS += -Wl,--export-dynamic
## Includes
LOCAL_C_INCLUDES:= \
$(TARGET_OUT_HEADERS)/libloc_pla
$(TARGET_OUT_HEADERS)/libloc_pla \
$(TARGET_OUT_HEADERS)/liblocation_api \
LOCAL_COPY_HEADERS_TO:= gps.utils/
LOCAL_COPY_HEADERS:= \
@ -56,7 +58,11 @@ LOCAL_COPY_HEADERS:= \
loc_target.h \
loc_timer.h \
LocSharedLock.h \
loc_misc_utils.h
loc_misc_utils.h \
loc_nmea.h \
gps_extended_c.h \
gps_extended.h \
loc_gps.h
LOCAL_MODULE := libgps.utils

25
utils/Makefile.am
View file

@ -1,10 +1,8 @@
AM_CFLAGS = -Wundef \
-MD \
-Wno-trigraphs \
-g -O0 \
-fno-inline \
-fno-short-enums \
-fpic \
-I./ \
$(LOCPLA_CFLAGS)
AM_CPPFLAGS = -Wundef \
-I./ \
$(LOCPLA_CFLAGS)
@ -19,7 +17,11 @@ libgps_utils_so_la_h_sources = \
LocHeap.h \
LocThread.h \
LocTimer.h \
loc_misc_utils.h
loc_misc_utils.h \
loc_nmea.h \
gps_extended_c.h \
gps_extended.h \
loc_gps.h
libgps_utils_so_la_c_sources = \
linked_list.c \
@ -31,9 +33,9 @@ libgps_utils_so_la_c_sources = \
LocTimer.cpp \
LocThread.cpp \
MsgTask.cpp \
loc_misc_utils.cpp
loc_misc_utils.cpp \
loc_nmea.cpp
library_includedir = $(pkgincludedir)/utils
library_include_HEADERS = $(libgps_utils_so_la_h_sources)
@ -53,3 +55,8 @@ libgps_utils_so_la_LIBADD = -lcutils -lstdc++ -llog $(LOCPLA_LIBS)
#Create and Install libraries
lib_LTLIBRARIES = libgps_utils_so.la
library_includedir = $(pkgincludedir)
pkgconfigdir = $(libdir)/pkgconfig
pkgconfig_DATA = gps-utils.pc
EXTRA_DIST = $(pkgconfig_DATA)

59
utils/configure.ac Normal file
View file

@ -0,0 +1,59 @@
# configure.ac -- Autoconf script for gps gps-utils
#
# Process this file with autoconf to produce a configure script
# Requires autoconf tool later than 2.61
AC_PREREQ(2.61)
# Initialize the gps gps-utils package version 1.0.0
AC_INIT([gps-utils],1.0.0)
# Does not strictly follow GNU Coding standards
AM_INIT_AUTOMAKE([foreign])
# Disables auto rebuilding of configure, Makefile.ins
AM_MAINTAINER_MODE
# Verifies the --srcdir is correct by checking for the path
AC_CONFIG_SRCDIR([Makefile.am])
# defines some macros variable to be included by source
AC_CONFIG_HEADERS([config.h])
AC_CONFIG_MACRO_DIR([m4])
# Checks for programs.
AC_PROG_LIBTOOL
AC_PROG_CXX
AC_PROG_CC
AM_PROG_CC_C_O
AC_PROG_AWK
AC_PROG_CPP
AC_PROG_INSTALL
AC_PROG_LN_S
AC_PROG_MAKE_SET
PKG_PROG_PKG_CONFIG
PKG_CHECK_MODULES([LOCPLA], [loc-pla])
AC_SUBST([LOCPLA_CFLAGS])
AC_SUBST([LOCPLA_LIBS])
AC_ARG_WITH([glib],
AC_HELP_STRING([--with-glib],
[enable glib, building HLOS systems which use glib]))
if (test "x${with_glib}" = "xyes"); then
AC_DEFINE(ENABLE_USEGLIB, 1, [Define if HLOS systems uses glib])
PKG_CHECK_MODULES(GTHREAD, gthread-2.0 >= 2.16, dummy=yes,
AC_MSG_ERROR(GThread >= 2.16 is required))
PKG_CHECK_MODULES(GLIB, glib-2.0 >= 2.16, dummy=yes,
AC_MSG_ERROR(GLib >= 2.16 is required))
GLIB_CFLAGS="$GLIB_CFLAGS $GTHREAD_CFLAGS"
GLIB_LIBS="$GLIB_LIBS $GTHREAD_LIBS"
AC_SUBST(GLIB_CFLAGS)
AC_SUBST(GLIB_LIBS)
fi
AM_CONDITIONAL(USE_GLIB, test "x${with_glib}" = "xyes")
AC_CONFIG_FILES([ \
Makefile \
gps-utils.pc
])
AC_OUTPUT

10
utils/gps-utils.pc.in Normal file
View file

@ -0,0 +1,10 @@
prefix=@prefix@
exec_prefix=@exec_prefix@
libdir=@libdir@
includedir=@includedir@
Name: gps-utils
Description: QTI GPS Location utils
Version: @VERSION
Libs: -L${libdir} -lgps_utils_so
Cflags: -I${includedir}/gps-utils

0
core/gps_extended.h → utils/gps_extended.h
View file

0
core/gps_extended_c.h → utils/gps_extended_c.h
View file

0
core/loc_gps.h → utils/loc_gps.h
View file

991
utils/loc_nmea.cpp Normal file
View file

@ -0,0 +1,991 @@
/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of The Linux Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
* BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
* BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
* OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
* IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#define LOG_NDDEBUG 0
#define LOG_TAG "LocSvc_nmea"
#include <loc_nmea.h>
#include <math.h>
#include <platform_lib_includes.h>
#define GLONASS_SV_ID_OFFSET 64
#define MAX_SATELLITES_IN_USE 12
typedef struct loc_nmea_sv_meta_s
{
char talker[3];
LocGnssConstellationType svType;
uint32_t mask;
uint32_t svIdOffset;
} loc_nmea_sv_meta;
typedef struct loc_sv_cache_info_s
{
uint32_t gps_used_mask;
uint32_t glo_used_mask;
uint32_t gal_used_mask;
} loc_sv_cache_info;
static loc_sv_cache_info sv_cache_info;
/*===========================================================================
FUNCTION loc_nmea_sv_meta_init
DESCRIPTION
Init loc_nmea_sv_meta passed in
DEPENDENCIES
NONE
RETURN VALUE
Pointer to loc_nmea_sv_meta
SIDE EFFECTS
N/A
===========================================================================*/
static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
GnssSvType svType,
bool needCombine)
{
memset(&sv_meta, 0, sizeof(sv_meta));
sv_meta.svType = svType;
sv_meta.talker[0] = 'G';
switch (svType)
{
case GNSS_SV_TYPE_GPS:
sv_meta.talker[1] = 'P';
sv_meta.mask = sv_cache_info.gps_used_mask;
break;
case GNSS_SV_TYPE_GLONASS:
sv_meta.talker[1] = 'L';
sv_meta.mask = sv_cache_info.glo_used_mask;
// GLONASS SV ids are from 65-96
sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET;
break;
case GNSS_SV_TYPE_GALILEO:
sv_meta.talker[1] = 'A';
sv_meta.mask = sv_cache_info.gal_used_mask;
break;
default:
LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
return NULL;
}
if (needCombine &&
(sv_cache_info.gps_used_mask ? 1 : 0) +
(sv_cache_info.glo_used_mask ? 1 : 0) +
(sv_cache_info.gal_used_mask ? 1 : 0) > 1)
{
// If GPS, GLONASS, Galileo etc. are combined
// to obtain the reported position solution,
// talker shall be set to GN, to indicate that
// the satellites are used in a combined solution
sv_meta.talker[1] = 'N';
}
return &sv_meta;
}
/*===========================================================================
FUNCTION loc_nmea_count_bits
DESCRIPTION
Count how many bits are set in mask
DEPENDENCIES
NONE
RETURN VALUE
Bits number set in mask
SIDE EFFECTS
N/A
===========================================================================*/
static uint32_t loc_nmea_count_bits(uint32_t mask)
{
uint32_t count = 0;
while (mask)
{
if (mask & 1)
count++;
mask = mask >> 1;
}
return count;
}
/*===========================================================================
FUNCTION loc_nmea_put_checksum
DESCRIPTION
Generate NMEA sentences generated based on position report
DEPENDENCIES
NONE
RETURN VALUE
Total length of the nmea sentence
SIDE EFFECTS
N/A
===========================================================================*/
static int loc_nmea_put_checksum(char *pNmea, int maxSize)
{
uint8_t checksum = 0;
int length = 0;
if(NULL == pNmea)
return 0;
pNmea++; //skip the $
while (*pNmea != '\0')
{
checksum ^= *pNmea++;
length++;
}
// length now contains nmea sentence string length not including $ sign.
int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum);
// total length of nmea sentence is length of nmea sentence inc $ sign plus
// length of checksum (+1 is to cover the $ character in the length).
return (length + checksumLength + 1);
}
/*===========================================================================
FUNCTION loc_nmea_generate_GSA
DESCRIPTION
Generate NMEA GSA sentences generated based on position report
Currently below sentences are generated:
- $GPGSA : GPS DOP and active SVs
- $GLGSA : GLONASS DOP and active SVs
- $GAGSA : GALILEO DOP and active SVs
- $GNGSA : GNSS DOP and active SVs
DEPENDENCIES
NONE
RETURN VALUE
Number of SVs used
SIDE EFFECTS
N/A
===========================================================================*/
static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended,
char* sentence,
int bufSize,
loc_nmea_sv_meta* sv_meta_p,
std::vector<std::string> &nmeaArraystr)
{
if (!sentence || bufSize <= 0 || !sv_meta_p)
{
LOC_LOGE("NMEA Error invalid arguments.");
return 0;
}
char* pMarker = sentence;
int lengthRemaining = bufSize;
int length = 0;
uint32_t svUsedCount = 0;
uint32_t svUsedList[32] = {0};
char fixType = '\0';
const char* talker = sv_meta_p->talker;
uint32_t svIdOffset = sv_meta_p->svIdOffset;
uint32_t mask = sv_meta_p->mask;
for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++)
{
if (mask & 1)
svUsedList[svUsedCount++] = i + svIdOffset;
mask = mask >> 1;
}
if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType)
return 0;
if (svUsedCount == 0)
fixType = '1'; // no fix
else if (svUsedCount <= 3)
fixType = '2'; // 2D fix
else
fixType = '3'; // 3D fix
// Start printing the sentence
// Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v*cc
// a : Mode : A : Automatic, allowed to automatically switch 2D/3D
// x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix)
// xx : 12 SV ID
// p.p : Position DOP (Dilution of Precision)
// h.h : Horizontal DOP
// v.v : Vertical DOP
// cc : Checksum value
length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return 0;
}
pMarker += length;
lengthRemaining -= length;
// Add first 12 satellite IDs
for (uint8_t i = 0; i < 12; i++)
{
if (i < svUsedCount)
length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]);
else
length = snprintf(pMarker, lengthRemaining, ",");
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return 0;
}
pMarker += length;
lengthRemaining -= length;
}
// Add the position/horizontal/vertical DOP values
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
{
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
locationExtended.pdop,
locationExtended.hdop,
locationExtended.vdop);
}
else
{ // no dop
length = snprintf(pMarker, lengthRemaining, ",,");
}
/* Sentence is ready, add checksum and broadcast */
length = loc_nmea_put_checksum(sentence, bufSize);
nmeaArraystr.push_back(sentence);
return svUsedCount;
}
/*===========================================================================
FUNCTION loc_nmea_generate_GSV
DESCRIPTION
Generate NMEA GSV sentences generated based on sv report
Currently below sentences are generated:
- $GPGSV: GPS Satellites in View
- $GNGSV: GLONASS Satellites in View
- $GAGSV: GALILEO Satellites in View
DEPENDENCIES
NONE
RETURN VALUE
NONE
SIDE EFFECTS
N/A
===========================================================================*/
static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
char* sentence,
int bufSize,
loc_nmea_sv_meta* sv_meta_p,
std::vector<std::string> &nmeaArraystr)
{
if (!sentence || bufSize <= 0)
{
LOC_LOGE("NMEA Error invalid argument.");
return;
}
char* pMarker = sentence;
int lengthRemaining = bufSize;
int length = 0;
int sentenceCount = 0;
int sentenceNumber = 1;
size_t svNumber = 1;
const char* talker = sv_meta_p->talker;
uint32_t svIdOffset = sv_meta_p->svIdOffset;
uint32_t mask = sv_meta_p->mask;
uint32_t svCount = loc_nmea_count_bits(mask);
if (svCount <= 0)
{
// no svs in view, so just send a blank $--GSV sentence
snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,", talker);
length = loc_nmea_put_checksum(sentence, bufSize);
nmeaArraystr.push_back(sentence);
return;
}
svNumber = 1;
sentenceNumber = 1;
sentenceCount = svCount / 4 + (svCount % 4 != 0);
while (sentenceNumber <= sentenceCount)
{
pMarker = sentence;
lengthRemaining = bufSize;
length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d",
talker, sentenceCount, sentenceNumber, svCount);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++)
{
if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type)
{
length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
svNotify.gnssSvs[svNumber - 1].svId,
(int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
(int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0)
{
length = snprintf(pMarker, lengthRemaining,"%02d",
(int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
}
i++;
}
}
length = loc_nmea_put_checksum(sentence, bufSize);
nmeaArraystr.push_back(sentence);
sentenceNumber++;
} //while
}
/*===========================================================================
FUNCTION loc_nmea_generate_pos
DESCRIPTION
Generate NMEA sentences generated based on position report
Currently below sentences are generated within this function:
- $GPGSA : GPS DOP and active SVs
- $GLGSA : GLONASS DOP and active SVs
- $GAGSA : GALILEO DOP and active SVs
- $GNGSA : GNSS DOP and active SVs
- $--VTG : Track made good and ground speed
- $--RMC : Recommended minimum navigation information
- $--GGA : Time, position and fix related data
DEPENDENCIES
NONE
RETURN VALUE
0
SIDE EFFECTS
N/A
===========================================================================*/
void loc_nmea_generate_pos(const UlpLocation &location,
const GpsLocationExtended &locationExtended,
unsigned char generate_nmea,
std::vector<std::string> &nmeaArraystr)
{
ENTRY_LOG();
time_t utcTime(location.gpsLocation.timestamp/1000);
tm * pTm = gmtime(&utcTime);
if (NULL == pTm) {
LOC_LOGE("gmtime failed");
return;
}
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
char* pMarker = sentence;
int lengthRemaining = sizeof(sentence);
int length = 0;
int utcYear = pTm->tm_year % 100; // 2 digit year
int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero
int utcDay = pTm->tm_mday;
int utcHours = pTm->tm_hour;
int utcMinutes = pTm->tm_min;
int utcSeconds = pTm->tm_sec;
int utcMSeconds = (location.gpsLocation.timestamp)%1000;
if (generate_nmea) {
char talker[3] = {'G', 'P', '\0'};
uint32_t svUsedCount = 0;
uint32_t count = 0;
loc_nmea_sv_meta sv_meta;
// -------------------
// ---$GPGSA/$GNGSA---
// -------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GPS, true), nmeaArraystr);
if (count > 0)
{
svUsedCount += count;
talker[1] = sv_meta.talker[1];
}
// -------------------
// ---$GLGSA/$GNGSA---
// -------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GLONASS, true), nmeaArraystr);
if (count > 0)
{
svUsedCount += count;
talker[1] = sv_meta.talker[1];
}
// -------------------
// ---$GAGSA/$GNGSA---
// -------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GALILEO, true), nmeaArraystr);
if (count > 0)
{
svUsedCount += count;
talker[1] = sv_meta.talker[1];
}
// -------------------
// ------$--VTG-------
// -------------------
pMarker = sentence;
lengthRemaining = sizeof(sentence);
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
{
float magTrack = location.gpsLocation.bearing;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
{
float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation;
if (magTrack < 0.0)
magTrack += 360.0;
else if (magTrack > 360.0)
magTrack -= 360.0;
}
length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack);
}
else
{
length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker);
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
{
float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
float speedKmPerHour = location.gpsLocation.speed * 3.6;
length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour);
}
else
{
length = snprintf(pMarker, lengthRemaining, ",N,,K,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
// N means no fix
length = snprintf(pMarker, lengthRemaining, "%c", 'N');
else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
// D means differential
length = snprintf(pMarker, lengthRemaining, "%c", 'D');
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
// E means estimated (dead reckoning)
length = snprintf(pMarker, lengthRemaining, "%c", 'E');
else // A means autonomous
length = snprintf(pMarker, lengthRemaining, "%c", 'A');
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
// -------------------
// ------$--RMC-------
// -------------------
pMarker = sentence;
lengthRemaining = sizeof(sentence);
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
{
double latitude = location.gpsLocation.latitude;
double longitude = location.gpsLocation.longitude;
char latHemisphere;
char lonHemisphere;
double latMinutes;
double lonMinutes;
if (latitude > 0)
{
latHemisphere = 'N';
}
else
{
latHemisphere = 'S';
latitude *= -1.0;
}
if (longitude < 0)
{
lonHemisphere = 'W';
longitude *= -1.0;
}
else
{
lonHemisphere = 'E';
}
latMinutes = fmod(latitude * 60.0 , 60.0);
lonMinutes = fmod(longitude * 60.0 , 60.0);
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
(uint8_t)floor(latitude), latMinutes, latHemisphere,
(uint8_t)floor(longitude),lonMinutes, lonHemisphere);
}
else
{
length = snprintf(pMarker, lengthRemaining,",,,,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
{
float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
}
else
{
length = snprintf(pMarker, lengthRemaining, ",");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
{
length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing);
}
else
{
length = snprintf(pMarker, lengthRemaining, ",");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
utcDay, utcMonth, utcYear);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
{
float magneticVariation = locationExtended.magneticDeviation;
char direction;
if (magneticVariation < 0.0)
{
direction = 'W';
magneticVariation *= -1.0;
}
else
{
direction = 'E';
}
length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
magneticVariation, direction);
}
else
{
length = snprintf(pMarker, lengthRemaining, ",,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
// N means no fix
length = snprintf(pMarker, lengthRemaining, "%c", 'N');
else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
// D means differential
length = snprintf(pMarker, lengthRemaining, "%c", 'D');
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
// E means estimated (dead reckoning)
length = snprintf(pMarker, lengthRemaining, "%c", 'E');
else // A means autonomous
length = snprintf(pMarker, lengthRemaining, "%c", 'A');
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
// -------------------
// ------$--GGA-------
// -------------------
pMarker = sentence;
lengthRemaining = sizeof(sentence);
length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
{
double latitude = location.gpsLocation.latitude;
double longitude = location.gpsLocation.longitude;
char latHemisphere;
char lonHemisphere;
double latMinutes;
double lonMinutes;
if (latitude > 0)
{
latHemisphere = 'N';
}
else
{
latHemisphere = 'S';
latitude *= -1.0;
}
if (longitude < 0)
{
lonHemisphere = 'W';
longitude *= -1.0;
}
else
{
lonHemisphere = 'E';
}
latMinutes = fmod(latitude * 60.0 , 60.0);
lonMinutes = fmod(longitude * 60.0 , 60.0);
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
(uint8_t)floor(latitude), latMinutes, latHemisphere,
(uint8_t)floor(longitude),lonMinutes, lonHemisphere);
}
else
{
length = snprintf(pMarker, lengthRemaining,",,,,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
char gpsQuality;
if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
gpsQuality = '0'; // 0 means no fix
else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
gpsQuality = '2'; // 2 means DGPS fix
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
gpsQuality = '6'; // 6 means estimated (dead reckoning)
else
gpsQuality = '1'; // 1 means GPS fix
// Number of satellites in use, 00-12
if (svUsedCount > MAX_SATELLITES_IN_USE)
svUsedCount = MAX_SATELLITES_IN_USE;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
{
length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
gpsQuality, svUsedCount, locationExtended.hdop);
}
else
{ // no hdop
length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
gpsQuality, svUsedCount);
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
{
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
locationExtended.altitudeMeanSeaLevel);
}
else
{
length = snprintf(pMarker, lengthRemaining,",,");
}
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
(locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
{
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
}
else
{
length = snprintf(pMarker, lengthRemaining,",,,");
}
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
// clear the cache so they can't be used again
sv_cache_info.gps_used_mask = 0;
sv_cache_info.glo_used_mask = 0;
sv_cache_info.gal_used_mask = 0;
}
//Send blank NMEA reports for non-final fixes
else {
strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
}
EXIT_LOG(%d, 0);
}
/*===========================================================================
FUNCTION loc_nmea_generate_sv
DESCRIPTION
Generate NMEA sentences generated based on sv report
DEPENDENCIES
NONE
RETURN VALUE
0
SIDE EFFECTS
N/A
===========================================================================*/
void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
std::vector<std::string> &nmeaArraystr)
{
ENTRY_LOG();
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
char* pMarker = sentence;
int lengthRemaining = sizeof(sentence);
int length = 0;
int svCount = svNotify.count;
int sentenceCount = 0;
int sentenceNumber = 1;
int svNumber = 1;
//Count GPS SVs for saparating GPS from GLONASS and throw others
sv_cache_info.gps_used_mask = 0;
sv_cache_info.glo_used_mask = 0;
sv_cache_info.gal_used_mask = 0;
for(svNumber=1; svNumber <= svCount; svNumber++) {
if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type)
{
// cache the used in fix mask, as it will be needed to send $GPGSA
// during the position report
if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
(svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
{
sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
}
}
else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type)
{
// cache the used in fix mask, as it will be needed to send $GNGSA
// during the position report
if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
(svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
{
sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
}
}
else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type)
{
// cache the used in fix mask, as it will be needed to send $GAGSA
// during the position report
if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
(svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
{
sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
}
}
}
loc_nmea_sv_meta sv_meta;
// ------------------
// ------$GPGSV------
// ------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GPS, false), nmeaArraystr);
// ------------------
// ------$GLGSV------
// ------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GLONASS, false), nmeaArraystr);
// ------------------
// ------$GAGSV------
// ------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GALILEO, false), nmeaArraystr);
EXIT_LOG(%d, 0);
}

46
utils/loc_nmea.h Normal file
View file

@ -0,0 +1,46 @@
/* Copyright (c) 2012-2013, 2015-2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of The Linux Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
* BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
* BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
* OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
* IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef LOC_ENG_NMEA_H
#define LOC_ENG_NMEA_H
#include <gps_extended.h>
#include <vector>
#include <string>
#define NMEA_SENTENCE_MAX_LENGTH 200
void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
std::vector<std::string> &nmeaArraystr);
void loc_nmea_generate_pos(const UlpLocation &location,
const GpsLocationExtended &locationExtended,
unsigned char generate_nmea,
std::vector<std::string> &nmeaArraystr);
#endif // LOC_ENG_NMEA_H