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(); } 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; }; diff --git a/etc/gps.conf b/etc/gps.conf index 957b6613..e5be3858 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 @@ -101,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/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index b7c57e8a..016356b4 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 @@ -1920,9 +1927,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 +1956,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 +3019,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 +3300,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 @@ -3716,6 +3739,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 +3747,7 @@ void GnssAdapter::initDefaultAgps() { if (cbInfo.statusV4Cb == nullptr) { LOC_LOGE("%s]: statusV4Cb is nullptr!", __func__); + dlclose(handle); return; } 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); 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) diff --git a/utils/loc_cfg.cpp b/utils/loc_cfg.cpp index af2ece68..3676bd58 100644 --- a/utils/loc_cfg.cpp +++ b/utils/loc_cfg.cpp @@ -57,14 +57,14 @@ /* Parameter data */ static uint32_t DEBUG_LEVEL = 0xff; static uint32_t TIMESTAMP = 0; -static uint32_t LOC_MODEM_EMULATOR = 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'}, - {"LOC_MODEM_EMULATOR", &LOC_MODEM_EMULATOR, NULL, 'n'}, + {"DATUM_TYPE", &DATUM_TYPE, NULL, 'n'}, }; static const int loc_param_num = sizeof(loc_param_table) / sizeof(loc_param_s_type); @@ -88,14 +88,26 @@ 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 +FUNCTION loc_get_datum_type DESCRIPTION - Provides access to Modem Emulator config item. + get datum type + +PARAMETERS: + N/A + +DEPENDENCIES + N/A + +RETURN VALUE + DATUM TYPE + +SIDE EFFECTS + N/A ===========================================================================*/ -uint32_t loc_modem_emulator_enabled() +int loc_get_datum_type() { - return LOC_MODEM_EMULATOR; + return DATUM_TYPE; } /*=========================================================================== diff --git a/utils/loc_cfg.h b/utils/loc_cfg.h index 3057abc2..aa001480 100644 --- a/utils/loc_cfg.h +++ b/utils/loc_cfg.h @@ -132,9 +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); - -uint32_t loc_modem_emulator_enabled(); - +int loc_get_datum_type(); #ifdef __cplusplus } #endif diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp index 5ef4277a..e9a8f848 100644 --- a/utils/loc_nmea.cpp +++ b/utils/loc_nmea.cpp @@ -33,9 +33,12 @@ #include #include #include +#include #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 @@ -111,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 @@ -605,6 +728,183 @@ 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 + +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 +931,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"); @@ -643,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; @@ -653,7 +964,26 @@ 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 = {}; + 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 + // 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 +997,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; @@ -808,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); @@ -828,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; @@ -971,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); @@ -994,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; @@ -1095,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 585a9c13..c6c83db4 100644 --- a/utils/loc_nmea.h +++ b/utils/loc_nmea.h @@ -35,11 +35,49 @@ #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); void loc_nmea_generate_pos(const UlpLocation &location, const GpsLocationExtended &locationExtended, + const LocationSystemInfo &systemInfo, unsigned char generate_nmea, std::vector &nmeaArraystr);