diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index 9a846c85..43b7e598 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -84,7 +84,8 @@ GnssAdapter::GnssAdapter() : mOdcpiInjectedPositionCount(0), mSystemStatus(SystemStatus::getInstance(mMsgTask)), mServerUrl(":"), - mXtraObserver(mSystemStatus->getOsObserver(), mMsgTask) + mXtraObserver(mSystemStatus->getOsObserver(), mMsgTask), + mBlockCPIInfo{} { LOC_LOGD("%s]: Constructor %p", __func__, this); mLocPositionMode.mode = LOC_POSITION_MODE_INVALID; @@ -1700,26 +1701,39 @@ GnssAdapter::injectLocationCommand(double latitude, double longitude, float accu struct MsgInjectLocation : public LocMsg { LocApiBase& mApi; ContextBase& mContext; + BlockCPIInfo& mBlockCPI; double mLatitude; double mLongitude; float mAccuracy; inline MsgInjectLocation(LocApiBase& api, ContextBase& context, + BlockCPIInfo& blockCPIInfo, double latitude, double longitude, float accuracy) : LocMsg(), mApi(api), mContext(context), + mBlockCPI(blockCPIInfo), mLatitude(latitude), mLongitude(longitude), mAccuracy(accuracy) {} inline virtual void proc() const { - mApi.injectPosition(mLatitude, mLongitude, mAccuracy); + if ((uptimeMillis() <= mBlockCPI.blockedTillTsMs) && + (fabs(mLatitude-mBlockCPI.latitude) <= mBlockCPI.latLonDiffThreshold) && + (fabs(mLongitude-mBlockCPI.longitude) <= mBlockCPI.latLonDiffThreshold)) { + + LOC_LOGD("%s]: positon injeciton blocked: lat: %f, lon: %f, accuracy: %f", + __func__, mLatitude, mLongitude, mAccuracy); + + } else { + mApi.injectPosition(mLatitude, mLongitude, mAccuracy); + } } }; - sendMsg(new MsgInjectLocation(*mLocApi, *mContext, latitude, longitude, accuracy)); + sendMsg(new MsgInjectLocation(*mLocApi, *mContext, mBlockCPIInfo, + latitude, longitude, accuracy)); } void @@ -1753,6 +1767,43 @@ GnssAdapter::injectTimeCommand(int64_t time, int64_t timeReference, int32_t unce sendMsg(new MsgInjectTime(*mLocApi, *mContext, time, timeReference, uncertainty)); } +// This command is to called to block the position to be injected to the modem. +// This can happen for network position that comes from modem. +void +GnssAdapter::blockCPICommand(double latitude, double longitude, + float accuracy, int blockDurationMsec, + double latLonDiffThreshold) +{ + struct MsgBlockCPI : public LocMsg { + BlockCPIInfo& mDstCPIInfo; + BlockCPIInfo mSrcCPIInfo; + + inline MsgBlockCPI(BlockCPIInfo& dstCPIInfo, + BlockCPIInfo& srcCPIInfo) : + mDstCPIInfo(dstCPIInfo), + mSrcCPIInfo(srcCPIInfo) {} + inline virtual void proc() const { + // in the same hal thread, save the cpi to be blocked + // the global variable + mDstCPIInfo = mSrcCPIInfo; + } + }; + + // construct the new block CPI info and queue on the same thread + // for processing + BlockCPIInfo blockCPIInfo; + blockCPIInfo.latitude = latitude; + blockCPIInfo.longitude = longitude; + blockCPIInfo.accuracy = accuracy; + blockCPIInfo.blockedTillTsMs = uptimeMillis() + blockDurationMsec; + blockCPIInfo.latLonDiffThreshold = latLonDiffThreshold; + + LOC_LOGD("%s]: block CPI lat: %f, lon: %f ", __func__, latitude, longitude); + // send a message to record down the coarse position + // to be blocked from injection in the master copy (mBlockCPIInfo) + sendMsg(new MsgBlockCPI(mBlockCPIInfo, blockCPIInfo)); +} + void GnssAdapter::addClientCommand(LocationAPI* client, const LocationCallbacks& callbacks) { @@ -3326,13 +3377,25 @@ void GnssAdapter::injectOdcpiCommand(const Location& location) void GnssAdapter::injectOdcpi(const Location& location) { - mLocApi->injectPosition(location, true); - if (mOdcpiRequestActive) { - mOdcpiInjectedPositionCount++; - if (mOdcpiInjectedPositionCount >= - ODCPI_INJECTED_POSITION_COUNT_PER_REQUEST) { - mOdcpiRequestActive = false; - mOdcpiInjectedPositionCount = 0; + if (LOCATION_HAS_LAT_LONG_BIT & location.flags) { + if ((uptimeMillis() <= mBlockCPIInfo.blockedTillTsMs) && + (fabs(location.latitude-mBlockCPIInfo.latitude) <= + mBlockCPIInfo.latLonDiffThreshold) && + (fabs(location.longitude-mBlockCPIInfo.longitude) <= + mBlockCPIInfo.latLonDiffThreshold)) { + + LOC_LOGD("%s]: positon injeciton blocked: lat: %f, lon: %f, accuracy: %f", + __func__, location.latitude, location.longitude, location.accuracy); + } else { + mLocApi->injectPosition(location, true); + if (mOdcpiRequestActive) { + mOdcpiInjectedPositionCount++; + if (mOdcpiInjectedPositionCount >= + ODCPI_INJECTED_POSITION_COUNT_PER_REQUEST) { + mOdcpiRequestActive = false; + mOdcpiInjectedPositionCount = 0; + } + } } } } diff --git a/gnss/GnssAdapter.h b/gnss/GnssAdapter.h index ad556877..6ed96552 100644 --- a/gnss/GnssAdapter.h +++ b/gnss/GnssAdapter.h @@ -76,6 +76,19 @@ typedef struct { uint32_t svIdOffset; } NmeaSvMeta; +typedef struct { + double latitude; + double longitude; + float accuracy; + // the CPI will be blocked until the boot time + // specified in blockedTillTsMs + int64_t blockedTillTsMs; + // CPIs whose both latitude and longitude differ + // no more than latLonThreshold will be blocked + // in units of degree + double latLonDiffThreshold; +} BlockCPIInfo; + using namespace loc_core; namespace loc_core { @@ -125,6 +138,9 @@ class GnssAdapter : public LocAdapterBase { std::string mMoServerUrl; XtraSystemStatusObserver mXtraObserver; + /* === Misc ===================================================================== */ + BlockCPIInfo mBlockCPIInfo; + /*==== CONVERSION ===================================================================*/ static void convertOptions(LocPosMode& out, const TrackingOptions& trackingOptions); static void convertLocation(Location& out, const UlpLocation& ulpLocation, @@ -337,6 +353,8 @@ public: void injectLocationCommand(double latitude, double longitude, float accuracy); void injectTimeCommand(int64_t time, int64_t timeReference, int32_t uncertainty); + void blockCPICommand(double latitude, double longitude, float accuracy, + int blockDurationMsec, double latLonDiffThreshold); }; #endif //GNSS_ADAPTER_H diff --git a/gnss/location_gnss.cpp b/gnss/location_gnss.cpp index 105145a4..800ea1b9 100644 --- a/gnss/location_gnss.cpp +++ b/gnss/location_gnss.cpp @@ -70,6 +70,9 @@ static void updateConnectionStatus(bool connected, int8_t type); static void odcpiInit(const OdcpiRequestCallback& callback); static void odcpiInject(const Location& location); +static void blockCPI(double latitude, double longitude, float accuracy, + int blockDurationMsec, double latLonDiffThreshold); + static const GnssInterface gGnssInterface = { sizeof(GnssInterface), initialize, @@ -101,6 +104,7 @@ static const GnssInterface gGnssInterface = { updateConnectionStatus, odcpiInit, odcpiInject, + blockCPI, }; #ifndef DEBUG_X86 @@ -327,3 +331,10 @@ static void odcpiInject(const Location& location) } } +static void blockCPI(double latitude, double longitude, float accuracy, + int blockDurationMsec, double latLonDiffThreshold) { + if (NULL != gGnssAdapter) { + gGnssAdapter->blockCPICommand(latitude, longitude, accuracy, + blockDurationMsec, latLonDiffThreshold); + } +} \ No newline at end of file diff --git a/location/location_interface.h b/location/location_interface.h index 3c1558e9..359cb2be 100644 --- a/location/location_interface.h +++ b/location/location_interface.h @@ -63,6 +63,8 @@ struct GnssInterface { void (*updateConnectionStatus)(bool connected, int8_t type); void (*odcpiInit)(const OdcpiRequestCallback& callback); void (*odcpiInject)(const Location& location); + void (*blockCPI)(double latitude, double longitude, float accuracy, + int blockDurationMsec, double latLonDiffThreshold); }; struct FlpInterface {