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