1e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo/* Copyright (c) 2012, The Linux Foundation. All rights reserved. 2e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * 3e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * Redistribution and use in source and binary forms, with or without 4e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * modification, are permitted provided that the following conditions are 5e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * met: 6e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * * Redistributions of source code must retain the above copyright 7e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * notice, this list of conditions and the following disclaimer. 8e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * * Redistributions in binary form must reproduce the above 9e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * copyright notice, this list of conditions and the following 10e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * disclaimer in the documentation and/or other materials provided 11e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * with the distribution. 12e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * * Neither the name of The Linux Foundation nor the names of its 13e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * contributors may be used to endorse or promote products derived 14e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * from this software without specific prior written permission. 15e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * 16e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 17e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 19e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 20e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 23e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 24e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 25e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 26e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo * 28e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo */ 29e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 30e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#define LOG_NDDEBUG 0 31e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#define LOG_TAG "LocSvc_eng_nmea" 3247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo#define GPS_PRN_START 1 3347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo#define GPS_PRN_END 32 3447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo#define GLONASS_PRN_START 65 3547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo#define GLONASS_PRN_END 96 36e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#include <loc_eng.h> 37e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#include <loc_eng_nmea.h> 38e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#include <math.h> 39e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo#include "log_util.h" 40e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 41e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo/*=========================================================================== 42e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoFUNCTION loc_eng_nmea_send 43e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 44e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDESCRIPTION 45e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo send out NMEA sentence 46e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 47e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDEPENDENCIES 48e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo NONE 49e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 50e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoRETURN VALUE 51e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo Total length of the nmea sentence 52e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 53e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoSIDE EFFECTS 54e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo N/A 55e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 56e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo===========================================================================*/ 57e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russovoid loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p) 58e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo{ 59e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo struct timeval tv; 60e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gettimeofday(&tv, (struct timezone *) NULL); 61e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000; 62e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo CALLBACK_LOG_CALLFLOW("nmea_cb", %p, pNmea); 6347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (loc_eng_data_p->nmea_cb != NULL) 6447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo loc_eng_data_p->nmea_cb(now, pNmea, length); 65e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGD("NMEA <%s", pNmea); 66e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo} 67e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 68e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo/*=========================================================================== 69e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoFUNCTION loc_eng_nmea_put_checksum 70e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 71e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDESCRIPTION 72e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo Generate NMEA sentences generated based on position report 73e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 74e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDEPENDENCIES 75e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo NONE 76e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 77e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoRETURN VALUE 78e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo Total length of the nmea sentence 79e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 80e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoSIDE EFFECTS 81e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo N/A 82e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 83e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo===========================================================================*/ 84e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russoint loc_eng_nmea_put_checksum(char *pNmea, int maxSize) 85e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo{ 86e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo uint8_t checksum = 0; 87e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int length = 0; 88e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 89e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pNmea++; //skip the $ 90e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo while (*pNmea != '\0') 91e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 92e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo checksum ^= *pNmea++; 93e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length++; 94e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 95e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 96e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum); 97e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return (length + checksumLength); 98e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo} 99e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 100e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo/*=========================================================================== 101e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoFUNCTION loc_eng_nmea_generate_pos 102e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 103e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDESCRIPTION 104e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo Generate NMEA sentences generated based on position report 105e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 106e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDEPENDENCIES 107e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo NONE 108e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 109e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoRETURN VALUE 110e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 0 111e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 112e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoSIDE EFFECTS 113e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo N/A 114e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 115e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo===========================================================================*/ 116e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russovoid loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p, 117e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo const UlpLocation &location, 118e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo const GpsLocationExtended &locationExtended, 119e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo unsigned char generate_nmea) 120e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo{ 121e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo ENTRY_LOG(); 122bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo time_t utcTime(location.gpsLocation.timestamp/1000); 123bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo tm * pTm = gmtime(&utcTime); 124bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo if (NULL == pTm) { 125bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo LOC_LOGE("gmtime failed"); 126bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo return; 127bfff6343845ad9ff062c5fd97bb3b9be1053340eDante Russo } 128e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 129e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 130e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char* pMarker = sentence; 131e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int lengthRemaining = sizeof(sentence); 132e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int length = 0; 133e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcYear = pTm->tm_year % 100; // 2 digit year 134e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero 135e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcDay = pTm->tm_mday; 136e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcHours = pTm->tm_hour; 137e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcMinutes = pTm->tm_min; 138e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int utcSeconds = pTm->tm_sec; 139e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 140e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (generate_nmea) { 141e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 142e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------$GPGSA------ 143e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 144e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 145e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo uint32_t svUsedCount = 0; 146e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo uint32_t svUsedList[32] = {0}; 147e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo uint32_t mask = loc_eng_data_p->sv_used_mask; 148e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) 149e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 150e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (mask & 1) 151e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo svUsedList[svUsedCount++] = i; 152e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo mask = mask >> 1; 153e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 154e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // clear the cache so they can't be used again 155e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->sv_used_mask = 0; 156e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 157e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char fixType; 158e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (svUsedCount == 0) 159e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo fixType = '1'; // no fix 160e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (svUsedCount <= 3) 161e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo fixType = '2'; // 2D fix 162e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 163e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo fixType = '3'; // 3D fix 164e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 165e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType); 166e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 167e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 168e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 169e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 170e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 171e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 172e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 173e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 174e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 175e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence 176e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 177e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (i < svUsedCount) 178e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); 179e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 180e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ","); 181e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 182e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 183e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 184e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 185e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 186e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 187e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 188e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 189e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 190e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 191e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 192e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // dop is in locationExtended, (QMI) 193e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 194e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo locationExtended.pdop, 195e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo locationExtended.hdop, 196e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo locationExtended.vdop); 197e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 198e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 199e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // dop was cached from sv report (RPC) 200e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 201e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->pdop, 202e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->hdop, 203e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->vdop); 204e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 205e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 206e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // no dop 207e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ",,"); 208e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 209e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 210e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 211e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 212e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 213e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 214e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------$GPVTG------ 215e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 216e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 217e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker = sentence; 218e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining = sizeof(sentence); 219e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 220e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 221e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 222e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float magTrack = location.gpsLocation.bearing; 223e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 224e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 225e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; 226e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (magTrack < 0.0) 227e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo magTrack += 360.0; 228e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (magTrack > 360.0) 229e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo magTrack -= 360.0; 230e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 231e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 232e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPVTG,%.1lf,T,%.1lf,M,", location.gpsLocation.bearing, magTrack); 233e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 234e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 235e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 236e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,"); 237e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 238e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 239e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 240e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 241e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 242e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 243e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 244e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 245e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 246e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 247e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 248e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 249e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 250e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float speedKmPerHour = location.gpsLocation.speed * 3.6; 251e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 252e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); 253e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 254e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 255e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 256e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ",N,,K,"); 257e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 258e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 259e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 260e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 261e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 262e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 263e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 264e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 265e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 266e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 267e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 268e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 269e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 270e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 271e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 272e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 273e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 274e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 275e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 276e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 277e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 278e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------$GPRMC------ 279e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 280e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 281e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker = sentence; 282e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining = sizeof(sentence); 283e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 284e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d,A," , 285e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo utcHours, utcMinutes, utcSeconds); 286e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 287e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 288e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 289e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 290e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 291e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 292e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 293e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 294e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 295e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 296e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 297e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double latitude = location.gpsLocation.latitude; 298e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double longitude = location.gpsLocation.longitude; 299e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char latHemisphere; 300e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char lonHemisphere; 301e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double latMinutes; 302e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double lonMinutes; 303e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 304e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (latitude > 0) 305e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 306e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latHemisphere = 'N'; 307e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 308e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 309e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 310e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latHemisphere = 'S'; 311e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latitude *= -1.0; 312e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 313e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 314e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (longitude < 0) 315e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 316e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonHemisphere = 'W'; 317e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo longitude *= -1.0; 318e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 319e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 320e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 321e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonHemisphere = 'E'; 322e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 323e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 324e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latMinutes = fmod(latitude * 60.0 , 60.0); 325e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonMinutes = fmod(longitude * 60.0 , 60.0); 326e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 327e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 328e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (uint8_t)floor(latitude), latMinutes, latHemisphere, 329e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 330e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 331e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 332e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 333e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining,",,,,"); 334e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 335e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 336e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 337e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 338e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 339e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 340e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 341e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 342e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 343e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 344e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 345e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 346e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 347e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); 348e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 349e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 350e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 351e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ","); 352e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 353e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 354e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 355e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 356e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 357e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 358e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 359e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 360e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 361e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 362e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 363e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 364e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing); 365e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 366e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 367e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 368e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ","); 369e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 370e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 371e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 372e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 373e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 374e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 375e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 376e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 377e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 378e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 379e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", 380e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo utcDay, utcMonth, utcYear); 381e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 382e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 383e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 384e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 385e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 386e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 387e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 388e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 389e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 390e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 391e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 392e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo float magneticVariation = locationExtended.magneticDeviation; 393e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char direction; 394e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (magneticVariation < 0.0) 395e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 396e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo direction = 'W'; 397e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo magneticVariation *= -1.0; 398e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 399e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 400e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 401e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo direction = 'E'; 402e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 403e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 404e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", 405e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo magneticVariation, direction); 406e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 407e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 408e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 409e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, ",,"); 410e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 411e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 412e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 413e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 414e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 415e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 416e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 417e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 418e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 419e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 420e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 421e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 422e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 423e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 424e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 425e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 426e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 427e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 428e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 429e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 430e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 431e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------$GPGGA------ 432e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 433e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 434e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker = sentence; 435e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining = sizeof(sentence); 436e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 437e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d," , 438e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo utcHours, utcMinutes, utcSeconds); 439e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 440e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 441e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 442e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 443e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 444e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 445e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 446e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 447e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 448e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 449e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 450e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double latitude = location.gpsLocation.latitude; 451e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double longitude = location.gpsLocation.longitude; 452e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char latHemisphere; 453e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char lonHemisphere; 454e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double latMinutes; 455e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo double lonMinutes; 456e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 457e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (latitude > 0) 458e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 459e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latHemisphere = 'N'; 460e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 461e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 462e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 463e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latHemisphere = 'S'; 464e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latitude *= -1.0; 465e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 466e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 467e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (longitude < 0) 468e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 469e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonHemisphere = 'W'; 470e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo longitude *= -1.0; 471e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 472e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 473e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 474e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonHemisphere = 'E'; 475e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 476e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 477e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo latMinutes = fmod(latitude * 60.0 , 60.0); 478e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lonMinutes = fmod(longitude * 60.0 , 60.0); 479e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 480e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 481e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (uint8_t)floor(latitude), latMinutes, latHemisphere, 482e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 483e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 484e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 485e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 486e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining,",,,,"); 487e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 488e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 489e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 490e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 491e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 492e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 493e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 494e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 495e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 496e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 497e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char gpsQuality; 498e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 499e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality = '0'; // 0 means no fix 500e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 501e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality = '1'; // 1 means GPS fix 502e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 503e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality = '2'; // 2 means DGPS fix 504e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 505e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 506e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // dop is in locationExtended, (QMI) 507e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 508e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality, svUsedCount, locationExtended.hdop); 509e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 510e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 511e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // dop was cached from sv report (RPC) 512e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 513e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality, svUsedCount, loc_eng_data_p->hdop); 514e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 515e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 516e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // no hdop 517e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", 518e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo gpsQuality, svUsedCount); 519e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 520e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 521e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 522e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 523e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 524e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 525e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 526e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 527e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 528e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 529e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) 530e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 531e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", 532e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo locationExtended.altitudeMeanSeaLevel); 533e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 534e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 535e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 536e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining,",,"); 537e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 538e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 539e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 540e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 541e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 542e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 543e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 544e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 545e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 546e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 547e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if ((location.gpsLocation.flags & GPS_LOCATION_HAS_ALTITUDE) && 548e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) 549e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 550e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", 551e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); 552e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 553e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 554e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 555e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining,",,,"); 556e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 557e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 558e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 559e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 560e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 561e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 562e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo //Send blank NMEA reports for non-final fixes 563e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else { 564e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 565e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 566e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 567e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 568e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 569e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 570e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 571e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 572e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 573e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 574e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 575e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 576e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 577e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 578e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 579e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 580e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // clear the dop cache so they can't be used again 581e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->pdop = 0; 582e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->hdop = 0; 583e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->vdop = 0; 584e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 585e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo EXIT_LOG(%d, 0); 586e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo} 587e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 588e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 589e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 590e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo/*=========================================================================== 591e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoFUNCTION loc_eng_nmea_generate_sv 592e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 593e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDESCRIPTION 594e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo Generate NMEA sentences generated based on sv report 595e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 596e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoDEPENDENCIES 597e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo NONE 598e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 599e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoRETURN VALUE 600e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 0 601e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 602e14a6c846df2ce4bb1847e4250991f7c52fd793dDante RussoSIDE EFFECTS 603e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo N/A 604e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 605e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo===========================================================================*/ 606e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russovoid loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p, 607e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo const GpsSvStatus &svStatus, const GpsLocationExtended &locationExtended) 608e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo{ 609e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo ENTRY_LOG(); 610e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 611e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 612e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo char* pMarker = sentence; 613e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int lengthRemaining = sizeof(sentence); 614e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo int length = 0; 61547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int svCount = svStatus.num_svs; 61647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int sentenceCount = 0; 61747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int sentenceNumber = 1; 61847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int svNumber = 1; 61947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int gpsCount = 0; 62047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo int glnCount = 0; 62147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 62247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo //Count GPS SVs for saparating GPS from GLONASS and throw others 62347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 62447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo for(svNumber=1; svNumber <= svCount; svNumber++) { 62547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START)&& 62647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) ) 62747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 62847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo gpsCount++; 62947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 63047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo else if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) && 63147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) ) 63247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 63347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo glnCount++; 63447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 63547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 636e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 637e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 638e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------$GPGSV------ 639e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // ------------------ 640e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 64147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (gpsCount <= 0) 642e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 643e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // no svs in view, so just send a blank $GPGSV sentence 644e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence)); 645e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 646e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 647e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 648e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 649e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 65047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo svNumber = 1; 65147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceNumber = 1; 65247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceCount = gpsCount/4 + (gpsCount % 4 != 0); 653e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 654e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo while (sentenceNumber <= sentenceCount) 655e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 656e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker = sentence; 657e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining = sizeof(sentence); 658e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 659e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d", 66047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceCount, sentenceNumber, gpsCount); 661e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 662e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 663e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 664e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 665e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 666e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 667e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 668e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 669e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 67047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++) 671e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 67247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if( (svStatus.sv_list[svNumber-1].prn >= GPS_PRN_START) && 67347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (svStatus.sv_list[svNumber-1].prn <= GPS_PRN_END) ) 67447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 67547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 676e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo svStatus.sv_list[svNumber-1].prn, 677e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int 678e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int 679e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 68047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (length < 0 || length >= lengthRemaining) 68147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 68247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo LOC_LOGE("NMEA Error in string formatting"); 68347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo return; 68447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 68547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo pMarker += length; 68647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo lengthRemaining -= length; 6878c61f8c1f1572ab769912264af3f31feb041fe79Dante Russo 68847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (svStatus.sv_list[svNumber-1].snr > 0) 68947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 69047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = snprintf(pMarker, lengthRemaining,"%02d", 69147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int 69247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 69347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (length < 0 || length >= lengthRemaining) 69447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 69547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo LOC_LOGE("NMEA Error in string formatting"); 69647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo return; 69747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 69847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo pMarker += length; 69947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo lengthRemaining -= length; 70047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 70147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 70247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo i++; 70347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 70447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 70547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 70647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 70747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 70847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 70947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceNumber++; 71047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 71147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } //while 71247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 71347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } //if 71447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 71547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo // ------------------ 71647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo // ------$GLGSV------ 71747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo // ------------------ 71847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 71947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (glnCount <= 0) 72047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 72147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo // no svs in view, so just send a blank $GLGSV sentence 72247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo strlcpy(sentence, "$GLGSV,1,1,0,", sizeof(sentence)); 72347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 72447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 72547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 72647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo else 72747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 72847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo svNumber = 1; 72947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceNumber = 1; 73047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceCount = glnCount/4 + (glnCount % 4 != 0); 73147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 73247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo while (sentenceNumber <= sentenceCount) 73347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 73447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo pMarker = sentence; 73547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo lengthRemaining = sizeof(sentence); 73647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 73747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = snprintf(pMarker, lengthRemaining, "$GLGSV,%d,%d,%02d", 73847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo sentenceCount, sentenceNumber, glnCount); 73947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 74047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (length < 0 || length >= lengthRemaining) 74147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 74247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo LOC_LOGE("NMEA Error in string formatting"); 74347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo return; 74447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 74547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo pMarker += length; 74647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo lengthRemaining -= length; 74747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 74847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo for (int i=0; (svNumber <= svCount) && (i < 4); svNumber++) 74947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 75047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if( (svStatus.sv_list[svNumber-1].prn >= GLONASS_PRN_START) && 75147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (svStatus.sv_list[svNumber-1].prn <= GLONASS_PRN_END) ) { 75247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 75347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 75447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo svStatus.sv_list[svNumber-1].prn, 75547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int 75647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int 757e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 758e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (length < 0 || length >= lengthRemaining) 759e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 760e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo LOC_LOGE("NMEA Error in string formatting"); 761e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo return; 762e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 763e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo pMarker += length; 764e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo lengthRemaining -= length; 76547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 76647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (svStatus.sv_list[svNumber-1].snr > 0) 76747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 76847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo length = snprintf(pMarker, lengthRemaining,"%02d", 76947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int 77047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 77147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo if (length < 0 || length >= lengthRemaining) 77247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo { 77347ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo LOC_LOGE("NMEA Error in string formatting"); 77447ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo return; 77547ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 77647ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo pMarker += length; 77747ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo lengthRemaining -= length; 77847ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 77947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 78047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo i++; 78147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } 78247ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 783e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 784e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 785e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 786e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 787e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo sentenceNumber++; 788e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 78947ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo } //while 79047ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo 79147ad5e4cf2f6810db3c0e7ec79696496a94b6f0dDante Russo }//if 792e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 793e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (svStatus.used_in_fix_mask == 0) 794e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // No sv used, so there will be no position report, so send 795e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // blank NMEA sentences 796e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 797e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 798e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 799e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 800e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 801e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 802e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 803e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 804e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 805e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 806e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 807e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 808e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 809e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 810e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 811e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 812e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 813e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { // cache the used in fix mask, as it will be needed to send $GPGSA 814e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // during the position report 815e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->sv_used_mask = svStatus.used_in_fix_mask; 816e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 817e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // For RPC, the DOP are sent during sv report, so cache them 818e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // now to be sent during position report. 819e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo // For QMI, the DOP will be in position report. 820e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 821e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 822e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->pdop = locationExtended.pdop; 823e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->hdop = locationExtended.hdop; 824e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->vdop = locationExtended.vdop; 825e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 826e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo else 827e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo { 828e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->pdop = 0; 829e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->hdop = 0; 830e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo loc_eng_data_p->vdop = 0; 831e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 832e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 833e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo } 834e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo 835e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo EXIT_LOG(%d, 0); 836e14a6c846df2ce4bb1847e4250991f7c52fd793dDante Russo} 837