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