lisa/android_device_xiaomi_sm8350-common
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
ec81b275ae
android_device_xiaomi_sm835.../gps/utils/loc_nmea.cpp

2348 lines
84 KiB
C++
Raw Normal View History

sm8350-common: Import gps hal from LA.UM.9.14.r1-16700-LAHAINA.0 Change-Id: I134d6418f1357a4d7ce37358481d6b765d93e1a5
2022-04-01 18:03:09 -04:00
/* Copyright (c) 2012-2020, 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_NDEBUG 0
#define LOG_TAG "LocSvc_nmea"
#include <loc_nmea.h>
#include <math.h>
#include <log_util.h>
#include <loc_pla.h>
#include <loc_cfg.h>
#define GLONASS_SV_ID_OFFSET 64
#define SBAS_SV_ID_OFFSET (87)
#define QZSS_SV_ID_OFFSET (192)
#define BDS_SV_ID_OFFSET (200)
#define GALILEO_SV_ID_OFFSET (300)
#define NAVIC_SV_ID_OFFSET (400)
#define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64
#define MAX_SATELLITES_IN_USE 12
#define MSEC_IN_ONE_WEEK 604800000ULL
#define UTC_GPS_OFFSET_MSECS 315964800000ULL
#define MAX_TAG_BLOCK_GROUP_CODE (99999)
// GNSS system id according to NMEA spec
#define SYSTEM_ID_GPS 1
#define SYSTEM_ID_GLONASS 2
#define SYSTEM_ID_GALILEO 3
#define SYSTEM_ID_BDS 4
#define SYSTEM_ID_QZSS 5
#define SYSTEM_ID_NAVIC 6
//GNSS signal id according to NMEA spec
#define SIGNAL_ID_ALL_SIGNALS 0
#define SIGNAL_ID_GPS_L1CA 1
#define SIGNAL_ID_GPS_L1P 2
#define SIGNAL_ID_GPS_L1M 3
#define SIGNAL_ID_GPS_L2P 4
#define SIGNAL_ID_GPS_L2CM 5
#define SIGNAL_ID_GPS_L2CL 6
#define SIGNAL_ID_GPS_L5I 7
#define SIGNAL_ID_GPS_L5Q 8
#define SIGNAL_ID_GLO_G1CA 1
#define SIGNAL_ID_GLO_G1P 2
#define SIGNAL_ID_GLO_G2CA 3
#define SIGNAL_ID_GLO_G2P 4
#define SIGNAL_ID_GAL_E5A 1
#define SIGNAL_ID_GAL_E5B 2
#define SIGNAL_ID_GAL_E5AB 3
#define SIGNAL_ID_GAL_E6A 4
#define SIGNAL_ID_GAL_E6BC 5
#define SIGNAL_ID_GAL_L1A 6
#define SIGNAL_ID_GAL_L1BC 7
#define SIGNAL_ID_BDS_B1I 1
#define SIGNAL_ID_BDS_B1Q 2
#define SIGNAL_ID_BDS_B1C 3
#define SIGNAL_ID_BDS_B1A 4
#define SIGNAL_ID_BDS_B2A 5
#define SIGNAL_ID_BDS_B2B 6
#define SIGNAL_ID_BDS_B2AB 7
#define SIGNAL_ID_BDS_B3I 8
#define SIGNAL_ID_BDS_B3Q 9
#define SIGNAL_ID_BDS_B3A 0xA
#define SIGNAL_ID_BDS_B2I 0xB
#define SIGNAL_ID_BDS_B2Q 0xC
#define SIGNAL_ID_QZSS_L1CA 1
#define SIGNAL_ID_QZSS_L1CD 2
#define SIGNAL_ID_QZSS_L1CP 3
#define SIGNAL_ID_QZSS_LIS 4
#define SIGNAL_ID_QZSS_L2CM 5
#define SIGNAL_ID_QZSS_L2CL 6
#define SIGNAL_ID_QZSS_L5I 7
#define SIGNAL_ID_QZSS_L5Q 8
#define SIGNAL_ID_QZSS_L6D 9
#define SIGNAL_ID_QZSS_L6E 0xA
#define SIGNAL_ID_NAVIC_L5SPS 1
#define SIGNAL_ID_NAVIC_SSPS 2
#define SIGNAL_ID_NAVIC_L5RS 3
#define SIGNAL_ID_NAVIC_SRS 4
#define SIGNAL_ID_NAVIC_L1SPS 5
typedef struct loc_nmea_sv_meta_s
{
char talker[3];
uint32_t svTypeMask;
uint64_t mask;
uint32_t svCount;
uint32_t totalSvUsedCount;
uint32_t svIdOffset;
uint32_t signalId;
uint32_t systemId;
} loc_nmea_sv_meta;
typedef struct loc_sv_cache_info_s
{
uint64_t gps_used_mask;
uint64_t glo_used_mask;
uint64_t gal_used_mask;
uint64_t qzss_used_mask;
uint64_t bds_used_mask;
uint64_t navic_used_mask;
uint32_t gps_l1_count;
uint32_t gps_l2_count;
uint32_t gps_l5_count;
uint32_t glo_g1_count;
uint32_t glo_g2_count;
uint32_t gal_e1_count;
uint32_t gal_e5_count;
uint32_t gal_e5b_count;
uint32_t qzss_l1_count;
uint32_t qzss_l2_count;
uint32_t qzss_l5_count;
uint32_t bds_b1i_count;
uint32_t bds_b1c_count;
uint32_t bds_b2_count;
uint32_t navic_l5_count;
float hdop;
float pdop;
float vdop;
} loc_sv_cache_info;
/*===========================================================================
FUNCTION convert_Lla_to_Ecef
DESCRIPTION
Convert LLA to ECEF
DEPENDENCIES
NONE
RETURN VALUE
NONE
SIDE EFFECTS
N/A
===========================================================================*/
static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef)
{
double r;
r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat));
pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon);
pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon);
pecef.Z = (r * OMES + plla.alt) * sin(plla.lat);
}
/*===========================================================================
FUNCTION convert_WGS84_to_PZ90
DESCRIPTION
Convert datum from WGS84 to PZ90
DEPENDENCIES
NONE
RETURN VALUE
NONE
SIDE EFFECTS
N/A
===========================================================================*/
static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90)
{
double deltaX = DatumConstFromWGS84[0];
double deltaY = DatumConstFromWGS84[1];
double deltaZ = DatumConstFromWGS84[2];
double deltaScale = DatumConstFromWGS84[3];
double rotX = DatumConstFromWGS84[4];
double rotY = DatumConstFromWGS84[5];
double rotZ = DatumConstFromWGS84[6];
pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z);
pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z);
pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y);
}
/*===========================================================================
FUNCTION convert_Ecef_to_Lla
DESCRIPTION
Convert ECEF to LLA
DEPENDENCIES
NONE
RETURN VALUE
NONE
SIDE EFFECTS
N/A
===========================================================================*/
static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla)
{
double p, r;
double EcefA = C_PZ90A;
double EcefB = C_PZ90B;
double Ecef1Mf;
double EcefE2;
double Mu;
double Smu;
double Cmu;
double Phi;
double Sphi;
double N;
p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y);
r = sqrt(p * p + pecef.Z * pecef.Z);
if (r < 1.0) {
plla.lat = 1.0;
plla.lon = 1.0;
plla.alt = 1.0;
}
Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA;
EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA);
if (p > 1.0) {
Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p);
} else {
if (pecef.Z > 0.0) {
Mu = M_PI / 2.0;
} else {
Mu = -M_PI / 2.0;
}
}
Smu = sin(Mu);
Cmu = cos(Mu);
Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu,
Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu));
Sphi = sin(Phi);
N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi);
plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N;
plla.lat = Phi;
if ( p > 1.0) {
plla.lon = atan2(pecef.Y, pecef.X);
} else {
plla.lon = 0.0;
}
}
/*===========================================================================
FUNCTION convert_signalType_to_signalId
DESCRIPTION
convert signalType to signal ID
DEPENDENCIES
NONE
RETURN VALUE
value of signal ID
SIDE EFFECTS
N/A
===========================================================================*/
static uint32_t convert_signalType_to_signalId(GnssSignalTypeMask signalType)
{
uint32_t signalId = SIGNAL_ID_ALL_SIGNALS;
switch (signalType) {
case GNSS_SIGNAL_GPS_L1CA:
case GNSS_SIGNAL_SBAS_L1:
signalId = SIGNAL_ID_GPS_L1CA;
break;
case GNSS_SIGNAL_GPS_L2:
signalId = SIGNAL_ID_GPS_L2CL;
break;
case GNSS_SIGNAL_GPS_L5:
signalId = SIGNAL_ID_GPS_L5Q;
break;
case GNSS_SIGNAL_GLONASS_G1:
signalId = SIGNAL_ID_GLO_G1CA;
break;
case GNSS_SIGNAL_GLONASS_G2:
signalId = SIGNAL_ID_GLO_G2CA;
break;
case GNSS_SIGNAL_GALILEO_E1:
signalId = SIGNAL_ID_GAL_L1BC;
break;
case GNSS_SIGNAL_GALILEO_E5A:
signalId = SIGNAL_ID_GAL_E5A;
break;
case GNSS_SIGNAL_GALILEO_E5B:
signalId = SIGNAL_ID_GAL_E5B;
break;
case GNSS_SIGNAL_QZSS_L1CA:
signalId = SIGNAL_ID_QZSS_L1CA;
break;
case GNSS_SIGNAL_QZSS_L2:
signalId = SIGNAL_ID_QZSS_L2CL;
break;
case GNSS_SIGNAL_QZSS_L5:
signalId = SIGNAL_ID_QZSS_L5Q;
break;
case GNSS_SIGNAL_BEIDOU_B1I:
signalId = SIGNAL_ID_BDS_B1I;
break;
case GNSS_SIGNAL_BEIDOU_B1C:
signalId = SIGNAL_ID_BDS_B1C;
break;
case GNSS_SIGNAL_BEIDOU_B2I:
signalId = SIGNAL_ID_BDS_B2I;
break;
case GNSS_SIGNAL_BEIDOU_B2AI:
case GNSS_SIGNAL_BEIDOU_B2AQ:
signalId = SIGNAL_ID_BDS_B2A;
break;
case GNSS_SIGNAL_NAVIC_L5:
signalId = SIGNAL_ID_NAVIC_L5SPS;
break;
default:
signalId = SIGNAL_ID_ALL_SIGNALS;
}
return signalId;
}
/*===========================================================================
FUNCTION get_sv_count_from_mask
DESCRIPTION
get the sv count from bit mask
DEPENDENCIES
NONE
RETURN VALUE
value of sv count
SIDE EFFECTS
N/A
===========================================================================*/
static uint32_t get_sv_count_from_mask(uint64_t svMask, int totalSvCount)
{
int index = 0;
uint32_t svCount = 0;
if(totalSvCount > MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION) {
LOC_LOGE("total SV count in this constellation %d exceeded limit %d",
totalSvCount, MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION);
}
for(index = 0; index < totalSvCount; index++) {
if(svMask & 0x1)
svCount += 1;
svMask >>= 1;
}
return svCount;
}
/*===========================================================================
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,
loc_sv_cache_info& sv_cache_info,
GnssSvType svType,
GnssSignalTypeMask signalType,
bool needCombine)
{
memset(&sv_meta, 0, sizeof(sv_meta));
sv_meta.svTypeMask = (1 << svType);
switch (svType)
{
case GNSS_SV_TYPE_GPS:
sv_meta.talker[0] = 'G';
sv_meta.talker[1] = 'P';
sv_meta.mask = sv_cache_info.gps_used_mask;
sv_meta.systemId = SYSTEM_ID_GPS;
sv_meta.svTypeMask |= (1 << GNSS_SV_TYPE_SBAS);
switch (signalType) {
case GNSS_SIGNAL_GPS_L1CA:
sv_meta.svCount = sv_cache_info.gps_l1_count;
break;
case GNSS_SIGNAL_GPS_L5:
sv_meta.svCount = sv_cache_info.gps_l5_count;
break;
case GNSS_SIGNAL_GPS_L2:
sv_meta.svCount = sv_cache_info.gps_l2_count;
break;
}
break;
case GNSS_SV_TYPE_GLONASS:
sv_meta.talker[0] = 'G';
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;
sv_meta.systemId = SYSTEM_ID_GLONASS;
switch (signalType) {
case GNSS_SIGNAL_GLONASS_G1:
sv_meta.svCount = sv_cache_info.glo_g1_count;
break;
case GNSS_SIGNAL_GLONASS_G2:
sv_meta.svCount = sv_cache_info.glo_g2_count;
break;
}
break;
case GNSS_SV_TYPE_GALILEO:
sv_meta.talker[0] = 'G';
sv_meta.talker[1] = 'A';
sv_meta.mask = sv_cache_info.gal_used_mask;
// GALILEO SV ids are from 301-336, So keep svIdOffset 300
sv_meta.svIdOffset = GALILEO_SV_ID_OFFSET;
sv_meta.systemId = SYSTEM_ID_GALILEO;
switch (signalType) {
case GNSS_SIGNAL_GALILEO_E1:
sv_meta.svCount = sv_cache_info.gal_e1_count;
break;
case GNSS_SIGNAL_GALILEO_E5A:
sv_meta.svCount = sv_cache_info.gal_e5_count;
break;
case GNSS_SIGNAL_GALILEO_E5B:
sv_meta.svCount = sv_cache_info.gal_e5b_count;
break;
}
break;
case GNSS_SV_TYPE_QZSS:
sv_meta.talker[0] = 'G';
sv_meta.talker[1] = 'Q';
sv_meta.mask = sv_cache_info.qzss_used_mask;
// QZSS SV ids are from 193-199. So keep svIdOffset 192
sv_meta.svIdOffset = QZSS_SV_ID_OFFSET;
sv_meta.systemId = SYSTEM_ID_QZSS;
switch (signalType) {
case GNSS_SIGNAL_QZSS_L1CA:
sv_meta.svCount = sv_cache_info.qzss_l1_count;
break;
case GNSS_SIGNAL_QZSS_L2:
sv_meta.svCount = sv_cache_info.qzss_l2_count;
break;
case GNSS_SIGNAL_QZSS_L5:
sv_meta.svCount = sv_cache_info.qzss_l5_count;
break;
}
break;
case GNSS_SV_TYPE_BEIDOU:
sv_meta.talker[0] = 'G';
sv_meta.talker[1] = 'B';
sv_meta.mask = sv_cache_info.bds_used_mask;
// BDS SV ids are from 201-237. So keep svIdOffset 200
sv_meta.svIdOffset = BDS_SV_ID_OFFSET;
sv_meta.systemId = SYSTEM_ID_BDS;
switch (signalType) {
case GNSS_SIGNAL_BEIDOU_B1I:
sv_meta.svCount = sv_cache_info.bds_b1i_count;
break;
case GNSS_SIGNAL_BEIDOU_B1C:
sv_meta.svCount = sv_cache_info.bds_b1c_count;
break;
case GNSS_SIGNAL_BEIDOU_B2AI:
sv_meta.svCount = sv_cache_info.bds_b2_count;
break;
}
break;
case GNSS_SV_TYPE_NAVIC:
sv_meta.talker[0] = 'G';
sv_meta.talker[1] = 'I';
sv_meta.mask = sv_cache_info.navic_used_mask;
// NAVIC SV ids are from 401-414. So keep svIdOffset 400
sv_meta.svIdOffset = NAVIC_SV_ID_OFFSET;
sv_meta.systemId = SYSTEM_ID_NAVIC;
switch (signalType) {
case GNSS_SIGNAL_NAVIC_L5:
sv_meta.svCount = sv_cache_info.navic_l5_count;
break;
}
break;
default:
LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
return NULL;
}
sv_meta.signalId = convert_signalType_to_signalId(signalType);
sv_meta.totalSvUsedCount =
get_sv_count_from_mask(sv_cache_info.gps_used_mask,
GPS_SV_PRN_MAX - GPS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.glo_used_mask,
GLO_SV_PRN_MAX - GLO_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.gal_used_mask,
GAL_SV_PRN_MAX - GAL_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.qzss_used_mask,
QZSS_SV_PRN_MAX - QZSS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.bds_used_mask,
BDS_SV_PRN_MAX - BDS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.navic_used_mask,
NAVIC_SV_PRN_MAX - NAVIC_SV_PRN_MIN + 1);
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) +
(sv_cache_info.qzss_used_mask ? 1 : 0) +
(sv_cache_info.bds_used_mask ? 1 : 0) +
(sv_cache_info.navic_used_mask ? 1 : 0) > 1)
{
// If GPS, GLONASS, Galileo, QZSS, BDS 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[0] = 'G';
sv_meta.talker[1] = 'N';
}
return &sv_meta;
}
/*===========================================================================
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, bool isTagBlock)
{
uint8_t checksum = 0;
int length = 0;
int checksumLength = 0;
if(NULL == pNmea)
return 0;
pNmea++; //skip the $ or / for Tag Block
while (*pNmea != '\0')
{
checksum ^= *pNmea++;
length++;
}
if (isTagBlock) {
// length now contains tag block sentence string length not including / sign.
checksumLength = snprintf(pNmea, (maxSize-length-1), "*%02X\\", checksum);
} else {
// length now contains nmea sentence string length not including $ sign.
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,
bool isTagBlockGroupingEnabled)
{
if (!sentence || bufSize <= 0 || !sv_meta_p)
{
LOC_LOGE("NMEA Error invalid arguments.");
return 0;
}
char* pMarker = sentence;
int lengthRemaining = bufSize;
int length = 0;
int lengthTagBlock = 0;
uint32_t svUsedCount = 0;
uint32_t svUsedList[64] = {0};
uint32_t sentenceCount = 0;
uint32_t sentenceNumber = 1;
size_t svNumber = 1;
static uint32_t code = 1;
char fixType = '\0';
const char* talker = sv_meta_p->talker;
uint32_t svIdOffset = sv_meta_p->svIdOffset;
uint64_t mask = sv_meta_p->mask;
if (!(sv_meta_p->svTypeMask & (1 << GNSS_SV_TYPE_GLONASS))) {
svIdOffset = 0;
}
for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++)
{
if (mask & 1)
svUsedList[svUsedCount++] = i + svIdOffset;
mask = mask >> 1;
}
if (svUsedCount == 0) {
return 0;
} else {
sentenceNumber = 1;
sentenceCount = svUsedCount / 12 + (svUsedCount % 12 != 0);
svNumber = 1;
}
while (sentenceNumber <= sentenceCount) {
pMarker = sentence;
lengthRemaining = bufSize;
if (svUsedCount > 12 && isTagBlockGroupingEnabled) {
lengthTagBlock = snprintf(pMarker, lengthRemaining, "\\g:%d-%d-%d", sentenceNumber,
sentenceCount, code);
if (MAX_TAG_BLOCK_GROUP_CODE == code) {
code = 1;
}
lengthTagBlock = loc_nmea_put_checksum(sentence, bufSize, true);
pMarker += lengthTagBlock;
lengthRemaining -= lengthTagBlock;
}
if (sv_meta_p->totalSvUsedCount == 0)
fixType = '1'; // no fix
else if (sv_meta_p->totalSvUsedCount <= 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,s*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
// s : GNSS System Id
// 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 12 satellite IDs
for (uint8_t i = 0; i < 12; i++, svNumber++)
{
if (svNumber <= svUsedCount)
length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[svNumber - 1]);
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, ",,,");
}
pMarker += length;
lengthRemaining -= length;
// system id
length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId);
pMarker += length;
lengthRemaining -= length;
/* Sentence is ready, add checksum and broadcast */
length = loc_nmea_put_checksum(sentence + lengthTagBlock, bufSize - lengthTagBlock, false);
nmeaArraystr.push_back(sentence);
sentenceNumber++;
if (!isTagBlockGroupingEnabled) {
break;
}
}
if (svUsedCount > 12 && isTagBlockGroupingEnabled) {
code++;
}
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
- $GLGSV: 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;
int svCount = sv_meta_p->svCount;
if (svCount <= 0)
{
LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId);
return;
}
if ((1 << GNSS_SV_TYPE_GLONASS) & sv_meta_p->svTypeMask) {
svIdOffset = 0;
}
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++)
{
GnssSignalTypeMask signalType = svNotify.gnssSvs[svNumber-1].gnssSignalTypeMask;
if (0 == signalType) {
// If no signal type in report, it means default L1,G1,E1,B1I
switch (svNotify.gnssSvs[svNumber - 1].type)
{
case GNSS_SV_TYPE_GPS:
signalType = GNSS_SIGNAL_GPS_L1CA;
break;
case GNSS_SV_TYPE_GLONASS:
signalType = GNSS_SIGNAL_GLONASS_G1;
break;
case GNSS_SV_TYPE_GALILEO:
signalType = GNSS_SIGNAL_GALILEO_E1;
break;
case GNSS_SV_TYPE_QZSS:
signalType = GNSS_SIGNAL_QZSS_L1CA;
break;
case GNSS_SV_TYPE_BEIDOU:
signalType = GNSS_SIGNAL_BEIDOU_B1I;
break;
case GNSS_SV_TYPE_SBAS:
signalType = GNSS_SIGNAL_SBAS_L1;
break;
case GNSS_SV_TYPE_NAVIC:
signalType = GNSS_SIGNAL_NAVIC_L5;
break;
default:
LOC_LOGE("NMEA Error unknow constellation type: %d",
svNotify.gnssSvs[svNumber - 1].type);
continue;
}
}
if ((sv_meta_p->svTypeMask & (1 << svNotify.gnssSvs[svNumber - 1].type)) &&
sv_meta_p->signalId == convert_signalType_to_signalId(signalType))
{
if (GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svNumber - 1].type) {
svIdOffset = SBAS_SV_ID_OFFSET;
}
if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type &&
GLO_SV_PRN_SLOT_UNKNOWN == svNotify.gnssSvs[svNumber - 1].svId) {
length = snprintf(pMarker, lengthRemaining, ",,%02d,%03d,",
(int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
(int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
} else {
length = snprintf(pMarker, lengthRemaining, ",%02d,%02d,%03d,",
svNotify.gnssSvs[svNumber - 1].svId - svIdOffset,
(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++;
}
}
// append signalId
length = snprintf(pMarker, lengthRemaining,",%X",sv_meta_p->signalId);
pMarker += length;
lengthRemaining -= length;
length = loc_nmea_put_checksum(sentence, bufSize, false);
nmeaArraystr.push_back(sentence);
sentenceNumber++;
} //while
}
/*===========================================================================
FUNCTION loc_nmea_generate_DTM
DESCRIPTION
Generate NMEA DTM sentences generated based on position report
DEPENDENCIES
NONE
RETURN VALUE
NONE
SIDE EFFECTS
N/A
===========================================================================*/
static void loc_nmea_generate_DTM(const LocLla &ref_lla,
const LocLla &local_lla,
char *talker,
char *sentence,
int bufSize)
{
char* pMarker = sentence;
int lengthRemaining = bufSize;
int length = 0;
int datum_type;
char ref_datum[4] = {0};
char local_datum[4] = {0};
double lla_offset[3] = {0};
char latHem, longHem;
double latMins, longMins;
datum_type = loc_get_datum_type();
switch (datum_type) {
case LOC_GNSS_DATUM_WGS84:
ref_datum[0] = 'W';
ref_datum[1] = '8';
ref_datum[2] = '4';
local_datum[0] = 'P';
local_datum[1] = '9';
local_datum[2] = '0';
break;
case LOC_GNSS_DATUM_PZ90:
ref_datum[0] = 'P';
ref_datum[1] = '9';
ref_datum[2] = '0';
local_datum[0] = 'W';
local_datum[1] = '8';
local_datum[2] = '4';
break;
default:
break;
}
length = snprintf(pMarker , lengthRemaining , "$%sDTM,%s,," , talker, local_datum);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
lla_offset[0] = local_lla.lat - ref_lla.lat;
lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0);
if (lla_offset[1] < -180.0) {
lla_offset[1] += 360.0;
} else if ( lla_offset[1] > 180.0) {
lla_offset[1] -= 360.0;
}
lla_offset[2] = local_lla.alt - ref_lla.alt;
if (lla_offset[0] > 0.0) {
latHem = 'N';
} else {
latHem = 'S';
lla_offset[0] *= -1.0;
}
latMins = fmod(lla_offset[0] * 60.0, 60.0);
if (lla_offset[1] < 0.0) {
longHem = 'W';
lla_offset[1] *= -1.0;
}else {
longHem = 'E';
}
longMins = fmod(lla_offset[1] * 60.0, 60.0);
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,",
(uint8_t)floor(lla_offset[0]), latMins, latHem,
(uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum);
if (length < 0 || length >= lengthRemaining) {
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
length = loc_nmea_put_checksum(sentence, bufSize, false);
}
/*===========================================================================
FUNCTION get_utctime_with_leapsecond_transition
DESCRIPTION
This function returns true if the position report is generated during
leap second transition period. If not, then the utc timestamp returned
will be set to the timestamp in the position report. If it is,
then the utc timestamp returned will need to take into account
of the leap second transition so that proper calendar year/month/date
can be calculated from the returned utc timestamp.
DEPENDENCIES
NONE
RETURN VALUE
true: position report is generated in leap second transition period.
SIDE EFFECTS
N/A
===========================================================================*/
static bool get_utctime_with_leapsecond_transition(
const UlpLocation &location,
const GpsLocationExtended &locationExtended,
const LocationSystemInfo &systemInfo,
LocGpsUtcTime &utcPosTimestamp)
{
bool inTransition = false;
// position report is not generated during leap second transition,
// we can use the UTC timestamp from position report as is
utcPosTimestamp = location.gpsLocation.timestamp;
// Check whether we are in leap second transition.
// If so, per NMEA spec, we need to display the extra second in format of 23:59:60
// with year/month/date not getting advanced.
if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) &&
((systemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) &&
(systemInfo.leapSecondSysInfo.leapSecondInfoMask &
LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT))) {
const LeapSecondChangeInfo &leapSecondChangeInfo =
systemInfo.leapSecondSysInfo.leapSecondChangeInfo;
const GnssSystemTimeStructType &gpsTimestampLsChange =
leapSecondChangeInfo.gpsTimestampLsChange;
uint64_t gpsTimeLsChange = gpsTimestampLsChange.systemWeek * MSEC_IN_ONE_WEEK +
gpsTimestampLsChange.systemMsec;
uint64_t gpsTimePosReport = locationExtended.gpsTime.gpsWeek * MSEC_IN_ONE_WEEK +
locationExtended.gpsTime.gpsTimeOfWeekMs;
// we are only dealing with positive leap second change, as negative
// leap second change has never occurred and should not occur in future
if (leapSecondChangeInfo.leapSecondsAfterChange >
leapSecondChangeInfo.leapSecondsBeforeChange) {
// leap second adjustment is always 1 second at a time. It can happen
// every quarter end and up to four times per year.
if ((gpsTimePosReport >= gpsTimeLsChange) &&
(gpsTimePosReport < (gpsTimeLsChange + 1000))) {
inTransition = true;
utcPosTimestamp = gpsTimeLsChange + UTC_GPS_OFFSET_MSECS -
leapSecondChangeInfo.leapSecondsBeforeChange * 1000;
// we substract 1000 milli-seconds from UTC timestmap in order to calculate the
// proper year, month and date during leap second transtion.
// Let us give an example, assuming leap second transition is scheduled on 2019,
// Dec 31st mid night. When leap second transition is happening,
// instead of outputting the time as 2020, Jan, 1st, 00 hour, 00 min, and 00 sec.
// The time need to be displayed as 2019, Dec, 31st, 23 hour, 59 min and 60 sec.
utcPosTimestamp -= 1000;
}
}
}
return inTransition;
}
/*===========================================================================
FUNCTION loc_nmea_get_fix_quality
DESCRIPTION
This function obtains the fix quality for GGA sentence, mode indicator
for RMC and VTG sentence based on nav solution mask and tech mask in
the postion report.
DEPENDENCIES
NONE
Output parameter
ggaGpsQuality: gps quality field in GGA sentence
rmcModeIndicator: mode indicator field in RMC sentence
vtgModeIndicator: mode indicator field in VTG sentence
SIDE EFFECTS
N/A
===========================================================================*/
static void loc_nmea_get_fix_quality(const UlpLocation & location,
const GpsLocationExtended & locationExtended,
bool custom_gga_fix_quality,
char ggaGpsQuality[3],
char & rmcModeIndicator,
char & vtgModeIndicator,
char gnsModeIndicator[7]) {
ggaGpsQuality[0] = '0'; // 0 means no fix
rmcModeIndicator = 'N'; // N means no fix
vtgModeIndicator = 'N'; // N means no fix
memset(gnsModeIndicator, 'N', 6); // N means no fix
gnsModeIndicator[6] = '\0';
do {
if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){
break;
}
// NOTE: Order of the check is important
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
ggaGpsQuality[0] = '2'; // 2 means DGPS fix
rmcModeIndicator = 'P'; // P means precise
vtgModeIndicator = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'P'; // P means precise
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'P'; // P means precise
break;
} else if (LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask){
ggaGpsQuality[0] = '4'; // 4 means RTK Fixed fix
rmcModeIndicator = 'R'; // use R (RTK fixed)
vtgModeIndicator = 'D'; // use D (differential) as
// no RTK fixed defined for VTG in NMEA 183 spec
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'R'; // R means RTK fixed
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'R'; // R means RTK fixed
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'R'; // R means RTK fixed
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'R'; // R means RTK fixed
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'R'; // R means RTK fixed
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'R'; // R means RTK fixed
break;
} else if (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask){
ggaGpsQuality[0] = '5'; // 5 means RTK float fix
rmcModeIndicator = 'F'; // F means RTK float fix
vtgModeIndicator = 'D'; // use D (differential) as
// no RTK float defined for VTG in NMEA 183 spec
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'F'; // F means RTK float fix
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'F'; // F means RTK float fix
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'F'; // F means RTK float fix
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'F'; // F means RTK float fix
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'F'; // F means RTK float fix
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'F'; // F means RTK float fix
break;
} else if (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask){
ggaGpsQuality[0] = '2'; // 2 means DGPS fix
rmcModeIndicator = 'D'; // D means differential
vtgModeIndicator = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'D'; // D means differential
break;
} else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask){
ggaGpsQuality[0] = '2'; // 2 means DGPS fix
rmcModeIndicator = 'D'; // D means differential
vtgModeIndicator = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'D'; // D means differential
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'D'; // D means differential
break;
}
}
// NOTE: Order of the check is important
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
if (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask){
ggaGpsQuality[0] = '1'; // 1 means GPS
rmcModeIndicator = 'A'; // A means autonomous
vtgModeIndicator = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[0] = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[1] = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[2] = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[3] = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[4] = 'A'; // A means autonomous
if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
gnsModeIndicator[5] = 'A'; // A means autonomous
break;
} else if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){
ggaGpsQuality[0] = '6'; // 6 means estimated (dead reckoning)
rmcModeIndicator = 'E'; // E means estimated (dead reckoning)
vtgModeIndicator = 'E'; // E means estimated (dead reckoning)
memset(gnsModeIndicator, 'E', 6); // E means estimated (dead reckoning)
break;
}
}
} while (0);
do {
// check for customized nmea enabled or not
// with customized GGA quality enabled
// PPP fix w/o sensor: 59, PPP fix w/ sensor: 69
// DGNSS/SBAS correction fix w/o sensor: 2, w/ sensor: 62
// RTK fixed fix w/o sensor: 4, w/ sensor: 64
// RTK float fix w/o sensor: 5, w/ sensor: 65
// SPE fix w/o sensor: 1, and w/ sensor: 61
// Sensor dead reckoning fix: 6
if (true == custom_gga_fix_quality) {
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
// PPP fix w/o sensor: fix quality will now be 59
// PPP fix w sensor: fix quality will now be 69
if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) &&
(LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask)) {
ggaGpsQuality[0] = '6';
ggaGpsQuality[1] = '9';
} else {
ggaGpsQuality[0] = '5';
ggaGpsQuality[1] = '9';
}
break;
}
}
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){
char ggaQuality_copy = ggaGpsQuality[0];
ggaGpsQuality[0] = '6'; // 6 sensor assisted
// RTK fixed fix w/ sensor: fix quality will now be 64
// RTK float fix w/ sensor: 65
// DGNSS and/or SBAS correction fix and w/ sensor: 62
// GPS fix without correction and w/ sensor: 61
if ((LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask)||
(LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask)||
(LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask)||
(LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)||
(LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask)) {
ggaGpsQuality[1] = ggaQuality_copy;
break;
}
}
}
}
} while (0);
LOC_LOGv("gps quality: %s, rmc mode indicator: %c, vtg mode indicator: %c",
ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
}
/*===========================================================================
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,
const LocationSystemInfo &systemInfo,
unsigned char generate_nmea,
bool custom_gga_fix_quality,
std::vector<std::string> &nmeaArraystr,
int& indexOfGGA,
bool isTagBlockGroupingEnabled)
{
ENTRY_LOG();
indexOfGGA = -1;
LocGpsUtcTime utcPosTimestamp = 0;
bool inLsTransition = false;
inLsTransition = get_utctime_with_leapsecond_transition
(location, locationExtended, systemInfo, utcPosTimestamp);
time_t utcTime(utcPosTimestamp/1000);
struct tm result;
tm * pTm = gmtime_r(&utcTime, &result);
if (NULL == pTm) {
LOC_LOGE("gmtime failed");
return;
}
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0};
char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0};
char sentence_GNS[NMEA_SENTENCE_MAX_LENGTH] = {0};
char sentence_GGA[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;
int datum_type = loc_get_datum_type();
LocEcef ecef_w84;
LocEcef ecef_p90;
LocLla lla_w84;
LocLla lla_p90;
LocLla ref_lla;
LocLla local_lla;
if (inLsTransition) {
// During leap second transition, we need to display the extra
// leap second of hour, minute, second as (23:59:60)
utcHours = 23;
utcMinutes = 59;
utcSeconds = 60;
// As UTC timestamp is freezing during leap second transition,
// retrieve milli-seconds portion from GPS timestamp.
utcMSeconds = locationExtended.gpsTime.gpsTimeOfWeekMs % 1000;
}
loc_sv_cache_info sv_cache_info = {};
if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
sv_cache_info.gps_used_mask =
locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask;
sv_cache_info.glo_used_mask =
locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask;
sv_cache_info.gal_used_mask =
locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask;
sv_cache_info.bds_used_mask =
locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
sv_cache_info.qzss_used_mask =
locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask;
sv_cache_info.navic_used_mask =
locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask;
}
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, sv_cache_info, GNSS_SV_TYPE_GPS,
GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled);
if (count > 0)
{
svUsedCount += count;
talker[0] = sv_meta.talker[0];
talker[1] = sv_meta.talker[1];
}
// -------------------
// ---$GLGSA/$GNGSA---
// -------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr, isTagBlockGroupingEnabled);
if (count > 0)
{
svUsedCount += count;
talker[0] = sv_meta.talker[0];
talker[1] = sv_meta.talker[1];
}
// -------------------
// ---$GAGSA/$GNGSA---
// -------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr, isTagBlockGroupingEnabled);
if (count > 0)
{
svUsedCount += count;
talker[0] = sv_meta.talker[0];
talker[1] = sv_meta.talker[1];
}
// ----------------------------
// ---$GBGSA/$GNGSA (BEIDOU)---
// ----------------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr, isTagBlockGroupingEnabled);
if (count > 0)
{
svUsedCount += count;
talker[0] = sv_meta.talker[0];
talker[1] = sv_meta.talker[1];
}
// --------------------------
// ---$GQGSA/$GNGSA (QZSS)---
// --------------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled);
if (count > 0)
{
svUsedCount += count;
talker[0] = sv_meta.talker[0];
talker[1] = sv_meta.talker[1];
}
// if svUsedCount is 0, it means we do not generate any GSA sentence yet.
// in this case, generate an empty GSA sentence
if (svUsedCount == 0) {
strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
}
char ggaGpsQuality[3] = {'0', '\0', '\0'};
char rmcModeIndicator = 'N';
char vtgModeIndicator = 'N';
char gnsModeIndicator[7] = {'N', 'N', 'N', 'N', 'N', 'N', '\0'};
loc_nmea_get_fix_quality(location, locationExtended, custom_gga_fix_quality,
ggaGpsQuality, rmcModeIndicator, vtgModeIndicator, gnsModeIndicator);
// -------------------
// ------$--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)
{
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;
length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator);
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
memset(&ecef_w84, 0, sizeof(ecef_w84));
memset(&ecef_p90, 0, sizeof(ecef_p90));
memset(&lla_w84, 0, sizeof(lla_w84));
memset(&lla_p90, 0, sizeof(lla_p90));
memset(&ref_lla, 0, sizeof(ref_lla));
memset(&local_lla, 0, sizeof(local_lla));
lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI;
lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI;
lla_w84.alt = location.gpsLocation.altitude;
convert_Lla_to_Ecef(lla_w84, ecef_w84);
convert_WGS84_to_PZ90(ecef_w84, ecef_p90);
convert_Ecef_to_Lla(ecef_p90, lla_p90);
switch (datum_type) {
case LOC_GNSS_DATUM_WGS84:
ref_lla.lat = location.gpsLocation.latitude;
ref_lla.lon = location.gpsLocation.longitude;
ref_lla.alt = location.gpsLocation.altitude;
local_lla.lat = lla_p90.lat / M_PI * 180.0;
local_lla.lon = lla_p90.lon / M_PI * 180.0;
local_lla.alt = lla_p90.alt;
break;
case LOC_GNSS_DATUM_PZ90:
ref_lla.lat = lla_p90.lat / M_PI * 180.0;
ref_lla.lon = lla_p90.lon / M_PI * 180.0;
ref_lla.alt = lla_p90.alt;
local_lla.lat = location.gpsLocation.latitude;
local_lla.lon = location.gpsLocation.longitude;
local_lla.alt = location.gpsLocation.altitude;
break;
default:
break;
}
// -------------------
// ------$--DTM-------
// -------------------
loc_nmea_generate_DTM(ref_lla, local_lla, talker, sentence_DTM, sizeof(sentence_DTM));
// -------------------
// ------$--RMC-------
// -------------------
pMarker = sentence_RMC;
lengthRemaining = sizeof(sentence_RMC);
bool validFix = ((0 != sv_cache_info.gps_used_mask) ||
(0 != sv_cache_info.glo_used_mask) ||
(0 != sv_cache_info.gal_used_mask) ||
(0 != sv_cache_info.qzss_used_mask) ||
(0 != sv_cache_info.bds_used_mask));
if (validFix) {
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A,",
talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
} else {
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,V,",
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 = ref_lla.lat;
double longitude = ref_lla.lon;
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;
length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator);
pMarker += length;
lengthRemaining -= length;
// hardcode Navigation Status field to 'V'
length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC), false);
// -------------------
// ------$--GNS-------
// -------------------
pMarker = sentence_GNS;
lengthRemaining = sizeof(sentence_GNS);
length = snprintf(pMarker, lengthRemaining, "$%sGNS,%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 = ref_lla.lat;
double longitude = ref_lla.lon;
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;
length = snprintf(pMarker, lengthRemaining, "%s,", gnsModeIndicator);
pMarker += length;
lengthRemaining -= length;
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
length = snprintf(pMarker, lengthRemaining, "%02d,%.1f,",
svUsedCount, locationExtended.hdop);
}
else { // no hdop
length = snprintf(pMarker, lengthRemaining, "%02d,,",
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,",
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,",
ref_lla.alt - 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 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE)
{
length = snprintf(pMarker, lengthRemaining, "%.1f,",
(float)locationExtended.dgnssDataAgeMsec / 1000);
}
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_DGNSS_REF_STATION_ID)
{
length = snprintf(pMarker, lengthRemaining, "%04d",
locationExtended.dgnssRefStationId);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
}
// hardcode Navigation Status field to 'V'
length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
pMarker += length;
lengthRemaining -= length;
length = loc_nmea_put_checksum(sentence_GNS, sizeof(sentence_GNS), false);
// -------------------
// ------$--GGA-------
// -------------------
pMarker = sentence_GGA;
lengthRemaining = sizeof(sentence_GGA);
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 = ref_lla.lat;
double longitude = ref_lla.lon;
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;
// 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, "%s,%02d,%.1f,",
ggaGpsQuality, svUsedCount, locationExtended.hdop);
}
else
{ // no hdop
length = snprintf(pMarker, lengthRemaining, "%s,%02d,,",
ggaGpsQuality, 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,",
ref_lla.alt - 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 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE)
{
length = snprintf(pMarker, lengthRemaining, "%.1f,",
(float)locationExtended.dgnssDataAgeMsec / 1000);
}
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_DGNSS_REF_STATION_ID)
{
length = snprintf(pMarker, lengthRemaining, "%04d",
locationExtended.dgnssRefStationId);
if (length < 0 || length >= lengthRemaining)
{
LOC_LOGE("NMEA Error in string formatting");
return;
}
pMarker += length;
lengthRemaining -= length;
}
length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA), false);
// ------$--DTM-------
nmeaArraystr.push_back(sentence_DTM);
// ------$--RMC-------
nmeaArraystr.push_back(sentence_RMC);
if(LOC_GNSS_DATUM_PZ90 == datum_type) {
// ------$--DTM-------
nmeaArraystr.push_back(sentence_DTM);
}
// ------$--GNS-------
nmeaArraystr.push_back(sentence_GNS);
if(LOC_GNSS_DATUM_PZ90 == datum_type) {
// ------$--DTM-------
nmeaArraystr.push_back(sentence_DTM);
}
// ------$--GGA-------
nmeaArraystr.push_back(sentence_GGA);
indexOfGGA = static_cast<int>(nmeaArraystr.size() - 1);
}
//Send blank NMEA reports for non-final fixes
else {
strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
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};
loc_sv_cache_info sv_cache_info = {};
//Count GPS SVs for saparating GPS from GLONASS and throw others
for (uint32_t svOffset = 0; svOffset < svNotify.count; svOffset++) {
if ((GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svOffset].type) ||
(GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svOffset].type))
{
if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.gps_l5_count++;
} else if (GNSS_SIGNAL_GPS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.gps_l2_count++;
} else {
// GNSS_SIGNAL_GPS_L1CA, GNSS_SIGNAL_SBAS_L1 or default
// If no signal type in report, it means default L1
sv_cache_info.gps_l1_count++;
}
}
else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svOffset].type)
{
if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){
sv_cache_info.glo_g2_count++;
} else {
// GNSS_SIGNAL_GLONASS_G1 or default
// If no signal type in report, it means default G1
sv_cache_info.glo_g1_count++;
}
}
else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svOffset].type)
{
if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){
sv_cache_info.gal_e5_count++;
} else if (GNSS_SIGNAL_GALILEO_E5B == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.gal_e5b_count++;
} else {
// GNSS_SIGNAL_GALILEO_E1 or default
// If no signal type in report, it means default E1
sv_cache_info.gal_e1_count++;
}
}
else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svOffset].type)
{
if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.qzss_l5_count++;
} else if (GNSS_SIGNAL_QZSS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.qzss_l2_count++;
} else {
// GNSS_SIGNAL_QZSS_L1CA or default
// If no signal type in report, it means default L1
sv_cache_info.qzss_l1_count++;
}
}
else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svOffset].type)
{
// cache the used in fix mask, as it will be needed to send $PQGSA
// during the position report
if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
(svNotify.gnssSvs[svOffset].gnssSvOptionsMask &
GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
{
setSvMask(sv_cache_info.bds_used_mask, svNotify.gnssSvs[svOffset].svId);
}
if ((GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) ||
(GNSS_SIGNAL_BEIDOU_B2AQ == svNotify.gnssSvs[svOffset].gnssSignalTypeMask)) {
sv_cache_info.bds_b2_count++;
} else if (GNSS_SIGNAL_BEIDOU_B1C == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
sv_cache_info.bds_b1c_count++;
} else {
// GNSS_SIGNAL_BEIDOU_B1I or default
// If no signal type in report, it means default B1I
sv_cache_info.bds_b1i_count++;
}
}
else if (GNSS_SV_TYPE_NAVIC == svNotify.gnssSvs[svOffset].type)
{
// GNSS_SIGNAL_NAVIC_L5 is the only signal type for NAVIC
sv_cache_info.navic_l5_count++;
}
}
loc_nmea_sv_meta sv_meta;
// ---------------------
// ------$GPGSV:L1CA----
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
GNSS_SIGNAL_GPS_L1CA, false), nmeaArraystr);
// ---------------------
// ------$GPGSV:L5------
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
GNSS_SIGNAL_GPS_L5, false), nmeaArraystr);
// ---------------------
// ------$GPGSV:L2------
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
GNSS_SIGNAL_GPS_L2, false), nmeaArraystr);
// ---------------------
// ------$GLGSV:G1------
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr);
// ---------------------
// ------$GLGSV:G2------
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
GNSS_SIGNAL_GLONASS_G2, false), nmeaArraystr);
// ---------------------
// ------$GAGSV:E1------
// ---------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
GNSS_SIGNAL_GALILEO_E1, false), nmeaArraystr);
// -------------------------
// ------$GAGSV:E5A---------
// -------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
GNSS_SIGNAL_GALILEO_E5A, false), nmeaArraystr);
// -------------------------
// ------$GAGSV:E5B---------
// -------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
GNSS_SIGNAL_GALILEO_E5B, false), nmeaArraystr);
// -----------------------------
// ------$GQGSV (QZSS):L1CA-----
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr);
// -----------------------------
// ------$GQGSV (QZSS):L5-------
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
GNSS_SIGNAL_QZSS_L5, false), nmeaArraystr);
// -----------------------------
// ------$GQGSV (QZSS):L2-------
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
GNSS_SIGNAL_QZSS_L2, false), nmeaArraystr);
// -----------------------------
// ------$GBGSV (BEIDOU:B1I)----
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
GNSS_SIGNAL_BEIDOU_B1I, false), nmeaArraystr);
// -----------------------------
// ------$GBGSV (BEIDOU:B1C)----
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
GNSS_SIGNAL_BEIDOU_B1C, false), nmeaArraystr);
// -----------------------------
// ------$GBGSV (BEIDOU:B2AI)---
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
GNSS_SIGNAL_BEIDOU_B2AI, false), nmeaArraystr);
// -----------------------------
// ------$GIGSV (NAVIC:L5)------
// -----------------------------
loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC,
GNSS_SIGNAL_NAVIC_L5,false), nmeaArraystr);
EXIT_LOG(%d, 0);
}
Reference in New Issue Copy Permalink