From 84a25954a72d6120ab340a01b3c92f8a0f77920f Mon Sep 17 00:00:00 2001 From: Kevin Tang Date: Wed, 31 Oct 2018 14:22:22 -0700 Subject: [PATCH 1/9] remove LOC_MODEM_EMULATOR from gps.utils Move LOC_MODEM_EMULATOR reading out of libgps.utils, as utils is being use by the whole world, and nobody but one cares about this valude. Change-Id: I95ad52222172c3f31e8013dd4d917713392e1a4d CRs-Fixed: 2343169 --- utils/loc_cfg.cpp | 13 ------------- utils/loc_cfg.h | 2 -- 2 files changed, 15 deletions(-) diff --git a/utils/loc_cfg.cpp b/utils/loc_cfg.cpp index af2ece68..71dfc665 100644 --- a/utils/loc_cfg.cpp +++ b/utils/loc_cfg.cpp @@ -57,14 +57,12 @@ /* Parameter data */ static uint32_t DEBUG_LEVEL = 0xff; static uint32_t TIMESTAMP = 0; -static uint32_t LOC_MODEM_EMULATOR = 0; /* Parameter spec table */ static const loc_param_s_type loc_param_table[] = { {"DEBUG_LEVEL", &DEBUG_LEVEL, NULL, 'n'}, {"TIMESTAMP", &TIMESTAMP, NULL, 'n'}, - {"LOC_MODEM_EMULATOR", &LOC_MODEM_EMULATOR, NULL, 'n'}, }; static const int loc_param_num = sizeof(loc_param_table) / sizeof(loc_param_s_type); @@ -87,17 +85,6 @@ const char LOC_PATH_APDR_CONF[] = LOC_PATH_APDR_CONF_STR; const char LOC_PATH_XTWIFI_CONF[] = LOC_PATH_XTWIFI_CONF_STR; const char LOC_PATH_QUIPC_CONF[] = LOC_PATH_QUIPC_CONF_STR; -/*=========================================================================== -FUNCTION loc_modem_emulator_enabled - -DESCRIPTION - Provides access to Modem Emulator config item. -===========================================================================*/ -uint32_t loc_modem_emulator_enabled() -{ - return LOC_MODEM_EMULATOR; -} - /*=========================================================================== FUNCTION loc_set_config_entry diff --git a/utils/loc_cfg.h b/utils/loc_cfg.h index 3057abc2..a8bdaede 100644 --- a/utils/loc_cfg.h +++ b/utils/loc_cfg.h @@ -133,8 +133,6 @@ extern const char LOC_PATH_QUIPC_CONF[]; int loc_read_process_conf(const char* conf_file_name, uint32_t * process_count_ptr, loc_process_info_s_type** process_info_table_ptr); -uint32_t loc_modem_emulator_enabled(); - #ifdef __cplusplus } #endif From 512388f80689620fae581302ce6d05111568951b Mon Sep 17 00:00:00 2001 From: Madhanraj Chelladurai Date: Mon, 5 Nov 2018 14:52:13 +0530 Subject: [PATCH 2/9] gps: add include files for LA compilation add include files for LA compilation Change-Id: I2781337a7b5d50eaaada20bcf2a6bfb97095749d CRs-Fixed: 2344727 --- pla/android/loc_pla.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pla/android/loc_pla.h b/pla/android/loc_pla.h index 43a09067..6104bfb2 100644 --- a/pla/android/loc_pla.h +++ b/pla/android/loc_pla.h @@ -42,6 +42,8 @@ extern "C" { #include #include #include +#include +#include #define UID_GPS (AID_GPS) #define GID_GPS (AID_GPS) From d6b0854a7050ca571a6eeb3b0302e000c8857fb2 Mon Sep 17 00:00:00 2001 From: Mike Cailean Date: Fri, 9 Nov 2018 14:15:28 -0800 Subject: [PATCH 3/9] Lowered the severity of debug message Lowered the severity of debug message when setting callback function for geofencing Change-Id: I2bbd0648d1dba5d7d655545bb0487223dbccb9cb CRs-fixed: 2347777 --- android/1.1/GnssGeofencing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/android/1.1/GnssGeofencing.cpp b/android/1.1/GnssGeofencing.cpp index 5c11afe0..d57a6667 100644 --- a/android/1.1/GnssGeofencing.cpp +++ b/android/1.1/GnssGeofencing.cpp @@ -53,7 +53,7 @@ GnssGeofencing::~GnssGeofencing() { // Methods from ::android::hardware::gnss::V1_0::IGnssGeofencing follow. Return GnssGeofencing::setCallback(const sp& callback) { if (mApi != nullptr) { - LOC_LOGE("%s]: mApi is NOT nullptr", __FUNCTION__); + LOC_LOGd("mApi is NOT nullptr"); return Void(); } From b00c1c26c9d7673c8bc84d7e8697958541912587 Mon Sep 17 00:00:00 2001 From: Saurabh Srivastava Date: Fri, 16 Nov 2018 11:49:40 +0530 Subject: [PATCH 4/9] Invoking dlclose in error case Invoking dlclose for loc_net_iface library dlopen call in error case. Change-Id: Ib8d4b05904214ebaa83608309679bc055bd7abe5 CRs-Fixed: 2347372 --- gnss/GnssAdapter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index b7c57e8a..fc59c224 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -3716,6 +3716,7 @@ void GnssAdapter::initDefaultAgps() { dlsym(handle, "LocNetIfaceAgps_getAgpsCbInfo"); if (getAgpsCbInfo == nullptr) { LOC_LOGE("%s]: Failed to get method LocNetIfaceAgps_getStatusCb", __func__); + dlclose(handle); return; } @@ -3723,6 +3724,7 @@ void GnssAdapter::initDefaultAgps() { if (cbInfo.statusV4Cb == nullptr) { LOC_LOGE("%s]: statusV4Cb is nullptr!", __func__); + dlclose(handle); return; } From 06d7e5bbba73be1399450ae1c3c077f7c177bf9b Mon Sep 17 00:00:00 2001 From: Harikrishnan Hariharan Date: Fri, 16 Nov 2018 17:58:35 +0530 Subject: [PATCH 5/9] Request for a data item value is not handle by SSO Subscribe for a data item first time adds the DI to internal SSO client list. When the request comes for after this, since item is already added, dataItemsToSubscribe will be empty. So the code for request value for DI is not invoked. Handle this scenario in SSO. Change-Id: Ife04fe4eedc9b3132e7967f781b0e29ac41c01e7 CRs-Fixed: 2347338 --- core/SystemStatusOsObserver.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/core/SystemStatusOsObserver.cpp b/core/SystemStatusOsObserver.cpp index 20ef1ae8..3fdb5130 100644 --- a/core/SystemStatusOsObserver.cpp +++ b/core/SystemStatusOsObserver.cpp @@ -97,6 +97,7 @@ void SystemStatusOsObserver::subscribe(const list& l, IDataItemObser list& l, IDataItemObserver* client, bool requestData) : mParent(parent), mClient(client), mDataItemSet(containerTransfer, unordered_set>(l)), + diItemlist(l), mToRequestData(requestData) {} void proc() const { @@ -107,16 +108,13 @@ void SystemStatusOsObserver::subscribe(const list& l, IDataItemObser mParent->sendCachedDataItems(mDataItemSet, mClient); // Send subscription set to framework - if (nullptr != mParent->mContext.mSubscriptionObj && !dataItemsToSubscribe.empty()) { - LOC_LOGD("Subscribe Request sent to framework for the following"); - mParent->logMe(dataItemsToSubscribe); - + if (nullptr != mParent->mContext.mSubscriptionObj) { if (mToRequestData) { - mParent->mContext.mSubscriptionObj->requestData( - containerTransfer, list>( - std::move(dataItemsToSubscribe)), - mParent); - } else { + LOC_LOGD("Request Data sent to framework for the following"); + mParent->mContext.mSubscriptionObj->requestData(diItemlist, mParent); + } else if (!dataItemsToSubscribe.empty()) { + LOC_LOGD("Subscribe Request sent to framework for the following"); + mParent->logMe(dataItemsToSubscribe); mParent->mContext.mSubscriptionObj->subscribe( containerTransfer, list>( std::move(dataItemsToSubscribe)), @@ -127,6 +125,7 @@ void SystemStatusOsObserver::subscribe(const list& l, IDataItemObser mutable SystemStatusOsObserver* mParent; IDataItemObserver* mClient; const unordered_set mDataItemSet; + const list diItemlist; bool mToRequestData; }; From dca1724d17700135ee806027dec12a82b4940462 Mon Sep 17 00:00:00 2001 From: Ankur Sharma Date: Wed, 21 Nov 2018 22:41:09 +0530 Subject: [PATCH 6/9] Remove XTRA URLs from gps.conf Remove default URL's for XTRA from gps.conf CRs-Fixed:2355372 Change-Id: Iea1caa42e8cd4f84a0d37bb6bcd69655609d368d --- etc/gps.conf | 6 ------ 1 file changed, 6 deletions(-) diff --git a/etc/gps.conf b/etc/gps.conf index 957b6613..2f2e5ca6 100644 --- a/etc/gps.conf +++ b/etc/gps.conf @@ -1,9 +1,3 @@ -#Uncommenting these urls would only enable -#the power up auto injection and force injection(test case). -#XTRA_SERVER_1=http://xtrapath1.izatcloud.net/xtra2.bin -#XTRA_SERVER_2=http://xtrapath2.izatcloud.net/xtra2.bin -#XTRA_SERVER_3=http://xtrapath3.izatcloud.net/xtra2.bin - #Version check for XTRA #DISABLE = 0 #AUTO = 1 From 730abcc3d4354badb431ec99ca10011b29d5eccd Mon Sep 17 00:00:00 2001 From: Wei Chen Date: Fri, 19 Oct 2018 15:52:40 -0700 Subject: [PATCH 7/9] GPS NMEA: generate proper nmea during leap second transition During leap second transition, the hour, minute and seconds displayed need to be in the format of 23:59:60 to denote the extra leap second Change-Id: Ice7c6f934b1251990ba26e2b7e399554a2862eb4 CRs-fixed: 2335188 --- gnss/GnssAdapter.cpp | 26 ++++++++--- utils/loc_nmea.cpp | 100 ++++++++++++++++++++++++++++++++++++++++++- utils/loc_nmea.h | 1 + 3 files changed, 120 insertions(+), 7 deletions(-) diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index fc59c224..6b3e8463 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -1920,9 +1920,6 @@ GnssAdapter::updateClientsEventMask() mask |= LOC_API_ADAPTER_BIT_NMEA_1HZ_REPORT; updateNmeaMask(mNmeaMask | LOC_NMEA_MASK_DEBUG_V02); } - if (it->second.locationSystemInfoCb != nullptr) { - mask |= LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO; - } } /* @@ -1952,6 +1949,9 @@ GnssAdapter::updateClientsEventMask() mask |= LOC_API_ADAPTER_BIT_REQUEST_WIFI; } + // need to register for leap second info + // for proper nmea generation + mask |= LOC_API_ADAPTER_BIT_LOC_SYSTEM_INFO; updateEvtMask(mask, LOC_REGISTRATION_MASK_SET); } @@ -3012,7 +3012,8 @@ GnssAdapter::reportPosition(const UlpLocation& ulpLocation, (LOC_RELIABILITY_NOT_SET == locationExtended.horizontal_reliability)); uint8_t generate_nmea = (reported && status != LOC_SESS_FAILURE && !blank_fix); std::vector nmeaArraystr; - loc_nmea_generate_pos(ulpLocation, locationExtended, generate_nmea, nmeaArraystr); + loc_nmea_generate_pos(ulpLocation, locationExtended, mLocSystemInfo, + generate_nmea, nmeaArraystr); stringstream ss; for (auto sentence : nmeaArraystr) { ss << sentence; @@ -3292,7 +3293,22 @@ GnssAdapter::reportLocationSystemInfo(const LocationSystemInfo & locationSystemI // may come at different time if (locationSystemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) { mLocSystemInfo.systemInfoMask |= LOCATION_SYS_INFO_LEAP_SECOND; - mLocSystemInfo.leapSecondSysInfo = locationSystemInfo.leapSecondSysInfo; + + const LeapSecondSystemInfo &srcLeapSecondSysInfo = locationSystemInfo.leapSecondSysInfo; + LeapSecondSystemInfo &dstLeapSecondSysInfo = mLocSystemInfo.leapSecondSysInfo; + if (srcLeapSecondSysInfo.leapSecondInfoMask & + LEAP_SECOND_SYS_INFO_CURRENT_LEAP_SECONDS_BIT) { + dstLeapSecondSysInfo.leapSecondInfoMask |= + LEAP_SECOND_SYS_INFO_CURRENT_LEAP_SECONDS_BIT; + dstLeapSecondSysInfo.leapSecondCurrent = srcLeapSecondSysInfo.leapSecondCurrent; + } + // once leap second change event is complete, modem may send up event invalidate the leap second + // change info while AP is still processing report during leap second transition + // so, we choose to keep this info around even though it is old + if (srcLeapSecondSysInfo.leapSecondInfoMask & LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT) { + dstLeapSecondSysInfo.leapSecondInfoMask |= LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT; + dstLeapSecondSysInfo.leapSecondChangeInfo = srcLeapSecondSysInfo.leapSecondChangeInfo; + } } // we received new info, inform client of the newly received info diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp index 5ef4277a..b4de6dba 100644 --- a/utils/loc_nmea.cpp +++ b/utils/loc_nmea.cpp @@ -36,6 +36,8 @@ #define GLONASS_SV_ID_OFFSET 64 #define MAX_SATELLITES_IN_USE 12 +#define MSEC_IN_ONE_WEEK 604800000ULL +#define UTC_GPS_OFFSET_MSECS 315964800000ULL // GNSS system id according to NMEA spec #define SYSTEM_ID_GPS 1 @@ -605,6 +607,79 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, } //while } +/*=========================================================================== +FUNCTION getUtcTimeWithLeapSecondTransition + +DESCRIPTION + This function returns true if the position report is generated during + leap second transition period. If not, then the utc timestamp returned + will be set to the timestamp in the position report. If it is, + then the utc timestamp returned will need to take into account + of the leap second transition so that proper calendar year/month/date + can be calculated from the returned utc timestamp. + +DEPENDENCIES + NONE + +RETURN VALUE + true: position report is generated in leap second transition period. + +SIDE EFFECTS + N/A + +===========================================================================*/ +bool getUtcTimeWithLeapSecondTransition(const UlpLocation &location, + const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, + LocGpsUtcTime &utcPosTimestamp) { + bool inTransition = false; + + // position report is not generated during leap second transition, + // we can use the UTC timestamp from position report as is + utcPosTimestamp = location.gpsLocation.timestamp; + + // Check whether we are in leap second transition. + // If so, per NMEA spec, we need to display the extra second in format of 23:59:60 + // with year/month/date not getting advanced. + if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) && + ((systemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) && + (systemInfo.leapSecondSysInfo.leapSecondInfoMask & + LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT))) { + + const LeapSecondChangeInfo &leapSecondChangeInfo = + systemInfo.leapSecondSysInfo.leapSecondChangeInfo; + const GnssSystemTimeStructType &gpsTimestampLsChange = + leapSecondChangeInfo.gpsTimestampLsChange; + + uint64_t gpsTimeLsChange = gpsTimestampLsChange.systemWeek * MSEC_IN_ONE_WEEK + + gpsTimestampLsChange.systemMsec; + uint64_t gpsTimePosReport = locationExtended.gpsTime.gpsWeek * MSEC_IN_ONE_WEEK + + locationExtended.gpsTime.gpsTimeOfWeekMs; + // we are only dealing with positive leap second change, as negative + // leap second change has never occurred and should not occur in future + if (leapSecondChangeInfo.leapSecondsAfterChange > + leapSecondChangeInfo.leapSecondsBeforeChange) { + // leap second adjustment is always 1 second at a time. It can happen + // every quarter end and up to four times per year. + if ((gpsTimePosReport >= gpsTimeLsChange) && + (gpsTimePosReport < (gpsTimeLsChange + 1000))) { + inTransition = true; + utcPosTimestamp = gpsTimeLsChange + UTC_GPS_OFFSET_MSECS - + leapSecondChangeInfo.leapSecondsBeforeChange * 1000; + + // we substract 1000 milli-seconds from UTC timestmap in order to calculate the + // proper year, month and date during leap second transtion. + // Let us give an example, assuming leap second transition is scheduled on 2019, + // Dec 31st mid night. When leap second transition is happening, + // instead of outputting the time as 2020, Jan, 1st, 00 hour, 00 min, and 00 sec. + // The time need to be displayed as 2019, Dec, 31st, 23 hour, 59 min and 60 sec. + utcPosTimestamp -= 1000; + } + } + } + return inTransition; +} + /*=========================================================================== FUNCTION loc_nmea_generate_pos @@ -631,11 +706,19 @@ SIDE EFFECTS ===========================================================================*/ void loc_nmea_generate_pos(const UlpLocation &location, const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, unsigned char generate_nmea, std::vector &nmeaArraystr) { ENTRY_LOG(); - time_t utcTime(location.gpsLocation.timestamp/1000); + + LocGpsUtcTime utcPosTimestamp = 0; + bool inLsTransition = false; + + inLsTransition = getUtcTimeWithLeapSecondTransition + (location, locationExtended, systemInfo, utcPosTimestamp); + + time_t utcTime(utcPosTimestamp/1000); tm * pTm = gmtime(&utcTime); if (NULL == pTm) { LOC_LOGE("gmtime failed"); @@ -653,7 +736,19 @@ void loc_nmea_generate_pos(const UlpLocation &location, int utcMinutes = pTm->tm_min; int utcSeconds = pTm->tm_sec; int utcMSeconds = (location.gpsLocation.timestamp)%1000; - loc_sv_cache_info sv_cache_info = {}; + + if (inLsTransition) { + // During leap second transition, we need to display the extra + // leap second of hour, minute, second as (23:59:60) + utcHours = 23; + utcMinutes = 59; + utcSeconds = 60; + // As UTC timestamp is freezing during leap second transition, + // retrieve milli-seconds portion from GPS timestamp. + utcMSeconds = locationExtended.gpsTime.gpsTimeOfWeekMs % 1000; + } + + loc_sv_cache_info sv_cache_info = {}; if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) { sv_cache_info.gps_used_mask = @@ -667,6 +762,7 @@ void loc_nmea_generate_pos(const UlpLocation &location, sv_cache_info.bds_used_mask = (uint32_t)locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask; } + if (generate_nmea) { char talker[3] = {'G', 'P', '\0'}; uint32_t svUsedCount = 0; diff --git a/utils/loc_nmea.h b/utils/loc_nmea.h index 585a9c13..fbf88a74 100644 --- a/utils/loc_nmea.h +++ b/utils/loc_nmea.h @@ -40,6 +40,7 @@ void loc_nmea_generate_sv(const GnssSvNotification &svNotify, void loc_nmea_generate_pos(const UlpLocation &location, const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, unsigned char generate_nmea, std::vector &nmeaArraystr); From e232986d84deb7178d1a57cc7f1e4d24cbaeeb3c Mon Sep 17 00:00:00 2001 From: Wei Chen Date: Thu, 15 Nov 2018 09:41:26 -0800 Subject: [PATCH 8/9] GPS location API: support destroy with callback for completion Support Location API destroy function with callback. Location API client that passes callback to Location API need to wait for the destroy complete callback to be invoked before releasing the memory that holds the callback Change-Id: I29b1c6d46feb79c789e6f1ec1500c941b022a3ac CRs-fixed: 2349398 --- gnss/GnssAdapter.cpp | 15 +++- gnss/GnssAdapter.h | 5 +- gnss/location_gnss.cpp | 8 +- location/LocationAPI.cpp | 148 ++++++++++++++++++++++++++++------ location/LocationAPI.h | 12 ++- location/LocationDataTypes.h | 3 + location/location_interface.h | 8 +- 7 files changed, 159 insertions(+), 40 deletions(-) diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index fc59c224..f7297d0e 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -1859,25 +1859,32 @@ GnssAdapter::addClientCommand(LocationAPI* client, const LocationCallbacks& call } void -GnssAdapter::removeClientCommand(LocationAPI* client) +GnssAdapter::removeClientCommand(LocationAPI* client, + removeClientCompleteCallback rmClientCb) { LOC_LOGD("%s]: client %p", __func__, client); struct MsgRemoveClient : public LocMsg { GnssAdapter& mAdapter; LocationAPI* mClient; + removeClientCompleteCallback mRmClientCb; inline MsgRemoveClient(GnssAdapter& adapter, - LocationAPI* client) : + LocationAPI* client, + removeClientCompleteCallback rmCb) : LocMsg(), mAdapter(adapter), - mClient(client) {} + mClient(client), + mRmClientCb(rmCb){} inline virtual void proc() const { mAdapter.stopClientSessions(mClient); mAdapter.eraseClient(mClient); + if (nullptr != mRmClientCb) { + mRmClientCb(mClient); + } } }; - sendMsg(new MsgRemoveClient(*this, client)); + sendMsg(new MsgRemoveClient(*this, client, rmClientCb)); } void diff --git a/gnss/GnssAdapter.h b/gnss/GnssAdapter.h index f382c6ad..1682f027 100644 --- a/gnss/GnssAdapter.h +++ b/gnss/GnssAdapter.h @@ -128,6 +128,8 @@ typedef std::function GnssEnergyConsumedCallback; +typedef void (*removeClientCompleteCallback)(LocationAPI* client); + class GnssAdapter : public LocAdapterBase { /* ==== Engine Hub ===================================================================== */ @@ -206,7 +208,8 @@ public: /* ==== CLIENT ========================================================================= */ /* ======== COMMANDS ====(Called from Client Thread)==================================== */ void addClientCommand(LocationAPI* client, const LocationCallbacks& callbacks); - void removeClientCommand(LocationAPI* client); + void removeClientCommand(LocationAPI* client, + removeClientCompleteCallback rmClientCb); void requestCapabilitiesCommand(LocationAPI* client); /* ======== UTILITIES ================================================================== */ void saveClient(LocationAPI* client, const LocationCallbacks& callbacks); diff --git a/gnss/location_gnss.cpp b/gnss/location_gnss.cpp index 5a7710a2..f9fbddc9 100644 --- a/gnss/location_gnss.cpp +++ b/gnss/location_gnss.cpp @@ -36,7 +36,7 @@ static void initialize(); static void deinitialize(); static void addClient(LocationAPI* client, const LocationCallbacks& callbacks); -static void removeClient(LocationAPI* client); +static void removeClient(LocationAPI* client, removeClientCompleteCallback rmClientCb); static void requestCapabilities(LocationAPI* client); static uint32_t startTracking(LocationAPI* client, TrackingOptions&); @@ -140,10 +140,10 @@ static void addClient(LocationAPI* client, const LocationCallbacks& callbacks) } } -static void removeClient(LocationAPI* client) +static void removeClient(LocationAPI* client, removeClientCompleteCallback rmClientCb) { if (NULL != gGnssAdapter) { - gGnssAdapter->removeClientCommand(client); + gGnssAdapter->removeClientCommand(client, rmClientCb); } } @@ -345,4 +345,4 @@ static void getGnssEnergyConsumed(GnssEnergyConsumedCallback energyConsumedCb) { if (NULL != gGnssAdapter) { gGnssAdapter->getGnssEnergyConsumedCommand(energyConsumedCb); } -} \ No newline at end of file +} diff --git a/location/LocationAPI.cpp b/location/LocationAPI.cpp index 9a67f39e..4d859b30 100644 --- a/location/LocationAPI.cpp +++ b/location/LocationAPI.cpp @@ -36,15 +36,37 @@ #include typedef void* (getLocationInterface)(); + +typedef uint16_t LocationAdapterTypeMask; +typedef enum { + LOCATION_ADAPTER_GNSS_TYPE_BIT = (1<<0), // adapter type is GNSS + LOCATION_ADAPTER_FLP_TYPE_BIT = (1<<1), // adapter type is FLP + LOCATION_ADAPTER_GEOFENCE_TYPE_BIT = (1<<2) // adapter type is geo fence +} LocationAdapterTypeBits; + +typedef struct { + // bit mask of the adpaters that we need to wait for the removeClientCompleteCallback + // before we invoke the registered locationApiDestroyCompleteCallback + LocationAdapterTypeMask waitAdapterMask; + locationApiDestroyCompleteCallback destroyCompleteCb; +} LocationAPIDestroyCbData; + +// This is the map for the client that has requested destroy with +// destroy callback provided. +typedef std::map + LocationClientDestroyCbMap; + typedef std::map LocationClientMap; typedef struct { LocationClientMap clientData; + LocationClientDestroyCbMap destroyClientData; LocationControlAPI* controlAPI; LocationControlCallbacks controlCallbacks; GnssInterface* gnssInterface; GeofenceInterface* geofenceInterface; FlpInterface* flpInterface; } LocationAPIData; + static LocationAPIData gData = {}; static pthread_mutex_t gDataMutex = PTHREAD_MUTEX_INITIALIZER; static bool gGnssLoadFailed = false; @@ -80,7 +102,8 @@ static bool isGeofenceClient(LocationCallbacks& locationCallbacks) locationCallbacks.geofenceStatusCb != nullptr); } -static void* loadLocationInterface(const char* library, const char* name) { +static void* loadLocationInterface(const char* library, const char* name) +{ LOC_LOGD("%s]: loading %s::%s ...", __func__, library, name); if (NULL == library || NULL == name) { return NULL; @@ -106,6 +129,46 @@ static void* loadLocationInterface(const char* library, const char* name) { } } +void onRemoveClientCompleteCb ( + LocationAPI* client, LocationAdapterTypeMask adapterType) +{ + bool invokeCallback = false; + locationApiDestroyCompleteCallback destroyCompleteCb; + LOC_LOGd("adatper type %x", adapterType); + pthread_mutex_lock(&gDataMutex); + auto it = gData.destroyClientData.find(client); + if (it != gData.destroyClientData.end()) { + it->second.waitAdapterMask &= ~adapterType; + if (it->second.waitAdapterMask == 0) { + invokeCallback = true; + destroyCompleteCb = it->second.destroyCompleteCb; + gData.destroyClientData.erase(it); + } + } + pthread_mutex_unlock(&gDataMutex); + + if ((true == invokeCallback) && (nullptr != destroyCompleteCb)) { + LOC_LOGd("invoke client destroy cb"); + (destroyCompleteCb) (); + LOC_LOGd("finish invoke client destroy cb"); + } +} + +void onGnssRemoveClientCompleteCb (LocationAPI* client) +{ + onRemoveClientCompleteCb (client, LOCATION_ADAPTER_GNSS_TYPE_BIT); +} + +void onFlpRemoveClientCompleteCb (LocationAPI* client) +{ + onRemoveClientCompleteCb (client, LOCATION_ADAPTER_FLP_TYPE_BIT); +} + +void onGeofenceRemoveClientCompleteCb (LocationAPI* client) +{ + onRemoveClientCompleteCb (client, LOCATION_ADAPTER_GEOFENCE_TYPE_BIT); +} + LocationAPI* LocationAPI::createInstance(LocationCallbacks& locationCallbacks) { @@ -188,9 +251,66 @@ LocationAPI::createInstance(LocationCallbacks& locationCallbacks) } void -LocationAPI::destroy() +LocationAPI::destroy(locationApiDestroyCompleteCallback destroyCompleteCb) { - delete this; + bool invokeDestroyCb = false; + + pthread_mutex_lock(&gDataMutex); + auto it = gData.clientData.find(this); + if (it != gData.clientData.end()) { + bool removeFromGnssInf = + (isGnssClient(it->second) && NULL != gData.gnssInterface); + bool removeFromFlpInf = + (isFlpClient(it->second) && NULL != gData.flpInterface); + bool removeFromGeofenceInf = + (isGeofenceClient(it->second) && NULL != gData.geofenceInterface); + bool needToWait = (removeFromGnssInf || removeFromFlpInf || removeFromGeofenceInf); + LOC_LOGe("removeFromGnssInf: %d, removeFromFlpInf: %d, removeFromGeofenceInf: %d, need %d", + removeFromGnssInf, removeFromFlpInf, removeFromGeofenceInf, needToWait); + + if ((NULL != destroyCompleteCb) && (true == needToWait)) { + LocationAPIDestroyCbData destroyCbData = {}; + destroyCbData.destroyCompleteCb = destroyCompleteCb; + // record down from which adapter we need to wait for the destroy complete callback + // only when we have received all the needed callbacks from all the associated stacks, + // we shall notify the client. + destroyCbData.waitAdapterMask = + (removeFromGnssInf ? LOCATION_ADAPTER_GNSS_TYPE_BIT : 0); + destroyCbData.waitAdapterMask |= + (removeFromFlpInf ? LOCATION_ADAPTER_FLP_TYPE_BIT : 0); + destroyCbData.waitAdapterMask |= + (removeFromGeofenceInf ? LOCATION_ADAPTER_GEOFENCE_TYPE_BIT : 0); + gData.destroyClientData[this] = destroyCbData; + LOC_LOGe("destroy data stored in the map: 0x%x", destroyCbData.waitAdapterMask); + } + + if (removeFromGnssInf) { + gData.gnssInterface->removeClient(it->first, + onGnssRemoveClientCompleteCb); + } + if (removeFromFlpInf) { + gData.flpInterface->removeClient(it->first, + onFlpRemoveClientCompleteCb); + } + if (removeFromGeofenceInf) { + gData.geofenceInterface->removeClient(it->first, + onGeofenceRemoveClientCompleteCb); + } + + gData.clientData.erase(it); + + if ((NULL != destroyCompleteCb) && (false == needToWait)) { + invokeDestroyCb = true; + } + } else { + LOC_LOGE("%s:%d]: Location API client %p not found in client data", + __func__, __LINE__, this); + } + + pthread_mutex_unlock(&gDataMutex); + if (invokeDestroyCb == true) { + (destroyCompleteCb) (); + } } LocationAPI::LocationAPI() @@ -198,29 +318,9 @@ LocationAPI::LocationAPI() LOC_LOGD("LOCATION API CONSTRUCTOR"); } +// private destructor LocationAPI::~LocationAPI() { - LOC_LOGD("LOCATION API DESTRUCTOR"); - pthread_mutex_lock(&gDataMutex); - - auto it = gData.clientData.find(this); - if (it != gData.clientData.end()) { - if (isGnssClient(it->second) && NULL != gData.gnssInterface) { - gData.gnssInterface->removeClient(it->first); - } - if (isFlpClient(it->second) && NULL != gData.flpInterface) { - gData.flpInterface->removeClient(it->first); - } - if (isGeofenceClient(it->second) && NULL != gData.geofenceInterface) { - gData.geofenceInterface->removeClient(it->first); - } - gData.clientData.erase(it); - } else { - LOC_LOGE("%s:%d]: Location API client %p not found in client data", - __func__, __LINE__, this); - } - - pthread_mutex_unlock(&gDataMutex); } void diff --git a/location/LocationAPI.h b/location/LocationAPI.h index f4f31f96..c270ef41 100644 --- a/location/LocationAPI.h +++ b/location/LocationAPI.h @@ -43,10 +43,14 @@ public: of instances have been reached */ static LocationAPI* createInstance(LocationCallbacks&); - /* destroy/cleans up the instance, which should be called when LocationAPI object is - no longer needed. LocationAPI* returned from createInstance will no longer valid - after destroy is called */ - void destroy(); + /* destroy/cleans up the instance, which should be called when LocationControlAPI object is + no longer needed. LocationControlAPI* returned from createInstance will no longer valid + after destroy is called. + If the caller allocates the memory for LocationControlCallbacks used in + LocationControlAPI::createInstance, then the caller must ensure that the memory still remains + valid until destroyCompleteCb is invoked. + */ + void destroy(locationApiDestroyCompleteCallback destroyCompleteCb=nullptr); /* updates/changes the callbacks that will be called. mandatory callbacks must be present for callbacks to be successfully updated diff --git a/location/LocationDataTypes.h b/location/LocationDataTypes.h index f35b2200..61598be8 100644 --- a/location/LocationDataTypes.h +++ b/location/LocationDataTypes.h @@ -1274,6 +1274,9 @@ typedef std::function locationSystemInfoCallback; +typedef std::function locationApiDestroyCompleteCallback; + typedef struct { size_t size; // set to sizeof(LocationCallbacks) capabilitiesCallback capabilitiesCb; // mandatory diff --git a/location/location_interface.h b/location/location_interface.h index e9e2fe89..480bbdb4 100644 --- a/location/location_interface.h +++ b/location/location_interface.h @@ -46,12 +46,14 @@ typedef std::function GnssEnergyConsumedCallback; +typedef void (*removeClientCompleteCallback)(LocationAPI* client); + struct GnssInterface { size_t size; void (*initialize)(void); void (*deinitialize)(void); void (*addClient)(LocationAPI* client, const LocationCallbacks& callbacks); - void (*removeClient)(LocationAPI* client); + void (*removeClient)(LocationAPI* client, removeClientCompleteCallback rmClientCb); void (*requestCapabilities)(LocationAPI* client); uint32_t (*startTracking)(LocationAPI* client, TrackingOptions&); void (*updateTrackingOptions)(LocationAPI* client, uint32_t id, TrackingOptions&); @@ -87,7 +89,7 @@ struct FlpInterface { void (*initialize)(void); void (*deinitialize)(void); void (*addClient)(LocationAPI* client, const LocationCallbacks& callbacks); - void (*removeClient)(LocationAPI* client); + void (*removeClient)(LocationAPI* client, removeClientCompleteCallback rmClientCb); void (*requestCapabilities)(LocationAPI* client); uint32_t (*startTracking)(LocationAPI* client, TrackingOptions&); void (*updateTrackingOptions)(LocationAPI* client, uint32_t id, TrackingOptions&); @@ -104,7 +106,7 @@ struct GeofenceInterface { void (*initialize)(void); void (*deinitialize)(void); void (*addClient)(LocationAPI* client, const LocationCallbacks& callbacks); - void (*removeClient)(LocationAPI* client); + void (*removeClient)(LocationAPI* client, removeClientCompleteCallback rmClientCb); void (*requestCapabilities)(LocationAPI* client); uint32_t* (*addGeofences)(LocationAPI* client, size_t count, GeofenceOption*, GeofenceInfo*); void (*removeGeofences)(LocationAPI* client, size_t count, uint32_t* ids); From 3203504a28c72cf9823f1864b46b8692cf7979ec Mon Sep 17 00:00:00 2001 From: Hoss Zhou Date: Tue, 27 Nov 2018 15:00:57 +0800 Subject: [PATCH 9/9] PZ90 datum support add a configuration DATUM_TYPE in gps.conf to configure datum type. Default datum type is WGS84. Change-Id: Ia8404aa5cf6d49741b9f487f8b086abd095f21ea CRs-fixed: 2338883 --- etc/gps.conf | 7 + utils/loc_cfg.cpp | 25 ++++ utils/loc_cfg.h | 2 +- utils/loc_nmea.cpp | 311 +++++++++++++++++++++++++++++++++++++++++++-- utils/loc_nmea.h | 37 ++++++ 5 files changed, 368 insertions(+), 14 deletions(-) diff --git a/etc/gps.conf b/etc/gps.conf index 2f2e5ca6..e5be3858 100644 --- a/etc/gps.conf +++ b/etc/gps.conf @@ -95,6 +95,13 @@ CAPABILITIES=0x37 # 3: Enable both LPP_User_Plane and LPP_Control_Plane LPP_PROFILE = 2 +#################################### +#Datum Type +#################################### +# 0: WGS-84 +# 1: PZ-90 +DATUM_TYPE = 0 + ################################ # EXTRA SETTINGS ################################ diff --git a/utils/loc_cfg.cpp b/utils/loc_cfg.cpp index 71dfc665..3676bd58 100644 --- a/utils/loc_cfg.cpp +++ b/utils/loc_cfg.cpp @@ -57,12 +57,14 @@ /* Parameter data */ static uint32_t DEBUG_LEVEL = 0xff; static uint32_t TIMESTAMP = 0; +static uint32_t DATUM_TYPE = 0; /* Parameter spec table */ static const loc_param_s_type loc_param_table[] = { {"DEBUG_LEVEL", &DEBUG_LEVEL, NULL, 'n'}, {"TIMESTAMP", &TIMESTAMP, NULL, 'n'}, + {"DATUM_TYPE", &DATUM_TYPE, NULL, 'n'}, }; static const int loc_param_num = sizeof(loc_param_table) / sizeof(loc_param_s_type); @@ -85,6 +87,29 @@ const char LOC_PATH_APDR_CONF[] = LOC_PATH_APDR_CONF_STR; const char LOC_PATH_XTWIFI_CONF[] = LOC_PATH_XTWIFI_CONF_STR; const char LOC_PATH_QUIPC_CONF[] = LOC_PATH_QUIPC_CONF_STR; +/*=========================================================================== +FUNCTION loc_get_datum_type + +DESCRIPTION + get datum type + +PARAMETERS: + N/A + +DEPENDENCIES + N/A + +RETURN VALUE + DATUM TYPE + +SIDE EFFECTS + N/A +===========================================================================*/ +int loc_get_datum_type() +{ + return DATUM_TYPE; +} + /*=========================================================================== FUNCTION loc_set_config_entry diff --git a/utils/loc_cfg.h b/utils/loc_cfg.h index a8bdaede..aa001480 100644 --- a/utils/loc_cfg.h +++ b/utils/loc_cfg.h @@ -132,7 +132,7 @@ extern const char LOC_PATH_QUIPC_CONF[]; int loc_read_process_conf(const char* conf_file_name, uint32_t * process_count_ptr, loc_process_info_s_type** process_info_table_ptr); - +int loc_get_datum_type(); #ifdef __cplusplus } #endif diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp index b4de6dba..e9a8f848 100644 --- a/utils/loc_nmea.cpp +++ b/utils/loc_nmea.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #define GLONASS_SV_ID_OFFSET 64 #define MAX_SATELLITES_IN_USE 12 @@ -113,6 +114,126 @@ typedef struct loc_sv_cache_info_s float vdop; } loc_sv_cache_info; +/*=========================================================================== +FUNCTION convert_Lla_to_Ecef + +DESCRIPTION + Convert LLA to ECEF + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef) +{ + double r; + + r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat)); + pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon); + pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon); + pecef.Z = (r * OMES + plla.alt) * sin(plla.lat); +} + +/*=========================================================================== +FUNCTION convert_WGS84_to_PZ90 + +DESCRIPTION + Convert datum from WGS84 to PZ90 + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90) +{ + double deltaX = DatumConstFromWGS84[0]; + double deltaY = DatumConstFromWGS84[1]; + double deltaZ = DatumConstFromWGS84[2]; + double deltaScale = DatumConstFromWGS84[3]; + double rotX = DatumConstFromWGS84[4]; + double rotY = DatumConstFromWGS84[5]; + double rotZ = DatumConstFromWGS84[6]; + + pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z); + pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z); + pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y); +} + +/*=========================================================================== +FUNCTION convert_Ecef_to_Lla + +DESCRIPTION + Convert ECEF to LLA + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla) +{ + double p, r; + double EcefA = C_PZ90A; + double EcefB = C_PZ90B; + double Ecef1Mf; + double EcefE2; + double Mu; + double Smu; + double Cmu; + double Phi; + double Sphi; + double N; + + p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y); + r = sqrt(p * p + pecef.Z * pecef.Z); + if (r < 1.0) { + plla.lat = 1.0; + plla.lon = 1.0; + plla.alt = 1.0; + } + Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA; + EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA); + if (p > 1.0) { + Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p); + } else { + if (pecef.Z > 0.0) { + Mu = M_PI / 2.0; + } else { + Mu = -M_PI / 2.0; + } + } + Smu = sin(Mu); + Cmu = cos(Mu); + Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu, + Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu)); + Sphi = sin(Phi); + N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi); + plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N; + plla.lat = Phi; + if ( p > 1.0) { + plla.lon = atan2(pecef.Y, pecef.X); + } else { + plla.lon = 0.0; + } +} + /*=========================================================================== FUNCTION convert_signalType_to_signalId @@ -607,6 +728,110 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify, } //while } +/*=========================================================================== +FUNCTION loc_nmea_generate_DTM + +DESCRIPTION + Generate NMEA DTM sentences generated based on position report + +DEPENDENCIES + NONE + +RETURN VALUE + NONE + +SIDE EFFECTS + N/A + +===========================================================================*/ +static void loc_nmea_generate_DTM(const LocLla &ref_lla, + const LocLla &local_lla, + char *sentence, + int bufSize) +{ + char* pMarker = sentence; + int lengthRemaining = bufSize; + int length = 0; + int datum_type; + char ref_datum[4] = {0}; + char local_datum[4] = {0}; + double lla_offset[3] = {0}; + char latHem, longHem; + double latMins, longMins; + + + + datum_type = loc_get_datum_type(); + switch (datum_type) { + case LOC_GNSS_DATUM_WGS84: + ref_datum[0] = 'W'; + ref_datum[1] = '8'; + ref_datum[2] = '4'; + local_datum[0] = 'P'; + local_datum[1] = '9'; + local_datum[2] = '0'; + break; + case LOC_GNSS_DATUM_PZ90: + ref_datum[0] = 'P'; + ref_datum[1] = '9'; + ref_datum[2] = '0'; + local_datum[0] = 'W'; + local_datum[1] = '8'; + local_datum[2] = '4'; + break; + default: + break; + } + length = snprintf(pMarker , lengthRemaining , "$GPDTM,%s,," , local_datum); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + lla_offset[0] = local_lla.lat - ref_lla.lat; + lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0); + if (lla_offset[1] < -180.0) { + lla_offset[1] += 360.0; + } else if ( lla_offset[1] > 180.0) { + lla_offset[1] -= 360.0; + } + lla_offset[2] = local_lla.alt - ref_lla.alt; + if (lla_offset[0] > 0.0) { + latHem = 'N'; + } else { + latHem = 'S'; + lla_offset[0] *= -1.0; + } + latMins = fmod(lla_offset[0] * 60.0, 60.0); + if (lla_offset[1] < 0.0) { + longHem = 'W'; + lla_offset[1] *= -1.0; + }else { + longHem = 'E'; + } + longMins = fmod(lla_offset[1] * 60.0, 60.0); + length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,", + (uint8_t)floor(lla_offset[0]), latMins, latHem, + (uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum); + if (length < 0 || length >= lengthRemaining) { + LOC_LOGE("NMEA Error in string formatting"); + return; + } + pMarker += length; + lengthRemaining -= length; + + length = loc_nmea_put_checksum(sentence, bufSize); +} + /*=========================================================================== FUNCTION getUtcTimeWithLeapSecondTransition @@ -726,6 +951,9 @@ void loc_nmea_generate_pos(const UlpLocation &location, } char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0}; + char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0}; char* pMarker = sentence; int lengthRemaining = sizeof(sentence); int length = 0; @@ -736,6 +964,13 @@ void loc_nmea_generate_pos(const UlpLocation &location, int utcMinutes = pTm->tm_min; int utcSeconds = pTm->tm_sec; int utcMSeconds = (location.gpsLocation.timestamp)%1000; + int datum_type = loc_get_datum_type(); + LocEcef ecef_w84; + LocEcef ecef_p90; + LocLla lla_w84; + LocLla lla_p90; + LocLla ref_lla; + LocLla local_lla; if (inLsTransition) { // During leap second transition, we need to display the extra @@ -904,12 +1139,52 @@ void loc_nmea_generate_pos(const UlpLocation &location, length = loc_nmea_put_checksum(sentence, sizeof(sentence)); nmeaArraystr.push_back(sentence); + memset(&ecef_w84, 0, sizeof(ecef_w84)); + memset(&ecef_p90, 0, sizeof(ecef_p90)); + memset(&lla_w84, 0, sizeof(lla_w84)); + memset(&lla_p90, 0, sizeof(lla_p90)); + memset(&ref_lla, 0, sizeof(ref_lla)); + memset(&local_lla, 0, sizeof(local_lla)); + lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI; + lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI; + lla_w84.alt = location.gpsLocation.altitude; + + convert_Lla_to_Ecef(lla_w84, ecef_w84); + convert_WGS84_to_PZ90(ecef_w84, ecef_p90); + convert_Ecef_to_Lla(ecef_p90, lla_p90); + + switch (datum_type) { + case LOC_GNSS_DATUM_WGS84: + ref_lla.lat = location.gpsLocation.latitude; + ref_lla.lon = location.gpsLocation.longitude; + ref_lla.alt = location.gpsLocation.altitude; + local_lla.lat = lla_p90.lat / M_PI * 180.0; + local_lla.lon = lla_p90.lon / M_PI * 180.0; + local_lla.alt = lla_p90.alt; + break; + case LOC_GNSS_DATUM_PZ90: + ref_lla.lat = lla_p90.lat / M_PI * 180.0; + ref_lla.lon = lla_p90.lon / M_PI * 180.0; + ref_lla.alt = lla_p90.alt; + local_lla.lat = location.gpsLocation.latitude; + local_lla.lon = location.gpsLocation.longitude; + local_lla.alt = location.gpsLocation.altitude; + break; + default: + break; + } + + // ------------------- + // ------$--DTM------- + // ------------------- + loc_nmea_generate_DTM(ref_lla, local_lla, sentence_DTM, sizeof(sentence_DTM)); + // ------------------- // ------$--RMC------- // ------------------- - pMarker = sentence; - lengthRemaining = sizeof(sentence); + pMarker = sentence_RMC; + lengthRemaining = sizeof(sentence_RMC); length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," , talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10); @@ -924,8 +1199,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) { - double latitude = location.gpsLocation.latitude; - double longitude = location.gpsLocation.longitude; + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; char latHemisphere; char lonHemisphere; double latMinutes; @@ -1067,15 +1342,14 @@ void loc_nmea_generate_pos(const UlpLocation &location, pMarker += length; lengthRemaining -= length; - length = loc_nmea_put_checksum(sentence, sizeof(sentence)); - nmeaArraystr.push_back(sentence); + length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC)); // ------------------- // ------$--GGA------- // ------------------- - pMarker = sentence; - lengthRemaining = sizeof(sentence); + pMarker = sentence_GGA; + lengthRemaining = sizeof(sentence_GGA); length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," , talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); @@ -1090,8 +1364,8 @@ void loc_nmea_generate_pos(const UlpLocation &location, if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG) { - double latitude = location.gpsLocation.latitude; - double longitude = location.gpsLocation.longitude; + double latitude = ref_lla.lat; + double longitude = ref_lla.lon; char latHemisphere; char lonHemisphere; double latMinutes; @@ -1191,15 +1465,26 @@ void loc_nmea_generate_pos(const UlpLocation &location, (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) { length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", - location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); + ref_lla.alt - locationExtended.altitudeMeanSeaLevel); } else { length = snprintf(pMarker, lengthRemaining,",,,"); } - length = loc_nmea_put_checksum(sentence, sizeof(sentence)); - nmeaArraystr.push_back(sentence); + length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA)); + + // ------$--DTM------- + nmeaArraystr.push_back(sentence_DTM); + // ------$--RMC------- + nmeaArraystr.push_back(sentence_RMC); + if(LOC_GNSS_DATUM_PZ90 == datum_type) { + // ------$--DTM------- + nmeaArraystr.push_back(sentence_DTM); + } + // ------$--GGA------- + nmeaArraystr.push_back(sentence_GGA); + } //Send blank NMEA reports for non-final fixes else { diff --git a/utils/loc_nmea.h b/utils/loc_nmea.h index fbf88a74..c6c83db4 100644 --- a/utils/loc_nmea.h +++ b/utils/loc_nmea.h @@ -35,6 +35,43 @@ #include #define NMEA_SENTENCE_MAX_LENGTH 200 +/** gnss datum type */ +#define LOC_GNSS_DATUM_WGS84 0 +#define LOC_GNSS_DATUM_PZ90 1 + +/* len of semi major axis of ref ellips*/ +#define MAJA (6378137.0) +/* flattening coef of ref ellipsoid*/ +#define FLAT (1.0/298.2572235630) +/* 1st eccentricity squared*/ +#define ESQR (FLAT*(2.0 - FLAT)) +/*1 minus eccentricity squared*/ +#define OMES (1.0 - ESQR) +#define MILARCSEC2RAD (4.848136811095361e-09) +/*semi major axis */ +#define C_PZ90A (6378136.0) +/*semi minor axis */ +#define C_PZ90B (6356751.3618) +/* Transformation from WGS84 to PZ90 + * Cx,Cy,Cz,Rs,Rx,Ry,Rz,C_SYS_A,C_SYS_B*/ +const double DatumConstFromWGS84[9] = + {+0.003, +0.001, 0.000, (1.0+(0.000*1E-6)), (-0.019*MILARCSEC2RAD), + (+0.042*MILARCSEC2RAD), (-0.002*MILARCSEC2RAD), C_PZ90A, C_PZ90B}; + +/** Represents a LTP*/ +typedef struct { + double lat; + double lon; + double alt; +} LocLla; + +/** Represents a ECEF*/ +typedef struct { + double X; + double Y; + double Z; +} LocEcef; + void loc_nmea_generate_sv(const GnssSvNotification &svNotify, std::vector &nmeaArraystr);