diff options
Diffstat (limited to 'gps/libloc_api_50001/loc_eng_nmea.cpp')
| -rw-r--r-- | gps/libloc_api_50001/loc_eng_nmea.cpp | 703 |
1 files changed, 703 insertions, 0 deletions
diff --git a/gps/libloc_api_50001/loc_eng_nmea.cpp b/gps/libloc_api_50001/loc_eng_nmea.cpp new file mode 100644 index 0000000..d2507a5 --- /dev/null +++ b/gps/libloc_api_50001/loc_eng_nmea.cpp @@ -0,0 +1,703 @@ +/* Copyright (c) 2012, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#define LOG_NDDEBUG 0 +#define LOG_TAG "LocSvc_eng_nmea" + +#include <loc_eng.h> +#include <loc_eng_nmea.h> +#include <math.h> +#include "log_util.h" + +/*=========================================================================== +FUNCTION loc_eng_nmea_send + +DESCRIPTION + send out NMEA sentence + +DEPENDENCIES + NONE + +RETURN VALUE + Total length of the nmea sentence + +SIDE EFFECTS + N/A + +===========================================================================*/ +void loc_eng_nmea_send(char *pNmea, int length, loc_eng_data_s_type *loc_eng_data_p) +{ + struct timeval tv; + gettimeofday(&tv, (struct timezone *) NULL); + int64_t now = tv.tv_sec * 1000LL + tv.tv_usec / 1000; + CALLBACK_LOG_CALLFLOW("nmea_cb", %p, pNmea); + loc_eng_data_p->nmea_cb(now, pNmea, length); + LOC_LOGD("NMEA <%s", pNmea); +} + +/*=========================================================================== +FUNCTION loc_eng_nmea_put_checksum + +DESCRIPTION + Generate NMEA sentences generated based on position report + +DEPENDENCIES + NONE + +RETURN VALUE + Total length of the nmea sentence + +SIDE EFFECTS + N/A + +===========================================================================*/ +int loc_eng_nmea_put_checksum(char *pNmea, int maxSize) +{ + uint8_t checksum = 0; + int length = 0; + + pNmea++; //skip the $ + while (*pNmea != '\0') + { + checksum ^= *pNmea++; + length++; + } + + int checksumLength = snprintf(pNmea, maxSize,"*%02X\r\n", checksum); + return (length + checksumLength); +} + +/*=========================================================================== +FUNCTION loc_eng_nmea_generate_pos + +DESCRIPTION + Generate NMEA sentences generated based on position report + +DEPENDENCIES + NONE + +RETURN VALUE + 0 + +SIDE EFFECTS + N/A + +===========================================================================*/ +void loc_eng_nmea_generate_pos(loc_eng_data_s_type *loc_eng_data_p, + const GpsLocation &location, const GpsLocationExtended &locationExtended) +{ + ENTRY_LOG(); + + char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char* pMarker = sentence; + int lengthRemaining = sizeof(sentence); + int length = 0; + + time_t utcTime(location.timestamp/1000); + tm * pTm = gmtime(&utcTime); + int utcYear = pTm->tm_year % 100; // 2 digit year + int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero + int utcDay = pTm->tm_mday; + int utcHours = pTm->tm_hour; + int utcMinutes = pTm->tm_min; + int utcSeconds = pTm->tm_sec; + + // ------------------ + // ------$GPGSA------ + // ------------------ + + uint32_t svUsedCount = 0; + uint32_t svUsedList[32] = {0}; + uint32_t mask = loc_eng_data_p->sv_used_mask; + for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) + { + if (mask & 1) + svUsedList[svUsedCount++] = i; + mask = mask >> 1; + } + // clear the cache so they can't be used again + loc_eng_data_p->sv_used_mask = 0; + + char fixType; + if (svUsedCount == 0) + fixType = '1'; // no fix + else if (svUsedCount <= 3) + fixType = '2'; // 2D fix + else + fixType = '3'; // 3D fix + + length = snprintf(pMarker, lengthRemaining, "$GPGSA,A,%c,", fixType); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + for (uint8_t i = 0; i < 12; i++) // only the first 12 sv go in sentence + { + if (i < svUsedCount) + length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]); + else + length = snprintf(pMarker, lengthRemaining, ","); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + } + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) + { // dop is in locationExtended, (QMI) + length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", + locationExtended.pdop, + locationExtended.hdop, + locationExtended.vdop); + } + else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) + { // dop was cached from sv report (RPC) + length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f", + loc_eng_data_p->pdop, + loc_eng_data_p->hdop, + loc_eng_data_p->vdop); + } + else + { // no dop + length = snprintf(pMarker, lengthRemaining, ",,"); + } + + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + // ------------------ + // ------$GPVTG------ + // ------------------ + + pMarker = sentence; + lengthRemaining = sizeof(sentence); + + if (location.flags & GPS_LOCATION_HAS_BEARING) + { + float magTrack = location.bearing; + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) + { + float magTrack = location.bearing - locationExtended.magneticDeviation; + if (magTrack < 0.0) + magTrack += 360.0; + else if (magTrack > 360.0) + magTrack -= 360.0; + } + + length = snprintf(pMarker, lengthRemaining, "$GPVTG,%.1lf,T,%.1lf,M,", location.bearing, magTrack); + } + else + { + length = snprintf(pMarker, lengthRemaining, "$GPVTG,,T,,M,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.flags & GPS_LOCATION_HAS_SPEED) + { + float speedKnots = location.speed * (3600.0/1852.0); + float speedKmPerHour = location.speed * 3.6; + + length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour); + } + else + { + length = snprintf(pMarker, lengthRemaining, ",N,,K,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (!(location.flags & GPS_LOCATION_HAS_LAT_LONG)) + length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix + else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->client_handle->getPositionMode().mode) + length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous + else + length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential + + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + // ------------------ + // ------$GPRMC------ + // ------------------ + + pMarker = sentence; + lengthRemaining = sizeof(sentence); + + length = snprintf(pMarker, lengthRemaining, "$GPRMC,%02d%02d%02d,A," , + utcHours, utcMinutes, utcSeconds); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.flags & GPS_LOCATION_HAS_LAT_LONG) + { + double latitude = location.latitude; + double longitude = location.longitude; + char latHemisphere; + char lonHemisphere; + double latMinutes; + double lonMinutes; + + if (latitude > 0) + { + latHemisphere = 'N'; + } + else + { + latHemisphere = 'S'; + latitude *= -1.0; + } + + if (longitude < 0) + { + lonHemisphere = 'W'; + longitude *= -1.0; + } + else + { + lonHemisphere = 'E'; + } + + latMinutes = fmod(latitude * 60.0 , 60.0); + lonMinutes = fmod(longitude * 60.0 , 60.0); + + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", + (uint8_t)floor(latitude), latMinutes, latHemisphere, + (uint8_t)floor(longitude),lonMinutes, lonHemisphere); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.flags & GPS_LOCATION_HAS_SPEED) + { + float speedKnots = location.speed * (3600.0/1852.0); + length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.flags & GPS_LOCATION_HAS_BEARING) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.bearing); + } + else + { + length = snprintf(pMarker, lengthRemaining, ","); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,", + utcDay, utcMonth, utcYear); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV) + { + float magneticVariation = locationExtended.magneticDeviation; + char direction; + if (magneticVariation < 0.0) + { + direction = 'W'; + magneticVariation *= -1.0; + } + else + { + direction = 'E'; + } + + length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,", + magneticVariation, direction); + } + else + { + length = snprintf(pMarker, lengthRemaining, ",,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (!(location.flags & GPS_LOCATION_HAS_LAT_LONG)) + length = snprintf(pMarker, lengthRemaining, "%c", 'N'); // N means no fix + else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->client_handle->getPositionMode().mode) + length = snprintf(pMarker, lengthRemaining, "%c", 'A'); // A means autonomous + else + length = snprintf(pMarker, lengthRemaining, "%c", 'D'); // D means differential + + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + // ------------------ + // ------$GPGGA------ + // ------------------ + + pMarker = sentence; + lengthRemaining = sizeof(sentence); + + length = snprintf(pMarker, lengthRemaining, "$GPGGA,%02d%02d%02d," , + utcHours, utcMinutes, utcSeconds); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (location.flags & GPS_LOCATION_HAS_LAT_LONG) + { + double latitude = location.latitude; + double longitude = location.longitude; + char latHemisphere; + char lonHemisphere; + double latMinutes; + double lonMinutes; + + if (latitude > 0) + { + latHemisphere = 'N'; + } + else + { + latHemisphere = 'S'; + latitude *= -1.0; + } + + if (longitude < 0) + { + lonHemisphere = 'W'; + longitude *= -1.0; + } + else + { + lonHemisphere = 'E'; + } + + latMinutes = fmod(latitude * 60.0 , 60.0); + lonMinutes = fmod(longitude * 60.0 , 60.0); + + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,", + (uint8_t)floor(latitude), latMinutes, latHemisphere, + (uint8_t)floor(longitude),lonMinutes, lonHemisphere); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + char gpsQuality; + if (!(location.flags & GPS_LOCATION_HAS_LAT_LONG)) + gpsQuality = '0'; // 0 means no fix + else if (LOC_POSITION_MODE_STANDALONE == loc_eng_data_p->client_handle->getPositionMode().mode) + gpsQuality = '1'; // 1 means GPS fix + else + gpsQuality = '2'; // 2 means DGPS fix + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) + { // dop is in locationExtended, (QMI) + length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", + gpsQuality, svUsedCount, locationExtended.hdop); + } + else if (loc_eng_data_p->pdop > 0 && loc_eng_data_p->hdop > 0 && loc_eng_data_p->vdop > 0) + { // dop was cached from sv report (RPC) + length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,", + gpsQuality, svUsedCount, loc_eng_data_p->hdop); + } + else + { // no hdop + length = snprintf(pMarker, lengthRemaining, "%c,%02d,,", + gpsQuality, svUsedCount); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,M,", + locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,"); + } + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if ((location.flags & GPS_LOCATION_HAS_ALTITUDE) && + (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) + { + length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", + location.altitude - locationExtended.altitudeMeanSeaLevel); + } + else + { + length = snprintf(pMarker, lengthRemaining,",,,"); + } + + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + // clear the dop cache so they can't be used again + loc_eng_data_p->pdop = 0; + loc_eng_data_p->hdop = 0; + loc_eng_data_p->vdop = 0; + + EXIT_LOG(%d, 0); +} + + + +/*=========================================================================== +FUNCTION loc_eng_nmea_generate_sv + +DESCRIPTION + Generate NMEA sentences generated based on sv report + +DEPENDENCIES + NONE + +RETURN VALUE + 0 + +SIDE EFFECTS + N/A + +===========================================================================*/ +void loc_eng_nmea_generate_sv(loc_eng_data_s_type *loc_eng_data_p, + const GpsSvStatus &svStatus, const GpsLocationExtended &locationExtended) +{ + ENTRY_LOG(); + + char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char* pMarker = sentence; + int lengthRemaining = sizeof(sentence); + int length = 0; + + // ------------------ + // ------$GPGSV------ + // ------------------ + + if (svStatus.num_svs <= 0) + { + // no svs in view, so just send a blank $GPGSV sentence + strlcpy(sentence, "$GPGSV,1,1,0,", sizeof(sentence)); + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + } + else + { + int svCount = svStatus.num_svs; + int sentenceCount = svCount / 4; + if (svStatus.num_svs % 4) + sentenceCount++; + int sentenceNumber = 1; + int svNumber = 1; + + while (sentenceNumber <= sentenceCount) + { + pMarker = sentence; + lengthRemaining = sizeof(sentence); + + length = snprintf(pMarker, lengthRemaining, "$GPGSV,%d,%d,%02d", + sentenceCount, sentenceNumber, svCount); + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + for (int i=0; (svNumber <= svCount) && (i < 4); i++, svNumber++) + { + length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,", + svStatus.sv_list[svNumber-1].prn, + (int)(0.5 + svStatus.sv_list[svNumber-1].elevation), //float to int + (int)(0.5 + svStatus.sv_list[svNumber-1].azimuth)); //float to int + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + if (svStatus.sv_list[svNumber-1].snr > 0) + { + length = snprintf(pMarker, lengthRemaining,"%02d", + (int)(0.5 + svStatus.sv_list[svNumber-1].snr)); //float to int + + if (length < 0 || length >= lengthRemaining) + { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + } + } + + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + sentenceNumber++; + + } + } + + if (svStatus.used_in_fix_mask == 0) + { // No sv used, so there will be no position report, so send + // blank NMEA sentences + strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,", sizeof(sentence)); + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N", sizeof(sentence)); + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + + strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence)); + length = loc_eng_nmea_put_checksum(sentence, sizeof(sentence)); + loc_eng_nmea_send(sentence, length, loc_eng_data_p); + } + else + { // cache the used in fix mask, as it will be needed to send $GPGSA + // during the position report + loc_eng_data_p->sv_used_mask = svStatus.used_in_fix_mask; + + // For RPC, the DOP are sent during sv report, so cache them + // now to be sent during position report. + // For QMI, the DOP will be in position report. + if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) + { + loc_eng_data_p->pdop = locationExtended.pdop; + loc_eng_data_p->hdop = locationExtended.hdop; + loc_eng_data_p->vdop = locationExtended.vdop; + } + else + { + loc_eng_data_p->pdop = 0; + loc_eng_data_p->hdop = 0; + loc_eng_data_p->vdop = 0; + } + + } + + EXIT_LOG(%d, 0); +} |