diff options
Diffstat (limited to 'gps/utils/loc_nmea.cpp')
| -rw-r--r-- | gps/utils/loc_nmea.cpp | 2347 |
1 files changed, 2347 insertions, 0 deletions
diff --git a/gps/utils/loc_nmea.cpp b/gps/utils/loc_nmea.cpp new file mode 100644 index 0000000..6b5e60e --- /dev/null +++ b/gps/utils/loc_nmea.cpp @@ -0,0 +1,2347 @@ +/* Copyright (c) 2012-2020, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#define LOG_NDEBUG 0 +#define LOG_TAG "LocSvc_nmea" +#include <loc_nmea.h> +#include <math.h> +#include <log_util.h> +#include <loc_pla.h> +#include <loc_cfg.h> + +#define GLONASS_SV_ID_OFFSET 64 +#define SBAS_SV_ID_OFFSET (87) +#define QZSS_SV_ID_OFFSET (192) +#define BDS_SV_ID_OFFSET (200) +#define GALILEO_SV_ID_OFFSET (300) +#define NAVIC_SV_ID_OFFSET (400) +#define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64 +#define MAX_SATELLITES_IN_USE 12 +#define MSEC_IN_ONE_WEEK 604800000ULL +#define UTC_GPS_OFFSET_MSECS 315964800000ULL +#define MAX_TAG_BLOCK_GROUP_CODE (99999) + +// GNSS system id according to NMEA spec +#define SYSTEM_ID_GPS 1 +#define SYSTEM_ID_GLONASS 2 +#define SYSTEM_ID_GALILEO 3 +#define SYSTEM_ID_BDS 4 +#define SYSTEM_ID_QZSS 5 +#define SYSTEM_ID_NAVIC 6 + +//GNSS signal id according to NMEA spec +#define SIGNAL_ID_ALL_SIGNALS 0 +#define SIGNAL_ID_GPS_L1CA 1 +#define SIGNAL_ID_GPS_L1P 2 +#define SIGNAL_ID_GPS_L1M 3 +#define SIGNAL_ID_GPS_L2P 4 +#define SIGNAL_ID_GPS_L2CM 5 +#define SIGNAL_ID_GPS_L2CL 6 +#define SIGNAL_ID_GPS_L5I 7 +#define SIGNAL_ID_GPS_L5Q 8 + + +#define SIGNAL_ID_GLO_G1CA 1 +#define SIGNAL_ID_GLO_G1P 2 +#define SIGNAL_ID_GLO_G2CA 3 +#define SIGNAL_ID_GLO_G2P 4 + + +#define SIGNAL_ID_GAL_E5A 1 +#define SIGNAL_ID_GAL_E5B 2 +#define SIGNAL_ID_GAL_E5AB 3 +#define SIGNAL_ID_GAL_E6A 4 +#define SIGNAL_ID_GAL_E6BC 5 +#define SIGNAL_ID_GAL_L1A 6 +#define SIGNAL_ID_GAL_L1BC 7 + +#define SIGNAL_ID_BDS_B1I 1 +#define SIGNAL_ID_BDS_B1Q 2 +#define SIGNAL_ID_BDS_B1C 3 +#define SIGNAL_ID_BDS_B1A 4 +#define SIGNAL_ID_BDS_B2A 5 +#define SIGNAL_ID_BDS_B2B 6 +#define SIGNAL_ID_BDS_B2AB 7 +#define SIGNAL_ID_BDS_B3I 8 +#define SIGNAL_ID_BDS_B3Q 9 +#define SIGNAL_ID_BDS_B3A 0xA +#define SIGNAL_ID_BDS_B2I 0xB +#define SIGNAL_ID_BDS_B2Q 0xC + +#define SIGNAL_ID_QZSS_L1CA 1 +#define SIGNAL_ID_QZSS_L1CD 2 +#define SIGNAL_ID_QZSS_L1CP 3 +#define SIGNAL_ID_QZSS_LIS 4 +#define SIGNAL_ID_QZSS_L2CM 5 +#define SIGNAL_ID_QZSS_L2CL 6 +#define SIGNAL_ID_QZSS_L5I 7 +#define SIGNAL_ID_QZSS_L5Q 8 +#define SIGNAL_ID_QZSS_L6D 9 +#define SIGNAL_ID_QZSS_L6E 0xA + +#define SIGNAL_ID_NAVIC_L5SPS 1 +#define SIGNAL_ID_NAVIC_SSPS 2 +#define SIGNAL_ID_NAVIC_L5RS 3 +#define SIGNAL_ID_NAVIC_SRS 4 +#define SIGNAL_ID_NAVIC_L1SPS 5 + + +typedef struct loc_nmea_sv_meta_s +{ + char talker[3]; + uint32_t svTypeMask; + uint64_t mask; + uint32_t svCount; + uint32_t totalSvUsedCount; + uint32_t svIdOffset; + uint32_t signalId; + uint32_t systemId; +} loc_nmea_sv_meta; + +typedef struct loc_sv_cache_info_s +{ + uint64_t gps_used_mask; + uint64_t glo_used_mask; + uint64_t gal_used_mask; + uint64_t qzss_used_mask; + uint64_t bds_used_mask; + uint64_t navic_used_mask; + uint32_t gps_l1_count; + uint32_t gps_l2_count; + uint32_t gps_l5_count; + uint32_t glo_g1_count; + uint32_t glo_g2_count; + uint32_t gal_e1_count; + uint32_t gal_e5_count; + uint32_t gal_e5b_count; + uint32_t qzss_l1_count; + uint32_t qzss_l2_count; + uint32_t qzss_l5_count; + uint32_t bds_b1i_count; + uint32_t bds_b1c_count; + uint32_t bds_b2_count; + uint32_t navic_l5_count; + float hdop; + float pdop; + float vdop; +} loc_sv_cache_info; + +/*=========================================================================== +FUNCTION convert_Lla_to_Ecef + +DESCRIPTION + Convert LLA to ECEF + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef) +{ + double r; + + r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat)); + pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon); + pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon); + pecef.Z = (r * OMES + plla.alt) * sin(plla.lat); +} + +/*=========================================================================== +FUNCTION convert_WGS84_to_PZ90 + +DESCRIPTION + Convert datum from WGS84 to PZ90 + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90) +{ + double deltaX = DatumConstFromWGS84[0]; + double deltaY = DatumConstFromWGS84[1]; + double deltaZ = DatumConstFromWGS84[2]; + double deltaScale = DatumConstFromWGS84[3]; + double rotX = DatumConstFromWGS84[4]; + double rotY = DatumConstFromWGS84[5]; + double rotZ = DatumConstFromWGS84[6]; + + pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z); + pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z); + pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y); +} + +/*=========================================================================== +FUNCTION convert_Ecef_to_Lla + +DESCRIPTION + Convert ECEF to LLA + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla) +{ + double p, r; + double EcefA = C_PZ90A; + double EcefB = C_PZ90B; + double Ecef1Mf; + double EcefE2; + double Mu; + double Smu; + double Cmu; + double Phi; + double Sphi; + double N; + + p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y); + r = sqrt(p * p + pecef.Z * pecef.Z); + if (r < 1.0) { + plla.lat = 1.0; + plla.lon = 1.0; + plla.alt = 1.0; + } + Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA; + EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA); + if (p > 1.0) { + Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p); + } else { + if (pecef.Z > 0.0) { + Mu = M_PI / 2.0; + } else { + Mu = -M_PI / 2.0; + } + } + Smu = sin(Mu); + Cmu = cos(Mu); + Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu, + Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu)); + Sphi = sin(Phi); + N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi); + plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N; + plla.lat = Phi; + if ( p > 1.0) { + plla.lon = atan2(pecef.Y, pecef.X); + } else { + plla.lon = 0.0; + } +} + +/*=========================================================================== +FUNCTION convert_signalType_to_signalId + +DESCRIPTION + convert signalType to signal ID + +DEPENDENCIES + NONE + +RETURN VALUE + value of signal ID + +SIDE EFFECTS + N/A + +===========================================================================*/ +static uint32_t convert_signalType_to_signalId(GnssSignalTypeMask signalType) +{ + uint32_t signalId = SIGNAL_ID_ALL_SIGNALS; + + switch (signalType) { + case GNSS_SIGNAL_GPS_L1CA: + case GNSS_SIGNAL_SBAS_L1: + signalId = SIGNAL_ID_GPS_L1CA; + break; + case GNSS_SIGNAL_GPS_L2: + signalId = SIGNAL_ID_GPS_L2CL; + break; + case GNSS_SIGNAL_GPS_L5: + signalId = SIGNAL_ID_GPS_L5Q; + break; + case GNSS_SIGNAL_GLONASS_G1: + signalId = SIGNAL_ID_GLO_G1CA; + break; + case GNSS_SIGNAL_GLONASS_G2: + signalId = SIGNAL_ID_GLO_G2CA; + break; + case GNSS_SIGNAL_GALILEO_E1: + signalId = SIGNAL_ID_GAL_L1BC; + break; + case GNSS_SIGNAL_GALILEO_E5A: + signalId = SIGNAL_ID_GAL_E5A; + break; + case GNSS_SIGNAL_GALILEO_E5B: + signalId = SIGNAL_ID_GAL_E5B; + break; + case GNSS_SIGNAL_QZSS_L1CA: + signalId = SIGNAL_ID_QZSS_L1CA; + break; + case GNSS_SIGNAL_QZSS_L2: + signalId = SIGNAL_ID_QZSS_L2CL; + break; + case GNSS_SIGNAL_QZSS_L5: + signalId = SIGNAL_ID_QZSS_L5Q; + break; + case GNSS_SIGNAL_BEIDOU_B1I: + signalId = SIGNAL_ID_BDS_B1I; + break; + case GNSS_SIGNAL_BEIDOU_B1C: + signalId = SIGNAL_ID_BDS_B1C; + break; + case GNSS_SIGNAL_BEIDOU_B2I: + signalId = SIGNAL_ID_BDS_B2I; + break; + case GNSS_SIGNAL_BEIDOU_B2AI: + case GNSS_SIGNAL_BEIDOU_B2AQ: + signalId = SIGNAL_ID_BDS_B2A; + break; + case GNSS_SIGNAL_NAVIC_L5: + signalId = SIGNAL_ID_NAVIC_L5SPS; + break; + default: + signalId = SIGNAL_ID_ALL_SIGNALS; + } + + return signalId; + +} + +/*=========================================================================== +FUNCTION get_sv_count_from_mask + +DESCRIPTION + get the sv count from bit mask + +DEPENDENCIES + NONE + +RETURN VALUE + value of sv count + +SIDE EFFECTS + N/A + +===========================================================================*/ +static uint32_t get_sv_count_from_mask(uint64_t svMask, int totalSvCount) +{ + int index = 0; + uint32_t svCount = 0; + + if(totalSvCount > MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION) { + LOC_LOGE("total SV count in this constellation %d exceeded limit %d", + totalSvCount, MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION); + } + for(index = 0; index < totalSvCount; index++) { + if(svMask & 0x1) + svCount += 1; + svMask >>= 1; + } + return svCount; +} + +/*=========================================================================== +FUNCTION loc_nmea_sv_meta_init + +DESCRIPTION + Init loc_nmea_sv_meta passed in + +DEPENDENCIES + NONE + +RETURN VALUE + Pointer to loc_nmea_sv_meta + +SIDE EFFECTS + N/A + +===========================================================================*/ +static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta, + loc_sv_cache_info& sv_cache_info, + GnssSvType svType, + GnssSignalTypeMask signalType, + bool needCombine) +{ + memset(&sv_meta, 0, sizeof(sv_meta)); + sv_meta.svTypeMask = (1 << svType); + + switch (svType) + { + case GNSS_SV_TYPE_GPS: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'P'; + sv_meta.mask = sv_cache_info.gps_used_mask; + sv_meta.systemId = SYSTEM_ID_GPS; + sv_meta.svTypeMask |= (1 << GNSS_SV_TYPE_SBAS); + switch (signalType) { + case GNSS_SIGNAL_GPS_L1CA: + sv_meta.svCount = sv_cache_info.gps_l1_count; + break; + case GNSS_SIGNAL_GPS_L5: + sv_meta.svCount = sv_cache_info.gps_l5_count; + break; + case GNSS_SIGNAL_GPS_L2: + sv_meta.svCount = sv_cache_info.gps_l2_count; + break; + } + break; + case GNSS_SV_TYPE_GLONASS: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'L'; + sv_meta.mask = sv_cache_info.glo_used_mask; + // GLONASS SV ids are from 65-96 + sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_GLONASS; + switch (signalType) { + case GNSS_SIGNAL_GLONASS_G1: + sv_meta.svCount = sv_cache_info.glo_g1_count; + break; + case GNSS_SIGNAL_GLONASS_G2: + sv_meta.svCount = sv_cache_info.glo_g2_count; + break; + } + break; + case GNSS_SV_TYPE_GALILEO: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'A'; + sv_meta.mask = sv_cache_info.gal_used_mask; + // GALILEO SV ids are from 301-336, So keep svIdOffset 300 + sv_meta.svIdOffset = GALILEO_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_GALILEO; + switch (signalType) { + case GNSS_SIGNAL_GALILEO_E1: + sv_meta.svCount = sv_cache_info.gal_e1_count; + break; + case GNSS_SIGNAL_GALILEO_E5A: + sv_meta.svCount = sv_cache_info.gal_e5_count; + break; + case GNSS_SIGNAL_GALILEO_E5B: + sv_meta.svCount = sv_cache_info.gal_e5b_count; + break; + } + break; + case GNSS_SV_TYPE_QZSS: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'Q'; + sv_meta.mask = sv_cache_info.qzss_used_mask; + // QZSS SV ids are from 193-199. So keep svIdOffset 192 + sv_meta.svIdOffset = QZSS_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_QZSS; + switch (signalType) { + case GNSS_SIGNAL_QZSS_L1CA: + sv_meta.svCount = sv_cache_info.qzss_l1_count; + break; + case GNSS_SIGNAL_QZSS_L2: + sv_meta.svCount = sv_cache_info.qzss_l2_count; + break; + case GNSS_SIGNAL_QZSS_L5: + sv_meta.svCount = sv_cache_info.qzss_l5_count; + break; + } + break; + case GNSS_SV_TYPE_BEIDOU: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'B'; + sv_meta.mask = sv_cache_info.bds_used_mask; + // BDS SV ids are from 201-237. So keep svIdOffset 200 + sv_meta.svIdOffset = BDS_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_BDS; + switch (signalType) { + case GNSS_SIGNAL_BEIDOU_B1I: + sv_meta.svCount = sv_cache_info.bds_b1i_count; + break; + case GNSS_SIGNAL_BEIDOU_B1C: + sv_meta.svCount = sv_cache_info.bds_b1c_count; + break; + case GNSS_SIGNAL_BEIDOU_B2AI: + sv_meta.svCount = sv_cache_info.bds_b2_count; + break; + } + break; + case GNSS_SV_TYPE_NAVIC: + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'I'; + sv_meta.mask = sv_cache_info.navic_used_mask; + // NAVIC SV ids are from 401-414. So keep svIdOffset 400 + sv_meta.svIdOffset = NAVIC_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_NAVIC; + switch (signalType) { + case GNSS_SIGNAL_NAVIC_L5: + sv_meta.svCount = sv_cache_info.navic_l5_count; + break; + } + break; + default: + LOC_LOGE("NMEA Error unknow constellation type: %d", svType); + return NULL; + } + sv_meta.signalId = convert_signalType_to_signalId(signalType); + sv_meta.totalSvUsedCount = + get_sv_count_from_mask(sv_cache_info.gps_used_mask, + GPS_SV_PRN_MAX - GPS_SV_PRN_MIN + 1) + + get_sv_count_from_mask(sv_cache_info.glo_used_mask, + GLO_SV_PRN_MAX - GLO_SV_PRN_MIN + 1) + + get_sv_count_from_mask(sv_cache_info.gal_used_mask, + GAL_SV_PRN_MAX - GAL_SV_PRN_MIN + 1) + + get_sv_count_from_mask(sv_cache_info.qzss_used_mask, + QZSS_SV_PRN_MAX - QZSS_SV_PRN_MIN + 1) + + get_sv_count_from_mask(sv_cache_info.bds_used_mask, + BDS_SV_PRN_MAX - BDS_SV_PRN_MIN + 1) + + get_sv_count_from_mask(sv_cache_info.navic_used_mask, + NAVIC_SV_PRN_MAX - NAVIC_SV_PRN_MIN + 1); + if (needCombine && + (sv_cache_info.gps_used_mask ? 1 : 0) + + (sv_cache_info.glo_used_mask ? 1 : 0) + + (sv_cache_info.gal_used_mask ? 1 : 0) + + (sv_cache_info.qzss_used_mask ? 1 : 0) + + (sv_cache_info.bds_used_mask ? 1 : 0) + + (sv_cache_info.navic_used_mask ? 1 : 0) > 1) + { + // If GPS, GLONASS, Galileo, QZSS, BDS etc. are combined + // to obtain the reported position solution, + // talker shall be set to GN, to indicate that + // the satellites are used in a combined solution + sv_meta.talker[0] = 'G'; + sv_meta.talker[1] = 'N'; + } + return &sv_meta; +} + +/*=========================================================================== +FUNCTION loc_nmea_put_checksum + +DESCRIPTION + Generate NMEA sentences generated based on position report + +DEPENDENCIES + NONE + +RETURN VALUE + Total length of the nmea sentence + +SIDE EFFECTS + N/A + +===========================================================================*/ +static int loc_nmea_put_checksum(char *pNmea, int maxSize, bool isTagBlock) +{ + uint8_t checksum = 0; + int length = 0; + int checksumLength = 0; + if(NULL == pNmea) + return 0; + + pNmea++; //skip the $ or / for Tag Block + while (*pNmea != '\0') + { + checksum ^= *pNmea++; + length++; + } + + if (isTagBlock) { + // length now contains tag block sentence string length not including / sign. + checksumLength = snprintf(pNmea, (maxSize-length-1), "*%02X\\", checksum); + } else { + // length now contains nmea sentence string length not including $ sign. + checksumLength = snprintf(pNmea, (maxSize-length-1), "*%02X\r\n", checksum); + } + // total length of nmea sentence is length of nmea sentence inc $ sign plus + // length of checksum (+1 is to cover the $ character in the length). + return (length + checksumLength + 1); +} + +/*=========================================================================== +FUNCTION loc_nmea_generate_GSA + +DESCRIPTION + Generate NMEA GSA sentences generated based on position report + Currently below sentences are generated: + - $GPGSA : GPS DOP and active SVs + - $GLGSA : GLONASS DOP and active SVs + - $GAGSA : GALILEO DOP and active SVs + - $GNGSA : GNSS DOP and active SVs + +DEPENDENCIES + NONE + +RETURN VALUE + Number of SVs used + +SIDE EFFECTS + N/A + +===========================================================================*/ +static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended, + char* sentence, + int bufSize, + loc_nmea_sv_meta* sv_meta_p, + std::vector<std::string> &nmeaArraystr, + bool isTagBlockGroupingEnabled) +{ + if (!sentence || bufSize <= 0 || !sv_meta_p) + { + LOC_LOGE("NMEA Error invalid arguments."); + return 0; + } + + char* pMarker = sentence; + int lengthRemaining = bufSize; + int length = 0; + int lengthTagBlock = 0; + + uint32_t svUsedCount = 0; + uint32_t svUsedList[64] = {0}; + uint32_t sentenceCount = 0; + uint32_t sentenceNumber = 1; + size_t svNumber = 1; + static uint32_t code = 1; + + char fixType = '\0'; + + const char* talker = sv_meta_p->talker; + uint32_t svIdOffset = sv_meta_p->svIdOffset; + uint64_t mask = sv_meta_p->mask; + + if (!(sv_meta_p->svTypeMask & (1 << GNSS_SV_TYPE_GLONASS))) { + svIdOffset = 0; + } + + for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++) + { + if (mask & 1) + svUsedList[svUsedCount++] = i + svIdOffset; + mask = mask >> 1; + } + + if (svUsedCount == 0) { + return 0; + } else { + sentenceNumber = 1; + sentenceCount = svUsedCount / 12 + (svUsedCount % 12 != 0); + svNumber = 1; + } + while (sentenceNumber <= sentenceCount) { + pMarker = sentence; + lengthRemaining = bufSize; + if (svUsedCount > 12 && isTagBlockGroupingEnabled) { + lengthTagBlock = snprintf(pMarker, lengthRemaining, "\\g:%d-%d-%d", sentenceNumber, + sentenceCount, code); + if (MAX_TAG_BLOCK_GROUP_CODE == code) { + code = 1; + } + lengthTagBlock = loc_nmea_put_checksum(sentence, bufSize, true); + pMarker += lengthTagBlock; + lengthRemaining -= lengthTagBlock; + } + if (sv_meta_p->totalSvUsedCount == 0) + fixType = '1'; // no fix + else if (sv_meta_p->totalSvUsedCount <= 3) + fixType = '2'; // 2D fix + else + fixType = '3'; // 3D fix + + // Start printing the sentence + // Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v,s*cc + // a : Mode : A : Automatic, allowed to automatically switch 2D/3D + // x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix) + // xx : 12 SV ID + // p.p : Position DOP (Dilution of Precision) + // h.h : Horizontal DOP + // v.v : Vertical DOP + // s : GNSS System Id + // cc : Checksum value + length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return 0; + } + pMarker += length; + lengthRemaining -= length; + + // Add 12 satellite IDs + for (uint8_t i = 0; i < 12; i++, svNumber++) + { + if (svNumber <= svUsedCount) + length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[svNumber - 1]); + else + length = snprintf(pMarker, lengthRemaining, ","); + + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return 0; + } + pMarker += length; + lengthRemaining -= length; + } + + // Add the position/horizontal/vertical DOP values + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) + { + length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,", + locationExtended.pdop, + locationExtended.hdop, + locationExtended.vdop); + } + else + { // no dop + length = snprintf(pMarker, lengthRemaining, ",,,"); + } + pMarker += length; + lengthRemaining -= length; + + // system id + length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId); + pMarker += length; + lengthRemaining -= length; + + /* Sentence is ready, add checksum and broadcast */ + length = loc_nmea_put_checksum(sentence + lengthTagBlock, bufSize - lengthTagBlock, false); + nmeaArraystr.push_back(sentence); + sentenceNumber++; + if (!isTagBlockGroupingEnabled) { + break; + } + } + if (svUsedCount > 12 && isTagBlockGroupingEnabled) { + code++; + } + return svUsedCount; +} + +/*=========================================================================== +FUNCTION loc_nmea_generate_GSV + +DESCRIPTION + Generate NMEA GSV sentences generated based on sv report + Currently below sentences are generated: + - $GPGSV: GPS Satellites in View + - $GLGSV: GLONASS Satellites in View + - $GAGSV: GALILEO Satellites in View + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, + char* sentence, + int bufSize, + loc_nmea_sv_meta* sv_meta_p, + std::vector<std::string> &nmeaArraystr) +{ + if (!sentence || bufSize <= 0) + { + LOC_LOGE("NMEA Error invalid argument."); + return; + } + + char* pMarker = sentence; + int lengthRemaining = bufSize; + int length = 0; + int sentenceCount = 0; + int sentenceNumber = 1; + size_t svNumber = 1; + + const char* talker = sv_meta_p->talker; + uint32_t svIdOffset = sv_meta_p->svIdOffset; + int svCount = sv_meta_p->svCount; + if (svCount <= 0) + { + LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId); + return; + } + + if ((1 << GNSS_SV_TYPE_GLONASS) & sv_meta_p->svTypeMask) { + svIdOffset = 0; + } + svNumber = 1; + sentenceNumber = 1; + sentenceCount = svCount / 4 + (svCount % 4 != 0); + + while (sentenceNumber <= sentenceCount) + { + pMarker = sentence; + lengthRemaining = bufSize; + + length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d", + talker, sentenceCount, sentenceNumber, svCount); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++) + { + GnssSignalTypeMask signalType = svNotify.gnssSvs[svNumber-1].gnssSignalTypeMask; + if (0 == signalType) { + // If no signal type in report, it means default L1,G1,E1,B1I + switch (svNotify.gnssSvs[svNumber - 1].type) + { + case GNSS_SV_TYPE_GPS: + signalType = GNSS_SIGNAL_GPS_L1CA; + break; + case GNSS_SV_TYPE_GLONASS: + signalType = GNSS_SIGNAL_GLONASS_G1; + break; + case GNSS_SV_TYPE_GALILEO: + signalType = GNSS_SIGNAL_GALILEO_E1; + break; + case GNSS_SV_TYPE_QZSS: + signalType = GNSS_SIGNAL_QZSS_L1CA; + break; + case GNSS_SV_TYPE_BEIDOU: + signalType = GNSS_SIGNAL_BEIDOU_B1I; + break; + case GNSS_SV_TYPE_SBAS: + signalType = GNSS_SIGNAL_SBAS_L1; + break; + case GNSS_SV_TYPE_NAVIC: + signalType = GNSS_SIGNAL_NAVIC_L5; + break; + default: + LOC_LOGE("NMEA Error unknow constellation type: %d", + svNotify.gnssSvs[svNumber - 1].type); + continue; + } + } + + if ((sv_meta_p->svTypeMask & (1 << svNotify.gnssSvs[svNumber - 1].type)) && + sv_meta_p->signalId == convert_signalType_to_signalId(signalType)) + { + if (GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svNumber - 1].type) { + svIdOffset = SBAS_SV_ID_OFFSET; + } + if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type && + GLO_SV_PRN_SLOT_UNKNOWN == svNotify.gnssSvs[svNumber - 1].svId) { + length = snprintf(pMarker, lengthRemaining, ",,%02d,%03d,", + (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int + (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int + } else { + length = snprintf(pMarker, lengthRemaining, ",%02d,%02d,%03d,", + svNotify.gnssSvs[svNumber - 1].svId - svIdOffset, + (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int + (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int + } + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0) + { + length = snprintf(pMarker, lengthRemaining,"%02d", + (int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + } + + i++; + } + + } + + // append signalId + length = snprintf(pMarker, lengthRemaining,",%X",sv_meta_p->signalId); + pMarker += length; + lengthRemaining -= length; + + length = loc_nmea_put_checksum(sentence, bufSize, false); + nmeaArraystr.push_back(sentence); + sentenceNumber++; + + } //while +} + +/*=========================================================================== +FUNCTION loc_nmea_generate_DTM + +DESCRIPTION + Generate NMEA DTM sentences generated based on position report + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void loc_nmea_generate_DTM(const LocLla &ref_lla, + const LocLla &local_lla, + char *talker, + char *sentence, + int bufSize) +{ + char* pMarker = sentence; + int lengthRemaining = bufSize; + int length = 0; + int datum_type; + char ref_datum[4] = {0}; + char local_datum[4] = {0}; + double lla_offset[3] = {0}; + char latHem, longHem; + double latMins, longMins; + + + + datum_type = loc_get_datum_type(); + switch (datum_type) { + case LOC_GNSS_DATUM_WGS84: + ref_datum[0] = 'W'; + ref_datum[1] = '8'; + ref_datum[2] = '4'; + local_datum[0] = 'P'; + local_datum[1] = '9'; + local_datum[2] = '0'; + break; + case LOC_GNSS_DATUM_PZ90: + ref_datum[0] = 'P'; + ref_datum[1] = '9'; + ref_datum[2] = '0'; + local_datum[0] = 'W'; + local_datum[1] = '8'; + local_datum[2] = '4'; + break; + default: + break; + } + length = snprintf(pMarker , lengthRemaining , "$%sDTM,%s,," , talker, local_datum); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + lla_offset[0] = local_lla.lat - ref_lla.lat; + lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0); + if (lla_offset[1] < -180.0) { + lla_offset[1] += 360.0; + } else if ( lla_offset[1] > 180.0) { + lla_offset[1] -= 360.0; + } + lla_offset[2] = local_lla.alt - ref_lla.alt; + if (lla_offset[0] > 0.0) { + latHem = 'N'; + } else { + latHem = 'S'; + lla_offset[0] *= -1.0; + } + latMins = fmod(lla_offset[0] * 60.0, 60.0); + if (lla_offset[1] < 0.0) { + longHem = 'W'; + lla_offset[1] *= -1.0; + }else { + longHem = 'E'; + } + longMins = fmod(lla_offset[1] * 60.0, 60.0); + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,", + (uint8_t)floor(lla_offset[0]), latMins, latHem, + (uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = loc_nmea_put_checksum(sentence, bufSize, false); +} + +/*=========================================================================== +FUNCTION get_utctime_with_leapsecond_transition + +DESCRIPTION + This function returns true if the position report is generated during + leap second transition period. If not, then the utc timestamp returned + will be set to the timestamp in the position report. If it is, + then the utc timestamp returned will need to take into account + of the leap second transition so that proper calendar year/month/date + can be calculated from the returned utc timestamp. + +DEPENDENCIES + NONE + +RETURN VALUE + true: position report is generated in leap second transition period. + +SIDE EFFECTS + N/A + +===========================================================================*/ +static bool get_utctime_with_leapsecond_transition( + const UlpLocation &location, + const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, + LocGpsUtcTime &utcPosTimestamp) +{ + bool inTransition = false; + + // position report is not generated during leap second transition, + // we can use the UTC timestamp from position report as is + utcPosTimestamp = location.gpsLocation.timestamp; + + // Check whether we are in leap second transition. + // If so, per NMEA spec, we need to display the extra second in format of 23:59:60 + // with year/month/date not getting advanced. + if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) && + ((systemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) && + (systemInfo.leapSecondSysInfo.leapSecondInfoMask & + LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT))) { + + const LeapSecondChangeInfo &leapSecondChangeInfo = + systemInfo.leapSecondSysInfo.leapSecondChangeInfo; + const GnssSystemTimeStructType &gpsTimestampLsChange = + leapSecondChangeInfo.gpsTimestampLsChange; + + uint64_t gpsTimeLsChange = gpsTimestampLsChange.systemWeek * MSEC_IN_ONE_WEEK + + gpsTimestampLsChange.systemMsec; + uint64_t gpsTimePosReport = locationExtended.gpsTime.gpsWeek * MSEC_IN_ONE_WEEK + + locationExtended.gpsTime.gpsTimeOfWeekMs; + // we are only dealing with positive leap second change, as negative + // leap second change has never occurred and should not occur in future + if (leapSecondChangeInfo.leapSecondsAfterChange > + leapSecondChangeInfo.leapSecondsBeforeChange) { + // leap second adjustment is always 1 second at a time. It can happen + // every quarter end and up to four times per year. + if ((gpsTimePosReport >= gpsTimeLsChange) && + (gpsTimePosReport < (gpsTimeLsChange + 1000))) { + inTransition = true; + utcPosTimestamp = gpsTimeLsChange + UTC_GPS_OFFSET_MSECS - + leapSecondChangeInfo.leapSecondsBeforeChange * 1000; + + // we substract 1000 milli-seconds from UTC timestmap in order to calculate the + // proper year, month and date during leap second transtion. + // Let us give an example, assuming leap second transition is scheduled on 2019, + // Dec 31st mid night. When leap second transition is happening, + // instead of outputting the time as 2020, Jan, 1st, 00 hour, 00 min, and 00 sec. + // The time need to be displayed as 2019, Dec, 31st, 23 hour, 59 min and 60 sec. + utcPosTimestamp -= 1000; + } + } + } + return inTransition; +} + +/*=========================================================================== +FUNCTION loc_nmea_get_fix_quality + +DESCRIPTION + This function obtains the fix quality for GGA sentence, mode indicator + for RMC and VTG sentence based on nav solution mask and tech mask in + the postion report. + +DEPENDENCIES + NONE + +Output parameter + ggaGpsQuality: gps quality field in GGA sentence + rmcModeIndicator: mode indicator field in RMC sentence + vtgModeIndicator: mode indicator field in VTG sentence + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void loc_nmea_get_fix_quality(const UlpLocation & location, + const GpsLocationExtended & locationExtended, + bool custom_gga_fix_quality, + char ggaGpsQuality[3], + char & rmcModeIndicator, + char & vtgModeIndicator, + char gnsModeIndicator[7]) { + + ggaGpsQuality[0] = '0'; // 0 means no fix + rmcModeIndicator = 'N'; // N means no fix + vtgModeIndicator = 'N'; // N means no fix + memset(gnsModeIndicator, 'N', 6); // N means no fix + gnsModeIndicator[6] = '\0'; + do { + if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){ + break; + } + // NOTE: Order of the check is important + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) { + if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) { + ggaGpsQuality[0] = '2'; // 2 means DGPS fix + rmcModeIndicator = 'P'; // P means precise + vtgModeIndicator = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'P'; // P means precise + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'P'; // P means precise + break; + } else if (LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask){ + ggaGpsQuality[0] = '4'; // 4 means RTK Fixed fix + rmcModeIndicator = 'R'; // use R (RTK fixed) + vtgModeIndicator = 'D'; // use D (differential) as + // no RTK fixed defined for VTG in NMEA 183 spec + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'R'; // R means RTK fixed + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'R'; // R means RTK fixed + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'R'; // R means RTK fixed + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'R'; // R means RTK fixed + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'R'; // R means RTK fixed + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'R'; // R means RTK fixed + break; + } else if (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask){ + ggaGpsQuality[0] = '5'; // 5 means RTK float fix + rmcModeIndicator = 'F'; // F means RTK float fix + vtgModeIndicator = 'D'; // use D (differential) as + // no RTK float defined for VTG in NMEA 183 spec + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'F'; // F means RTK float fix + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'F'; // F means RTK float fix + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'F'; // F means RTK float fix + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'F'; // F means RTK float fix + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'F'; // F means RTK float fix + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'F'; // F means RTK float fix + break; + } else if (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask){ + ggaGpsQuality[0] = '2'; // 2 means DGPS fix + rmcModeIndicator = 'D'; // D means differential + vtgModeIndicator = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'D'; // D means differential + break; + } else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask){ + ggaGpsQuality[0] = '2'; // 2 means DGPS fix + rmcModeIndicator = 'D'; // D means differential + vtgModeIndicator = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'D'; // D means differential + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'D'; // D means differential + break; + } + } + // NOTE: Order of the check is important + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) { + if (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask){ + ggaGpsQuality[0] = '1'; // 1 means GPS + rmcModeIndicator = 'A'; // A means autonomous + vtgModeIndicator = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[0] = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[1] = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[2] = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[3] = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[4] = 'A'; // A means autonomous + if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0) + gnsModeIndicator[5] = 'A'; // A means autonomous + break; + } else if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){ + ggaGpsQuality[0] = '6'; // 6 means estimated (dead reckoning) + rmcModeIndicator = 'E'; // E means estimated (dead reckoning) + vtgModeIndicator = 'E'; // E means estimated (dead reckoning) + memset(gnsModeIndicator, 'E', 6); // E means estimated (dead reckoning) + break; + } + } + } while (0); + + do { + // check for customized nmea enabled or not + // with customized GGA quality enabled + // PPP fix w/o sensor: 59, PPP fix w/ sensor: 69 + // DGNSS/SBAS correction fix w/o sensor: 2, w/ sensor: 62 + // RTK fixed fix w/o sensor: 4, w/ sensor: 64 + // RTK float fix w/o sensor: 5, w/ sensor: 65 + // SPE fix w/o sensor: 1, and w/ sensor: 61 + // Sensor dead reckoning fix: 6 + if (true == custom_gga_fix_quality) { + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) { + // PPP fix w/o sensor: fix quality will now be 59 + // PPP fix w sensor: fix quality will now be 69 + if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) { + if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) && + (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask)) { + ggaGpsQuality[0] = '6'; + ggaGpsQuality[1] = '9'; + } else { + ggaGpsQuality[0] = '5'; + ggaGpsQuality[1] = '9'; + } + break; + } + } + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) { + if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){ + char ggaQuality_copy = ggaGpsQuality[0]; + ggaGpsQuality[0] = '6'; // 6 sensor assisted + // RTK fixed fix w/ sensor: fix quality will now be 64 + // RTK float fix w/ sensor: 65 + // DGNSS and/or SBAS correction fix and w/ sensor: 62 + // GPS fix without correction and w/ sensor: 61 + if ((LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask)|| + (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask)|| + (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask)|| + (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)|| + (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask)) { + ggaGpsQuality[1] = ggaQuality_copy; + break; + } + } + } + } + } while (0); + + LOC_LOGv("gps quality: %s, rmc mode indicator: %c, vtg mode indicator: %c", + ggaGpsQuality, rmcModeIndicator, vtgModeIndicator); +} + +/*=========================================================================== +FUNCTION loc_nmea_generate_pos + +DESCRIPTION + Generate NMEA sentences generated based on position report + Currently below sentences are generated within this function: + - $GPGSA : GPS DOP and active SVs + - $GLGSA : GLONASS DOP and active SVs + - $GAGSA : GALILEO DOP and active SVs + - $GNGSA : GNSS DOP and active SVs + - $--VTG : Track made good and ground speed + - $--RMC : Recommended minimum navigation information + - $--GGA : Time, position and fix related data + +DEPENDENCIES + NONE + +RETURN VALUE + 0 + +SIDE EFFECTS + N/A + +===========================================================================*/ +void loc_nmea_generate_pos(const UlpLocation &location, + const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, + unsigned char generate_nmea, + bool custom_gga_fix_quality, + std::vector<std::string> &nmeaArraystr, + int& indexOfGGA, + bool isTagBlockGroupingEnabled) +{ + ENTRY_LOG(); + + indexOfGGA = -1; + LocGpsUtcTime utcPosTimestamp = 0; + bool inLsTransition = false; + + inLsTransition = get_utctime_with_leapsecond_transition + (location, locationExtended, systemInfo, utcPosTimestamp); + + time_t utcTime(utcPosTimestamp/1000); + struct tm result; + tm * pTm = gmtime_r(&utcTime, &result); + if (NULL == pTm) { + LOC_LOGE("gmtime failed"); + return; + } + + char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_GNS[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char* pMarker = sentence; + int lengthRemaining = sizeof(sentence); + int length = 0; + int utcYear = pTm->tm_year % 100; // 2 digit year + int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero + int utcDay = pTm->tm_mday; + int utcHours = pTm->tm_hour; + int utcMinutes = pTm->tm_min; + int utcSeconds = pTm->tm_sec; + int utcMSeconds = (location.gpsLocation.timestamp)%1000; + int datum_type = loc_get_datum_type(); + LocEcef ecef_w84; + LocEcef ecef_p90; + LocLla lla_w84; + LocLla lla_p90; + LocLla ref_lla; + LocLla local_lla; + + if (inLsTransition) { + // During leap second transition, we need to display the extra + // leap second of hour, minute, second as (23:59:60) + utcHours = 23; + utcMinutes = 59; + utcSeconds = 60; + // As UTC timestamp is freezing during leap second transition, + // retrieve milli-seconds portion from GPS timestamp. + utcMSeconds = locationExtended.gpsTime.gpsTimeOfWeekMs % 1000; + } + + loc_sv_cache_info sv_cache_info = {}; + + if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) { + sv_cache_info.gps_used_mask = + locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask; + sv_cache_info.glo_used_mask = + locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask; + sv_cache_info.gal_used_mask = + locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask; + sv_cache_info.bds_used_mask = + locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask; + sv_cache_info.qzss_used_mask = + locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask; + sv_cache_info.navic_used_mask = + locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask; + } + + if (generate_nmea) { + char talker[3] = {'G', 'P', '\0'}; + uint32_t svUsedCount = 0; + uint32_t count = 0; + loc_nmea_sv_meta sv_meta; + // ------------------- + // ---$GPGSA/$GNGSA--- + // ------------------- + + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled); + if (count > 0) + { + svUsedCount += count; + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; + } + + // ------------------- + // ---$GLGSA/$GNGSA--- + // ------------------- + + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, + GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr, isTagBlockGroupingEnabled); + if (count > 0) + { + svUsedCount += count; + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; + } + + // ------------------- + // ---$GAGSA/$GNGSA--- + // ------------------- + + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, + GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr, isTagBlockGroupingEnabled); + if (count > 0) + { + svUsedCount += count; + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; + } + + // ---------------------------- + // ---$GBGSA/$GNGSA (BEIDOU)--- + // ---------------------------- + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr, isTagBlockGroupingEnabled); + if (count > 0) + { + svUsedCount += count; + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; + } + + // -------------------------- + // ---$GQGSA/$GNGSA (QZSS)--- + // -------------------------- + + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, + GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled); + if (count > 0) + { + svUsedCount += count; + talker[0] = sv_meta.talker[0]; + talker[1] = sv_meta.talker[1]; + } + + // if svUsedCount is 0, it means we do not generate any GSA sentence yet. + // in this case, generate an empty GSA sentence + if (svUsedCount == 0) { + strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + } + + char ggaGpsQuality[3] = {'0', '\0', '\0'}; + char rmcModeIndicator = 'N'; + char vtgModeIndicator = 'N'; + char gnsModeIndicator[7] = {'N', 'N', 'N', 'N', 'N', 'N', '\0'}; + loc_nmea_get_fix_quality(location, locationExtended, custom_gga_fix_quality, + ggaGpsQuality, rmcModeIndicator, vtgModeIndicator, gnsModeIndicator); + + // ------------------- + // ------$--VTG------- + // ------------------- + + pMarker = sentence; + lengthRemaining = sizeof(sentence); + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING) + { + float magTrack = location.gpsLocation.bearing; + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) + { + magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; + if (magTrack < 0.0) + magTrack += 360.0; + else if (magTrack > 360.0) + magTrack -= 360.0; + } + + length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack); + } + else + { + length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) + { + float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); + float speedKmPerHour = location.gpsLocation.speed * 3.6; + + length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); + } + else + { + length = snprintf(pMarker, lengthRemaining, ",N,,K,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator); + + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + memset(&ecef_w84, 0, sizeof(ecef_w84)); + memset(&ecef_p90, 0, sizeof(ecef_p90)); + memset(&lla_w84, 0, sizeof(lla_w84)); + memset(&lla_p90, 0, sizeof(lla_p90)); + memset(&ref_lla, 0, sizeof(ref_lla)); + memset(&local_lla, 0, sizeof(local_lla)); + lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI; + lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI; + lla_w84.alt = location.gpsLocation.altitude; + + convert_Lla_to_Ecef(lla_w84, ecef_w84); + convert_WGS84_to_PZ90(ecef_w84, ecef_p90); + convert_Ecef_to_Lla(ecef_p90, lla_p90); + + switch (datum_type) { + case LOC_GNSS_DATUM_WGS84: + ref_lla.lat = location.gpsLocation.latitude; + ref_lla.lon = location.gpsLocation.longitude; + ref_lla.alt = location.gpsLocation.altitude; + local_lla.lat = lla_p90.lat / M_PI * 180.0; + local_lla.lon = lla_p90.lon / M_PI * 180.0; + local_lla.alt = lla_p90.alt; + break; + case LOC_GNSS_DATUM_PZ90: + ref_lla.lat = lla_p90.lat / M_PI * 180.0; + ref_lla.lon = lla_p90.lon / M_PI * 180.0; + ref_lla.alt = lla_p90.alt; + local_lla.lat = location.gpsLocation.latitude; + local_lla.lon = location.gpsLocation.longitude; + local_lla.alt = location.gpsLocation.altitude; + break; + default: + break; + } + + // ------------------- + // ------$--DTM------- + // ------------------- + loc_nmea_generate_DTM(ref_lla, local_lla, talker, sentence_DTM, sizeof(sentence_DTM)); + + // ------------------- + // ------$--RMC------- + // ------------------- + + pMarker = sentence_RMC; + lengthRemaining = sizeof(sentence_RMC); + + bool validFix = ((0 != sv_cache_info.gps_used_mask) || + (0 != sv_cache_info.glo_used_mask) || + (0 != sv_cache_info.gal_used_mask) || + (0 != sv_cache_info.qzss_used_mask) || + (0 != sv_cache_info.bds_used_mask)); + + if (validFix) { + length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A,", + talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); + } else { + length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,V,", + talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) + { + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; + char latHemisphere; + char lonHemisphere; + double latMinutes; + double lonMinutes; + + if (latitude > 0) + { + latHemisphere = 'N'; + } + else + { + latHemisphere = 'S'; + latitude *= -1.0; + } + + if (longitude < 0) + { + lonHemisphere = 'W'; + longitude *= -1.0; + } + else + { + lonHemisphere = 'E'; + } + + latMinutes = fmod(latitude * 60.0 , 60.0); + lonMinutes = fmod(longitude * 60.0 , 60.0); + + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", + (uint8_t)floor(latitude), latMinutes, latHemisphere, + (uint8_t)floor(longitude),lonMinutes, lonHemisphere); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) + { + float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); + length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", + utcDay, utcMonth, utcYear); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) + { + float magneticVariation = locationExtended.magneticDeviation; + char direction; + if (magneticVariation < 0.0) + { + direction = 'W'; + magneticVariation *= -1.0; + } + else + { + direction = 'E'; + } + + length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", + magneticVariation, direction); + } + else + { + length = snprintf(pMarker, lengthRemaining, ",,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator); + pMarker += length; + lengthRemaining -= length; + + // hardcode Navigation Status field to 'V' + length = snprintf(pMarker, lengthRemaining, ",%c", 'V'); + + length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC), false); + + // ------------------- + // ------$--GNS------- + // ------------------- + + pMarker = sentence_GNS; + lengthRemaining = sizeof(sentence_GNS); + + length = snprintf(pMarker, lengthRemaining, "$%sGNS,%02d%02d%02d.%02d," , + talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) + { + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; + char latHemisphere; + char lonHemisphere; + double latMinutes; + double lonMinutes; + + if (latitude > 0) + { + latHemisphere = 'N'; + } + else + { + latHemisphere = 'S'; + latitude *= -1.0; + } + + if (longitude < 0) + { + lonHemisphere = 'W'; + longitude *= -1.0; + } + else + { + lonHemisphere = 'E'; + } + + latMinutes = fmod(latitude * 60.0 , 60.0); + lonMinutes = fmod(longitude * 60.0 , 60.0); + + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", + (uint8_t)floor(latitude), latMinutes, latHemisphere, + (uint8_t)floor(longitude),lonMinutes, lonHemisphere); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = snprintf(pMarker, lengthRemaining, "%s,", gnsModeIndicator); + + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) { + length = snprintf(pMarker, lengthRemaining, "%02d,%.1f,", + svUsedCount, locationExtended.hdop); + } + else { // no hdop + length = snprintf(pMarker, lengthRemaining, "%02d,,", + svUsedCount); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,", + locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining,","); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) && + (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,", + ref_lla.alt - locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE) + { + length = snprintf(pMarker, lengthRemaining, "%.1f,", + (float)locationExtended.dgnssDataAgeMsec / 1000); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_REF_STATION_ID) + { + length = snprintf(pMarker, lengthRemaining, "%04d", + locationExtended.dgnssRefStationId); + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + } + + // hardcode Navigation Status field to 'V' + length = snprintf(pMarker, lengthRemaining, ",%c", 'V'); + pMarker += length; + lengthRemaining -= length; + + length = loc_nmea_put_checksum(sentence_GNS, sizeof(sentence_GNS), false); + + // ------------------- + // ------$--GGA------- + // ------------------- + + pMarker = sentence_GGA; + lengthRemaining = sizeof(sentence_GGA); + + length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," , + talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) + { + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; + char latHemisphere; + char lonHemisphere; + double latMinutes; + double lonMinutes; + + if (latitude > 0) + { + latHemisphere = 'N'; + } + else + { + latHemisphere = 'S'; + latitude *= -1.0; + } + + if (longitude < 0) + { + lonHemisphere = 'W'; + longitude *= -1.0; + } + else + { + lonHemisphere = 'E'; + } + + latMinutes = fmod(latitude * 60.0 , 60.0); + lonMinutes = fmod(longitude * 60.0 , 60.0); + + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", + (uint8_t)floor(latitude), latMinutes, latHemisphere, + (uint8_t)floor(longitude),lonMinutes, lonHemisphere); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + // Number of satellites in use, 00-12 + if (svUsedCount > MAX_SATELLITES_IN_USE) + svUsedCount = MAX_SATELLITES_IN_USE; + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) + { + length = snprintf(pMarker, lengthRemaining, "%s,%02d,%.1f,", + ggaGpsQuality, svUsedCount, locationExtended.hdop); + } + else + { // no hdop + length = snprintf(pMarker, lengthRemaining, "%s,%02d,,", + ggaGpsQuality, svUsedCount); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", + locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) && + (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", + ref_lla.alt - locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining, ",,"); + } + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE) + { + length = snprintf(pMarker, lengthRemaining, "%.1f,", + (float)locationExtended.dgnssDataAgeMsec / 1000); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_REF_STATION_ID) + { + length = snprintf(pMarker, lengthRemaining, "%04d", + locationExtended.dgnssRefStationId); + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + } + + length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA), false); + + // ------$--DTM------- + nmeaArraystr.push_back(sentence_DTM); + // ------$--RMC------- + nmeaArraystr.push_back(sentence_RMC); + if(LOC_GNSS_DATUM_PZ90 == datum_type) { + // ------$--DTM------- + nmeaArraystr.push_back(sentence_DTM); + } + // ------$--GNS------- + nmeaArraystr.push_back(sentence_GNS); + if(LOC_GNSS_DATUM_PZ90 == datum_type) { + // ------$--DTM------- + nmeaArraystr.push_back(sentence_DTM); + } + // ------$--GGA------- + nmeaArraystr.push_back(sentence_GGA); + indexOfGGA = static_cast<int>(nmeaArraystr.size() - 1); + } + //Send blank NMEA reports for non-final fixes + else { + strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + + strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); + length = loc_nmea_put_checksum(sentence, sizeof(sentence), false); + nmeaArraystr.push_back(sentence); + } + + EXIT_LOG(%d, 0); +} + + + +/*=========================================================================== +FUNCTION loc_nmea_generate_sv + +DESCRIPTION + Generate NMEA sentences generated based on sv report + +DEPENDENCIES + NONE + +RETURN VALUE + 0 + +SIDE EFFECTS + N/A + +===========================================================================*/ +void loc_nmea_generate_sv(const GnssSvNotification &svNotify, + std::vector<std::string> &nmeaArraystr) +{ + ENTRY_LOG(); + + char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; + loc_sv_cache_info sv_cache_info = {}; + + //Count GPS SVs for saparating GPS from GLONASS and throw others + for (uint32_t svOffset = 0; svOffset < svNotify.count; svOffset++) { + if ((GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svOffset].type) || + (GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svOffset].type)) + { + if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.gps_l5_count++; + } else if (GNSS_SIGNAL_GPS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.gps_l2_count++; + } else { + // GNSS_SIGNAL_GPS_L1CA, GNSS_SIGNAL_SBAS_L1 or default + // If no signal type in report, it means default L1 + sv_cache_info.gps_l1_count++; + } + } + else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svOffset].type) + { + if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){ + sv_cache_info.glo_g2_count++; + } else { + // GNSS_SIGNAL_GLONASS_G1 or default + // If no signal type in report, it means default G1 + sv_cache_info.glo_g1_count++; + } + } + else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svOffset].type) + { + if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){ + sv_cache_info.gal_e5_count++; + } else if (GNSS_SIGNAL_GALILEO_E5B == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.gal_e5b_count++; + } else { + // GNSS_SIGNAL_GALILEO_E1 or default + // If no signal type in report, it means default E1 + sv_cache_info.gal_e1_count++; + } + } + else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svOffset].type) + { + if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.qzss_l5_count++; + } else if (GNSS_SIGNAL_QZSS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.qzss_l2_count++; + } else { + // GNSS_SIGNAL_QZSS_L1CA or default + // If no signal type in report, it means default L1 + sv_cache_info.qzss_l1_count++; + } + } + else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svOffset].type) + { + // cache the used in fix mask, as it will be needed to send $PQGSA + // during the position report + if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == + (svNotify.gnssSvs[svOffset].gnssSvOptionsMask & + GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) + { + setSvMask(sv_cache_info.bds_used_mask, svNotify.gnssSvs[svOffset].svId); + } + if ((GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) || + (GNSS_SIGNAL_BEIDOU_B2AQ == svNotify.gnssSvs[svOffset].gnssSignalTypeMask)) { + sv_cache_info.bds_b2_count++; + } else if (GNSS_SIGNAL_BEIDOU_B1C == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) { + sv_cache_info.bds_b1c_count++; + } else { + // GNSS_SIGNAL_BEIDOU_B1I or default + // If no signal type in report, it means default B1I + sv_cache_info.bds_b1i_count++; + } + } + else if (GNSS_SV_TYPE_NAVIC == svNotify.gnssSvs[svOffset].type) + { + // GNSS_SIGNAL_NAVIC_L5 is the only signal type for NAVIC + sv_cache_info.navic_l5_count++; + } + } + + loc_nmea_sv_meta sv_meta; + // --------------------- + // ------$GPGSV:L1CA---- + // --------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L1CA, false), nmeaArraystr); + + // --------------------- + // ------$GPGSV:L5------ + // --------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L5, false), nmeaArraystr); + + // --------------------- + // ------$GPGSV:L2------ + // --------------------- + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, + GNSS_SIGNAL_GPS_L2, false), nmeaArraystr); + + // --------------------- + // ------$GLGSV:G1------ + // --------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, + GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr); + + // --------------------- + // ------$GLGSV:G2------ + // --------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, + GNSS_SIGNAL_GLONASS_G2, false), nmeaArraystr); + + // --------------------- + // ------$GAGSV:E1------ + // --------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, + GNSS_SIGNAL_GALILEO_E1, false), nmeaArraystr); + + // ------------------------- + // ------$GAGSV:E5A--------- + // ------------------------- + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, + GNSS_SIGNAL_GALILEO_E5A, false), nmeaArraystr); + + // ------------------------- + // ------$GAGSV:E5B--------- + // ------------------------- + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, + GNSS_SIGNAL_GALILEO_E5B, false), nmeaArraystr); + + // ----------------------------- + // ------$GQGSV (QZSS):L1CA----- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, + GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr); + + // ----------------------------- + // ------$GQGSV (QZSS):L5------- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, + GNSS_SIGNAL_QZSS_L5, false), nmeaArraystr); + + // ----------------------------- + // ------$GQGSV (QZSS):L2------- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, + GNSS_SIGNAL_QZSS_L2, false), nmeaArraystr); + + + // ----------------------------- + // ------$GBGSV (BEIDOU:B1I)---- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B1I, false), nmeaArraystr); + + // ----------------------------- + // ------$GBGSV (BEIDOU:B1C)---- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B1C, false), nmeaArraystr); + + // ----------------------------- + // ------$GBGSV (BEIDOU:B2AI)--- + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, + GNSS_SIGNAL_BEIDOU_B2AI, false), nmeaArraystr); + + // ----------------------------- + // ------$GIGSV (NAVIC:L5)------ + // ----------------------------- + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC, + GNSS_SIGNAL_NAVIC_L5,false), nmeaArraystr); + + EXIT_LOG(%d, 0); +} |