From 0029fafd92d25e884f11e316e9b1b29a6a537cf9 Mon Sep 17 00:00:00 2001 From: Harikrishnan Hariharan Date: Mon, 24 Apr 2017 18:13:05 +0530 Subject: Add support for generating NMEA sentences for QZSS. Change-Id: Ib1350fceeabcb2dc67812641b1f967b1b240cefd CRs-Fixed: 2036931 --- utils/loc_nmea.cpp | 95 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 90 insertions(+), 5 deletions(-) (limited to 'utils/loc_nmea.cpp') diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp index e288af4..5a15863 100644 --- a/utils/loc_nmea.cpp +++ b/utils/loc_nmea.cpp @@ -35,6 +35,15 @@ #define GLONASS_SV_ID_OFFSET 64 #define MAX_SATELLITES_IN_USE 12 + +// GNSS system id according to NMEA spec +#define SYSTEM_ID_GPS 1 +#define SYSTEM_ID_GLONASS 2 +#define SYSTEM_ID_GALILEO 3 +// Extended systems +#define SYSTEM_ID_BEIDOU 4 +#define SYSTEM_ID_QZSS 5 + typedef struct loc_nmea_sv_meta_s { char talker[3]; @@ -42,6 +51,7 @@ typedef struct loc_nmea_sv_meta_s uint32_t mask; uint32_t svCount; uint32_t svIdOffset; + uint32_t systemId; } loc_nmea_sv_meta; typedef struct loc_sv_cache_info_s @@ -49,9 +59,11 @@ typedef struct loc_sv_cache_info_s uint32_t gps_used_mask; uint32_t glo_used_mask; uint32_t gal_used_mask; + uint32_t qzss_used_mask; uint32_t gps_count; uint32_t glo_count; uint32_t gal_count; + uint32_t qzss_count; float hdop; float pdop; float vdop; @@ -81,26 +93,39 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta, { memset(&sv_meta, 0, sizeof(sv_meta)); sv_meta.svType = svType; - sv_meta.talker[0] = 'G'; 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.svCount = sv_cache_info.gps_count; + sv_meta.systemId = SYSTEM_ID_GPS; break; case GNSS_SV_TYPE_GLONASS: + sv_meta.talker[0] = 'G'; sv_meta.talker[1] = 'L'; sv_meta.mask = sv_cache_info.glo_used_mask; sv_meta.svCount = sv_cache_info.glo_count; // GLONASS SV ids are from 65-96 sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET; + sv_meta.systemId = SYSTEM_ID_GLONASS; break; case GNSS_SV_TYPE_GALILEO: + sv_meta.talker[0] = 'G'; sv_meta.talker[1] = 'A'; sv_meta.mask = sv_cache_info.gal_used_mask; sv_meta.svCount = sv_cache_info.gal_count; + sv_meta.systemId = SYSTEM_ID_GALILEO; + break; + case GNSS_SV_TYPE_QZSS: + sv_meta.talker[0] = 'P'; + sv_meta.talker[1] = 'Q'; + sv_meta.mask = sv_cache_info.qzss_used_mask; + sv_meta.svCount = sv_cache_info.qzss_count; + // QZSS SV ids are from 193-197. So keep svIdOffset 0 + sv_meta.systemId = SYSTEM_ID_QZSS; break; default: LOC_LOGE("NMEA Error unknow constellation type: %d", svType); @@ -109,12 +134,14 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta, 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) > 1) + (sv_cache_info.gal_used_mask ? 1 : 0) + + (sv_cache_info.qzss_used_mask ? 1 : 0) > 1) { - // If GPS, GLONASS, Galileo etc. are combined + // If GPS, GLONASS, Galileo, QZSS 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; @@ -260,15 +287,22 @@ static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtende // Add the position/horizontal/vertical DOP values if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) { - length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", + length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,", locationExtended.pdop, locationExtended.hdop, locationExtended.vdop); } else { // no dop - length = snprintf(pMarker, lengthRemaining, ",,"); + 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); @@ -385,6 +419,15 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, } + // The following entries are specific to QZSS and BDS + if (sv_meta_p->svType == GNSS_SV_TYPE_QZSS) + { + // last one is System id and second last is Signal Id which is always zero + length = snprintf(pMarker, lengthRemaining,",%d,%d",0,sv_meta_p->systemId); + pMarker += length; + lengthRemaining -= length; + } + length = loc_nmea_put_checksum(sentence, bufSize); nmeaArraystr.push_back(sentence); sentenceNumber++; @@ -455,6 +498,7 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (count > 0) { svUsedCount += count; + talker[0] = sv_meta.talker[0]; talker[1] = sv_meta.talker[1]; } @@ -467,6 +511,7 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (count > 0) { svUsedCount += count; + talker[0] = sv_meta.talker[0]; talker[1] = sv_meta.talker[1]; } @@ -479,9 +524,22 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (count > 0) { svUsedCount += count; + talker[0] = sv_meta.talker[0]; talker[1] = sv_meta.talker[1]; } + // ------------------- + // ---$PQGSA/$GNGSA--- + // ------------------- + + count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_QZSS, false), nmeaArraystr); + if (count > 0) + { + svUsedCount += count; + // talker should be default "GP". If GPS, GLO etc is used, it should be "GN" + } + // ------------------- // ------$--VTG------- // ------------------- @@ -844,6 +902,7 @@ void loc_nmea_generate_pos(const UlpLocation &location, 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; } //Send blank NMEA reports for non-final fixes else { @@ -855,6 +914,10 @@ void loc_nmea_generate_pos(const UlpLocation &location, length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); + strlcpy(sentence, "$PQGSA,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); @@ -908,10 +971,12 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, 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.gps_count = 0; sv_cache_info.glo_count = 0; sv_cache_info.gal_count = 0; + sv_cache_info.qzss_count = 0; for(svNumber=1; svNumber <= svCount; svNumber++) { if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type) { @@ -949,6 +1014,19 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, } sv_cache_info.gal_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)) + { + sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); + } + sv_cache_info.qzss_count++; + } + } loc_nmea_sv_meta sv_meta; @@ -973,5 +1051,12 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GALILEO, false), nmeaArraystr); + // ------------------ + // ------$PQGSV------ + // ------------------ + + loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), + loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_QZSS, false), nmeaArraystr); + EXIT_LOG(%d, 0); } -- cgit v1.2.3