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