925 lines
31 KiB
C++
925 lines
31 KiB
C++
/* Copyright (c) 2012, 2016, 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_eng_nmea"
|
|
#include <loc_eng.h>
|
|
#include <loc_eng_nmea.h>
|
|
#include <math.h>
|
|
#include "log_util.h"
|
|
|
|
/*===========================================================================
|
|
FUNCTION loc_eng_nmea_send
|
|
|
|
DESCRIPTION
|
|
send out NMEA sentence
|
|
|
|
DEPENDENCIES
|
|
NONE
|
|
|
|
RETURN VALUE
|
|
Total length of the nmea sentence
|
|
|
|
SIDE EFFECTS
|
|
N/A
|
|
|
|
===========================================================================*/
|
|
void loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p)
|
|
{
|
|
struct timeval tv;
|
|
gettimeofday(&tv, (struct timezone *) NULL);
|
|
int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000;
|
|
if (loc_eng_data_p->nmea_cb != NULL)
|
|
loc_eng_data_p->nmea_cb(now, pNmea, length);
|
|
LOC_LOGD("NMEA <%s", pNmea);
|
|
}
|
|
|
|
/*===========================================================================
|
|
FUNCTION loc_eng_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
|
|
|
|
===========================================================================*/
|
|
int loc_eng_nmea_put_checksum(char *pNmea, int maxSize)
|
|
{
|
|
uint8_t checksum = 0;
|
|
int length = 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_eng_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
|
|
- $GNGSA : GLONASS DOP and active SVs
|
|
- $GPVTG : Track made good and ground speed
|
|
- $GPRMC : Recommended minimum navigation information
|
|
- $GPGGA : Time, position and fix related data
|
|
|
|
DEPENDENCIES
|
|
NONE
|
|
|
|
RETURN VALUE
|
|
0
|
|
|
|
SIDE EFFECTS
|
|
N/A
|
|
|
|
===========================================================================*/
|
|
void loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p,
|
|
const UlpLocation &location,
|
|
const GpsLocationExtended &locationExtended,
|
|
unsigned char generate_nmea)
|
|
{
|
|
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) {
|
|
// ------------------
|
|
// ------$GPGSA------
|
|
// ------------------
|
|
|
|
uint32_t svUsedCount = 0;
|
|
uint32_t svUsedList[32] = {0};
|
|
uint32_t mask = loc_eng_data_p->gps_used_mask;
|
|
for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++)
|
|
{
|
|
if (mask & 1)
|
|
svUsedList[svUsedCount++] = i;
|
|
mask = mask >> 1;
|
|
}
|
|
// clear the cache so they can't be used again
|
|
loc_eng_data_p->gps_used_mask = 0;
|
|
|
|
char fixType;
|
|
if (svUsedCount == 0)
|
|
fixType = '1'; // no fix
|
|
else if (svUsedCount <= 3)
|
|
fixType = '2'; // 2D fix
|
|
else
|
|
fixType = '3'; // 3D fix
|
|
|
|
length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType);
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence
|
|
{
|
|
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;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
}
|
|
|
|
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
|
|
{ // dop is in locationExtended, (QMI)
|
|
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
|
|
locationExtended.pdop,
|
|
locationExtended.hdop,
|
|
locationExtended.vdop);
|
|
}
|
|
else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0)
|
|
{ // dop was cached from sv report (RPC)
|
|
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
|
|
loc_eng_data_p->pdop,
|
|
loc_eng_data_p->hdop,
|
|
loc_eng_data_p->vdop);
|
|
}
|
|
else
|
|
{ // no dop
|
|
length = snprintf(pMarker, lengthRemaining, ",,");
|
|
}
|
|
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
// ------------------
|
|
// ------$GNGSA------
|
|
// ------------------
|
|
uint32_t gloUsedCount = 0;
|
|
uint32_t gloUsedList[32] = {0};
|
|
|
|
// Reset locals for GNGSA sentence generation
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
mask = loc_eng_data_p->glo_used_mask;
|
|
fixType = '\0';
|
|
|
|
// Parse the glonass sv mask, and fetch glo sv ids
|
|
// Mask corresponds to the offset.
|
|
// GLONASS SV ids are from 65-96
|
|
const int GLONASS_SV_ID_OFFSET = 64;
|
|
for (uint8_t i = 1; mask > 0 && gloUsedCount < 32; i++)
|
|
{
|
|
if (mask & 1)
|
|
gloUsedList[gloUsedCount++] = i + GLONASS_SV_ID_OFFSET;
|
|
mask = mask >> 1;
|
|
}
|
|
// clear the cache so they can't be used again
|
|
loc_eng_data_p->glo_used_mask = 0;
|
|
|
|
if (gloUsedCount == 0)
|
|
fixType = '1'; // no fix
|
|
else if (gloUsedCount <= 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
|
|
// GNGSA : for glonass SVs
|
|
// 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, "$GNGSA,A,%c,", fixType);
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
// Add first 12 GLONASS satellite IDs
|
|
for (uint8_t i = 0; i < 12; i++)
|
|
{
|
|
if (i < gloUsedCount)
|
|
length = snprintf(pMarker, lengthRemaining, "%02d,", gloUsedList[i]);
|
|
else
|
|
length = snprintf(pMarker, lengthRemaining, ",");
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
}
|
|
|
|
// Add the position/horizontal/vertical DOP values
|
|
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
|
|
{ // dop is in locationExtended, (QMI)
|
|
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
|
|
locationExtended.pdop,
|
|
locationExtended.hdop,
|
|
locationExtended.vdop);
|
|
}
|
|
else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0)
|
|
{ // dop was cached from sv report (RPC)
|
|
length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
|
|
loc_eng_data_p->pdop,
|
|
loc_eng_data_p->hdop,
|
|
loc_eng_data_p->vdop);
|
|
}
|
|
else
|
|
{ // no dop
|
|
length = snprintf(pMarker, lengthRemaining, ",,");
|
|
}
|
|
|
|
/* Sentence is ready, add checksum and broadcast */
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
// ------------------
|
|
// ------$GPVTG------
|
|
// ------------------
|
|
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
|
|
if (location.gpsLocation.flags & 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, "$GPVTG,%.1lf,T,%.1lf,M,", location.gpsLocation.bearing, magTrack);
|
|
}
|
|
else
|
|
{
|
|
length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,");
|
|
}
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
if (location.gpsLocation.flags & 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 & GPS_LOCATION_HAS_LAT_LONG))
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
|
|
else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
|
|
else
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
|
|
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
// ------------------
|
|
// ------$GPRMC------
|
|
// ------------------
|
|
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
|
|
length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d.%02d,A," ,
|
|
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 & 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 & 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 & 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 & GPS_LOCATION_HAS_LAT_LONG))
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
|
|
else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
|
|
else
|
|
length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
|
|
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
// ------------------
|
|
// ------$GPGGA------
|
|
// ------------------
|
|
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
|
|
length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d.%02d," ,
|
|
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 & 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 & GPS_LOCATION_HAS_LAT_LONG))
|
|
gpsQuality = '0'; // 0 means no fix
|
|
else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
|
|
gpsQuality = '1'; // 1 means GPS fix
|
|
else
|
|
gpsQuality = '2'; // 2 means DGPS fix
|
|
|
|
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
|
|
{ // dop is in locationExtended, (QMI)
|
|
length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
|
|
gpsQuality, svUsedCount, locationExtended.hdop);
|
|
}
|
|
else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0)
|
|
{ // dop was cached from sv report (RPC)
|
|
length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
|
|
gpsQuality, svUsedCount, loc_eng_data_p->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 & 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_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
}
|
|
//Send blank NMEA reports for non-final fixes
|
|
else {
|
|
strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
|
|
strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
}
|
|
// clear the dop cache so they can't be used again
|
|
loc_eng_data_p->pdop = 0;
|
|
loc_eng_data_p->hdop = 0;
|
|
loc_eng_data_p->vdop = 0;
|
|
|
|
EXIT_LOG(%d, 0);
|
|
}
|
|
|
|
|
|
|
|
/*===========================================================================
|
|
FUNCTION loc_eng_nmea_generate_sv
|
|
|
|
DESCRIPTION
|
|
Generate NMEA sentences generated based on sv report
|
|
|
|
DEPENDENCIES
|
|
NONE
|
|
|
|
RETURN VALUE
|
|
0
|
|
|
|
SIDE EFFECTS
|
|
N/A
|
|
|
|
===========================================================================*/
|
|
void loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p,
|
|
const GnssSvStatus &svStatus, const GpsLocationExtended &locationExtended)
|
|
{
|
|
ENTRY_LOG();
|
|
|
|
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
|
|
char* pMarker = sentence;
|
|
int lengthRemaining = sizeof(sentence);
|
|
int length = 0;
|
|
int svCount = svStatus.num_svs;
|
|
int sentenceCount = 0;
|
|
int sentenceNumber = 1;
|
|
int svNumber = 1;
|
|
int gpsCount = 0;
|
|
int glnCount = 0;
|
|
|
|
//Count GPS SVs for saparating GPS from GLONASS and throw others
|
|
|
|
loc_eng_data_p->gps_used_mask = 0;
|
|
loc_eng_data_p->glo_used_mask = 0;
|
|
for(svNumber=1; svNumber <= svCount; svNumber++) {
|
|
if (GNSS_CONSTELLATION_GPS == svStatus.gnss_sv_list[svNumber - 1].constellation)
|
|
{
|
|
// cache the used in fix mask, as it will be needed to send $GPGSA
|
|
// during the position report
|
|
if (GNSS_SV_FLAGS_USED_IN_FIX == (svStatus.gnss_sv_list[svNumber - 1].flags & GNSS_SV_FLAGS_USED_IN_FIX))
|
|
{
|
|
loc_eng_data_p->gps_used_mask |= (1 << (svStatus.gnss_sv_list[svNumber - 1].svid - 1));
|
|
}
|
|
gpsCount++;
|
|
}
|
|
else if (GNSS_CONSTELLATION_GLONASS == svStatus.gnss_sv_list[svNumber - 1].constellation)
|
|
{
|
|
// cache the used in fix mask, as it will be needed to send $GNGSA
|
|
// during the position report
|
|
if (GNSS_SV_FLAGS_USED_IN_FIX == (svStatus.gnss_sv_list[svNumber - 1].flags & GNSS_SV_FLAGS_USED_IN_FIX))
|
|
{
|
|
loc_eng_data_p->glo_used_mask |= (1 << (svStatus.gnss_sv_list[svNumber - 1].svid - 1));
|
|
}
|
|
glnCount++;
|
|
}
|
|
}
|
|
|
|
// ------------------
|
|
// ------$GPGSV------
|
|
// ------------------
|
|
|
|
if (gpsCount <= 0)
|
|
{
|
|
// no svs in view, so just send a blank $GPGSV sentence
|
|
strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
}
|
|
else
|
|
{
|
|
svNumber = 1;
|
|
sentenceNumber = 1;
|
|
sentenceCount = gpsCount/4 + (gpsCount % 4 != 0);
|
|
|
|
while (sentenceNumber <= sentenceCount)
|
|
{
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
|
|
length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d",
|
|
sentenceCount, sentenceNumber, gpsCount);
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++)
|
|
{
|
|
if (GNSS_CONSTELLATION_GPS == svStatus.gnss_sv_list[svNumber - 1].constellation)
|
|
{
|
|
length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
|
|
svStatus.gnss_sv_list[svNumber-1].svid,
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber-1].elevation), //float to int
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber-1].azimuth)); //float to int
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
if (svStatus.gnss_sv_list[svNumber-1].c_n0_dbhz > 0)
|
|
{
|
|
length = snprintf(pMarker, lengthRemaining,"%02d",
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber-1].c_n0_dbhz)); //float to int
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
}
|
|
|
|
i++;
|
|
}
|
|
|
|
}
|
|
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
sentenceNumber++;
|
|
|
|
} //while
|
|
|
|
} //if
|
|
|
|
// ------------------
|
|
// ------$GLGSV------
|
|
// ------------------
|
|
|
|
if (glnCount <= 0)
|
|
{
|
|
// no svs in view, so just send a blank $GLGSV sentence
|
|
strlcpy(sentence, "$GLGSV,1,1,0,", sizeof(sentence));
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
}
|
|
else
|
|
{
|
|
svNumber = 1;
|
|
sentenceNumber = 1;
|
|
sentenceCount = glnCount/4 + (glnCount % 4 != 0);
|
|
|
|
while (sentenceNumber <= sentenceCount)
|
|
{
|
|
pMarker = sentence;
|
|
lengthRemaining = sizeof(sentence);
|
|
|
|
length = snprintf(pMarker, lengthRemaining, "$GLGSV,%d,%d,%02d",
|
|
sentenceCount, sentenceNumber, glnCount);
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++)
|
|
{
|
|
if (GNSS_CONSTELLATION_GLONASS == svStatus.gnss_sv_list[svNumber - 1].constellation)
|
|
{
|
|
|
|
length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
|
|
svStatus.gnss_sv_list[svNumber - 1].svid,
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber - 1].elevation), //float to int
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber - 1].azimuth)); //float to int
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
|
|
if (svStatus.gnss_sv_list[svNumber - 1].c_n0_dbhz > 0)
|
|
{
|
|
length = snprintf(pMarker, lengthRemaining,"%02d",
|
|
(int)(0.5 + svStatus.gnss_sv_list[svNumber - 1].c_n0_dbhz)); //float to int
|
|
|
|
if (length < 0 || length >= lengthRemaining)
|
|
{
|
|
LOC_LOGE("NMEA Error in string formatting");
|
|
return;
|
|
}
|
|
pMarker += length;
|
|
lengthRemaining -= length;
|
|
}
|
|
|
|
i++;
|
|
}
|
|
|
|
}
|
|
|
|
length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
|
|
loc_eng_nmea_send(sentence, length, loc_eng_data_p);
|
|
sentenceNumber++;
|
|
|
|
} //while
|
|
|
|
}//if
|
|
|
|
// For RPC, the DOP are sent during sv report, so cache them
|
|
// now to be sent during position report.
|
|
// For QMI, the DOP will be in position report.
|
|
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
|
|
{
|
|
loc_eng_data_p->pdop = locationExtended.pdop;
|
|
loc_eng_data_p->hdop = locationExtended.hdop;
|
|
loc_eng_data_p->vdop = locationExtended.vdop;
|
|
}
|
|
else
|
|
{
|
|
loc_eng_data_p->pdop = 0;
|
|
loc_eng_data_p->hdop = 0;
|
|
loc_eng_data_p->vdop = 0;
|
|
}
|
|
|
|
EXIT_LOG(%d, 0);
|
|
}
|