1ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo/* Copyright (c) 2012, The Linux Foundation. All rights reserved. 2ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * 3ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * Redistribution and use in source and binary forms, with or without 4ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * modification, are permitted provided that the following conditions are 5ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * met: 6ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * * Redistributions of source code must retain the above copyright 7ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * notice, this list of conditions and the following disclaimer. 8ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * * Redistributions in binary form must reproduce the above 9ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * copyright notice, this list of conditions and the following 10ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * disclaimer in the documentation and/or other materials provided 11ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * with the distribution. 12ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * * Neither the name of The Linux Foundation nor the names of its 13ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * contributors may be used to endorse or promote products derived 14ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * from this software without specific prior written permission. 15ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * 16ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 17ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 19ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 20ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 23ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 24ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 25ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 26ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo * 28ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo */ 29ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 30ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#define LOG_NDDEBUG 0 31ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#define LOG_TAG "LocSvc_eng_nmea" 32ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 33ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#include <loc_eng.h> 34ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#include <loc_eng_nmea.h> 35ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#include <math.h> 36ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo#include "log_util.h" 37ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 38ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo/*=========================================================================== 39ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoFUNCTION loc_eng_nmea_send 40ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 41ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDESCRIPTION 42ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo send out NMEA sentence 43ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 44ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDEPENDENCIES 45ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo NONE 46ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 47ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoRETURN VALUE 48ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo Total length of the nmea sentence 49ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 50ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoSIDE EFFECTS 51ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo N/A 52ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 53ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo===========================================================================*/ 54ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russovoid loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p) 55ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo{ 56ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo struct timeval tv; 57ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gettimeofday(&tv, (struct timezone *) NULL); 58ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000; 59ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo CALLBACK_LOG_CALLFLOW("nmea_cb", %p, pNmea); 60ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->nmea_cb(now, pNmea, length); 61ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGD("NMEA <%s", pNmea); 62ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo} 63ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 64ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo/*=========================================================================== 65ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoFUNCTION loc_eng_nmea_put_checksum 66ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 67ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDESCRIPTION 68ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo Generate NMEA sentences generated based on position report 69ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 70ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDEPENDENCIES 71ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo NONE 72ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 73ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoRETURN VALUE 74ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo Total length of the nmea sentence 75ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 76ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoSIDE EFFECTS 77ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo N/A 78ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 79ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo===========================================================================*/ 80ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russoint loc_eng_nmea_put_checksum(char *pNmea, int maxSize) 81ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo{ 82ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo uint8_t checksum = 0; 83ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int length = 0; 84ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 85ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pNmea++; //skip the $ 86ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo while (*pNmea != '\0') 87ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 88ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo checksum ^= *pNmea++; 89ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length++; 90ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 91ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 92ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum); 93ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return (length + checksumLength); 94ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo} 95ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 96ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo/*=========================================================================== 97ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoFUNCTION loc_eng_nmea_generate_pos 98ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 99ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDESCRIPTION 100ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo Generate NMEA sentences generated based on position report 101ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 102ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDEPENDENCIES 103ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo NONE 104ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 105ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoRETURN VALUE 106ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 0 107ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 108ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoSIDE EFFECTS 109ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo N/A 110ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 111ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo===========================================================================*/ 112ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russovoid loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p, 113ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo const UlpLocation &location, 114ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo const GpsLocationExtended &locationExtended, 115ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo unsigned char generate_nmea) 116ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo{ 117ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo ENTRY_LOG(); 118ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 119ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 120ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char* pMarker = sentence; 121ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int lengthRemaining = sizeof(sentence); 122ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int length = 0; 123ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 124ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo time_t utcTime(location.gpsLocation.timestamp/1000); 125ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo tm * pTm = gmtime(&utcTime); 126ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcYear = pTm->tm_year % 100; // 2 digit year 127ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero 128ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcDay = pTm->tm_mday; 129ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcHours = pTm->tm_hour; 130ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcMinutes = pTm->tm_min; 131ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int utcSeconds = pTm->tm_sec; 132ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 133ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (generate_nmea) { 134ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 135ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------$GPGSA------ 136ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 137ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 138ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo uint32_t svUsedCount = 0; 139ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo uint32_t svUsedList[32] = {0}; 140ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo uint32_t mask = loc_eng_data_p->sv_used_mask; 141ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) 142ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 143ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (mask & 1) 144ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo svUsedList[svUsedCount++] = i; 145ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo mask = mask >> 1; 146ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 147ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // clear the cache so they can't be used again 148ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->sv_used_mask = 0; 149ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 150ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char fixType; 151ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (svUsedCount == 0) 152ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo fixType = '1'; // no fix 153ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (svUsedCount <= 3) 154ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo fixType = '2'; // 2D fix 155ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 156ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo fixType = '3'; // 3D fix 157ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 158ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType); 159ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 160ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 161ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 162ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 163ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 164ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 165ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 166ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 167ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 168ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence 169ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 170ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (i < svUsedCount) 171ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); 172ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 173ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ","); 174ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 175ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 176ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 177ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 178ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 179ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 180ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 181ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 182ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 183ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 184ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 185ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // dop is in locationExtended, (QMI) 186ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 187ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo locationExtended.pdop, 188ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo locationExtended.hdop, 189ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo locationExtended.vdop); 190ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 191ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 192ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // dop was cached from sv report (RPC) 193ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", 194ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->pdop, 195ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->hdop, 196ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->vdop); 197ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 198ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 199ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // no dop 200ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ",,"); 201ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 202ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 203ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 204ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 205ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 206ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 207ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------$GPVTG------ 208ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 209ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 210ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker = sentence; 211ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining = sizeof(sentence); 212ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 213ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 214ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 215ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float magTrack = location.gpsLocation.bearing; 216ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 217ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 218ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation; 219ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (magTrack < 0.0) 220ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo magTrack += 360.0; 221ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (magTrack > 360.0) 222ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo magTrack -= 360.0; 223ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 224ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 225ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPVTG,%.1lf,T,%.1lf,M,", location.gpsLocation.bearing, magTrack); 226ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 227ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 228ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 229ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,"); 230ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 231ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 232ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 233ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 234ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 235ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 236ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 237ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 238ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 239ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 240ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 241ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 242ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 243ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float speedKmPerHour = location.gpsLocation.speed * 3.6; 244ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 245ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); 246ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 247ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 248ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 249ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ",N,,K,"); 250ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 251ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 252ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 253ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 254ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 255ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 256ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 257ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 258ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 259ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 260ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 261ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 262ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 263ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 264ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 265ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 266ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 267ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 268ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 269ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 270ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 271ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------$GPRMC------ 272ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 273ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 274ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker = sentence; 275ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining = sizeof(sentence); 276ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 277ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d,A," , 278ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo utcHours, utcMinutes, utcSeconds); 279ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 280ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 281ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 282ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 283ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 284ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 285ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 286ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 287ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 288ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 289ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 290ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double latitude = location.gpsLocation.latitude; 291ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double longitude = location.gpsLocation.longitude; 292ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char latHemisphere; 293ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char lonHemisphere; 294ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double latMinutes; 295ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double lonMinutes; 296ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 297ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (latitude > 0) 298ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 299ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latHemisphere = 'N'; 300ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 301ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 302ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 303ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latHemisphere = 'S'; 304ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latitude *= -1.0; 305ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 306ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 307ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (longitude < 0) 308ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 309ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonHemisphere = 'W'; 310ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo longitude *= -1.0; 311ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 312ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 313ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 314ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonHemisphere = 'E'; 315ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 316ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 317ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latMinutes = fmod(latitude * 60.0 , 60.0); 318ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonMinutes = fmod(longitude * 60.0 , 60.0); 319ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 320ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 321ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (uint8_t)floor(latitude), latMinutes, latHemisphere, 322ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 323ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 324ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 325ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 326ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,",,,,"); 327ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 328ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 329ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 330ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 331ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 332ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 333ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 334ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 335ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 336ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 337ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_SPEED) 338ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 339ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float speedKnots = location.gpsLocation.speed * (3600.0/1852.0); 340ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); 341ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 342ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 343ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 344ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ","); 345ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 346ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 347ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 348ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 349ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 350ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 351ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 352ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 353ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 354ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 355ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_BEARING) 356ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 357ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing); 358ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 359ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 360ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 361ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ","); 362ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 363ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 364ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 365ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 366ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 367ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 368ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 369ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 370ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 371ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 372ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", 373ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo utcDay, utcMonth, utcYear); 374ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 375ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 376ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 377ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 378ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 379ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 380ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 381ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 382ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 383ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) 384ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 385ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo float magneticVariation = locationExtended.magneticDeviation; 386ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char direction; 387ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (magneticVariation < 0.0) 388ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 389ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo direction = 'W'; 390ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo magneticVariation *= -1.0; 391ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 392ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 393ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 394ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo direction = 'E'; 395ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 396ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 397ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", 398ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo magneticVariation, direction); 399ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 400ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 401ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 402ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, ",,"); 403ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 404ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 405ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 406ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 407ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 408ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 409ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 410ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 411ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 412ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 413ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 414ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix 415ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 416ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous 417ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 418ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential 419ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 420ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 421ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 422ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 423ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 424ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------$GPGGA------ 425ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 426ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 427ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker = sentence; 428ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining = sizeof(sentence); 429ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 430ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d," , 431ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo utcHours, utcMinutes, utcSeconds); 432ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 433ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 434ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 435ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 436ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 437ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 438ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 439ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 440ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 441ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG) 442ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 443ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double latitude = location.gpsLocation.latitude; 444ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double longitude = location.gpsLocation.longitude; 445ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char latHemisphere; 446ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char lonHemisphere; 447ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double latMinutes; 448ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo double lonMinutes; 449ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 450ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (latitude > 0) 451ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 452ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latHemisphere = 'N'; 453ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 454ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 455ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 456ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latHemisphere = 'S'; 457ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latitude *= -1.0; 458ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 459ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 460ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (longitude < 0) 461ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 462ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonHemisphere = 'W'; 463ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo longitude *= -1.0; 464ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 465ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 466ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 467ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonHemisphere = 'E'; 468ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 469ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 470ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo latMinutes = fmod(latitude * 60.0 , 60.0); 471ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lonMinutes = fmod(longitude * 60.0 , 60.0); 472ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 473ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", 474ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (uint8_t)floor(latitude), latMinutes, latHemisphere, 475ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (uint8_t)floor(longitude),lonMinutes, lonHemisphere); 476ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 477ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 478ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 479ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,",,,,"); 480ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 481ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 482ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 483ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 484ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 485ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 486ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 487ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 488ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 489ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 490ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char gpsQuality; 491ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (!(location.gpsLocation.flags & GPS_LOCATION_HAS_LAT_LONG)) 492ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality = '0'; // 0 means no fix 493ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->adapter->getPositionMode().mode) 494ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality = '1'; // 1 means GPS fix 495ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 496ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality = '2'; // 2 means DGPS fix 497ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 498ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 499ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // dop is in locationExtended, (QMI) 500ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 501ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality, svUsedCount, locationExtended.hdop); 502ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 503ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) 504ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // dop was cached from sv report (RPC) 505ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", 506ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality, svUsedCount, loc_eng_data_p->hdop); 507ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 508ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 509ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // no hdop 510ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", 511ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo gpsQuality, svUsedCount); 512ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 513ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 514ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 515ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 516ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 517ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 518ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 519ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 520ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 521ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 522ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) 523ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 524ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", 525ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo locationExtended.altitudeMeanSeaLevel); 526ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 527ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 528ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 529ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,",,"); 530ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 531ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 532ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 533ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 534ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 535ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 536ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 537ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 538ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 539ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 540ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if ((location.gpsLocation.flags & GPS_LOCATION_HAS_ALTITUDE) && 541ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) 542ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 543ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", 544ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); 545ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 546ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 547ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 548ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,",,,"); 549ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 550ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 551ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 552ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 553ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 554ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 555ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo //Send blank NMEA reports for non-final fixes 556ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else { 557ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 558ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 559ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 560ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 561ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 562ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 563ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 564ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 565ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 566ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 567ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 568ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 569ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 570ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 571ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 572ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 573ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // clear the dop cache so they can't be used again 574ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->pdop = 0; 575ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->hdop = 0; 576ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->vdop = 0; 577ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 578ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo EXIT_LOG(%d, 0); 579ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo} 580ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 581ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 582ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 583ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo/*=========================================================================== 584ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoFUNCTION loc_eng_nmea_generate_sv 585ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 586ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDESCRIPTION 587ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo Generate NMEA sentences generated based on sv report 588ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 589ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoDEPENDENCIES 590ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo NONE 591ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 592ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoRETURN VALUE 593ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 0 594ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 595ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante RussoSIDE EFFECTS 596ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo N/A 597ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 598ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo===========================================================================*/ 599ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russovoid loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p, 600ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo const GpsSvStatus &svStatus, const GpsLocationExtended &locationExtended) 601ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo{ 602ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo ENTRY_LOG(); 603ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 604ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 605ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo char* pMarker = sentence; 606ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int lengthRemaining = sizeof(sentence); 607ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int length = 0; 608ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 609ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 610ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------$GPGSV------ 611ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // ------------------ 612ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 613ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (svStatus.num_svs <= 0) 614ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 615ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // no svs in view, so just send a blank $GPGSV sentence 616ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence)); 617ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 618ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 619ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 620ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 621ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 622ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int svCount = svStatus.num_svs; 623ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int sentenceCount = svCount / 4; 624ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (svStatus.num_svs % 4) 625ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo sentenceCount++; 626ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int sentenceNumber = 1; 627ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo int svNumber = 1; 628ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 629ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo while (sentenceNumber <= sentenceCount) 630ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 631ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker = sentence; 632ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining = sizeof(sentence); 633ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 634ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d", 635ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo sentenceCount, sentenceNumber, svCount); 636ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 637ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 638ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 639ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 640ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 641ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 642ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 643ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 644ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 645ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo for (int i=0; (svNumber <= svCount) && (i < 4); i++, svNumber++) 646ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 647ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", 648ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo svStatus.sv_list[svNumber-1].prn, 649ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int 650ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int 651ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 652ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 653ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 654ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 655ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 656ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 657ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 658ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 659ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 660ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (svStatus.sv_list[svNumber-1].snr > 0) 661ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 662ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = snprintf(pMarker, lengthRemaining,"%02d", 663ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int 664ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 665ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (length < 0 || length >= lengthRemaining) 666ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 667ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo LOC_LOGE("NMEA Error in string formatting"); 668ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo return; 669ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 670ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo pMarker += length; 671ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo lengthRemaining -= length; 672ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 673ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 674ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 675ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 676ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 677ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo sentenceNumber++; 678ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 679ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 680ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 681ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 682ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (svStatus.used_in_fix_mask == 0) 683ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // No sv used, so there will be no position report, so send 684ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // blank NMEA sentences 685ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); 686ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 687ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 688ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 689ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); 690ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 691ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 692ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 693ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); 694ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 695ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 696ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 697ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); 698ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); 699ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_nmea_send(sentence, length, loc_eng_data_p); 700ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 701ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 702ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { // cache the used in fix mask, as it will be needed to send $GPGSA 703ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // during the position report 704ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->sv_used_mask = svStatus.used_in_fix_mask; 705ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 706ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // For RPC, the DOP are sent during sv report, so cache them 707ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // now to be sent during position report. 708ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo // For QMI, the DOP will be in position report. 709ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) 710ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 711ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->pdop = locationExtended.pdop; 712ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->hdop = locationExtended.hdop; 713ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->vdop = locationExtended.vdop; 714ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 715ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo else 716ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo { 717ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->pdop = 0; 718ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->hdop = 0; 719ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo loc_eng_data_p->vdop = 0; 720ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 721ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 722ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo } 723ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo 724ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo EXIT_LOG(%d, 0); 725ec6e5d3a2597d37d5b1d98911cb06218cdf19bf1Dante Russo} 726