1 /* Copyright (c) 2012, 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_eng_nmea"
32 #define GPS_PRN_START 1
33 #define GPS_PRN_END 32
34 #define GLONASS_PRN_START 65
35 #define GLONASS_PRN_END 96
36 #include <loc_eng.h>
37 #include <loc_eng_nmea.h>
38 #include <math.h>
39 #include "log_util.h"
40
41 /*===========================================================================
42 FUNCTION loc_eng_nmea_send
43
44 DESCRIPTION
45 send out NMEA sentence
46
47 DEPENDENCIES
48 NONE
49
50 RETURN VALUE
51 Total length of the nmea sentence
52
53 SIDE EFFECTS
54 N/A
55
56 ===========================================================================*/
loc_eng_nmea_send(char * pNmea,int length,loc_eng_data_s_type * loc_eng_data_p)57 void loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p)
58 {
59 struct timeval tv;
60 gettimeofday(&tv, (struct timezone *) NULL);
61 int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000;
62 if (loc_eng_data_p->nmea_cb != NULL)
63 loc_eng_data_p->nmea_cb(now, pNmea, length);
64 LOC_LOGD("NMEA <%s", pNmea);
65 }
66
67 /*===========================================================================
68 FUNCTION loc_eng_nmea_put_checksum
69
70 DESCRIPTION
71 Generate NMEA sentences generated based on position report
72
73 DEPENDENCIES
74 NONE
75
76 RETURN VALUE
77 Total length of the nmea sentence
78
79 SIDE EFFECTS
80 N/A
81
82 ===========================================================================*/
loc_eng_nmea_put_checksum(char * pNmea,int maxSize)83 int loc_eng_nmea_put_checksum(char *pNmea, int maxSize)
84 {
85 uint8_t checksum = 0;
86 int length = 0;
87
88 pNmea++; //skip the $
89 while (*pNmea != '\0')
90 {
91 checksum ^= *pNmea++;
92 length++;
93 }
94
95 int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum);
96 return (length + checksumLength);
97 }
98
99 /*===========================================================================
100 FUNCTION loc_eng_nmea_generate_pos
101
102 DESCRIPTION
103 Generate NMEA sentences generated based on position report
104
105 DEPENDENCIES
106 NONE
107
108 RETURN VALUE
109 0
110
111 SIDE EFFECTS
112 N/A
113
114 ===========================================================================*/
loc_eng_nmea_generate_pos(loc_eng_data_s_type * loc_eng_data_p,const UlpLocation & location,const GpsLocationExtended & locationExtended,unsigned char generate_nmea)115 void loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p,
116 const UlpLocation &location,
117 const GpsLocationExtended &locationExtended,
118 unsigned char generate_nmea)
119 {
120 ENTRY_LOG();
121 time_t utcTime(location.gpsLocation.timestamp/1000);
122 tm * pTm = gmtime(&utcTime);
123 if (NULL == pTm) {
124 LOC_LOGE("gmtime failed");
125 return;
126 }
127
128 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
129 char* pMarker = sentence;
130 int lengthRemaining = sizeof(sentence);
131 int length = 0;
132 int utcYear = pTm->tm_year % 100; // 2 digit year
133 int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero
134 int utcDay = pTm->tm_mday;
135 int utcHours = pTm->tm_hour;
136 int utcMinutes = pTm->tm_min;
137 int utcSeconds = pTm->tm_sec;
138
139 if (generate_nmea) {
140 // ------------------
141 // ------$GPGSA------
142 // ------------------
143
144 uint32_t svUsedCount = 0;
145 uint32_t svUsedList[32] = {0};
146 uint32_t mask = loc_eng_data_p->sv_used_mask;
147 for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++)
148 {
149 if (mask & 1)
150 svUsedList[svUsedCount++] = i;
151 mask = mask >> 1;
152 }
153 // clear the cache so they can't be used again
154 loc_eng_data_p->sv_used_mask = 0;
155
156 char fixType;
157 if (svUsedCount == 0)
158 fixType = '1'; // no fix
159 else if (svUsedCount <= 3)
160 fixType = '2'; // 2D fix
161 else
162 fixType = '3'; // 3D fix
163
164 length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType);
165
166 if (length < 0 || length >= lengthRemaining)
167 {
168 LOC_LOGE("NMEA Error in string formatting");
169 return;
170 }
171 pMarker += length;
172 lengthRemaining -= length;
173
174 for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence
175 {
176 if (i < svUsedCount)
177 length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]);
178 else
179 length = snprintf(pMarker, lengthRemaining, ",");
180
181 if (length < 0 || length >= lengthRemaining)
182 {
183 LOC_LOGE("NMEA Error in string formatting");
184 return;
185 }
186 pMarker += length;
187 lengthRemaining -= length;
188 }
189
190 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
191 { // dop is in locationExtended, (QMI)
192 length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
193 locationExtended.pdop,
194 locationExtended.hdop,
195 locationExtended.vdop);
196 }
197 else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0)
198 { // dop was cached from sv report (RPC)
199 length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f",
200 loc_eng_data_p->pdop,
201 loc_eng_data_p->hdop,
202 loc_eng_data_p->vdop);
203 }
204 else
205 { // no dop
206 length = snprintf(pMarker, lengthRemaining, ",,");
207 }
208
209 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
210 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
211
212 // ------------------
213 // ------$GPVTG------
214 // ------------------
215
216 pMarker = sentence;
217 lengthRemaining = sizeof(sentence);
218
219 if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING)
220 {
221 float magTrack = location.gpsLocation.bearing;
222 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
223 {
224 float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation;
225 if (magTrack < 0.0)
226 magTrack += 360.0;
227 else if (magTrack > 360.0)
228 magTrack -= 360.0;
229 }
230
231 length = snprintf(pMarker, lengthRemaining, "$GPVTG,%.1lf,T,%.1lf,M,", location.gpsLocation.bearing, magTrack);
232 }
233 else
234 {
235 length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,");
236 }
237
238 if (length < 0 || length >= lengthRemaining)
239 {
240 LOC_LOGE("NMEA Error in string formatting");
241 return;
242 }
243 pMarker += length;
244 lengthRemaining -= length;
245
246 if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED)
247 {
248 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
249 float speedKmPerHour = location.gpsLocation.speed * 3.6;
250
251 length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour);
252 }
253 else
254 {
255 length = snprintf(pMarker, lengthRemaining, ",N,,K,");
256 }
257
258 if (length < 0 || length >= lengthRemaining)
259 {
260 LOC_LOGE("NMEA Error in string formatting");
261 return;
262 }
263 pMarker += length;
264 lengthRemaining -= length;
265
266 if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG))
267 length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
268 else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
269 length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
270 else
271 length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
272
273 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
274 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
275
276 // ------------------
277 // ------$GPRMC------
278 // ------------------
279
280 pMarker = sentence;
281 lengthRemaining = sizeof(sentence);
282
283 length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d,A," ,
284 utcHours, utcMinutes, utcSeconds);
285
286 if (length < 0 || length >= lengthRemaining)
287 {
288 LOC_LOGE("NMEA Error in string formatting");
289 return;
290 }
291 pMarker += length;
292 lengthRemaining -= length;
293
294 if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)
295 {
296 double latitude = location.gpsLocation.latitude;
297 double longitude = location.gpsLocation.longitude;
298 char latHemisphere;
299 char lonHemisphere;
300 double latMinutes;
301 double lonMinutes;
302
303 if (latitude > 0)
304 {
305 latHemisphere = 'N';
306 }
307 else
308 {
309 latHemisphere = 'S';
310 latitude *= -1.0;
311 }
312
313 if (longitude < 0)
314 {
315 lonHemisphere = 'W';
316 longitude *= -1.0;
317 }
318 else
319 {
320 lonHemisphere = 'E';
321 }
322
323 latMinutes = fmod(latitude * 60.0 , 60.0);
324 lonMinutes = fmod(longitude * 60.0 , 60.0);
325
326 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
327 (uint8_t)floor(latitude), latMinutes, latHemisphere,
328 (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
329 }
330 else
331 {
332 length = snprintf(pMarker, lengthRemaining,",,,,");
333 }
334
335 if (length < 0 || length >= lengthRemaining)
336 {
337 LOC_LOGE("NMEA Error in string formatting");
338 return;
339 }
340 pMarker += length;
341 lengthRemaining -= length;
342
343 if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED)
344 {
345 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
346 length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
347 }
348 else
349 {
350 length = snprintf(pMarker, lengthRemaining, ",");
351 }
352
353 if (length < 0 || length >= lengthRemaining)
354 {
355 LOC_LOGE("NMEA Error in string formatting");
356 return;
357 }
358 pMarker += length;
359 lengthRemaining -= length;
360
361 if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING)
362 {
363 length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing);
364 }
365 else
366 {
367 length = snprintf(pMarker, lengthRemaining, ",");
368 }
369
370 if (length < 0 || length >= lengthRemaining)
371 {
372 LOC_LOGE("NMEA Error in string formatting");
373 return;
374 }
375 pMarker += length;
376 lengthRemaining -= length;
377
378 length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
379 utcDay, utcMonth, utcYear);
380
381 if (length < 0 || length >= lengthRemaining)
382 {
383 LOC_LOGE("NMEA Error in string formatting");
384 return;
385 }
386 pMarker += length;
387 lengthRemaining -= length;
388
389 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
390 {
391 float magneticVariation = locationExtended.magneticDeviation;
392 char direction;
393 if (magneticVariation < 0.0)
394 {
395 direction = 'W';
396 magneticVariation *= -1.0;
397 }
398 else
399 {
400 direction = 'E';
401 }
402
403 length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
404 magneticVariation, direction);
405 }
406 else
407 {
408 length = snprintf(pMarker, lengthRemaining, ",,");
409 }
410
411 if (length < 0 || length >= lengthRemaining)
412 {
413 LOC_LOGE("NMEA Error in string formatting");
414 return;
415 }
416 pMarker += length;
417 lengthRemaining -= length;
418
419 if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG))
420 length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix
421 else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
422 length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous
423 else
424 length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential
425
426 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
427 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
428
429 // ------------------
430 // ------$GPGGA------
431 // ------------------
432
433 pMarker = sentence;
434 lengthRemaining = sizeof(sentence);
435
436 length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d," ,
437 utcHours, utcMinutes, utcSeconds);
438
439 if (length < 0 || length >= lengthRemaining)
440 {
441 LOC_LOGE("NMEA Error in string formatting");
442 return;
443 }
444 pMarker += length;
445 lengthRemaining -= length;
446
447 if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)
448 {
449 double latitude = location.gpsLocation.latitude;
450 double longitude = location.gpsLocation.longitude;
451 char latHemisphere;
452 char lonHemisphere;
453 double latMinutes;
454 double lonMinutes;
455
456 if (latitude > 0)
457 {
458 latHemisphere = 'N';
459 }
460 else
461 {
462 latHemisphere = 'S';
463 latitude *= -1.0;
464 }
465
466 if (longitude < 0)
467 {
468 lonHemisphere = 'W';
469 longitude *= -1.0;
470 }
471 else
472 {
473 lonHemisphere = 'E';
474 }
475
476 latMinutes = fmod(latitude * 60.0 , 60.0);
477 lonMinutes = fmod(longitude * 60.0 , 60.0);
478
479 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
480 (uint8_t)floor(latitude), latMinutes, latHemisphere,
481 (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
482 }
483 else
484 {
485 length = snprintf(pMarker, lengthRemaining,",,,,");
486 }
487
488 if (length < 0 || length >= lengthRemaining)
489 {
490 LOC_LOGE("NMEA Error in string formatting");
491 return;
492 }
493 pMarker += length;
494 lengthRemaining -= length;
495
496 char gpsQuality;
497 if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG))
498 gpsQuality = '0'; // 0 means no fix
499 else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode)
500 gpsQuality = '1'; // 1 means GPS fix
501 else
502 gpsQuality = '2'; // 2 means DGPS fix
503
504 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
505 { // dop is in locationExtended, (QMI)
506 length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
507 gpsQuality, svUsedCount, locationExtended.hdop);
508 }
509 else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0)
510 { // dop was cached from sv report (RPC)
511 length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
512 gpsQuality, svUsedCount, loc_eng_data_p->hdop);
513 }
514 else
515 { // no hdop
516 length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
517 gpsQuality, svUsedCount);
518 }
519
520 if (length < 0 || length >= lengthRemaining)
521 {
522 LOC_LOGE("NMEA Error in string formatting");
523 return;
524 }
525 pMarker += length;
526 lengthRemaining -= length;
527
528 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
529 {
530 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
531 locationExtended.altitudeMeanSeaLevel);
532 }
533 else
534 {
535 length = snprintf(pMarker, lengthRemaining,",,");
536 }
537
538 if (length < 0 || length >= lengthRemaining)
539 {
540 LOC_LOGE("NMEA Error in string formatting");
541 return;
542 }
543 pMarker += length;
544 lengthRemaining -= length;
545
546 if ((location.gpsLocation.flags & GPS_LOCATION_HAS_ALTITUDE) &&
547 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
548 {
549 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
550 location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
551 }
552 else
553 {
554 length = snprintf(pMarker, lengthRemaining,",,,");
555 }
556
557 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
558 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
559
560 }
561 //Send blank NMEA reports for non-final fixes
562 else {
563 strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
564 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
565 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
566
567 strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
568 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
569 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
570
571 strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
572 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
573 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
574
575 strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
576 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
577 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
578 }
579 // clear the dop cache so they can't be used again
580 loc_eng_data_p->pdop = 0;
581 loc_eng_data_p->hdop = 0;
582 loc_eng_data_p->vdop = 0;
583
584 EXIT_LOG(%d, 0);
585 }
586
587
588
589 /*===========================================================================
590 FUNCTION loc_eng_nmea_generate_sv
591
592 DESCRIPTION
593 Generate NMEA sentences generated based on sv report
594
595 DEPENDENCIES
596 NONE
597
598 RETURN VALUE
599 0
600
601 SIDE EFFECTS
602 N/A
603
604 ===========================================================================*/
loc_eng_nmea_generate_sv(loc_eng_data_s_type * loc_eng_data_p,const GpsSvStatus & svStatus,const GpsLocationExtended & locationExtended)605 void loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p,
606 const GpsSvStatus &svStatus, const GpsLocationExtended &locationExtended)
607 {
608 ENTRY_LOG();
609
610 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
611 char* pMarker = sentence;
612 int lengthRemaining = sizeof(sentence);
613 int length = 0;
614 int svCount = svStatus.num_svs;
615 int sentenceCount = 0;
616 int sentenceNumber = 1;
617 int svNumber = 1;
618 int gpsCount = 0;
619 int glnCount = 0;
620
621 //Count GPS SVs for saparating GPS from GLONASS and throw others
622
623 for(svNumber=1; svNumber <= svCount; svNumber++) {
624 if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START)&&
625 (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) )
626 {
627 gpsCount++;
628 }
629 else if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) &&
630 (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) )
631 {
632 glnCount++;
633 }
634 }
635
636 // ------------------
637 // ------$GPGSV------
638 // ------------------
639
640 if (gpsCount <= 0)
641 {
642 // no svs in view, so just send a blank $GPGSV sentence
643 strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence));
644 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
645 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
646 }
647 else
648 {
649 svNumber = 1;
650 sentenceNumber = 1;
651 sentenceCount = gpsCount/4 + (gpsCount % 4 != 0);
652
653 while (sentenceNumber <= sentenceCount)
654 {
655 pMarker = sentence;
656 lengthRemaining = sizeof(sentence);
657
658 length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d",
659 sentenceCount, sentenceNumber, gpsCount);
660
661 if (length < 0 || length >= lengthRemaining)
662 {
663 LOC_LOGE("NMEA Error in string formatting");
664 return;
665 }
666 pMarker += length;
667 lengthRemaining -= length;
668
669 for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++)
670 {
671 if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START) &&
672 (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) )
673 {
674 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
675 svStatus.sv_list[svNumber-1].prn,
676 (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int
677 (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int
678
679 if (length < 0 || length >= lengthRemaining)
680 {
681 LOC_LOGE("NMEA Error in string formatting");
682 return;
683 }
684 pMarker += length;
685 lengthRemaining -= length;
686
687 if (svStatus.sv_list[svNumber-1].snr > 0)
688 {
689 length = snprintf(pMarker, lengthRemaining,"%02d",
690 (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int
691
692 if (length < 0 || length >= lengthRemaining)
693 {
694 LOC_LOGE("NMEA Error in string formatting");
695 return;
696 }
697 pMarker += length;
698 lengthRemaining -= length;
699 }
700
701 i++;
702 }
703
704 }
705
706 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
707 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
708 sentenceNumber++;
709
710 } //while
711
712 } //if
713
714 // ------------------
715 // ------$GLGSV------
716 // ------------------
717
718 if (glnCount <= 0)
719 {
720 // no svs in view, so just send a blank $GLGSV sentence
721 strlcpy(sentence, "$GLGSV,1,1,0,", sizeof(sentence));
722 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
723 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
724 }
725 else
726 {
727 svNumber = 1;
728 sentenceNumber = 1;
729 sentenceCount = glnCount/4 + (glnCount % 4 != 0);
730
731 while (sentenceNumber <= sentenceCount)
732 {
733 pMarker = sentence;
734 lengthRemaining = sizeof(sentence);
735
736 length = snprintf(pMarker, lengthRemaining, "$GLGSV,%d,%d,%02d",
737 sentenceCount, sentenceNumber, glnCount);
738
739 if (length < 0 || length >= lengthRemaining)
740 {
741 LOC_LOGE("NMEA Error in string formatting");
742 return;
743 }
744 pMarker += length;
745 lengthRemaining -= length;
746
747 for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++)
748 {
749 if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) &&
750 (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) ) {
751
752 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
753 svStatus.sv_list[svNumber-1].prn,
754 (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int
755 (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int
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 (svStatus.sv_list[svNumber-1].snr > 0)
766 {
767 length = snprintf(pMarker, lengthRemaining,"%02d",
768 (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int
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
779 i++;
780 }
781
782 }
783
784 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
785 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
786 sentenceNumber++;
787
788 } //while
789
790 }//if
791
792 if (svStatus.used_in_fix_mask == 0)
793 { // No sv used, so there will be no position report, so send
794 // blank NMEA sentences
795 strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence));
796 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
797 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
798
799 strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
800 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
801 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
802
803 strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence));
804 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
805 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
806
807 strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
808 length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence));
809 loc_eng_nmea_send(sentence, length, loc_eng_data_p);
810 }
811 else
812 { // cache the used in fix mask, as it will be needed to send $GPGSA
813 // during the position report
814 loc_eng_data_p->sv_used_mask = svStatus.used_in_fix_mask;
815
816 // For RPC, the DOP are sent during sv report, so cache them
817 // now to be sent during position report.
818 // For QMI, the DOP will be in position report.
819 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
820 {
821 loc_eng_data_p->pdop = locationExtended.pdop;
822 loc_eng_data_p->hdop = locationExtended.hdop;
823 loc_eng_data_p->vdop = locationExtended.vdop;
824 }
825 else
826 {
827 loc_eng_data_p->pdop = 0;
828 loc_eng_data_p->hdop = 0;
829 loc_eng_data_p->vdop = 0;
830 }
831
832 }
833
834 EXIT_LOG(%d, 0);
835 }
836