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