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/*=========================================================================== 38FUNCTION loc_eng_nmea_send 39 40DESCRIPTION 41 send out NMEA sentence 42 43DEPENDENCIES 44 NONE 45 46RETURN VALUE 47 Total length of the nmea sentence 48 49SIDE EFFECTS 50 N/A 51 52===========================================================================*/ 53void 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/*=========================================================================== 64FUNCTION loc_eng_nmea_put_checksum 65 66DESCRIPTION 67 Generate NMEA sentences generated based on position report 68 69DEPENDENCIES 70 NONE 71 72RETURN VALUE 73 Total length of the nmea sentence 74 75SIDE EFFECTS 76 N/A 77 78===========================================================================*/ 79int 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/*=========================================================================== 100FUNCTION loc_eng_nmea_generate_pos 101 102DESCRIPTION 103 Generate NMEA sentences generated based on position report 104 105DEPENDENCIES 106 NONE 107 108RETURN VALUE 109 0 110 111SIDE EFFECTS 112 N/A 113 114===========================================================================*/ 115void 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/*=========================================================================== 591FUNCTION loc_eng_nmea_generate_sv 592 593DESCRIPTION 594 Generate NMEA sentences generated based on sv report 595 596DEPENDENCIES 597 NONE 598 599RETURN VALUE 600 0 601 602SIDE EFFECTS 603 N/A 604 605===========================================================================*/ 606void 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