1 /* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
2  *
3  * Redistribution and use in source and binary forms, with or without
4  * modification, are permitted provided that the following conditions are
5  * met:
6  *     * Redistributions of source code must retain the above copyright
7  *       notice, this list of conditions and the following disclaimer.
8  *     * Redistributions in binary form must reproduce the above
9  *       copyright notice, this list of conditions and the following
10  *       disclaimer in the documentation and/or other materials provided
11  *       with the distribution.
12  *     * Neither the name of The Linux Foundation nor the names of its
13  *       contributors may be used to endorse or promote products derived
14  *       from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
17  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
20  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23  * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
26  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27  *
28  */
29 
30 #define LOG_NDDEBUG 0
31 #define LOG_TAG "LocSvc_nmea"
32 #include <loc_nmea.h>
33 #include <math.h>
34 #include <platform_lib_includes.h>
35 
36 #define GLONASS_SV_ID_OFFSET 64
37 #define MAX_SATELLITES_IN_USE 12
38 
39 // GNSS system id according to NMEA spec
40 #define SYSTEM_ID_GPS          1
41 #define SYSTEM_ID_GLONASS      2
42 #define SYSTEM_ID_GALILEO      3
43 // Extended systems
44 #define SYSTEM_ID_BEIDOU       4
45 #define SYSTEM_ID_QZSS         5
46 
47 typedef struct loc_nmea_sv_meta_s
48 {
49     char talker[3];
50     LocGnssConstellationType svType;
51     uint32_t mask;
52     uint32_t svCount;
53     uint32_t svIdOffset;
54     uint32_t systemId;
55 } loc_nmea_sv_meta;
56 
57 typedef struct loc_sv_cache_info_s
58 {
59     uint32_t gps_used_mask;
60     uint32_t glo_used_mask;
61     uint32_t gal_used_mask;
62     uint32_t qzss_used_mask;
63     uint32_t bds_used_mask;
64     uint32_t gps_count;
65     uint32_t glo_count;
66     uint32_t gal_count;
67     uint32_t qzss_count;
68     uint32_t bds_count;
69     float hdop;
70     float pdop;
71     float vdop;
72 } loc_sv_cache_info;
73 
74 /*===========================================================================
75 FUNCTION    loc_nmea_sv_meta_init
76 
77 DESCRIPTION
78    Init loc_nmea_sv_meta passed in
79 
80 DEPENDENCIES
81    NONE
82 
83 RETURN VALUE
84    Pointer to loc_nmea_sv_meta
85 
86 SIDE EFFECTS
87    N/A
88 
89 ===========================================================================*/
loc_nmea_sv_meta_init(loc_nmea_sv_meta & sv_meta,loc_sv_cache_info & sv_cache_info,GnssSvType svType,bool needCombine)90 static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
91                                                loc_sv_cache_info& sv_cache_info,
92                                                GnssSvType svType,
93                                                bool needCombine)
94 {
95     memset(&sv_meta, 0, sizeof(sv_meta));
96     sv_meta.svType = svType;
97 
98     switch (svType)
99     {
100         case GNSS_SV_TYPE_GPS:
101             sv_meta.talker[0] = 'G';
102             sv_meta.talker[1] = 'P';
103             sv_meta.mask = sv_cache_info.gps_used_mask;
104             sv_meta.svCount = sv_cache_info.gps_count;
105             sv_meta.systemId = SYSTEM_ID_GPS;
106             break;
107         case GNSS_SV_TYPE_GLONASS:
108             sv_meta.talker[0] = 'G';
109             sv_meta.talker[1] = 'L';
110             sv_meta.mask = sv_cache_info.glo_used_mask;
111             sv_meta.svCount = sv_cache_info.glo_count;
112             // GLONASS SV ids are from 65-96
113             sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET;
114             sv_meta.systemId = SYSTEM_ID_GLONASS;
115             break;
116         case GNSS_SV_TYPE_GALILEO:
117             sv_meta.talker[0] = 'G';
118             sv_meta.talker[1] = 'A';
119             sv_meta.mask = sv_cache_info.gal_used_mask;
120             sv_meta.svCount = sv_cache_info.gal_count;
121             sv_meta.systemId = SYSTEM_ID_GALILEO;
122             break;
123         case GNSS_SV_TYPE_QZSS:
124             sv_meta.talker[0] = 'P';
125             sv_meta.talker[1] = 'Q';
126             sv_meta.mask = sv_cache_info.qzss_used_mask;
127             sv_meta.svCount = sv_cache_info.qzss_count;
128             // QZSS SV ids are from 193-197. So keep svIdOffset 0
129             sv_meta.systemId = SYSTEM_ID_QZSS;
130             break;
131         case GNSS_SV_TYPE_BEIDOU:
132             sv_meta.talker[0] = 'P';
133             sv_meta.talker[1] = 'Q';
134             sv_meta.mask = sv_cache_info.bds_used_mask;
135             sv_meta.svCount = sv_cache_info.bds_count;
136             // BDS SV ids are from 201-235. So keep svIdOffset 0
137             sv_meta.systemId = SYSTEM_ID_BEIDOU;
138             break;
139         default:
140             LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
141             return NULL;
142     }
143     if (needCombine &&
144                 (sv_cache_info.gps_used_mask ? 1 : 0) +
145                 (sv_cache_info.glo_used_mask ? 1 : 0) +
146                 (sv_cache_info.gal_used_mask ? 1 : 0) +
147                 (sv_cache_info.qzss_used_mask ? 1 : 0) +
148                 (sv_cache_info.bds_used_mask ? 1 : 0) > 1)
149     {
150         // If GPS, GLONASS, Galileo, QZSS, BDS etc. are combined
151         // to obtain the reported position solution,
152         // talker shall be set to GN, to indicate that
153         // the satellites are used in a combined solution
154         sv_meta.talker[0] = 'G';
155         sv_meta.talker[1] = 'N';
156     }
157     return &sv_meta;
158 }
159 
160 /*===========================================================================
161 FUNCTION    loc_nmea_put_checksum
162 
163 DESCRIPTION
164    Generate NMEA sentences generated based on position report
165 
166 DEPENDENCIES
167    NONE
168 
169 RETURN VALUE
170    Total length of the nmea sentence
171 
172 SIDE EFFECTS
173    N/A
174 
175 ===========================================================================*/
loc_nmea_put_checksum(char * pNmea,int maxSize)176 static int loc_nmea_put_checksum(char *pNmea, int maxSize)
177 {
178     uint8_t checksum = 0;
179     int length = 0;
180     if(NULL == pNmea)
181         return 0;
182 
183     pNmea++; //skip the $
184     while (*pNmea != '\0')
185     {
186         checksum ^= *pNmea++;
187         length++;
188     }
189 
190     // length now contains nmea sentence string length not including $ sign.
191     int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum);
192 
193     // total length of nmea sentence is length of nmea sentence inc $ sign plus
194     // length of checksum (+1 is to cover the $ character in the length).
195     return (length + checksumLength + 1);
196 }
197 
198 /*===========================================================================
199 FUNCTION    loc_nmea_generate_GSA
200 
201 DESCRIPTION
202    Generate NMEA GSA sentences generated based on position report
203    Currently below sentences are generated:
204    - $GPGSA : GPS DOP and active SVs
205    - $GLGSA : GLONASS DOP and active SVs
206    - $GAGSA : GALILEO DOP and active SVs
207    - $GNGSA : GNSS DOP and active SVs
208 
209 DEPENDENCIES
210    NONE
211 
212 RETURN VALUE
213    Number of SVs used
214 
215 SIDE EFFECTS
216    N/A
217 
218 ===========================================================================*/
loc_nmea_generate_GSA(const GpsLocationExtended & locationExtended,char * sentence,int bufSize,loc_nmea_sv_meta * sv_meta_p,std::vector<std::string> & nmeaArraystr)219 static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended,
220                               char* sentence,
221                               int bufSize,
222                               loc_nmea_sv_meta* sv_meta_p,
223                               std::vector<std::string> &nmeaArraystr)
224 {
225     if (!sentence || bufSize <= 0 || !sv_meta_p)
226     {
227         LOC_LOGE("NMEA Error invalid arguments.");
228         return 0;
229     }
230 
231     char* pMarker = sentence;
232     int lengthRemaining = bufSize;
233     int length = 0;
234 
235     uint32_t svUsedCount = 0;
236     uint32_t svUsedList[32] = {0};
237 
238     char fixType = '\0';
239 
240     const char* talker = sv_meta_p->talker;
241     uint32_t svIdOffset = sv_meta_p->svIdOffset;
242     uint32_t mask = sv_meta_p->mask;
243 
244     for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++)
245     {
246         if (mask & 1)
247             svUsedList[svUsedCount++] = i + svIdOffset;
248         mask = mask >> 1;
249     }
250 
251     if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType)
252         return 0;
253 
254     if (svUsedCount == 0)
255         fixType = '1'; // no fix
256     else if (svUsedCount <= 3)
257         fixType = '2'; // 2D fix
258     else
259         fixType = '3'; // 3D fix
260 
261     // Start printing the sentence
262     // Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v*cc
263     // a : Mode  : A : Automatic, allowed to automatically switch 2D/3D
264     // x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix)
265     // xx : 12 SV ID
266     // p.p : Position DOP (Dilution of Precision)
267     // h.h : Horizontal DOP
268     // v.v : Vertical DOP
269     // cc : Checksum value
270     length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType);
271 
272     if (length < 0 || length >= lengthRemaining)
273     {
274         LOC_LOGE("NMEA Error in string formatting");
275         return 0;
276     }
277     pMarker += length;
278     lengthRemaining -= length;
279 
280     // Add first 12 satellite IDs
281     for (uint8_t i = 0; i < 12; i++)
282     {
283         if (i < svUsedCount)
284             length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]);
285         else
286             length = snprintf(pMarker, lengthRemaining, ",");
287 
288         if (length < 0 || length >= lengthRemaining)
289         {
290             LOC_LOGE("NMEA Error in string formatting");
291             return 0;
292         }
293         pMarker += length;
294         lengthRemaining -= length;
295     }
296 
297     // Add the position/horizontal/vertical DOP values
298     if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
299     {
300         length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,",
301                 locationExtended.pdop,
302                 locationExtended.hdop,
303                 locationExtended.vdop);
304     }
305     else
306     {   // no dop
307         length = snprintf(pMarker, lengthRemaining, ",,,");
308     }
309     pMarker += length;
310     lengthRemaining -= length;
311 
312     // system id
313     length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId);
314     pMarker += length;
315     lengthRemaining -= length;
316 
317     /* Sentence is ready, add checksum and broadcast */
318     length = loc_nmea_put_checksum(sentence, bufSize);
319     nmeaArraystr.push_back(sentence);
320 
321     return svUsedCount;
322 }
323 
324 /*===========================================================================
325 FUNCTION    loc_nmea_generate_GSV
326 
327 DESCRIPTION
328    Generate NMEA GSV sentences generated based on sv report
329    Currently below sentences are generated:
330    - $GPGSV: GPS Satellites in View
331    - $GNGSV: GLONASS Satellites in View
332    - $GAGSV: GALILEO Satellites in View
333 
334 DEPENDENCIES
335    NONE
336 
337 RETURN VALUE
338    NONE
339 
340 SIDE EFFECTS
341    N/A
342 
343 ===========================================================================*/
loc_nmea_generate_GSV(const GnssSvNotification & svNotify,char * sentence,int bufSize,loc_nmea_sv_meta * sv_meta_p,std::vector<std::string> & nmeaArraystr)344 static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
345                               char* sentence,
346                               int bufSize,
347                               loc_nmea_sv_meta* sv_meta_p,
348                               std::vector<std::string> &nmeaArraystr)
349 {
350     if (!sentence || bufSize <= 0)
351     {
352         LOC_LOGE("NMEA Error invalid argument.");
353         return;
354     }
355 
356     char* pMarker = sentence;
357     int lengthRemaining = bufSize;
358     int length = 0;
359     int sentenceCount = 0;
360     int sentenceNumber = 1;
361     size_t svNumber = 1;
362 
363     const char* talker = sv_meta_p->talker;
364     uint32_t svIdOffset = sv_meta_p->svIdOffset;
365     int svCount = sv_meta_p->svCount;
366 
367     if (svCount <= 0)
368     {
369         // no svs in view, so just send a blank $--GSV sentence
370         snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,", talker);
371         length = loc_nmea_put_checksum(sentence, bufSize);
372         nmeaArraystr.push_back(sentence);
373         return;
374     }
375 
376     svNumber = 1;
377     sentenceNumber = 1;
378     sentenceCount = svCount / 4 + (svCount % 4 != 0);
379 
380     while (sentenceNumber <= sentenceCount)
381     {
382         pMarker = sentence;
383         lengthRemaining = bufSize;
384 
385         length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d",
386                 talker, sentenceCount, sentenceNumber, svCount);
387 
388         if (length < 0 || length >= lengthRemaining)
389         {
390             LOC_LOGE("NMEA Error in string formatting");
391             return;
392         }
393         pMarker += length;
394         lengthRemaining -= length;
395 
396         for (int i=0; (svNumber <= svNotify.count) && (i < 4);  svNumber++)
397         {
398             if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type)
399             {
400                 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
401                         svNotify.gnssSvs[svNumber - 1].svId + svIdOffset,
402                         (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
403                         (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
404 
405                 if (length < 0 || length >= lengthRemaining)
406                 {
407                     LOC_LOGE("NMEA Error in string formatting");
408                     return;
409                 }
410                 pMarker += length;
411                 lengthRemaining -= length;
412 
413                 if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0)
414                 {
415                     length = snprintf(pMarker, lengthRemaining,"%02d",
416                             (int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int
417 
418                     if (length < 0 || length >= lengthRemaining)
419                     {
420                         LOC_LOGE("NMEA Error in string formatting");
421                         return;
422                     }
423                     pMarker += length;
424                     lengthRemaining -= length;
425                 }
426 
427                 i++;
428             }
429 
430         }
431 
432         // The following entries are specific to QZSS and BDS
433         if ((sv_meta_p->svType == GNSS_SV_TYPE_QZSS) ||
434             (sv_meta_p->svType == GNSS_SV_TYPE_BEIDOU))
435         {
436             // last one is System id and second last is Signal Id which is always zero
437             length = snprintf(pMarker, lengthRemaining,",%d,%d",0,sv_meta_p->systemId);
438             pMarker += length;
439             lengthRemaining -= length;
440         }
441 
442         length = loc_nmea_put_checksum(sentence, bufSize);
443         nmeaArraystr.push_back(sentence);
444         sentenceNumber++;
445 
446     }  //while
447 }
448 
449 /*===========================================================================
450 FUNCTION    loc_nmea_generate_pos
451 
452 DESCRIPTION
453    Generate NMEA sentences generated based on position report
454    Currently below sentences are generated within this function:
455    - $GPGSA : GPS DOP and active SVs
456    - $GLGSA : GLONASS DOP and active SVs
457    - $GAGSA : GALILEO DOP and active SVs
458    - $GNGSA : GNSS DOP and active SVs
459    - $--VTG : Track made good and ground speed
460    - $--RMC : Recommended minimum navigation information
461    - $--GGA : Time, position and fix related data
462 
463 DEPENDENCIES
464    NONE
465 
466 RETURN VALUE
467    0
468 
469 SIDE EFFECTS
470    N/A
471 
472 ===========================================================================*/
loc_nmea_generate_pos(const UlpLocation & location,const GpsLocationExtended & locationExtended,unsigned char generate_nmea,std::vector<std::string> & nmeaArraystr)473 void loc_nmea_generate_pos(const UlpLocation &location,
474                                const GpsLocationExtended &locationExtended,
475                                unsigned char generate_nmea,
476                                std::vector<std::string> &nmeaArraystr)
477 {
478     ENTRY_LOG();
479     time_t utcTime(location.gpsLocation.timestamp/1000);
480     tm * pTm = gmtime(&utcTime);
481     if (NULL == pTm) {
482         LOC_LOGE("gmtime failed");
483         return;
484     }
485 
486     char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
487     char* pMarker = sentence;
488     int lengthRemaining = sizeof(sentence);
489     int length = 0;
490     int utcYear = pTm->tm_year % 100; // 2 digit year
491     int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero
492     int utcDay = pTm->tm_mday;
493     int utcHours = pTm->tm_hour;
494     int utcMinutes = pTm->tm_min;
495     int utcSeconds = pTm->tm_sec;
496     int utcMSeconds = (location.gpsLocation.timestamp)%1000;
497     loc_sv_cache_info sv_cache_info = {};
498 
499     if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
500         sv_cache_info.gps_used_mask =
501                 (uint32_t)locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask;
502         sv_cache_info.glo_used_mask =
503                 (uint32_t)locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask;
504         sv_cache_info.gal_used_mask =
505                 (uint32_t)locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask;
506         sv_cache_info.qzss_used_mask =
507                 (uint32_t)locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask;
508         sv_cache_info.bds_used_mask =
509                 (uint32_t)locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
510     }
511     if (generate_nmea) {
512         char talker[3] = {'G', 'P', '\0'};
513         uint32_t svUsedCount = 0;
514         uint32_t count = 0;
515         loc_nmea_sv_meta sv_meta;
516         // -------------------
517         // ---$GPGSA/$GNGSA---
518         // -------------------
519 
520         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
521                         loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, true),
522                         nmeaArraystr);
523         if (count > 0)
524         {
525             svUsedCount += count;
526             talker[0] = sv_meta.talker[0];
527             talker[1] = sv_meta.talker[1];
528         }
529 
530         // -------------------
531         // ---$GLGSA/$GNGSA---
532         // -------------------
533 
534         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
535                         loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, true),
536                         nmeaArraystr);
537         if (count > 0)
538         {
539             svUsedCount += count;
540             talker[0] = sv_meta.talker[0];
541             talker[1] = sv_meta.talker[1];
542         }
543 
544         // -------------------
545         // ---$GAGSA/$GNGSA---
546         // -------------------
547 
548         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
549                         loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, true),
550                         nmeaArraystr);
551         if (count > 0)
552         {
553             svUsedCount += count;
554             talker[0] = sv_meta.talker[0];
555             talker[1] = sv_meta.talker[1];
556         }
557 
558         // --------------------------
559         // ---$PQGSA/$GNGSA (QZSS)---
560         // --------------------------
561 
562         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
563                         loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, false),
564                         nmeaArraystr);
565         if (count > 0)
566         {
567             svUsedCount += count;
568             // talker should be default "GP". If GPS, GLO etc is used, it should be "GN"
569         }
570 
571         // ----------------------------
572         // ---$PQGSA/$GNGSA (BEIDOU)---
573         // ----------------------------
574         count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
575                         loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, false),
576                         nmeaArraystr);
577         if (count > 0)
578         {
579             svUsedCount += count;
580             // talker should be default "GP". If GPS, GLO etc is used, it should be "GN"
581         }
582 
583         // -------------------
584         // ------$--VTG-------
585         // -------------------
586 
587         pMarker = sentence;
588         lengthRemaining = sizeof(sentence);
589 
590         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
591         {
592             float magTrack = location.gpsLocation.bearing;
593             if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
594             {
595                 float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation;
596                 if (magTrack < 0.0)
597                     magTrack += 360.0;
598                 else if (magTrack > 360.0)
599                     magTrack -= 360.0;
600             }
601 
602             length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack);
603         }
604         else
605         {
606             length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker);
607         }
608 
609         if (length < 0 || length >= lengthRemaining)
610         {
611             LOC_LOGE("NMEA Error in string formatting");
612             return;
613         }
614         pMarker += length;
615         lengthRemaining -= length;
616 
617         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
618         {
619             float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
620             float speedKmPerHour = location.gpsLocation.speed * 3.6;
621 
622             length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour);
623         }
624         else
625         {
626             length = snprintf(pMarker, lengthRemaining, ",N,,K,");
627         }
628 
629         if (length < 0 || length >= lengthRemaining)
630         {
631             LOC_LOGE("NMEA Error in string formatting");
632             return;
633         }
634         pMarker += length;
635         lengthRemaining -= length;
636 
637         if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
638             // N means no fix
639             length = snprintf(pMarker, lengthRemaining, "%c", 'N');
640         else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
641             // D means differential
642             length = snprintf(pMarker, lengthRemaining, "%c", 'D');
643         else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
644             // E means estimated (dead reckoning)
645             length = snprintf(pMarker, lengthRemaining, "%c", 'E');
646         else // A means autonomous
647             length = snprintf(pMarker, lengthRemaining, "%c", 'A');
648 
649         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
650         nmeaArraystr.push_back(sentence);
651 
652         // -------------------
653         // ------$--RMC-------
654         // -------------------
655 
656         pMarker = sentence;
657         lengthRemaining = sizeof(sentence);
658 
659         length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
660                           talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
661 
662         if (length < 0 || length >= lengthRemaining)
663         {
664             LOC_LOGE("NMEA Error in string formatting");
665             return;
666         }
667         pMarker += length;
668         lengthRemaining -= length;
669 
670         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
671         {
672             double latitude = location.gpsLocation.latitude;
673             double longitude = location.gpsLocation.longitude;
674             char latHemisphere;
675             char lonHemisphere;
676             double latMinutes;
677             double lonMinutes;
678 
679             if (latitude > 0)
680             {
681                 latHemisphere = 'N';
682             }
683             else
684             {
685                 latHemisphere = 'S';
686                 latitude *= -1.0;
687             }
688 
689             if (longitude < 0)
690             {
691                 lonHemisphere = 'W';
692                 longitude *= -1.0;
693             }
694             else
695             {
696                 lonHemisphere = 'E';
697             }
698 
699             latMinutes = fmod(latitude * 60.0 , 60.0);
700             lonMinutes = fmod(longitude * 60.0 , 60.0);
701 
702             length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
703                               (uint8_t)floor(latitude), latMinutes, latHemisphere,
704                               (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
705         }
706         else
707         {
708             length = snprintf(pMarker, lengthRemaining,",,,,");
709         }
710 
711         if (length < 0 || length >= lengthRemaining)
712         {
713             LOC_LOGE("NMEA Error in string formatting");
714             return;
715         }
716         pMarker += length;
717         lengthRemaining -= length;
718 
719         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
720         {
721             float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
722             length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
723         }
724         else
725         {
726             length = snprintf(pMarker, lengthRemaining, ",");
727         }
728 
729         if (length < 0 || length >= lengthRemaining)
730         {
731             LOC_LOGE("NMEA Error in string formatting");
732             return;
733         }
734         pMarker += length;
735         lengthRemaining -= length;
736 
737         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
738         {
739             length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing);
740         }
741         else
742         {
743             length = snprintf(pMarker, lengthRemaining, ",");
744         }
745 
746         if (length < 0 || length >= lengthRemaining)
747         {
748             LOC_LOGE("NMEA Error in string formatting");
749             return;
750         }
751         pMarker += length;
752         lengthRemaining -= length;
753 
754         length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
755                           utcDay, utcMonth, utcYear);
756 
757         if (length < 0 || length >= lengthRemaining)
758         {
759             LOC_LOGE("NMEA Error in string formatting");
760             return;
761         }
762         pMarker += length;
763         lengthRemaining -= length;
764 
765         if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
766         {
767             float magneticVariation = locationExtended.magneticDeviation;
768             char direction;
769             if (magneticVariation < 0.0)
770             {
771                 direction = 'W';
772                 magneticVariation *= -1.0;
773             }
774             else
775             {
776                 direction = 'E';
777             }
778 
779             length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
780                               magneticVariation, direction);
781         }
782         else
783         {
784             length = snprintf(pMarker, lengthRemaining, ",,");
785         }
786 
787         if (length < 0 || length >= lengthRemaining)
788         {
789             LOC_LOGE("NMEA Error in string formatting");
790             return;
791         }
792         pMarker += length;
793         lengthRemaining -= length;
794 
795         if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
796             // N means no fix
797             length = snprintf(pMarker, lengthRemaining, "%c", 'N');
798         else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
799             // D means differential
800             length = snprintf(pMarker, lengthRemaining, "%c", 'D');
801         else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
802             // E means estimated (dead reckoning)
803             length = snprintf(pMarker, lengthRemaining, "%c", 'E');
804         else  // A means autonomous
805             length = snprintf(pMarker, lengthRemaining, "%c", 'A');
806 
807         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
808         nmeaArraystr.push_back(sentence);
809 
810         // -------------------
811         // ------$--GGA-------
812         // -------------------
813 
814         pMarker = sentence;
815         lengthRemaining = sizeof(sentence);
816 
817         length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
818                           talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
819 
820         if (length < 0 || length >= lengthRemaining)
821         {
822             LOC_LOGE("NMEA Error in string formatting");
823             return;
824         }
825         pMarker += length;
826         lengthRemaining -= length;
827 
828         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
829         {
830             double latitude = location.gpsLocation.latitude;
831             double longitude = location.gpsLocation.longitude;
832             char latHemisphere;
833             char lonHemisphere;
834             double latMinutes;
835             double lonMinutes;
836 
837             if (latitude > 0)
838             {
839                 latHemisphere = 'N';
840             }
841             else
842             {
843                 latHemisphere = 'S';
844                 latitude *= -1.0;
845             }
846 
847             if (longitude < 0)
848             {
849                 lonHemisphere = 'W';
850                 longitude *= -1.0;
851             }
852             else
853             {
854                 lonHemisphere = 'E';
855             }
856 
857             latMinutes = fmod(latitude * 60.0 , 60.0);
858             lonMinutes = fmod(longitude * 60.0 , 60.0);
859 
860             length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
861                               (uint8_t)floor(latitude), latMinutes, latHemisphere,
862                               (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
863         }
864         else
865         {
866             length = snprintf(pMarker, lengthRemaining,",,,,");
867         }
868 
869         if (length < 0 || length >= lengthRemaining)
870         {
871             LOC_LOGE("NMEA Error in string formatting");
872             return;
873         }
874         pMarker += length;
875         lengthRemaining -= length;
876 
877         char gpsQuality;
878         if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
879             gpsQuality = '0'; // 0 means no fix
880         else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
881             gpsQuality = '2'; // 2 means DGPS fix
882         else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
883             gpsQuality = '6'; // 6 means estimated (dead reckoning)
884         else
885             gpsQuality = '1'; // 1 means GPS fix
886 
887         // Number of satellites in use, 00-12
888         if (svUsedCount > MAX_SATELLITES_IN_USE)
889             svUsedCount = MAX_SATELLITES_IN_USE;
890         if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
891         {
892             length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
893                               gpsQuality, svUsedCount, locationExtended.hdop);
894         }
895         else
896         {   // no hdop
897             length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
898                               gpsQuality, svUsedCount);
899         }
900 
901         if (length < 0 || length >= lengthRemaining)
902         {
903             LOC_LOGE("NMEA Error in string formatting");
904             return;
905         }
906         pMarker += length;
907         lengthRemaining -= length;
908 
909         if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
910         {
911             length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
912                               locationExtended.altitudeMeanSeaLevel);
913         }
914         else
915         {
916             length = snprintf(pMarker, lengthRemaining,",,");
917         }
918 
919         if (length < 0 || length >= lengthRemaining)
920         {
921             LOC_LOGE("NMEA Error in string formatting");
922             return;
923         }
924         pMarker += length;
925         lengthRemaining -= length;
926 
927         if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
928             (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
929         {
930             length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
931                               location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
932         }
933         else
934         {
935             length = snprintf(pMarker, lengthRemaining,",,,");
936         }
937 
938         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
939         nmeaArraystr.push_back(sentence);
940     }
941     //Send blank NMEA reports for non-final fixes
942     else {
943         strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
944         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
945         nmeaArraystr.push_back(sentence);
946 
947         strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
948         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
949         nmeaArraystr.push_back(sentence);
950 
951         strlcpy(sentence, "$PQGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
952         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
953         nmeaArraystr.push_back(sentence);
954 
955         strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
956         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
957         nmeaArraystr.push_back(sentence);
958 
959         strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
960         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
961         nmeaArraystr.push_back(sentence);
962 
963         strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
964         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
965         nmeaArraystr.push_back(sentence);
966     }
967 
968     EXIT_LOG(%d, 0);
969 }
970 
971 
972 
973 /*===========================================================================
974 FUNCTION    loc_nmea_generate_sv
975 
976 DESCRIPTION
977    Generate NMEA sentences generated based on sv report
978 
979 DEPENDENCIES
980    NONE
981 
982 RETURN VALUE
983    0
984 
985 SIDE EFFECTS
986    N/A
987 
988 ===========================================================================*/
loc_nmea_generate_sv(const GnssSvNotification & svNotify,std::vector<std::string> & nmeaArraystr)989 void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
990                               std::vector<std::string> &nmeaArraystr)
991 {
992     ENTRY_LOG();
993 
994     char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
995     char* pMarker = sentence;
996     int lengthRemaining = sizeof(sentence);
997     int length = 0;
998     int svCount = svNotify.count;
999     int sentenceCount = 0;
1000     int sentenceNumber = 1;
1001     int svNumber = 1;
1002     loc_sv_cache_info sv_cache_info = {};
1003 
1004     //Count GPS SVs for saparating GPS from GLONASS and throw others
1005     for(svNumber=1; svNumber <= svCount; svNumber++) {
1006         if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type)
1007         {
1008             // cache the used in fix mask, as it will be needed to send $GPGSA
1009             // during the position report
1010             if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1011                     (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1012                       GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1013             {
1014                 sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1015             }
1016             sv_cache_info.gps_count++;
1017         }
1018         else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type)
1019         {
1020             // cache the used in fix mask, as it will be needed to send $GNGSA
1021             // during the position report
1022             if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1023                     (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1024                       GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1025             {
1026                 sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1027             }
1028             sv_cache_info.glo_count++;
1029         }
1030         else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type)
1031         {
1032             // cache the used in fix mask, as it will be needed to send $GAGSA
1033             // during the position report
1034             if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1035                     (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1036                       GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1037             {
1038                 sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1039             }
1040             sv_cache_info.gal_count++;
1041         }
1042         else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type)
1043         {
1044             // cache the used in fix mask, as it will be needed to send $PQGSA
1045             // during the position report
1046             if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1047                 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1048                   GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1049             {
1050                 sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1051             }
1052             sv_cache_info.qzss_count++;
1053         }
1054         else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svNumber - 1].type)
1055         {
1056             // cache the used in fix mask, as it will be needed to send $PQGSA
1057             // during the position report
1058             if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1059                 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1060                   GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1061             {
1062                 sv_cache_info.bds_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1063             }
1064             sv_cache_info.bds_count++;
1065         }
1066     }
1067 
1068     loc_nmea_sv_meta sv_meta;
1069     // ------------------
1070     // ------$GPGSV------
1071     // ------------------
1072 
1073     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
1074             loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS, false), nmeaArraystr);
1075 
1076     // ------------------
1077     // ------$GLGSV------
1078     // ------------------
1079 
1080     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
1081             loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS, false),
1082             nmeaArraystr);
1083 
1084     // ------------------
1085     // ------$GAGSV------
1086     // ------------------
1087 
1088     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
1089             loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO, false),
1090             nmeaArraystr);
1091 
1092     // -------------------------
1093     // ------$PQGSV (QZSS)------
1094     // -------------------------
1095 
1096     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
1097             loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, false), nmeaArraystr);
1098 
1099     // ---------------------------
1100     // ------$PQGSV (BEIDOU)------
1101     // ---------------------------
1102 
1103     loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
1104             loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, false),
1105             nmeaArraystr);
1106 
1107     EXIT_LOG(%d, 0);
1108 }
1109