14035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin/* Copyright (c) 2012, The Linux Foundation. All rights reserved. 24035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * 34035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * Redistribution and use in source and binary forms, with or without 44035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * modification, are permitted provided that the following conditions are 54035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * met: 64035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * * Redistributions of source code must retain the above copyright 74035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * notice, this list of conditions and the following disclaimer. 84035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * * Redistributions in binary form must reproduce the above 94035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * copyright notice, this list of conditions and the following 104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * disclaimer in the documentation and/or other materials provided 114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * with the distribution. 124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * * Neither the name of The Linux Foundation nor the names of its 134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * contributors may be used to endorse or promote products derived 144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * from this software without specific prior written permission. 154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * 164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin * 284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin */ 294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define LOG_NDDEBUG 0 314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define LOG_TAG "LocSvc_eng_nmea" 324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define GPS_PRN_START 1 334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define GPS_PRN_END 32 344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define GLONASS_PRN_START 65 354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#define GLONASS_PRN_END 96 364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#include <loc_eng.h> 374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#include <loc_eng_nmea.h> 384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#include <math.h> 394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin#include "log_util.h" 404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin/*=========================================================================== 424035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinFUNCTION loc_eng_nmea_send 434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 444035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDESCRIPTION 454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin send out NMEA sentence 464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 474035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDEPENDENCIES 484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin NONE 494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 504035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinRETURN VALUE 514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin Total length of the nmea sentence 524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 534035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinSIDE EFFECTS 544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin N/A 554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin===========================================================================*/ 574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjinvoid loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p) 584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin{ 594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin struct timeval tv; 604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gettimeofday(&tv, (struct timezone *) NULL); 614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000; 624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin CALLBACK_LOG_CALLFLOW("nmea_cb", %p, pNmea); 634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (loc_eng_data_p->nmea_cb != NULL) 644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->nmea_cb(now, pNmea, length); 654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGD("NMEA <%s", pNmea); 664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin} 674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin/*=========================================================================== 694035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinFUNCTION loc_eng_nmea_put_checksum 704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 714035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDESCRIPTION 724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin Generate NMEA sentences generated based on position report 734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 744035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDEPENDENCIES 754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin NONE 764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 774035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinRETURN VALUE 784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin Total length of the nmea sentence 794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 804035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinSIDE EFFECTS 814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin N/A 824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin===========================================================================*/ 844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjinint loc_eng_nmea_put_checksum(char *pNmea, int maxSize) 854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin{ 864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin uint8_t checksum = 0; 874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int length = 0; 884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pNmea++; //skip the $ 904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin while (*pNmea != '\0') 914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin checksum ^= *pNmea++; 934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length++; 944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum); 974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return (length + checksumLength); 984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin} 994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin/*=========================================================================== 1014035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinFUNCTION loc_eng_nmea_generate_pos 1024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1034035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDESCRIPTION 1044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin Generate NMEA sentences generated based on position report 1054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1064035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDEPENDENCIES 1074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin NONE 1084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1094035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinRETURN VALUE 1104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 0 1114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1124035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinSIDE EFFECTS 1134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin N/A 1144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin===========================================================================*/ 1164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjinvoid loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p, 1174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin const UlpLocation &location, 1184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin const GpsLocationExtended &locationExtended, 1194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin unsigned char generate_nmea) 1204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin{ 1214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin ENTRY_LOG(); 1224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin time_t utcTime(location.gpsLocation.timestamp/1000); 1234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin tm * pTm = gmtime(&utcTime); 1244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (NULL == pTm) { 1254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("gmtime failed"); 1264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 1274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 1304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char* pMarker = sentence; 1314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int lengthRemaining = sizeof(sentence); 1324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int length = 0; 1334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcYear = pTm->tm_year % 100; // 2 digit year 1344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero 1354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcDay = pTm->tm_mday; 1364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcHours = pTm->tm_hour; 1374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcMinutes = pTm->tm_min; 1384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int utcSeconds = pTm->tm_sec; 139f53ac3ae2fb797f3e3f00685b3e691b218d3d930Steve Pfetsch int utcMSeconds = (location.gpsLocation.timestamp)%1000; 1404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (generate_nmea) { 1424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 1434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GPGSA------ 1444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 1454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin uint32_t svUsedCount = 0; 1474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin uint32_t svUsedList[32] = {0}; 1484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin uint32_t mask = loc_eng_data_p->sv_used_mask; 1494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) 1504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 1514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (mask & 1) 1524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin svUsedList[svUsedCount++] = i; 1534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin mask = mask >> 1; 1544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // clear the cache so they can't be used again 1564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->sv_used_mask = 0; 1574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char fixType; 1594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (svUsedCount == 0) 1604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin fixType = '1'; // no fix 1614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (svUsedCount <= 3) 1624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin fixType = '2'; // 2D fix 1634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 1644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin fixType = '3'; // 3D fix 1654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType); 1674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 1694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 1704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 1714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 1724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 1744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 1754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence 1774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 1784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (i < svUsedCount) 1794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); 1804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 1814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ","); 1824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 1844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 1854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 1864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 1874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 1894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 1904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 1924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 1934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // dop is in locationExtended, (QMI) 1944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 1954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin locationExtended.pdop, 1964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin locationExtended.hdop, 1974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin locationExtended.vdop); 1984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 1994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 2004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // dop was cached from sv report (RPC) 2014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 2024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->pdop, 2034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->hdop, 2044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->vdop); 2054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 2074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // no dop 2084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ",,"); 2094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 2124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 2134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 2154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GPVTG------ 2164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 2174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker = sentence; 2194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining = sizeof(sentence); 2204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 2224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float magTrack = location.gpsLocation.bearing; 2244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 2254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; 2274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (magTrack < 0.0) 2284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin magTrack += 360.0; 2294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (magTrack > 360.0) 2304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin magTrack -= 360.0; 2314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "$GPVTG,%.1lf,T,%.1lf,M,", location.gpsLocation.bearing, magTrack); 2344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 2364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,"); 2384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 2414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 2434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 2444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 2464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 2474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 2494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 2514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float speedKmPerHour = location.gpsLocation.speed * 3.6; 2524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); 2544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 2564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ",N,,K,"); 2584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 2614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 2634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 2644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 2664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 2674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 2694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 2704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 2714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 2724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 2734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 2744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 2764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 2774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 2794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GPRMC------ 2804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 2814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker = sentence; 2834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining = sizeof(sentence); 2844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 285f53ac3ae2fb797f3e3f00685b3e691b218d3d930Steve Pfetsch length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d.%02d,A," , 286f53ac3ae2fb797f3e3f00685b3e691b218d3d930Steve Pfetsch utcHours, utcMinutes, utcSeconds,utcMSeconds/10); 2874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 2894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 2914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 2924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 2934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 2944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 2954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 2964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 2974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 2984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double latitude = location.gpsLocation.latitude; 2994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double longitude = location.gpsLocation.longitude; 3004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char latHemisphere; 3014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char lonHemisphere; 3024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double latMinutes; 3034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double lonMinutes; 3044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (latitude > 0) 3064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latHemisphere = 'N'; 3084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 3104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latHemisphere = 'S'; 3124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latitude *= -1.0; 3134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (longitude < 0) 3164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonHemisphere = 'W'; 3184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin longitude *= -1.0; 3194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 3214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonHemisphere = 'E'; 3234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latMinutes = fmod(latitude * 60.0 , 60.0); 3264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonMinutes = fmod(longitude * 60.0 , 60.0); 3274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 3294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (uint8_t)floor(latitude), latMinutes, latHemisphere, 3304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 3314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 3334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",,,,"); 3354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 3384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 3404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 3414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 3434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 3444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 3464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 3484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); 3494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 3514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ","); 3534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 3564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 3584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 3594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 3614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 3624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 3644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing); 3664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 3684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ","); 3704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 3734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 3754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 3764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 3784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 3794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", 3814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin utcDay, utcMonth, utcYear); 3824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 3844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 3864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 3874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 3884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 3894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 3904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 3914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 3924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin float magneticVariation = locationExtended.magneticDeviation; 3944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char direction; 3954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (magneticVariation < 0.0) 3964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 3974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin direction = 'W'; 3984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin magneticVariation *= -1.0; 3994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin direction = 'E'; 4034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", 4064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin magneticVariation, direction); 4074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, ",,"); 4114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 4144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 4164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 4174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 4194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 4204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 4224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 4234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 4244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 4254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 4274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 4294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 4304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 4324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GPGGA------ 4334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 4344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker = sentence; 4364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining = sizeof(sentence); 4374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 438f53ac3ae2fb797f3e3f00685b3e691b218d3d930Steve Pfetsch length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d.%02d," , 439f53ac3ae2fb797f3e3f00685b3e691b218d3d930Steve Pfetsch utcHours, utcMinutes, utcSeconds, utcMSeconds/10); 4404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 4424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 4444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 4454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 4474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 4484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 4504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double latitude = location.gpsLocation.latitude; 4524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double longitude = location.gpsLocation.longitude; 4534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char latHemisphere; 4544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char lonHemisphere; 4554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double latMinutes; 4564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin double lonMinutes; 4574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (latitude > 0) 4594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latHemisphere = 'N'; 4614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latHemisphere = 'S'; 4654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latitude *= -1.0; 4664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (longitude < 0) 4694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonHemisphere = 'W'; 4714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin longitude *= -1.0; 4724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonHemisphere = 'E'; 4764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin latMinutes = fmod(latitude * 60.0 , 60.0); 4794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lonMinutes = fmod(longitude * 60.0 , 60.0); 4804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 4824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (uint8_t)floor(latitude), latMinutes, latHemisphere, 4834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 4844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 4864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",,,,"); 4884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 4914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 4924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 4934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 4944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 4954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 4964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 4974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 4984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char gpsQuality; 4994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 5004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality = '0'; // 0 means no fix 5014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 5024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality = '1'; // 1 means GPS fix 5034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 5044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality = '2'; // 2 means DGPS fix 5054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 5074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // dop is in locationExtended, (QMI) 5084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 5094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality, svUsedCount, locationExtended.hdop); 5104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 5124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // dop was cached from sv report (RPC) 5134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 5144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality, svUsedCount, loc_eng_data_p->hdop); 5154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 5174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { // no hdop 5184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", 5194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsQuality, svUsedCount); 5204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 5234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 5254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 5264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 5284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 5294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) 5314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", 5334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin locationExtended.altitudeMeanSeaLevel); 5344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 5364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",,"); 5384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 5414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 5434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 5444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 5464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 5474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if ((location.gpsLocation.flags & GPS_LOCATION_HAS_ALTITUDE) && 5494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) 5504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", 5524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); 5534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 5554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 5564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",,,"); 5574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 5604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 5614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin //Send blank NMEA reports for non-final fixes 5644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else { 5654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 5664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 5674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 5684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 5704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 5714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 5724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 5744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 5754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 5764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 5784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 5794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 5804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 5814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // clear the dop cache so they can't be used again 5824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->pdop = 0; 5834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->hdop = 0; 5844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->vdop = 0; 5854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin EXIT_LOG(%d, 0); 5874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin} 5884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin/*=========================================================================== 5924035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinFUNCTION loc_eng_nmea_generate_sv 5934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5944035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDESCRIPTION 5954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin Generate NMEA sentences generated based on sv report 5964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 5974035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinDEPENDENCIES 5984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin NONE 5994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6004035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinRETURN VALUE 6014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 0 6024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6034035be28a255eaa5605dbd9abeb2340db584249cPatrick TjinSIDE EFFECTS 6044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin N/A 6054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin===========================================================================*/ 6074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjinvoid loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p, 6082e0c5f2f0d31e06a817de185fda694ba79f82ce0Dante Russo const QtiGnssSvStatus &svStatus, const GpsLocationExtended &locationExtended) 6094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin{ 6104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin ENTRY_LOG(); 6114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 6134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin char* pMarker = sentence; 6144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int lengthRemaining = sizeof(sentence); 6154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int length = 0; 6164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int svCount = svStatus.num_svs; 6174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int sentenceCount = 0; 6184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int sentenceNumber = 1; 6194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int svNumber = 1; 6204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int gpsCount = 0; 6214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin int glnCount = 0; 6224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin //Count GPS SVs for saparating GPS from GLONASS and throw others 6244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin for(svNumber=1; svNumber <= svCount; svNumber++) { 6264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START)&& 6274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) ) 6284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin gpsCount++; 6304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) && 6324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) ) 6334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin glnCount++; 6354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 6394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GPGSV------ 6404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 6414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (gpsCount <= 0) 6434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // no svs in view, so just send a blank $GPGSV sentence 6454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence)); 6464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 6474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 6484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 6504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin svNumber = 1; 6524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceNumber = 1; 6534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceCount = gpsCount/4 + (gpsCount % 4 != 0); 6544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin while (sentenceNumber <= sentenceCount) 6564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker = sentence; 6584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining = sizeof(sentence); 6594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d", 6614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceCount, sentenceNumber, gpsCount); 6624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 6644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 6664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 6674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 6694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 6704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++) 6724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START) && 6744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) ) 6754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 6774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin svStatus.sv_list[svNumber-1].prn, 6784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int 6794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int 6804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 6824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 6844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 6854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 6874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 6884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (svStatus.sv_list[svNumber-1].snr > 0) 6904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,"%02d", 6924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int 6934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 6944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 6954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 6964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 6974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 6984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 6994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 7004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 7014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin i++; 7044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 7094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 7104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceNumber++; 7114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } //while 7134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } //if 7154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7164035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 7174035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------$GLGSV------ 7184035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // ------------------ 7194035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7204035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (glnCount <= 0) 7214035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7224035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // no svs in view, so just send a blank $GLGSV sentence 7234035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin strlcpy(sentence, "$GLGSV,1,1,0,", sizeof(sentence)); 7244035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 7254035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 7264035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7274035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 7284035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7294035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin svNumber = 1; 7304035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceNumber = 1; 7314035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceCount = glnCount/4 + (glnCount % 4 != 0); 7324035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7334035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin while (sentenceNumber <= sentenceCount) 7344035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7354035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker = sentence; 7364035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining = sizeof(sentence); 7374035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7384035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining, "$GLGSV,%d,%d,%02d", 7394035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceCount, sentenceNumber, glnCount); 7404035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7414035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 7424035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7434035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 7444035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 7454035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7464035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 7474035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 7484035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7494035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++) 7504035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7514035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) && 7524035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) ) { 7534035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7544035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 7554035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin svStatus.sv_list[svNumber-1].prn, 7564035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int 7574035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int 7584035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7594035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 7604035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7614035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 7624035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 7634035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7644035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 7654035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 7664035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7674035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (svStatus.sv_list[svNumber-1].snr > 0) 7684035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7694035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = snprintf(pMarker, lengthRemaining,"%02d", 7704035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int 7714035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7724035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (length < 0 || length >= lengthRemaining) 7734035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 7744035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin LOC_LOGE("NMEA Error in string formatting"); 7754035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin return; 7764035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7774035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin pMarker += length; 7784035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin lengthRemaining -= length; 7794035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7804035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7814035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin i++; 7824035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7834035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7844035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 7854035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7864035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 7874035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_nmea_send(sentence, length, loc_eng_data_p); 7884035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin sentenceNumber++; 7894035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7904035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } //while 7914035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7924035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin }//if 7934035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7944035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // cache the used in fix mask, as it will be needed to send $GPGSA 7954035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // during the position report 7964035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->sv_used_mask = svStatus.gps_used_in_fix_mask; 7974035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 7984035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // For RPC, the DOP are sent during sv report, so cache them 7994035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // now to be sent during position report. 8004035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin // For QMI, the DOP will be in position report. 8014035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 8024035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 8034035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->pdop = locationExtended.pdop; 8044035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->hdop = locationExtended.hdop; 8054035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->vdop = locationExtended.vdop; 8064035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 8074035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin else 8084035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin { 8094035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->pdop = 0; 8104035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->hdop = 0; 8114035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin loc_eng_data_p->vdop = 0; 8124035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin } 8134035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin 8144035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin EXIT_LOG(%d, 0); 8154035be28a255eaa5605dbd9abeb2340db584249cPatrick Tjin} 816