From a740abcb1819b583d1bede6eaf4f075167864a0e Mon Sep 17 00:00:00 2001 From: DerTeufel Date: Sat, 29 Nov 2014 23:42:59 +0100 Subject: update gps hal from jf Change-Id: I8d08e1c871d9b9e4338f4823c8cdd1d33f6a2b6b --- gps/libloc_api_50001/loc_eng_nmea.cpp | 703 ---------------------------------- 1 file changed, 703 deletions(-) delete mode 100644 gps/libloc_api_50001/loc_eng_nmea.cpp (limited to 'gps/libloc_api_50001/loc_eng_nmea.cpp') diff --git a/gps/libloc_api_50001/loc_eng_nmea.cpp b/gps/libloc_api_50001/loc_eng_nmea.cpp deleted file mode 100644 index d2507a5..0000000 --- a/gps/libloc_api_50001/loc_eng_nmea.cpp +++ /dev/null @@ -1,703 +0,0 @@ -/* 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 -#include -#include -#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); -} -- cgit v1.2.3