MiPad4/android_device_xiaomi_sdm660-common
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge location.lnx.4.0 into location.lnx.5.0

Browse source 
Fastforward from location.lnx.4.0

Change-Id: I08144304a318a075ba27f6b3dc5947606ffcbcef
This commit is contained in:
Dante Russo 2018-12-18 14:14:30 -08:00
commit 2f34b1ab42
15 changed files with 653 additions and 85 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
android/1.1/GnssGeofencing.cpp
View file

@ -53,7 +53,7 @@ GnssGeofencing::~GnssGeofencing() {
// Methods from ::android::hardware::gnss::V1_0::IGnssGeofencing follow.
Return<void> GnssGeofencing::setCallback(const sp<IGnssGeofenceCallback>& callback) {
if (mApi != nullptr) {
LOC_LOGE("%s]: mApi is NOT nullptr", __FUNCTION__);
LOC_LOGd("mApi is NOT nullptr");
return Void();
}

17
core/SystemStatusOsObserver.cpp
View file

@ -97,6 +97,7 @@ void SystemStatusOsObserver::subscribe(const list<DataItemId>& l, IDataItemObser
list<DataItemId>& l, IDataItemObserver* client, bool requestData) :
mParent(parent), mClient(client),
mDataItemSet(containerTransfer<list<DataItemId>, unordered_set<DataItemId>>(l)),
diItemlist(l),
mToRequestData(requestData) {}
void proc() const {
@ -107,16 +108,13 @@ void SystemStatusOsObserver::subscribe(const list<DataItemId>& 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<unordered_set<DataItemId>, list<DataItemId>>(
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<unordered_set<DataItemId>, list<DataItemId>>(
std::move(dataItemsToSubscribe)),
@ -127,6 +125,7 @@ void SystemStatusOsObserver::subscribe(const list<DataItemId>& l, IDataItemObser
mutable SystemStatusOsObserver* mParent;
IDataItemObserver* mClient;
const unordered_set<DataItemId> mDataItemSet;
const list<DataItemId> diItemlist;
bool mToRequestData;
};

13
etc/gps.conf
View file

@ -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
################################

43
gnss/GnssAdapter.cpp
View file

@ -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<std::string> 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;
}

5
gnss/GnssAdapter.h
View file

@ -128,6 +128,8 @@ typedef std::function<void(
uint64_t gnssEnergyConsumedFromFirstBoot
)> 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);

8
gnss/location_gnss.cpp
View file

@ -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);
}
}
}

148
location/LocationAPI.cpp
View file

@ -36,15 +36,37 @@
#include <map>
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<LocationAPI*, LocationAPIDestroyCbData>
LocationClientDestroyCbMap;
typedef std::map<LocationAPI*, LocationCallbacks> 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

12
location/LocationAPI.h
View file

@ -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

3
location/LocationDataTypes.h
View file

@ -1274,6 +1274,9 @@ typedef std::function<void(
LocationSystemInfo locationSystemInfo
)> locationSystemInfoCallback;
typedef std::function<void(
)> locationApiDestroyCompleteCallback;
typedef struct {
size_t size; // set to sizeof(LocationCallbacks)
capabilitiesCallback capabilitiesCb; // mandatory

8
location/location_interface.h
View file

@ -46,12 +46,14 @@ typedef std::function<void(
uint64_t gnssEnergyConsumedFromFirstBoot
)> 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);

2
pla/android/loc_pla.h
View file

@ -42,6 +42,8 @@ extern "C" {
#include <cutils/threads.h>
#include <cutils/sched_policy.h>
#include <cutils/android_filesystem_config.h>
#include <string.h>
#include <stdlib.h>
#define UID_GPS (AID_GPS)
#define GID_GPS (AID_GPS)

24
utils/loc_cfg.cpp
View file

@ -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;
}
/*===========================================================================

4
utils/loc_cfg.h
View file

@ -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

411
utils/loc_nmea.cpp
View file

@ -33,9 +33,12 @@
#include <math.h>
#include <log_util.h>
#include <loc_pla.h>
#include <loc_cfg.h>
#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<std::string> &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 {

38
utils/loc_nmea.h
View file

@ -35,11 +35,49 @@
#include <string>
#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<std::string> &nmeaArraystr);
void loc_nmea_generate_pos(const UlpLocation &location,
const GpsLocationExtended &locationExtended,
const LocationSystemInfo &systemInfo,
unsigned char generate_nmea,
std::vector<std::string> &nmeaArraystr);