1 /* Copyright (c) 2012-2017, 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_nmea" 32 #include <loc_nmea.h> 33 #include <math.h> 34 #include <platform_lib_includes.h> 35 36 #define GLONASS_SV_ID_OFFSET 64 37 #define MAX_SATELLITES_IN_USE 12 38 39 // GNSS system id according to NMEA spec 40 #define SYSTEM_ID_GPS 1 41 #define SYSTEM_ID_GLONASS 2 42 #define SYSTEM_ID_GALILEO 3 43 // Extended systems 44 #define SYSTEM_ID_BEIDOU 4 45 #define SYSTEM_ID_QZSS 5 46 47 typedef struct loc_nmea_sv_meta_s 48 { 49 char talker[3]; 50 LocGnssConstellationType svType; 51 uint32_t mask; 52 uint32_t svCount; 53 uint32_t svIdOffset; 54 uint32_t systemId; 55 } loc_nmea_sv_meta; 56 57 typedef struct loc_sv_cache_info_s 58 { 59 uint32_t gps_used_mask; 60 uint32_t glo_used_mask; 61 uint32_t gal_used_mask; 62 uint32_t qzss_used_mask; 63 uint32_t bds_used_mask; 64 uint32_t gps_count; 65 uint32_t glo_count; 66 uint32_t gal_count; 67 uint32_t qzss_count; 68 uint32_t bds_count; 69 float hdop; 70 float pdop; 71 float vdop; 72 } loc_sv_cache_info; 73 74 static loc_sv_cache_info sv_cache_info; 75 76 /*=========================================================================== 77 FUNCTION loc_nmea_sv_meta_init 78 79 DESCRIPTION 80 Init loc_nmea_sv_meta passed in 81 82 DEPENDENCIES 83 NONE 84 85 RETURN VALUE 86 Pointer to loc_nmea_sv_meta 87 88 SIDE EFFECTS 89 N/A 90 91 ===========================================================================*/ 92 static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta, 93 GnssSvType svType, 94 bool needCombine) 95 { 96 memset(&sv_meta, 0, sizeof(sv_meta)); 97 sv_meta.svType = svType; 98 99 switch (svType) 100 { 101 case GNSS_SV_TYPE_GPS: 102 sv_meta.talker[0] = 'G'; 103 sv_meta.talker[1] = 'P'; 104 sv_meta.mask = sv_cache_info.gps_used_mask; 105 sv_meta.svCount = sv_cache_info.gps_count; 106 sv_meta.systemId = SYSTEM_ID_GPS; 107 break; 108 case GNSS_SV_TYPE_GLONASS: 109 sv_meta.talker[0] = 'G'; 110 sv_meta.talker[1] = 'L'; 111 sv_meta.mask = sv_cache_info.glo_used_mask; 112 sv_meta.svCount = sv_cache_info.glo_count; 113 // GLONASS SV ids are from 65-96 114 sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET; 115 sv_meta.systemId = SYSTEM_ID_GLONASS; 116 break; 117 case GNSS_SV_TYPE_GALILEO: 118 sv_meta.talker[0] = 'G'; 119 sv_meta.talker[1] = 'A'; 120 sv_meta.mask = sv_cache_info.gal_used_mask; 121 sv_meta.svCount = sv_cache_info.gal_count; 122 sv_meta.systemId = SYSTEM_ID_GALILEO; 123 break; 124 case GNSS_SV_TYPE_QZSS: 125 sv_meta.talker[0] = 'P'; 126 sv_meta.talker[1] = 'Q'; 127 sv_meta.mask = sv_cache_info.qzss_used_mask; 128 sv_meta.svCount = sv_cache_info.qzss_count; 129 // QZSS SV ids are from 193-197. So keep svIdOffset 0 130 sv_meta.systemId = SYSTEM_ID_QZSS; 131 break; 132 case GNSS_SV_TYPE_BEIDOU: 133 sv_meta.talker[0] = 'P'; 134 sv_meta.talker[1] = 'Q'; 135 sv_meta.mask = sv_cache_info.bds_used_mask; 136 sv_meta.svCount = sv_cache_info.bds_count; 137 // BDS SV ids are from 201-235. So keep svIdOffset 0 138 sv_meta.systemId = SYSTEM_ID_BEIDOU; 139 break; 140 default: 141 LOC_LOGE("NMEA Error unknow constellation type: %d", svType); 142 return NULL; 143 } 144 if (needCombine && 145 (sv_cache_info.gps_used_mask ? 1 : 0) + 146 (sv_cache_info.glo_used_mask ? 1 : 0) + 147 (sv_cache_info.gal_used_mask ? 1 : 0) + 148 (sv_cache_info.qzss_used_mask ? 1 : 0) + 149 (sv_cache_info.bds_used_mask ? 1 : 0) > 1) 150 { 151 // If GPS, GLONASS, Galileo, QZSS, BDS etc. are combined 152 // to obtain the reported position solution, 153 // talker shall be set to GN, to indicate that 154 // the satellites are used in a combined solution 155 sv_meta.talker[0] = 'G'; 156 sv_meta.talker[1] = 'N'; 157 } 158 return &sv_meta; 159 } 160 161 /*=========================================================================== 162 FUNCTION loc_nmea_put_checksum 163 164 DESCRIPTION 165 Generate NMEA sentences generated based on position report 166 167 DEPENDENCIES 168 NONE 169 170 RETURN VALUE 171 Total length of the nmea sentence 172 173 SIDE EFFECTS 174 N/A 175 176 ===========================================================================*/ 177 static int loc_nmea_put_checksum(char *pNmea, int maxSize) 178 { 179 uint8_t checksum = 0; 180 int length = 0; 181 if(NULL == pNmea) 182 return 0; 183 184 pNmea++; //skip the $ 185 while (*pNmea != '\0') 186 { 187 checksum ^= *pNmea++; 188 length++; 189 } 190 191 // length now contains nmea sentence string length not including $ sign. 192 int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum); 193 194 // total length of nmea sentence is length of nmea sentence inc $ sign plus 195 // length of checksum (+1 is to cover the $ character in the length). 196 return (length + checksumLength + 1); 197 } 198 199 /*=========================================================================== 200 FUNCTION loc_nmea_generate_GSA 201 202 DESCRIPTION 203 Generate NMEA GSA sentences generated based on position report 204 Currently below sentences are generated: 205 - $GPGSA : GPS DOP and active SVs 206 - $GLGSA : GLONASS DOP and active SVs 207 - $GAGSA : GALILEO DOP and active SVs 208 - $GNGSA : GNSS DOP and active SVs 209 210 DEPENDENCIES 211 NONE 212 213 RETURN VALUE 214 Number of SVs used 215 216 SIDE EFFECTS 217 N/A 218 219 ===========================================================================*/ 220 static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended, 221 char* sentence, 222 int bufSize, 223 loc_nmea_sv_meta* sv_meta_p, 224 std::vector<std::string> &nmeaArraystr) 225 { 226 if (!sentence || bufSize <= 0 || !sv_meta_p) 227 { 228 LOC_LOGE("NMEA Error invalid arguments."); 229 return 0; 230 } 231 232 char* pMarker = sentence; 233 int lengthRemaining = bufSize; 234 int length = 0; 235 236 uint32_t svUsedCount = 0; 237 uint32_t svUsedList[32] = {0}; 238 239 char fixType = '\0'; 240 241 const char* talker = sv_meta_p->talker; 242 uint32_t svIdOffset = sv_meta_p->svIdOffset; 243 uint32_t mask = sv_meta_p->mask; 244 245 for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) 246 { 247 if (mask & 1) 248 svUsedList[svUsedCount++] = i + svIdOffset; 249 mask = mask >> 1; 250 } 251 252 if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType) 253 return 0; 254 255 if (svUsedCount == 0) 256 fixType = '1'; // no fix 257 else if (svUsedCount <= 3) 258 fixType = '2'; // 2D fix 259 else 260 fixType = '3'; // 3D fix 261 262 // Start printing the sentence 263 // Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v*cc 264 // a : Mode : A : Automatic, allowed to automatically switch 2D/3D 265 // x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix) 266 // xx : 12 SV ID 267 // p.p : Position DOP (Dilution of Precision) 268 // h.h : Horizontal DOP 269 // v.v : Vertical DOP 270 // cc : Checksum value 271 length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType); 272 273 if (length < 0 || length >= lengthRemaining) 274 { 275 LOC_LOGE("NMEA Error in string formatting"); 276 return 0; 277 } 278 pMarker += length; 279 lengthRemaining -= length; 280 281 // Add first 12 satellite IDs 282 for (uint8_t i = 0; i < 12; i++) 283 { 284 if (i < svUsedCount) 285 length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); 286 else 287 length = snprintf(pMarker, lengthRemaining, ","); 288 289 if (length < 0 || length >= lengthRemaining) 290 { 291 LOC_LOGE("NMEA Error in string formatting"); 292 return 0; 293 } 294 pMarker += length; 295 lengthRemaining -= length; 296 } 297 298 // Add the position/horizontal/vertical DOP values 299 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 300 { 301 length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,", 302 locationExtended.pdop, 303 locationExtended.hdop, 304 locationExtended.vdop); 305 } 306 else 307 { // no dop 308 length = snprintf(pMarker, lengthRemaining, ",,,"); 309 } 310 pMarker += length; 311 lengthRemaining -= length; 312 313 // system id 314 length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId); 315 pMarker += length; 316 lengthRemaining -= length; 317 318 /* Sentence is ready, add checksum and broadcast */ 319 length = loc_nmea_put_checksum(sentence, bufSize); 320 nmeaArraystr.push_back(sentence); 321 322 return svUsedCount; 323 } 324 325 /*=========================================================================== 326 FUNCTION loc_nmea_generate_GSV 327 328 DESCRIPTION 329 Generate NMEA GSV sentences generated based on sv report 330 Currently below sentences are generated: 331 - $GPGSV: GPS Satellites in View 332 - $GNGSV: GLONASS Satellites in View 333 - $GAGSV: GALILEO Satellites in View 334 335 DEPENDENCIES 336 NONE 337 338 RETURN VALUE 339 NONE 340 341 SIDE EFFECTS 342 N/A 343 344 ===========================================================================*/ 345 static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, 346 char* sentence, 347 int bufSize, 348 loc_nmea_sv_meta* sv_meta_p, 349 std::vector<std::string> &nmeaArraystr) 350 { 351 if (!sentence || bufSize <= 0) 352 { 353 LOC_LOGE("NMEA Error invalid argument."); 354 return; 355 } 356 357 char* pMarker = sentence; 358 int lengthRemaining = bufSize; 359 int length = 0; 360 int sentenceCount = 0; 361 int sentenceNumber = 1; 362 size_t svNumber = 1; 363 364 const char* talker = sv_meta_p->talker; 365 uint32_t svIdOffset = sv_meta_p->svIdOffset; 366 int svCount = sv_meta_p->svCount; 367 368 if (svCount <= 0) 369 { 370 // no svs in view, so just send a blank $--GSV sentence 371 snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,", talker); 372 length = loc_nmea_put_checksum(sentence, bufSize); 373 nmeaArraystr.push_back(sentence); 374 return; 375 } 376 377 svNumber = 1; 378 sentenceNumber = 1; 379 sentenceCount = svCount / 4 + (svCount % 4 != 0); 380 381 while (sentenceNumber <= sentenceCount) 382 { 383 pMarker = sentence; 384 lengthRemaining = bufSize; 385 386 length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d", 387 talker, sentenceCount, sentenceNumber, svCount); 388 389 if (length < 0 || length >= lengthRemaining) 390 { 391 LOC_LOGE("NMEA Error in string formatting"); 392 return; 393 } 394 pMarker += length; 395 lengthRemaining -= length; 396 397 for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++) 398 { 399 if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type) 400 { 401 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 402 svNotify.gnssSvs[svNumber - 1].svId + svIdOffset, 403 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int 404 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int 405 406 if (length < 0 || length >= lengthRemaining) 407 { 408 LOC_LOGE("NMEA Error in string formatting"); 409 return; 410 } 411 pMarker += length; 412 lengthRemaining -= length; 413 414 if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0) 415 { 416 length = snprintf(pMarker, lengthRemaining,"%02d", 417 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int 418 419 if (length < 0 || length >= lengthRemaining) 420 { 421 LOC_LOGE("NMEA Error in string formatting"); 422 return; 423 } 424 pMarker += length; 425 lengthRemaining -= length; 426 } 427 428 i++; 429 } 430 431 } 432 433 // The following entries are specific to QZSS and BDS 434 if ((sv_meta_p->svType == GNSS_SV_TYPE_QZSS) || 435 (sv_meta_p->svType == GNSS_SV_TYPE_BEIDOU)) 436 { 437 // last one is System id and second last is Signal Id which is always zero 438 length = snprintf(pMarker, lengthRemaining,",%d,%d",0,sv_meta_p->systemId); 439 pMarker += length; 440 lengthRemaining -= length; 441 } 442 443 length = loc_nmea_put_checksum(sentence, bufSize); 444 nmeaArraystr.push_back(sentence); 445 sentenceNumber++; 446 447 } //while 448 } 449 450 /*=========================================================================== 451 FUNCTION loc_nmea_generate_pos 452 453 DESCRIPTION 454 Generate NMEA sentences generated based on position report 455 Currently below sentences are generated within this function: 456 - $GPGSA : GPS DOP and active SVs 457 - $GLGSA : GLONASS DOP and active SVs 458 - $GAGSA : GALILEO DOP and active SVs 459 - $GNGSA : GNSS DOP and active SVs 460 - $--VTG : Track made good and ground speed 461 - $--RMC : Recommended minimum navigation information 462 - $--GGA : Time, position and fix related data 463 464 DEPENDENCIES 465 NONE 466 467 RETURN VALUE 468 0 469 470 SIDE EFFECTS 471 N/A 472 473 ===========================================================================*/ 474 void loc_nmea_generate_pos(const UlpLocation &location, 475 const GpsLocationExtended &locationExtended, 476 unsigned char generate_nmea, 477 std::vector<std::string> &nmeaArraystr) 478 { 479 ENTRY_LOG(); 480 time_t utcTime(location.gpsLocation.timestamp/1000); 481 tm * pTm = gmtime(&utcTime); 482 if (NULL == pTm) { 483 LOC_LOGE("gmtime failed"); 484 return; 485 } 486 487 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 488 char* pMarker = sentence; 489 int lengthRemaining = sizeof(sentence); 490 int length = 0; 491 int utcYear = pTm->tm_year % 100; // 2 digit year 492 int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero 493 int utcDay = pTm->tm_mday; 494 int utcHours = pTm->tm_hour; 495 int utcMinutes = pTm->tm_min; 496 int utcSeconds = pTm->tm_sec; 497 int utcMSeconds = (location.gpsLocation.timestamp)%1000; 498 499 if (generate_nmea) { 500 char talker[3] = {'G', 'P', '\0'}; 501 uint32_t svUsedCount = 0; 502 uint32_t count = 0; 503 loc_nmea_sv_meta sv_meta; 504 // ------------------- 505 // ---$GPGSA/$GNGSA--- 506 // ------------------- 507 508 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), 509 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GPS, true), nmeaArraystr); 510 if (count > 0) 511 { 512 svUsedCount += count; 513 talker[0] = sv_meta.talker[0]; 514 talker[1] = sv_meta.talker[1]; 515 } 516 517 // ------------------- 518 // ---$GLGSA/$GNGSA--- 519 // ------------------- 520 521 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), 522 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GLONASS, true), nmeaArraystr); 523 if (count > 0) 524 { 525 svUsedCount += count; 526 talker[0] = sv_meta.talker[0]; 527 talker[1] = sv_meta.talker[1]; 528 } 529 530 // ------------------- 531 // ---$GAGSA/$GNGSA--- 532 // ------------------- 533 534 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), 535 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GALILEO, true), nmeaArraystr); 536 if (count > 0) 537 { 538 svUsedCount += count; 539 talker[0] = sv_meta.talker[0]; 540 talker[1] = sv_meta.talker[1]; 541 } 542 543 // -------------------------- 544 // ---$PQGSA/$GNGSA (QZSS)--- 545 // -------------------------- 546 547 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), 548 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_QZSS, false), nmeaArraystr); 549 if (count > 0) 550 { 551 svUsedCount += count; 552 // talker should be default "GP". If GPS, GLO etc is used, it should be "GN" 553 } 554 555 // ---------------------------- 556 // ---$PQGSA/$GNGSA (BEIDOU)--- 557 // ---------------------------- 558 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), 559 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_BEIDOU, false), nmeaArraystr); 560 if (count > 0) 561 { 562 svUsedCount += count; 563 // talker should be default "GP". If GPS, GLO etc is used, it should be "GN" 564 } 565 566 // ------------------- 567 // ------$--VTG------- 568 // ------------------- 569 570 pMarker = sentence; 571 lengthRemaining = sizeof(sentence); 572 573 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING) 574 { 575 float magTrack = location.gpsLocation.bearing; 576 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 577 { 578 float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; 579 if (magTrack < 0.0) 580 magTrack += 360.0; 581 else if (magTrack > 360.0) 582 magTrack -= 360.0; 583 } 584 585 length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack); 586 } 587 else 588 { 589 length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker); 590 } 591 592 if (length < 0 || length >= lengthRemaining) 593 { 594 LOC_LOGE("NMEA Error in string formatting"); 595 return; 596 } 597 pMarker += length; 598 lengthRemaining -= length; 599 600 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) 601 { 602 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 603 float speedKmPerHour = location.gpsLocation.speed * 3.6; 604 605 length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); 606 } 607 else 608 { 609 length = snprintf(pMarker, lengthRemaining, ",N,,K,"); 610 } 611 612 if (length < 0 || length >= lengthRemaining) 613 { 614 LOC_LOGE("NMEA Error in string formatting"); 615 return; 616 } 617 pMarker += length; 618 lengthRemaining -= length; 619 620 if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) 621 // N means no fix 622 length = snprintf(pMarker, lengthRemaining, "%c", 'N'); 623 else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) 624 // D means differential 625 length = snprintf(pMarker, lengthRemaining, "%c", 'D'); 626 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) 627 // E means estimated (dead reckoning) 628 length = snprintf(pMarker, lengthRemaining, "%c", 'E'); 629 else // A means autonomous 630 length = snprintf(pMarker, lengthRemaining, "%c", 'A'); 631 632 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 633 nmeaArraystr.push_back(sentence); 634 635 // ------------------- 636 // ------$--RMC------- 637 // ------------------- 638 639 pMarker = sentence; 640 lengthRemaining = sizeof(sentence); 641 642 length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," , 643 talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10); 644 645 if (length < 0 || length >= lengthRemaining) 646 { 647 LOC_LOGE("NMEA Error in string formatting"); 648 return; 649 } 650 pMarker += length; 651 lengthRemaining -= length; 652 653 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) 654 { 655 double latitude = location.gpsLocation.latitude; 656 double longitude = location.gpsLocation.longitude; 657 char latHemisphere; 658 char lonHemisphere; 659 double latMinutes; 660 double lonMinutes; 661 662 if (latitude > 0) 663 { 664 latHemisphere = 'N'; 665 } 666 else 667 { 668 latHemisphere = 'S'; 669 latitude *= -1.0; 670 } 671 672 if (longitude < 0) 673 { 674 lonHemisphere = 'W'; 675 longitude *= -1.0; 676 } 677 else 678 { 679 lonHemisphere = 'E'; 680 } 681 682 latMinutes = fmod(latitude * 60.0 , 60.0); 683 lonMinutes = fmod(longitude * 60.0 , 60.0); 684 685 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 686 (uint8_t)floor(latitude), latMinutes, latHemisphere, 687 (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 688 } 689 else 690 { 691 length = snprintf(pMarker, lengthRemaining,",,,,"); 692 } 693 694 if (length < 0 || length >= lengthRemaining) 695 { 696 LOC_LOGE("NMEA Error in string formatting"); 697 return; 698 } 699 pMarker += length; 700 lengthRemaining -= length; 701 702 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED) 703 { 704 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 705 length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); 706 } 707 else 708 { 709 length = snprintf(pMarker, lengthRemaining, ","); 710 } 711 712 if (length < 0 || length >= lengthRemaining) 713 { 714 LOC_LOGE("NMEA Error in string formatting"); 715 return; 716 } 717 pMarker += length; 718 lengthRemaining -= length; 719 720 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING) 721 { 722 length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing); 723 } 724 else 725 { 726 length = snprintf(pMarker, lengthRemaining, ","); 727 } 728 729 if (length < 0 || length >= lengthRemaining) 730 { 731 LOC_LOGE("NMEA Error in string formatting"); 732 return; 733 } 734 pMarker += length; 735 lengthRemaining -= length; 736 737 length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", 738 utcDay, utcMonth, utcYear); 739 740 if (length < 0 || length >= lengthRemaining) 741 { 742 LOC_LOGE("NMEA Error in string formatting"); 743 return; 744 } 745 pMarker += length; 746 lengthRemaining -= length; 747 748 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 749 { 750 float magneticVariation = locationExtended.magneticDeviation; 751 char direction; 752 if (magneticVariation < 0.0) 753 { 754 direction = 'W'; 755 magneticVariation *= -1.0; 756 } 757 else 758 { 759 direction = 'E'; 760 } 761 762 length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", 763 magneticVariation, direction); 764 } 765 else 766 { 767 length = snprintf(pMarker, lengthRemaining, ",,"); 768 } 769 770 if (length < 0 || length >= lengthRemaining) 771 { 772 LOC_LOGE("NMEA Error in string formatting"); 773 return; 774 } 775 pMarker += length; 776 lengthRemaining -= length; 777 778 if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) 779 // N means no fix 780 length = snprintf(pMarker, lengthRemaining, "%c", 'N'); 781 else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) 782 // D means differential 783 length = snprintf(pMarker, lengthRemaining, "%c", 'D'); 784 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) 785 // E means estimated (dead reckoning) 786 length = snprintf(pMarker, lengthRemaining, "%c", 'E'); 787 else // A means autonomous 788 length = snprintf(pMarker, lengthRemaining, "%c", 'A'); 789 790 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 791 nmeaArraystr.push_back(sentence); 792 793 // ------------------- 794 // ------$--GGA------- 795 // ------------------- 796 797 pMarker = sentence; 798 lengthRemaining = sizeof(sentence); 799 800 length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," , 801 talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); 802 803 if (length < 0 || length >= lengthRemaining) 804 { 805 LOC_LOGE("NMEA Error in string formatting"); 806 return; 807 } 808 pMarker += length; 809 lengthRemaining -= length; 810 811 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) 812 { 813 double latitude = location.gpsLocation.latitude; 814 double longitude = location.gpsLocation.longitude; 815 char latHemisphere; 816 char lonHemisphere; 817 double latMinutes; 818 double lonMinutes; 819 820 if (latitude > 0) 821 { 822 latHemisphere = 'N'; 823 } 824 else 825 { 826 latHemisphere = 'S'; 827 latitude *= -1.0; 828 } 829 830 if (longitude < 0) 831 { 832 lonHemisphere = 'W'; 833 longitude *= -1.0; 834 } 835 else 836 { 837 lonHemisphere = 'E'; 838 } 839 840 latMinutes = fmod(latitude * 60.0 , 60.0); 841 lonMinutes = fmod(longitude * 60.0 , 60.0); 842 843 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 844 (uint8_t)floor(latitude), latMinutes, latHemisphere, 845 (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 846 } 847 else 848 { 849 length = snprintf(pMarker, lengthRemaining,",,,,"); 850 } 851 852 if (length < 0 || length >= lengthRemaining) 853 { 854 LOC_LOGE("NMEA Error in string formatting"); 855 return; 856 } 857 pMarker += length; 858 lengthRemaining -= length; 859 860 char gpsQuality; 861 if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) 862 gpsQuality = '0'; // 0 means no fix 863 else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) 864 gpsQuality = '2'; // 2 means DGPS fix 865 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) 866 gpsQuality = '6'; // 6 means estimated (dead reckoning) 867 else 868 gpsQuality = '1'; // 1 means GPS fix 869 870 // Number of satellites in use, 00-12 871 if (svUsedCount > MAX_SATELLITES_IN_USE) 872 svUsedCount = MAX_SATELLITES_IN_USE; 873 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 874 { 875 length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 876 gpsQuality, svUsedCount, locationExtended.hdop); 877 } 878 else 879 { // no hdop 880 length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", 881 gpsQuality, svUsedCount); 882 } 883 884 if (length < 0 || length >= lengthRemaining) 885 { 886 LOC_LOGE("NMEA Error in string formatting"); 887 return; 888 } 889 pMarker += length; 890 lengthRemaining -= length; 891 892 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) 893 { 894 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", 895 locationExtended.altitudeMeanSeaLevel); 896 } 897 else 898 { 899 length = snprintf(pMarker, lengthRemaining,",,"); 900 } 901 902 if (length < 0 || length >= lengthRemaining) 903 { 904 LOC_LOGE("NMEA Error in string formatting"); 905 return; 906 } 907 pMarker += length; 908 lengthRemaining -= length; 909 910 if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) && 911 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) 912 { 913 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", 914 location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); 915 } 916 else 917 { 918 length = snprintf(pMarker, lengthRemaining,",,,"); 919 } 920 921 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 922 nmeaArraystr.push_back(sentence); 923 924 // clear the cache so they can't be used again 925 sv_cache_info.gps_used_mask = 0; 926 sv_cache_info.glo_used_mask = 0; 927 sv_cache_info.gal_used_mask = 0; 928 sv_cache_info.qzss_used_mask = 0; 929 sv_cache_info.bds_used_mask = 0; 930 } 931 //Send blank NMEA reports for non-final fixes 932 else { 933 strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 934 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 935 nmeaArraystr.push_back(sentence); 936 937 strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 938 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 939 nmeaArraystr.push_back(sentence); 940 941 strlcpy(sentence, "$PQGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 942 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 943 nmeaArraystr.push_back(sentence); 944 945 strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 946 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 947 nmeaArraystr.push_back(sentence); 948 949 strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 950 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 951 nmeaArraystr.push_back(sentence); 952 953 strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 954 length = loc_nmea_put_checksum(sentence, sizeof(sentence)); 955 nmeaArraystr.push_back(sentence); 956 } 957 958 EXIT_LOG(%d, 0); 959 } 960 961 962 963 /*=========================================================================== 964 FUNCTION loc_nmea_generate_sv 965 966 DESCRIPTION 967 Generate NMEA sentences generated based on sv report 968 969 DEPENDENCIES 970 NONE 971 972 RETURN VALUE 973 0 974 975 SIDE EFFECTS 976 N/A 977 978 ===========================================================================*/ 979 void loc_nmea_generate_sv(const GnssSvNotification &svNotify, 980 std::vector<std::string> &nmeaArraystr) 981 { 982 ENTRY_LOG(); 983 984 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 985 char* pMarker = sentence; 986 int lengthRemaining = sizeof(sentence); 987 int length = 0; 988 int svCount = svNotify.count; 989 int sentenceCount = 0; 990 int sentenceNumber = 1; 991 int svNumber = 1; 992 993 //Count GPS SVs for saparating GPS from GLONASS and throw others 994 995 sv_cache_info.gps_used_mask = 0; 996 sv_cache_info.glo_used_mask = 0; 997 sv_cache_info.gal_used_mask = 0; 998 sv_cache_info.qzss_used_mask = 0; 999 sv_cache_info.bds_used_mask = 0; 1000 1001 sv_cache_info.gps_count = 0; 1002 sv_cache_info.glo_count = 0; 1003 sv_cache_info.gal_count = 0; 1004 sv_cache_info.qzss_count = 0; 1005 sv_cache_info.bds_count = 0; 1006 for(svNumber=1; svNumber <= svCount; svNumber++) { 1007 if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type) 1008 { 1009 // cache the used in fix mask, as it will be needed to send $GPGSA 1010 // during the position report 1011 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == 1012 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & 1013 GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) 1014 { 1015 sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); 1016 } 1017 sv_cache_info.gps_count++; 1018 } 1019 else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type) 1020 { 1021 // cache the used in fix mask, as it will be needed to send $GNGSA 1022 // during the position report 1023 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == 1024 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & 1025 GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) 1026 { 1027 sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); 1028 } 1029 sv_cache_info.glo_count++; 1030 } 1031 else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type) 1032 { 1033 // cache the used in fix mask, as it will be needed to send $GAGSA 1034 // during the position report 1035 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == 1036 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & 1037 GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) 1038 { 1039 sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); 1040 } 1041 sv_cache_info.gal_count++; 1042 } 1043 else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type) 1044 { 1045 // cache the used in fix mask, as it will be needed to send $PQGSA 1046 // during the position report 1047 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == 1048 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & 1049 GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) 1050 { 1051 sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); 1052 } 1053 sv_cache_info.qzss_count++; 1054 } 1055 else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svNumber - 1].type) 1056 { 1057 // cache the used in fix mask, as it will be needed to send $PQGSA 1058 // during the position report 1059 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT == 1060 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask & 1061 GNSS_SV_OPTIONS_USED_IN_FIX_BIT)) 1062 { 1063 sv_cache_info.bds_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1)); 1064 } 1065 sv_cache_info.bds_count++; 1066 } 1067 } 1068 1069 loc_nmea_sv_meta sv_meta; 1070 // ------------------ 1071 // ------$GPGSV------ 1072 // ------------------ 1073 1074 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), 1075 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GPS, false), nmeaArraystr); 1076 1077 // ------------------ 1078 // ------$GLGSV------ 1079 // ------------------ 1080 1081 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), 1082 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GLONASS, false), nmeaArraystr); 1083 1084 // ------------------ 1085 // ------$GAGSV------ 1086 // ------------------ 1087 1088 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), 1089 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_GALILEO, false), nmeaArraystr); 1090 1091 // ------------------------- 1092 // ------$PQGSV (QZSS)------ 1093 // ------------------------- 1094 1095 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), 1096 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_QZSS, false), nmeaArraystr); 1097 1098 // --------------------------- 1099 // ------$PQGSV (BEIDOU)------ 1100 // --------------------------- 1101 1102 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence), 1103 loc_nmea_sv_meta_init(sv_meta, GNSS_SV_TYPE_BEIDOU, false), nmeaArraystr); 1104 1105 EXIT_LOG(%d, 0); 1106 } 1107
