support signal ID in NMEA

support GPS L5, GALILEO E5 and BDS B2 in NMEA.

Change-Id: I7302ec8dfabaf28832fc93206f70cd1942f48183
CRs-Fixed: 2307787

1 files changed, 241 insertions, 53 deletions

diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp
index 558dc65..03a1ffd 100644
--- a/utils/loc_nmea.cpp
+++ b/utils/loc_nmea.cpp
@@ -45,6 +45,39 @@
 #define SYSTEM_ID_BEIDOU       4
 #define SYSTEM_ID_QZSS         5
 
+//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
+
+//Extended signal id
+#define SIGNAL_ID_BDS_B1I      1
+#define SIGNAL_ID_BDS_B1C      2
+#define SIGNAL_ID_BDS_B2I      3
+#define SIGNAL_ID_BDS_B2AI     4
+
+
 typedef struct loc_nmea_sv_meta_s
 {
     char talker[3];
@@ -63,17 +96,96 @@ typedef struct loc_sv_cache_info_s
     uint32_t gal_used_mask;
     uint32_t qzss_used_mask;
     uint32_t bds_used_mask;
-    uint32_t gps_count;
-    uint32_t glo_count;
-    uint32_t gal_count;
-    uint32_t qzss_count;
-    uint32_t bds_count;
+    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;
     float hdop;
     float pdop;
     float vdop;
 } loc_sv_cache_info;
 
 /*===========================================================================
+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_GPS_L1CA;
+            break;
+        case GNSS_SIGNAL_QZSS_L2:
+            signalId = SIGNAL_ID_GPS_L2CL;
+            break;
+        case GNSS_SIGNAL_QZSS_L5:
+            signalId = SIGNAL_ID_GPS_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:
+            signalId = SIGNAL_ID_BDS_B2AI;
+            break;
+        default:
+            signalId = SIGNAL_ID_ALL_SIGNALS;
+    }
+
+    return signalId;
+
+}
+
+/*===========================================================================
 FUNCTION    loc_nmea_sv_meta_init
 
 DESCRIPTION
@@ -92,6 +204,7 @@ SIDE EFFECTS
 static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
                                                loc_sv_cache_info& sv_cache_info,
                                                GnssSvType svType,
+                                               GnssSignalTypeMask signalType,
                                                bool needCombine)
 {
     memset(&sv_meta, 0, sizeof(sv_meta));
@@ -103,50 +216,66 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
             sv_meta.talker[0] = 'G';
             sv_meta.talker[1] = 'P';
             sv_meta.mask = sv_cache_info.gps_used_mask;
-            sv_meta.svCount = sv_cache_info.gps_count;
-            sv_meta.signalId = 1;
             sv_meta.systemId = SYSTEM_ID_GPS;
+            if (GNSS_SIGNAL_GPS_L1CA == signalType) {
+                sv_meta.svCount = sv_cache_info.gps_l1_count;
+            } else if (GNSS_SIGNAL_GPS_L5 == signalType) {
+                sv_meta.svCount = sv_cache_info.gps_l5_count;
+            }
             break;
         case GNSS_SV_TYPE_GLONASS:
             sv_meta.talker[0] = 'G';
             sv_meta.talker[1] = 'L';
             sv_meta.mask = sv_cache_info.glo_used_mask;
-            sv_meta.svCount = sv_cache_info.glo_count;
             // GLONASS SV ids are from 65-96
             sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET;
-            sv_meta.signalId = 1;
             sv_meta.systemId = SYSTEM_ID_GLONASS;
+            if (GNSS_SIGNAL_GLONASS_G1 == signalType) {
+                sv_meta.svCount = sv_cache_info.glo_g1_count;
+            } else if (GNSS_SIGNAL_GLONASS_G2 == signalType) {
+                sv_meta.svCount = sv_cache_info.glo_g2_count;
+            }
             break;
         case GNSS_SV_TYPE_GALILEO:
             sv_meta.talker[0] = 'G';
             sv_meta.talker[1] = 'A';
             sv_meta.mask = sv_cache_info.gal_used_mask;
-            sv_meta.svCount = sv_cache_info.gal_count;
-            sv_meta.signalId = 7;
             sv_meta.systemId = SYSTEM_ID_GALILEO;
+            if (GNSS_SIGNAL_GALILEO_E1 == signalType) {
+                sv_meta.svCount = sv_cache_info.gal_e1_count;
+            } else if (GNSS_SIGNAL_GALILEO_E5A == signalType) {
+                sv_meta.svCount = sv_cache_info.gal_e5_count;
+            }
             break;
         case GNSS_SV_TYPE_QZSS:
             sv_meta.talker[0] = 'P';
             sv_meta.talker[1] = 'Q';
             sv_meta.mask = sv_cache_info.qzss_used_mask;
-            sv_meta.svCount = sv_cache_info.qzss_count;
             // QZSS SV ids are from 193-197. So keep svIdOffset 0
-            sv_meta.signalId = 0;
             sv_meta.systemId = SYSTEM_ID_QZSS;
+            if (GNSS_SIGNAL_QZSS_L1CA == signalType) {
+                sv_meta.svCount = sv_cache_info.qzss_l1_count;
+            } else if (GNSS_SIGNAL_QZSS_L5 == signalType) {
+                sv_meta.svCount = sv_cache_info.qzss_l5_count;
+            }
             break;
         case GNSS_SV_TYPE_BEIDOU:
             sv_meta.talker[0] = 'P';
             sv_meta.talker[1] = 'Q';
             sv_meta.mask = sv_cache_info.bds_used_mask;
-            sv_meta.svCount = sv_cache_info.bds_count;
             // BDS SV ids are from 201-235. So keep svIdOffset 0
-            sv_meta.signalId = 0;
             sv_meta.systemId = SYSTEM_ID_BEIDOU;
+            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;
         default:
             LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
             return NULL;
     }
+    sv_meta.signalId = convert_signalType_to_signalId(signalType);
     if (needCombine &&
                 (sv_cache_info.gps_used_mask ? 1 : 0) +
                 (sv_cache_info.glo_used_mask ? 1 : 0) +
@@ -371,7 +500,6 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
     const char* talker = sv_meta_p->talker;
     uint32_t svIdOffset = sv_meta_p->svIdOffset;
     int svCount = sv_meta_p->svCount;
-
     if (svCount <= 0)
     {
         // no svs in view, so just send a blank $--GSV sentence
@@ -403,7 +531,8 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
 
         for (int i=0; (svNumber <= svNotify.count) && (i < 4);  svNumber++)
         {
-            if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type)
+            if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type && sv_meta_p->signalId ==
+                convert_signalType_to_signalId(svNotify.gnssSvs[svNumber-1].gnssSignalTypeMask))
             {
                 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
                         svNotify.gnssSvs[svNumber - 1].svId + svIdOffset,
@@ -521,8 +650,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         // -------------------
 
         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
-                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, true),
-                        nmeaArraystr);
+                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
+                        GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr);
         if (count > 0)
         {
             svUsedCount += count;
@@ -535,8 +664,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         // -------------------
 
         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
-                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, true),
-                        nmeaArraystr);
+                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
+                        GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr);
         if (count > 0)
         {
             svUsedCount += count;
@@ -549,8 +678,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         // -------------------
 
         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
-                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, true),
-                        nmeaArraystr);
+                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
+                        GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr);
         if (count > 0)
         {
             svUsedCount += count;
@@ -563,8 +692,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         // --------------------------
 
         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
-                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, false),
-                        nmeaArraystr);
+                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
+                        GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr);
         if (count > 0)
         {
             svUsedCount += count;
@@ -575,8 +704,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         // ---$PQGSA/$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, false),
-                        nmeaArraystr);
+                        loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
+                        GNSS_SIGNAL_BEIDOU_B1I, false), nmeaArraystr);
         if (count > 0)
         {
             svUsedCount += count;
@@ -1019,7 +1148,11 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
             {
                 sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
             }
-            sv_cache_info.gps_count++;
+            if (GNSS_SIGNAL_GPS_L1CA == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.gps_l1_count++;
+            } else if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.gps_l5_count++;
+            }
         }
         else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type)
         {
@@ -1031,7 +1164,11 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
             {
                 sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
             }
-            sv_cache_info.glo_count++;
+            if (GNSS_SIGNAL_GLONASS_G1 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.glo_g1_count++;
+            } else if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
+                sv_cache_info.glo_g2_count++;
+            }
         }
         else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type)
         {
@@ -1043,7 +1180,11 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
             {
                 sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
             }
-            sv_cache_info.gal_count++;
+            if (GNSS_SIGNAL_GALILEO_E1 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.gal_e1_count++;
+            } else if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
+                sv_cache_info.gal_e5_count++;
+            }
         }
         else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type)
         {
@@ -1055,7 +1196,11 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
             {
                 sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
             }
-            sv_cache_info.qzss_count++;
+            if (GNSS_SIGNAL_QZSS_L1CA == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.qzss_l1_count++;
+            } else if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.qzss_l5_count++;
+            }
         }
         else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svNumber - 1].type)
         {
@@ -1067,48 +1212,91 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
             {
                 sv_cache_info.bds_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
             }
-            sv_cache_info.bds_count++;
+            if (GNSS_SIGNAL_BEIDOU_B1I == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
+                sv_cache_info.bds_b1_count++;
+            } else if(GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
+                sv_cache_info.bds_b2_count++;
+            }
         }
     }
 
     loc_nmea_sv_meta sv_meta;
-    // ------------------
-    // ------$GPGSV------
-    // ------------------
+    // ---------------------
+    // ------$GPGSV:L1CA----
+    // ---------------------
+
+    loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
+            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_GPS, false), nmeaArraystr);
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
+            GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr);
 
-    // ------------------
-    // ------$GLGSV------
-    // ------------------
+    // ---------------------
+    // ------$GLGSV:G2------
+    // ---------------------
 
     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
-            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, false),
-            nmeaArraystr);
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
+            GNSS_SIGNAL_GLONASS_G2, false), nmeaArraystr);
 
-    // ------------------
-    // ------$GAGSV------
-    // ------------------
+    // ---------------------
+    // ------$GAGSV:E1------
+    // ---------------------
 
     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
-            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, false),
-            nmeaArraystr);
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
+            GNSS_SIGNAL_GALILEO_E1, false), nmeaArraystr);
 
     // -------------------------
-    // ------$PQGSV (QZSS)------
+    // ------$GAGSV:E5A---------
     // -------------------------
+    loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
+            GNSS_SIGNAL_GALILEO_E5A, false), nmeaArraystr);
+
+    // -----------------------------
+    // ------$PQGSV (QZSS):L1CA-----
+    // -----------------------------
+
+    loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
+            GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr);
+
+    // -----------------------------
+    // ------$PQGSV (QZSS):L5-------
+    // -----------------------------
+
+    loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
+            GNSS_SIGNAL_QZSS_L5, false), nmeaArraystr);
+    // -----------------------------
+    // ------$PQGSV (BEIDOU:B1I)----
+    // -----------------------------
 
     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
-            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, false), nmeaArraystr);
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
+            GNSS_SIGNAL_BEIDOU_B1I,false), nmeaArraystr);
 
-    // ---------------------------
-    // ------$PQGSV (BEIDOU)------
-    // ---------------------------
+    // -----------------------------
+    // ------$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, false),
-            nmeaArraystr);
+            loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
+            GNSS_SIGNAL_BEIDOU_B2AI,false), nmeaArraystr);
 
     EXIT_LOG(%d, 0);
 }