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