diff options
Diffstat (limited to 'gps/utils/loc_nmea.cpp')
| -rw-r--r-- | gps/utils/loc_nmea.cpp | 1347 |
1 files changed, 1193 insertions, 154 deletions
diff --git a/gps/utils/loc_nmea.cpp b/gps/utils/loc_nmea.cpp index 558dc65..24197ff 100644 --- a/gps/utils/loc_nmea.cpp +++ b/gps/utils/loc_nmea.cpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved. +/* Copyright (c) 2012-2019, 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 @@ -33,24 +33,87 @@ #include <math.h> #include <log_util.h> #include <loc_pla.h> +#include <loc_cfg.h> #define GLONASS_SV_ID_OFFSET 64 +#define QZSS_SV_ID_OFFSET (-192) +#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 // GNSS system id according to NMEA spec #define SYSTEM_ID_GPS 1 #define SYSTEM_ID_GLONASS 2 #define SYSTEM_ID_GALILEO 3 -// Extended systems -#define SYSTEM_ID_BEIDOU 4 +#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]; LocGnssConstellationType svType; - uint32_t mask; + uint64_t mask; uint32_t svCount; + uint32_t totalSvUsedCount; uint32_t svIdOffset; uint32_t signalId; uint32_t systemId; @@ -58,22 +121,260 @@ typedef struct loc_nmea_sv_meta_s typedef struct loc_sv_cache_info_s { - uint32_t gps_used_mask; - uint32_t glo_used_mask; - uint32_t gal_used_mask; - uint32_t qzss_used_mask; - uint32_t bds_used_mask; - uint32_t gps_count; - uint32_t glo_count; - uint32_t gal_count; - uint32_t qzss_count; - uint32_t bds_count; + 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_l5_count; + uint32_t glo_g1_count; + uint32_t glo_g2_count; + uint32_t gal_e1_count; + uint32_t gal_e5_count; + uint32_t qzss_l1_count; + uint32_t qzss_l5_count; + uint32_t bds_b1_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: + 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 @@ -92,6 +393,7 @@ SIDE EFFECTS 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)); @@ -103,56 +405,97 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta, sv_meta.talker[0] = 'G'; sv_meta.talker[1] = 'P'; sv_meta.mask = sv_cache_info.gps_used_mask; - sv_meta.svCount = sv_cache_info.gps_count; - sv_meta.signalId = 1; sv_meta.systemId = SYSTEM_ID_GPS; + if (GNSS_SIGNAL_GPS_L1CA == signalType) { + sv_meta.svCount = sv_cache_info.gps_l1_count; + } else if (GNSS_SIGNAL_GPS_L5 == signalType) { + sv_meta.svCount = sv_cache_info.gps_l5_count; + } 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; - sv_meta.svCount = sv_cache_info.glo_count; // GLONASS SV ids are from 65-96 sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET; - sv_meta.signalId = 1; sv_meta.systemId = SYSTEM_ID_GLONASS; + if (GNSS_SIGNAL_GLONASS_G1 == signalType) { + sv_meta.svCount = sv_cache_info.glo_g1_count; + } else if (GNSS_SIGNAL_GLONASS_G2 == signalType) { + sv_meta.svCount = sv_cache_info.glo_g2_count; + } 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; - sv_meta.svCount = sv_cache_info.gal_count; - sv_meta.signalId = 7; sv_meta.systemId = SYSTEM_ID_GALILEO; + if (GNSS_SIGNAL_GALILEO_E1 == signalType) { + sv_meta.svCount = sv_cache_info.gal_e1_count; + } else if (GNSS_SIGNAL_GALILEO_E5A == signalType) { + sv_meta.svCount = sv_cache_info.gal_e5_count; + } break; case GNSS_SV_TYPE_QZSS: - sv_meta.talker[0] = 'P'; + sv_meta.talker[0] = 'G'; sv_meta.talker[1] = 'Q'; sv_meta.mask = sv_cache_info.qzss_used_mask; - sv_meta.svCount = sv_cache_info.qzss_count; - // QZSS SV ids are from 193-197. So keep svIdOffset 0 - sv_meta.signalId = 0; + // QZSS SV ids are from 193-199. So keep svIdOffset -192 + sv_meta.svIdOffset = QZSS_SV_ID_OFFSET; sv_meta.systemId = SYSTEM_ID_QZSS; + if (GNSS_SIGNAL_QZSS_L1CA == signalType) { + sv_meta.svCount = sv_cache_info.qzss_l1_count; + } else if (GNSS_SIGNAL_QZSS_L5 == signalType) { + sv_meta.svCount = sv_cache_info.qzss_l5_count; + } break; case GNSS_SV_TYPE_BEIDOU: - sv_meta.talker[0] = 'P'; - sv_meta.talker[1] = 'Q'; + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'B'; sv_meta.mask = sv_cache_info.bds_used_mask; - sv_meta.svCount = sv_cache_info.bds_count; // BDS SV ids are from 201-235. So keep svIdOffset 0 - sv_meta.signalId = 0; - sv_meta.systemId = SYSTEM_ID_BEIDOU; + sv_meta.systemId = SYSTEM_ID_BDS; + if (GNSS_SIGNAL_BEIDOU_B1I == signalType) { + sv_meta.svCount = sv_cache_info.bds_b1_count; + } else if (GNSS_SIGNAL_BEIDOU_B2AI == signalType) { + sv_meta.svCount = sv_cache_info.bds_b2_count; + } + 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 0 + sv_meta.systemId = SYSTEM_ID_NAVIC; + if (GNSS_SIGNAL_NAVIC_L5 == signalType) { + sv_meta.svCount = sv_cache_info.navic_l5_count; + } 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) > 1) + (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, @@ -240,27 +583,31 @@ static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtende int length = 0; uint32_t svUsedCount = 0; - uint32_t svUsedList[32] = {0}; + uint32_t svUsedList[64] = {0}; char fixType = '\0'; const char* talker = sv_meta_p->talker; uint32_t svIdOffset = sv_meta_p->svIdOffset; - uint32_t mask = sv_meta_p->mask; + uint64_t mask = sv_meta_p->mask; + + if(sv_meta_p->svType != GNSS_SV_TYPE_GLONASS) { + svIdOffset = 0; + } - for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) + for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++) { if (mask & 1) svUsedList[svUsedCount++] = i + svIdOffset; mask = mask >> 1; } - if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType) + if (svUsedCount == 0) return 0; - if (svUsedCount == 0) + if (sv_meta_p->totalSvUsedCount == 0) fixType = '1'; // no fix - else if (svUsedCount <= 3) + else if (sv_meta_p->totalSvUsedCount <= 3) fixType = '2'; // 2D fix else fixType = '3'; // 3D fix @@ -371,13 +718,9 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, const char* talker = sv_meta_p->talker; uint32_t svIdOffset = sv_meta_p->svIdOffset; int svCount = sv_meta_p->svCount; - if (svCount <= 0) { - // no svs in view, so just send a blank $--GSV sentence - snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,%d", talker, sv_meta_p->signalId); - length = loc_nmea_put_checksum(sentence, bufSize); - nmeaArraystr.push_back(sentence); + LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId); return; } @@ -403,10 +746,49 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++) { - if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type) + 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->svType == svNotify.gnssSvs[svNumber - 1].type && + sv_meta_p->signalId == convert_signalType_to_signalId(signalType)) { + uint16_t svId = svNotify.gnssSvs[svNumber - 1].svId; + // For QZSS we adjusted SV id's in GnssAdapter, we need to re-adjust here + if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type) { + svId = svId - (QZSS_SV_PRN_MIN - 1); + } length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", - svNotify.gnssSvs[svNumber - 1].svId + svIdOffset, + svId + svIdOffset, (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int @@ -438,7 +820,7 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, } // append signalId - length = snprintf(pMarker, lengthRemaining,",%d",sv_meta_p->signalId); + length = snprintf(pMarker, lengthRemaining,",%X",sv_meta_p->signalId); pMarker += length; lengthRemaining -= length; @@ -450,6 +832,330 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, } /*=========================================================================== +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); +} + +/*=========================================================================== +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) { + + ggaGpsQuality[0] = '0'; // 0 means no fix + rmcModeIndicator = 'N'; // N means no fix + vtgModeIndicator = 'N'; // N means no fix + + do { + // GGA fix quality is defined in NMEA spec as below: + // https://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_GGA.html + // Fix quality: 0 = invalid + // 1 = GPS fix (SPS) + // 2 = DGPS fix + // 3 = PPS fix + // 4 = Real Time Kinematic + // 5 = Float RTK + // 6 = estimated (dead reckoning) (2.3 feature) + // 7 = Manual input mode + // 8 = Simulation mode + 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 + 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 + 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 + 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 + 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 + 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 + 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) + 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 @@ -475,11 +1181,20 @@ SIDE EFFECTS ===========================================================================*/ 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) { ENTRY_LOG(); - time_t utcTime(location.gpsLocation.timestamp/1000); + + LocGpsUtcTime utcPosTimestamp = 0; + bool inLsTransition = false; + + inLsTransition = get_utctime_with_leapsecond_transition + (location, locationExtended, systemInfo, utcPosTimestamp); + + time_t utcTime(utcPosTimestamp/1000); tm * pTm = gmtime(&utcTime); if (NULL == pTm) { LOC_LOGE("gmtime failed"); @@ -487,6 +1202,10 @@ void loc_nmea_generate_pos(const UlpLocation &location, } 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; @@ -497,22 +1216,45 @@ void loc_nmea_generate_pos(const UlpLocation &location, int utcMinutes = pTm->tm_min; int utcSeconds = pTm->tm_sec; int utcMSeconds = (location.gpsLocation.timestamp)%1000; - loc_sv_cache_info sv_cache_info = {}; + 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 = - (uint32_t)locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask; + locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask; sv_cache_info.glo_used_mask = - (uint32_t)locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask; + locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask; sv_cache_info.gal_used_mask = - (uint32_t)locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask; - sv_cache_info.qzss_used_mask = - (uint32_t)locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask; + locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask; sv_cache_info.bds_used_mask = - (uint32_t)locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_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'}; + char modeIndicator[7] = {0}; uint32_t svUsedCount = 0; uint32_t count = 0; loc_nmea_sv_meta sv_meta; @@ -521,8 +1263,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, // ------------------- count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, true), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr); if (count > 0) { svUsedCount += count; @@ -535,8 +1277,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, // ------------------- count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, true), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, + GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr); if (count > 0) { svUsedCount += count; @@ -549,8 +1291,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, // ------------------- count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, true), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, + GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr); if (count > 0) { svUsedCount += count; @@ -558,31 +1300,39 @@ void loc_nmea_generate_pos(const UlpLocation &location, talker[1] = sv_meta.talker[1]; } - // -------------------------- - // ---$PQGSA/$GNGSA (QZSS)--- - // -------------------------- - + // ---------------------------- + // ---$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_QZSS, false), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr); if (count > 0) { svUsedCount += count; - // talker should be default "GP". If GPS, GLO etc is used, it should be "GN" + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; } - // ---------------------------- - // ---$PQGSA/$GNGSA (BEIDOU)--- - // ---------------------------- + // -------------------------- + // ---$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_BEIDOU, false), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, + GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr); if (count > 0) { svUsedCount += count; - // talker should be default "GP". If GPS, GLO etc is used, it should be "GN" + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; } + char ggaGpsQuality[3] = {'0', '\0', '\0'}; + char rmcModeIndicator = 'N'; + char vtgModeIndicator = 'N'; + loc_nmea_get_fix_quality(location, locationExtended, custom_gga_fix_quality, + ggaGpsQuality, rmcModeIndicator, vtgModeIndicator); + // ------------------- // ------$--VTG------- // ------------------- @@ -637,27 +1387,57 @@ void loc_nmea_generate_pos(const UlpLocation &location, pMarker += length; lengthRemaining -= length; - if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) - // N means no fix - length = snprintf(pMarker, lengthRemaining, "%c", 'N'); - else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) - // D means differential - length = snprintf(pMarker, lengthRemaining, "%c", 'D'); - else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) - // E means estimated (dead reckoning) - length = snprintf(pMarker, lengthRemaining, "%c", 'E'); - else // A means autonomous - length = snprintf(pMarker, lengthRemaining, "%c", 'A'); + length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 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; - lengthRemaining = sizeof(sentence); + pMarker = sentence_RMC; + lengthRemaining = sizeof(sentence_RMC); length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," , talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10); @@ -672,8 +1452,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) { - double latitude = location.gpsLocation.latitude; - double longitude = location.gpsLocation.longitude; + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; char latHemisphere; char lonHemisphere; double latMinutes; @@ -795,17 +1575,176 @@ void loc_nmea_generate_pos(const UlpLocation &location, pMarker += length; lengthRemaining -= length; - if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) - // N means no fix - length = snprintf(pMarker, lengthRemaining, "%c", 'N'); + length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator); + 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_RMC, sizeof(sentence_RMC)); + + // ------------------- + // ------$--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; + + if(!(sv_cache_info.gps_used_mask ? 1 : 0)) + modeIndicator[0] = 'N'; else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) - // D means differential - length = snprintf(pMarker, lengthRemaining, "%c", 'D'); + modeIndicator[0] = 'D'; else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) - // E means estimated (dead reckoning) - length = snprintf(pMarker, lengthRemaining, "%c", 'E'); - else // A means autonomous - length = snprintf(pMarker, lengthRemaining, "%c", 'A'); + modeIndicator[0] = 'E'; + else + modeIndicator[0] = 'A'; + if(!(sv_cache_info.glo_used_mask ? 1 : 0)) + modeIndicator[1] = 'N'; + else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) + modeIndicator[1] = 'E'; + else + modeIndicator[1] = 'A'; + if(!(sv_cache_info.gal_used_mask ? 1 : 0)) + modeIndicator[2] = 'N'; + else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) + modeIndicator[2] = 'E'; + else + modeIndicator[2] = 'A'; + if(!(sv_cache_info.bds_used_mask ? 1 : 0)) + modeIndicator[3] = 'N'; + else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) + modeIndicator[3] = 'E'; + else + modeIndicator[3] = 'A'; + if(!(sv_cache_info.qzss_used_mask ? 1 : 0)) + modeIndicator[4] = 'N'; + else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) + modeIndicator[4] = 'E'; + else + modeIndicator[4] = 'A'; + if(!(sv_cache_info.navic_used_mask ? 1 : 0)) + modeIndicator[5] = 'N'; + else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) + modeIndicator[5] = 'E'; + else + modeIndicator[5] = 'A'; + modeIndicator[6] = '\0'; + for(int index = 5; index > 0 && 'N' == modeIndicator[index]; index--) { + modeIndicator[index] = '\0'; + } + length = snprintf(pMarker, lengthRemaining,"%s,", modeIndicator); + + 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,",,"); + } pMarker += length; lengthRemaining -= length; @@ -815,15 +1754,15 @@ void loc_nmea_generate_pos(const UlpLocation &location, pMarker += length; lengthRemaining -= length; - length = loc_nmea_put_checksum(sentence, sizeof(sentence)); - nmeaArraystr.push_back(sentence); + length = loc_nmea_put_checksum(sentence_GNS, sizeof(sentence_GNS)); + // ------------------- // ------$--GGA------- // ------------------- - pMarker = sentence; - lengthRemaining = sizeof(sentence); + pMarker = sentence_GGA; + lengthRemaining = sizeof(sentence_GGA); length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," , talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); @@ -838,8 +1777,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) { - double latitude = location.gpsLocation.latitude; - double longitude = location.gpsLocation.longitude; + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; char latHemisphere; char lonHemisphere; double latMinutes; @@ -885,28 +1824,18 @@ void loc_nmea_generate_pos(const UlpLocation &location, pMarker += length; lengthRemaining -= length; - char gpsQuality; - if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) - gpsQuality = '0'; // 0 means no fix - else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) - gpsQuality = '2'; // 2 means DGPS fix - else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) - gpsQuality = '6'; // 6 means estimated (dead reckoning) - else - gpsQuality = '1'; // 1 means GPS fix - // Number of satellites in use, 00-12 if (svUsedCount > MAX_SATELLITES_IN_USE) svUsedCount = MAX_SATELLITES_IN_USE; if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) { - length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", - gpsQuality, svUsedCount, locationExtended.hdop); + length = snprintf(pMarker, lengthRemaining, "%s,%02d,%.1f,", + ggaGpsQuality, svUsedCount, locationExtended.hdop); } else { // no hdop - length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", - gpsQuality, svUsedCount); + length = snprintf(pMarker, lengthRemaining, "%s,%02d,,", + ggaGpsQuality, svUsedCount); } if (length < 0 || length >= lengthRemaining) @@ -939,35 +1868,52 @@ void loc_nmea_generate_pos(const UlpLocation &location, (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) { length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", - location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); + ref_lla.alt - locationExtended.altitudeMeanSeaLevel); } else { length = snprintf(pMarker, lengthRemaining,",,,"); } - length = loc_nmea_put_checksum(sentence, sizeof(sentence)); - nmeaArraystr.push_back(sentence); + length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA)); + + // ------$--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); + } //Send blank NMEA reports for non-final fixes else { - strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); + strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); - strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); + strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); - strlcpy(sentence, "$PQGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); + strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); - strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); + strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); - strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence)); + strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); @@ -1017,9 +1963,15 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { - sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + sv_cache_info.gps_used_mask |= (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + } + if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) { + sv_cache_info.gps_l5_count++; + } else { + // GNSS_SIGNAL_GPS_L1CA or default + // If no signal type in report, it means default L1 + sv_cache_info.gps_l1_count++; } - sv_cache_info.gps_count++; } else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type) { @@ -1029,9 +1981,15 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { - sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + sv_cache_info.glo_used_mask |= (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + } + if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svNumber - 1].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++; } - sv_cache_info.glo_count++; } else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type) { @@ -1041,9 +1999,15 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { - sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + sv_cache_info.gal_used_mask |= (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + } + if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){ + sv_cache_info.gal_e5_count++; + } else { + // GNSS_SIGNAL_GALILEO_E1 or default + // If no signal type in report, it means default E1 + sv_cache_info.gal_e1_count++; } - sv_cache_info.gal_count++; } else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type) { @@ -1053,9 +2017,17 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { - sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + // For QZSS we adjusted SV id's in GnssAdapter, we need to re-adjust here + sv_cache_info.qzss_used_mask |= + (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - (QZSS_SV_PRN_MIN - 1) - 1)); + } + if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) { + sv_cache_info.qzss_l5_count++; + } else { + // GNSS_SIGNAL_QZSS_L1CA or default + // If no signal type in report, it means default L1 + sv_cache_info.qzss_l1_count++; } - sv_cache_info.qzss_count++; } else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svNumber - 1].type) { @@ -1065,50 +2037,117 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { - sv_cache_info.bds_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + sv_cache_info.bds_used_mask |= (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + } + if(GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){ + sv_cache_info.bds_b2_count++; + } else { + // GNSS_SIGNAL_BEIDOU_B1I or default + // If no signal type in report, it means default B1I + sv_cache_info.bds_b1_count++; + } + } + else if (GNSS_SV_TYPE_NAVIC == svNotify.gnssSvs[svNumber - 1].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[svNumber - 1].gnssSvOptionsMask & + GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) + { + sv_cache_info.navic_used_mask |= + (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); } - sv_cache_info.bds_count++; + // GNSS_SIGNAL_NAVIC_L5 is the only signal type for NAVIC + sv_cache_info.navic_l5_count++; } } loc_nmea_sv_meta sv_meta; - // ------------------ - // ------$GPGSV------ - // ------------------ + // --------------------- + // ------$GPGSV:L1CA---- + // --------------------- loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, false), nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L1CA, false), nmeaArraystr); + + // --------------------- + // ------$GPGSV:L5------ + // --------------------- - // ------------------ - // ------$GLGSV------ - // ------------------ + 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); + // --------------------- + // ------$GLGSV:G1------ + // --------------------- loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, false), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, + GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr); - // ------------------ - // ------$GAGSV------ - // ------------------ + // --------------------- + // ------$GLGSV:G2------ + // --------------------- loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, false), - nmeaArraystr); + 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); // ------------------------- - // ------$PQGSV (QZSS)------ + // ------$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); + + // ----------------------------- + // ------$PQGSV (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); + + // ----------------------------- + // ------$PQGSV (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); + // ----------------------------- + // ------$PQGSV (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); + + // ----------------------------- + // ------$PQGSV (BEIDOU:B2AI)--- + // ----------------------------- loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, false), nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B2AI,false), nmeaArraystr); - // --------------------------- - // ------$PQGSV (BEIDOU)------ - // --------------------------- + // ----------------------------- + // ------$GIGSV (NAVIC:L5)------ + // ----------------------------- loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), - loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, false), - nmeaArraystr); + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC, + GNSS_SIGNAL_NAVIC_L5,false), nmeaArraystr); EXIT_LOG(%d, 0); } |