/* 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 #include #include #include #include #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 #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; 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_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 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.svType = 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; 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; // GLONASS SV ids are from 65-96 sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET; 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.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] = '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; 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] = 'G'; sv_meta.talker[1] = 'B'; sv_meta.mask = sv_cache_info.bds_used_mask; // BDS SV ids are from 201-235. So keep svIdOffset 0 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) + (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) { uint8_t checksum = 0; int length = 0; if(NULL == pNmea) return 0; pNmea++; //skip the $ while (*pNmea != '\0') { checksum ^= *pNmea++; length++; } // length now contains nmea sentence string length not including $ sign. int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum); // total length of nmea sentence is length of nmea sentence inc $ sign plus // length of checksum (+1 is to cover the $ character in the length). return (length + checksumLength + 1); } /*=========================================================================== FUNCTION loc_nmea_generate_GSA DESCRIPTION Generate NMEA GSA sentences generated based on position report Currently below sentences are generated: - $GPGSA : GPS DOP and active SVs - $GLGSA : GLONASS DOP and active SVs - $GAGSA : GALILEO DOP and active SVs - $GNGSA : GNSS DOP and active SVs DEPENDENCIES NONE RETURN VALUE Number of SVs used SIDE EFFECTS N/A ===========================================================================*/ static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended, char* sentence, int bufSize, loc_nmea_sv_meta* sv_meta_p, std::vector &nmeaArraystr) { if (!sentence || bufSize <= 0 || !sv_meta_p) { LOC_LOGE("NMEA Error invalid arguments."); return 0; } char* pMarker = sentence; int lengthRemaining = bufSize; int length = 0; uint32_t svUsedCount = 0; uint32_t svUsedList[64] = {0}; 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->svType != 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; 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 first 12 satellite IDs for (uint8_t i = 0; i < 12; i++) { if (i < svUsedCount) length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); else length = snprintf(pMarker, lengthRemaining, ","); if (length < 0 || length >= lengthRemaining) { LOC_LOGE("NMEA Error in string formatting"); return 0; } pMarker += length; lengthRemaining -= length; } // Add the position/horizontal/vertical DOP values if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) { length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,", locationExtended.pdop, locationExtended.hdop, locationExtended.vdop); } else { // no dop length = snprintf(pMarker, lengthRemaining, ",,,"); } 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, bufSize); nmeaArraystr.push_back(sentence); return svUsedCount; } /*=========================================================================== FUNCTION loc_nmea_generate_GSV DESCRIPTION Generate NMEA GSV sentences generated based on sv report Currently below sentences are generated: - $GPGSV: GPS Satellites in View - $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 &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; } 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->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,", 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); 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); } /*=========================================================================== 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 { // 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 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 &nmeaArraystr) { ENTRY_LOG(); 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); 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); 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); 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); 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); 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)); 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) { float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; if (magTrack < 0.0) magTrack += 360.0; else if (magTrack > 360.0) magTrack -= 360.0; } length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack); } else { length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker); } if (length < 0 || length >= lengthRemaining) { LOC_LOGE("NMEA Error in string formatting"); return; } pMarker += length; lengthRemaining -= length; if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) { float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); float speedKmPerHour = location.gpsLocation.speed * 3.6; length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); } else { length = snprintf(pMarker, lengthRemaining, ",N,,K,"); } if (length < 0 || length >= lengthRemaining) { LOC_LOGE("NMEA Error in string formatting"); return; } pMarker += length; lengthRemaining -= length; 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_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)); // ------------------- // ------$--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)); // ------------------- // ------$--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)); // ------$--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)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); } EXIT_LOG(%d, 0); } /*=========================================================================== FUNCTION loc_nmea_generate_sv DESCRIPTION Generate NMEA sentences generated based on sv report DEPENDENCIES NONE RETURN VALUE 0 SIDE EFFECTS N/A ===========================================================================*/ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, std::vector &nmeaArraystr) { ENTRY_LOG(); char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; int svCount = svNotify.count; int svNumber = 1; loc_sv_cache_info sv_cache_info = {}; //Count GPS SVs for saparating GPS from GLONASS and throw others for(svNumber=1; svNumber <= svCount; svNumber++) { if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type) { // cache the used in fix mask, as it will be needed to send $GPGSA // during the position report if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { sv_cache_info.gps_used_mask |= (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++; } } else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type) { // cache the used in fix mask, as it will be needed to send $GNGSA // during the position report if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { sv_cache_info.glo_used_mask |= (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++; } } else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type) { // cache the used in fix mask, as it will be needed to send $GAGSA // during the position report if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) { sv_cache_info.gal_used_mask |= (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++; } } else if (GNSS_SV_TYPE_QZSS == 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)) { // 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++; } } else if (GNSS_SV_TYPE_BEIDOU == 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.bds_used_mask |= (1ULL << (svNotify.gnssSvs[svNumber - 1].svId - 1)); } if ((GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) || (GNSS_SIGNAL_BEIDOU_B2AQ == 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)); } // 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); // --------------------- // ------$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); // ----------------------------- // ------$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_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); }