Skip to content

Commit

Permalink
Merge "GNSS HAL: Support GPTP time in position reports, GPTP time, El…
Browse files Browse the repository at this point in the history 
…apsed real time in Meas reports"
  • Loading branch information
qctecmdr authored and Gerrit - the friendly Code Review server committed Dec 23, 2023
2 parents 9a97004 + 129dc2e commit 74b2a9d
Show file tree
Hide file tree
Showing 7 changed files with 119 additions and 61 deletions.
42 changes: 27 additions & 15 deletions core/LocApiBase.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <LocContext.h>
#include <loc_misc_utils.h>

#ifdef PTP_SUPPORTED
#include <gptp_helper.h>
#endif

namespace loc_core {

#define MSEC_IN_ONE_WEEK 604800000LL
Expand Down Expand Up @@ -373,11 +377,11 @@ void LocApiBase::reportPosition(UlpLocation& location,
{
// print the location info before delivering
LOC_LOGd("\n flags: 0x%x\n source: %d\n latitude: %f\n longitude: %f\n "
"altitude: %f\n speed: %f\n bearing: %f\n accuracy: %f\n "
"timestamp: %" PRId64 "\n "
"session status: %d\n technology mask: 0x%x\n time bias unc %f msec\n "
"SV used in fix (gps/glo/bds/gal/qzss/navic) : \n"
"(0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 ")",
"altitude: %f\n speed: %f\n bearing: %f\n accuracy: %f\n "
"timestamp: %" PRId64 "\n "
"session status: %d\n technology mask: 0x%x\n time bias unc %f msec\n "
"SV used in fix (gps/glo/bds/gal/qzss/navic) : \n"
"(0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 "/0x%" PRIx64 ")",
location.gpsLocation.flags, location.position_source,
location.gpsLocation.latitude, location.gpsLocation.longitude,
location.gpsLocation.altitude, location.gpsLocation.speed,
Expand Down Expand Up @@ -1017,7 +1021,7 @@ DEFAULT_IMPL()
void LocApiBase::configOsnmaEnablement(bool /*enable*/, LocApiResponse* /*adapterResponse*/)
DEFAULT_IMPL()

int64_t ElapsedRealtimeEstimator::getElapsedRealtimeEstimateNanos(int64_t curDataTimeNanos,
int64_t RealtimeEstimator::getElapsedRealtimeEstimateNanos(int64_t curDataTimeNanos,
bool isCurDataTimeTrustable, int64_t tbfNanos) {
//The algorithm works follow below steps:
//When isCurDataTimeTrustable is meet (means Modem timestamp is already stable),
Expand Down Expand Up @@ -1069,7 +1073,7 @@ int64_t ElapsedRealtimeEstimator::getElapsedRealtimeEstimateNanos(int64_t curDat
return (sinceBootTimeNanos - currentTravelTimeNanos);
}

void ElapsedRealtimeEstimator::reset() {
void RealtimeEstimator::reset() {
mCurrentClockDiff = 0;
mPrevDataTimeNanos = 0;
mPrevUtcTimeNanos = 0;
Expand All @@ -1079,7 +1083,7 @@ void ElapsedRealtimeEstimator::reset() {
memset(&mTimePairMeasReport, 0, sizeof(mTimePairMeasReport));
}

int64_t ElapsedRealtimeEstimator::getElapsedRealtimeQtimer(int64_t qtimerTicksAtOrigin) {
int64_t RealtimeEstimator::getElapsedRealtimeQtimer(int64_t qtimerTicksAtOrigin) {
struct timespec currentTime = {};
int64_t sinceBootTimeNanos = 0;
int64_t elapsedRealTimeNanos = 0;
Expand Down Expand Up @@ -1118,7 +1122,7 @@ int64_t ElapsedRealtimeEstimator::getElapsedRealtimeQtimer(int64_t qtimerTicksAt
return elapsedRealTimeNanos;
}

void ElapsedRealtimeEstimator::saveGpsTimeAndQtimerPairInPvtReport(
void RealtimeEstimator::saveGpsTimeAndQtimerPairInPvtReport(
const GpsLocationExtended& locationExtended) {

// Use GPS timestamp and qtimer tick for 1Hz PVT report for association
Expand All @@ -1138,7 +1142,7 @@ void ElapsedRealtimeEstimator::saveGpsTimeAndQtimerPairInPvtReport(
}
}

void ElapsedRealtimeEstimator::saveGpsTimeAndQtimerPairInMeasReport(
void RealtimeEstimator::saveGpsTimeAndQtimerPairInMeasReport(
const GnssSvMeasurementSet& svMeasurementSet) {

const GnssSvMeasurementHeader& svMeasSetHeader = svMeasurementSet.svMeasSetHeader;
Expand All @@ -1164,9 +1168,10 @@ void ElapsedRealtimeEstimator::saveGpsTimeAndQtimerPairInMeasReport(
}
}

bool ElapsedRealtimeEstimator::getElapsedRealtimeForGpsTime(
bool RealtimeEstimator::fillAdditionalTimestamps(
const GpsLocationExtended& locationExtended,
int64_t &bootTimeNsAtOrigin, float & bootTimeUnc) {
int64_t &bootTimeNsAtOrigin, float &bootTimeUnc,
uint64_t &gptpTime, bool &gPTPValidity) {
struct timespec curBootTime = {};
int64_t curBootTimeNs = 0;
int64_t curQTimerNSec = 0;
Expand Down Expand Up @@ -1211,13 +1216,20 @@ bool ElapsedRealtimeEstimator::getElapsedRealtimeForGpsTime(
bootTimeNsAtOrigin = curBootTimeNs - (curQTimerNSec - qtimerNsecAtOrigin);

bootTimeUnc = timePair.timeUncMsec;
#ifdef PTP_SUPPORTED
if (gptpGetPtpTimeFromQTimeNs(&gptpTime, qtimerNsecAtOrigin)) {
gPTPValidity = true;
}
#endif

LOC_LOGv("gpsTimeAtOrigin (%d, %d), timepair: gps (%d, %d), "
"qtimer nsec =%" PRIi64 ", curQTimerNSec=%" PRIi64 " qtimerNsecAtOrigin=%" PRIi64 ""
" curBoottimeNSec=%" PRIi64 " bootimeNsecAtOrigin=%" PRIi64 ", boottime unc =%f",
" curBoottimeNSec=%" PRIi64 " bootimeNsecAtOrigin=%" PRIi64 ", boottime unc =%f"
" gptp Time =%" PRIu64 " gPTPValidity = %d",
gpsTimeAtOrigin.gpsWeek, gpsTimeAtOrigin.gpsTimeOfWeekMs,
timePair.gpsTime.gpsWeek, timePair.gpsTime.gpsTimeOfWeekMs,
timePairQtimerNsec, curQTimerNSec, qtimerNsecAtOrigin,
curBootTimeNs, bootTimeNsAtOrigin, bootTimeUnc);
curBootTimeNs, bootTimeNsAtOrigin, bootTimeUnc, gptpTime, gPTPValidity);

if (bootTimeNsAtOrigin > 0) {
return true;
Expand All @@ -1226,7 +1238,7 @@ bool ElapsedRealtimeEstimator::getElapsedRealtimeForGpsTime(
}
}

bool ElapsedRealtimeEstimator::getCurrentTime(
bool RealtimeEstimator::getCurrentTime(
struct timespec& currentTime, int64_t& sinceBootTimeNanos)
{
struct timespec sinceBootTime = {};
Expand Down
15 changes: 7 additions & 8 deletions core/LocApiBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,14 +107,12 @@ class LocAdapterBase;
struct LocSsrMsg;
struct LocOpenMsg;

typedef struct
{
typedef struct {
uint32_t accumulatedDistance;
uint32_t numOfBatchedPositions;
} LocApiBatchData;

typedef struct
{
typedef struct {
uint32_t hwId;
} LocApiGeofenceData;

Expand Down Expand Up @@ -409,7 +407,7 @@ class LocApiBase {
virtual void configOsnmaEnablement(bool enable, LocApiResponse* adapterResponse=nullptr);
};

class ElapsedRealtimeEstimator {
class RealtimeEstimator {
typedef struct {
GPSTimeStruct gpsTime;
int64_t qtimerTick;
Expand All @@ -430,7 +428,7 @@ class ElapsedRealtimeEstimator {
GpsTimeQtimerTickPair mTimePairMeasReport;

public:
inline ElapsedRealtimeEstimator(int64_t travelTimeNanosEstimate) :
inline RealtimeEstimator(int64_t travelTimeNanosEstimate) :
mInitialTravelTime(travelTimeNanosEstimate) {
reset();
}
Expand All @@ -439,8 +437,9 @@ class ElapsedRealtimeEstimator {
inline int64_t getElapsedRealtimeUncNanos() { return 5000000;}
void reset();
static int64_t getElapsedRealtimeQtimer(int64_t qtimerTicksAtOrigin);
bool getElapsedRealtimeForGpsTime(const GpsLocationExtended& locationExtended,
int64_t &elapsedTime, float & elpasedTimeUnc);
bool fillAdditionalTimestamps(const GpsLocationExtended& locationExtended,
int64_t &elapsedTime, float & elpasedTimeUnc,
uint64_t &gptpTime, bool &gPTPValidity);
void saveGpsTimeAndQtimerPairInPvtReport(const GpsLocationExtended& locationExtended);
void saveGpsTimeAndQtimerPairInMeasReport(const GnssSvMeasurementSet& svMeasurementSet);
static bool getCurrentTime(struct timespec& currentTime, int64_t& sinceBootTimeNanos);
Expand Down
8 changes: 8 additions & 0 deletions core/Makefile.am
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,10 @@ libloc_core_la_c_sources = \
if USE_EXTERNAL_AP
AM_CFLAGS += -DFEATURE_EXTERNAL_AP
endif
if PTP_SUPPORTED
AM_CFLAGS += -DPTP_SUPPORTED
AM_CFLAGS += -I$(WORKSPACE)/external/open-avb/lib/libgptp
endif

library_includedir = $(pkgincludedir)

Expand All @@ -68,6 +72,10 @@ endif

libloc_core_la_LIBADD = -ldl $(GPSUTILS_LIBS)

if PTP_SUPPORTED
libloc_core_la_LIBADD += -lgptp
endif

#Create and Install libraries
lib_LTLIBRARIES = libloc_core.la

Expand Down
10 changes: 10 additions & 0 deletions core/configure.ac
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,16 @@ fi

AM_CONDITIONAL(USE_GLIB, test "x${with_glib}" = "xyes")

AC_ARG_ENABLE([ptp],
[ --enable-ptp enable ptp],
[case "${enableval}" in
yes) ptp=true ;;
no) ptp=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-ptp]) ;;
esac],[ptp=false])

AM_CONDITIONAL(PTP_SUPPORTED, [test x$ptp = xtrue])

AC_ARG_WITH([auto_feature],
AC_HELP_STRING([--with-auto_feature=@<:@dir@:>@],
[Using Automotive feature]),
Expand Down
70 changes: 43 additions & 27 deletions gnss/GnssAdapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ static int loadEngHubForExternalEngine = 0;
static int sUseZppInDBH = 0;
static loc_param_s_type izatConfParamTable[] = {
{"LOAD_ENGHUB_FOR_EXTERNAL_ENGINE", &loadEngHubForExternalEngine, nullptr, 'n'},
{"USE_ZPP_IN_DBH", &sUseZppInDBH, nullptr,'n'}
{"USE_ZPP_IN_DBH", &sUseZppInDBH, nullptr, 'n'}
};

/* Method to fetch status cb from loc_net_iface library */
Expand Down Expand Up @@ -463,37 +463,52 @@ GnssAdapter::convertLocation(Location& out, const UlpLocation& ulpLocation,
}

void GnssAdapter::fillElapsedRealTime(const GpsLocationExtended& locationExtended,
Location& out) {
GnssLocationInfoNotification& out) {
if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) {
int64_t elapsedTimeNs = 0;
float elapsedTimeUncMsec = 0.0;
if (mPositionElapsedRealTimeCal.getElapsedRealtimeForGpsTime(
locationExtended, elapsedTimeNs, elapsedTimeUncMsec)) {
out.flags |= LOCATION_HAS_ELAPSED_REAL_TIME_BIT;
out.elapsedRealTime = elapsedTimeNs;
out.elapsedRealTimeUnc = (int64_t) (elapsedTimeUncMsec * 1000000);
uint64_t elapsedgPTPTimeNsec = 0;
bool gptpTimeValid = false;
if (mPositionElapsedRealTimeCal.fillAdditionalTimestamps(
locationExtended, elapsedTimeNs, elapsedTimeUncMsec,
elapsedgPTPTimeNsec, gptpTimeValid)) {
out.location.flags |= LOCATION_HAS_ELAPSED_REAL_TIME_BIT;
out.location.elapsedRealTime = elapsedTimeNs;
out.location.elapsedRealTimeUnc = (int64_t) (elapsedTimeUncMsec * 1000000);

if (gptpTimeValid) {
out.flags |= LDT_GNSS_LOCATION_INFO_GPTP_TIME_BIT;
out.elapsedgPTPTime = elapsedgPTPTimeNsec;
out.flags |= LDT_GNSS_LOCATION_INFO_GPTP_TIME_UNC_BIT;
out.elapsedgPTPTimeUnc = 0;
} else {
out.elapsedgPTPTime = 0;
out.elapsedgPTPTimeUnc = 0;
}
}
#ifndef FEATURE_AUTOMOTIVE
else if ((out.timestamp > 0) &&
else if ((out.location.timestamp > 0) &&
(locationExtended.gpsTime.gpsWeek != UNKNOWN_GPS_WEEK_NUM)) {
int64_t locationTimeNanos = (int64_t)out.timestamp * 1000000;
bool isCurDataTimeTrustable = (out.timestamp % mLocPositionMode.min_interval == 0);
int64_t locationTimeNanos = (int64_t)out.location.timestamp * 1000000;
bool isCurDataTimeTrustable =
(out.location.timestamp % mLocPositionMode.min_interval == 0);
int64_t elapsedRealTime = mPositionElapsedRealTimeCal.getElapsedRealtimeEstimateNanos(
locationTimeNanos, isCurDataTimeTrustable,
(int64_t)mLocPositionMode.min_interval * 1000000);

if (elapsedRealTime != -1) {
out.flags |= LOCATION_HAS_ELAPSED_REAL_TIME_BIT;
out.elapsedRealTime = elapsedRealTime;
out.elapsedRealTimeUnc = mPositionElapsedRealTimeCal.getElapsedRealtimeUncNanos();
out.location.flags |= LOCATION_HAS_ELAPSED_REAL_TIME_BIT;
out.location.elapsedRealTime = elapsedRealTime;
out.location.elapsedRealTimeUnc =
mPositionElapsedRealTimeCal.getElapsedRealtimeUncNanos();
}
}
#endif //FEATURE_AUTOMOTIVE
}
#ifndef FEATURE_AUTOMOTIVE
if (!(out.flags & LOCATION_HAS_ELAPSED_REAL_TIME_BIT)) {
out.elapsedRealTime = getBootTimeMilliSec() * 1000000;
out.elapsedRealTimeUnc = mPositionElapsedRealTimeCal.getElapsedRealtimeUncNanos();
if (!(out.location.flags & LOCATION_HAS_ELAPSED_REAL_TIME_BIT)) {
out.location.elapsedRealTime = getBootTimeMilliSec() * 1000000;
out.location.elapsedRealTimeUnc = mPositionElapsedRealTimeCal.getElapsedRealtimeUncNanos();
}
#endif //FEATURE_AUTOMOTIVE
}
Expand Down Expand Up @@ -2032,7 +2047,7 @@ GnssAdapter::convertToGnssSvIdConfig(
uint64_t* svMaskPtr = NULL;
GnssSvId initialSvId = 0;
uint16_t svIndexOffset = 0;
switch(source.constellation) {
switch (source.constellation) {
case GNSS_SV_TYPE_GLONASS:
svMaskPtr = &config.gloBlacklistSvMask;
initialSvId = GNSS_SV_CONFIG_GLO_INITIAL_SV_ID;
Expand Down Expand Up @@ -3639,7 +3654,7 @@ GnssAdapter::updateTracking(LocationAPI* client, uint32_t sessionId,
// use a local copy of TrackingOptions as the TBF may get modified in the
// checkAndSetSPEToRunforNHz function
TrackingOptions tempOptions(updatedOptions);
if(!checkAndSetSPEToRunforNHz(tempOptions)) {
if (!checkAndSetSPEToRunforNHz(tempOptions)) {
mLocApi->startTimeBasedTracking(tempOptions, new LocApiResponse(*getContext(),
[this, client, sessionId, oldOptions] (LocationError err) {
if (ENGINE_LOCK_STATE_DISABLED != mLocApi->getEngineLockState() &&
Expand Down Expand Up @@ -4440,7 +4455,8 @@ bool GnssAdapter::needToGenerateNmeaReport(const uint32_t &gpsTimeOfWeekMs,
bool retVal = false;
uint64_t currentTimeNsec = 0;

if (NMEA_PROVIDER_AP == ContextBase::mGps_conf.NMEA_PROVIDER && !mTimeBasedTrackingSessions.empty()) {
if (NMEA_PROVIDER_AP == ContextBase::mGps_conf.NMEA_PROVIDER &&
!mTimeBasedTrackingSessions.empty()) {
currentTimeNsec = (apTimeStamp.tv_sec * BILLION_NSEC + apTimeStamp.tv_nsec);
if ((GNSS_NMEA_REPORT_RATE_NHZ == ContextBase::sNmeaReportRate) ||
(GPS_DEFAULT_FIX_INTERVAL_MS <= mLocPositionMode.min_interval)) {
Expand Down Expand Up @@ -4660,7 +4676,7 @@ GnssAdapter::reportPosition(const UlpLocation& ulpLocation,
list<trackingCallback> cbRunnables;
convertLocationInfo(locationInfo, locationExtended, status);
convertLocation(locationInfo.location, ulpLocation, locationExtended);
fillElapsedRealTime(locationExtended, locationInfo.location);
fillElapsedRealTime(locationExtended, locationInfo);
logLatencyInfo();

for (auto it=mClientData.begin(); it != mClientData.end(); ++it) {
Expand Down Expand Up @@ -4803,7 +4819,7 @@ GnssAdapter::reportEnginePositions(unsigned int count,
engLocation->location,
engLocation->locationExtended);
fillElapsedRealTime(engLocation->locationExtended,
locationInfo[i].location);
locationInfo[i]);
}

reportPositionNmea(engLocation->location,
Expand Down Expand Up @@ -6052,7 +6068,7 @@ GnssAdapter::invokeGnssEnergyConsumedCallback(uint64_t energyConsumedSinceFirstB
mBootReferenceEnergy = energyConsumedSinceFirstBoot;
if (NULL != (fp = fopen("/data/vendor/location/energy.conf", "a+b"))) {
rewind(fp);
if (ElapsedRealtimeEstimator::getCurrentTime(currentTime, sinceBootTimeNanos)) {
if (RealtimeEstimator::getCurrentTime(currentTime, sinceBootTimeNanos)) {
LOC_LOGv("sinceBootTimeNanos: %" PRIu64 " ", sinceBootTimeNanos);
if ((uint32_t)(sinceBootTimeNanos / 1000000000) > 30) {
int fr = fread(&mBootReferenceEnergy, sizeof(mBootReferenceEnergy), 1, fp);
Expand Down Expand Up @@ -6547,7 +6563,7 @@ void GnssAdapter::convertSatelliteInfo(std::vector<GnssDebugSatelliteInfo>& out,
}

// extract each sv info from systemstatus report
for(uint32_t i=0; i<svid_num && (svid_idx+i)<SV_ALL_NUM; i++) {
for (uint32_t i=0; i<svid_num && (svid_idx+i)<SV_ALL_NUM; i++) {

GnssDebugSatelliteInfo s = {};
s.size = sizeof(s);
Expand Down Expand Up @@ -6610,7 +6626,7 @@ bool GnssAdapter::getDebugReport(GnssDebugReport& r)

// location block
r.mLocation.size = sizeof(r.mLocation);
if(!reports.mLocation.empty() && reports.mLocation.back().mValid) {
if (!reports.mLocation.empty() && reports.mLocation.back().mValid) {
r.mLocation.mValid = true;
r.mLocation.mLocation.latitude =
reports.mLocation.back().mLocation.gpsLocation.latitude;
Expand All @@ -6635,7 +6651,7 @@ bool GnssAdapter::getDebugReport(GnssDebugReport& r)
r.mLocation.mUtcReported =
reports.mLocation.back().mUtcReported;
}
else if(!reports.mBestPosition.empty() && reports.mBestPosition.back().mValid) {
else if (!reports.mBestPosition.empty() && reports.mBestPosition.back().mValid) {
r.mLocation.mValid = true;
r.mLocation.mLocation.latitude =
(double)(reports.mBestPosition.back().mBestLat) * RAD2DEG;
Expand All @@ -6661,7 +6677,7 @@ bool GnssAdapter::getDebugReport(GnssDebugReport& r)

// time block
r.mTime.size = sizeof(r.mTime);
if(!reports.mTimeAndClock.empty() && reports.mTimeAndClock.back().mTimeValid) {
if (!reports.mTimeAndClock.empty() && reports.mTimeAndClock.back().mTimeValid) {
r.mTime.mValid = true;
r.mTime.timeEstimate =
(((int64_t)(reports.mTimeAndClock.back().mGpsWeek)*7 +
Expand Down Expand Up @@ -8150,7 +8166,7 @@ GnssAdapter::initEngHubProxy() {
};

getEngHubProxyFn* getter = (getEngHubProxyFn*) dlsym(handle, "getEngHubProxy");
if(getter != nullptr) {
if (getter != nullptr) {
// Wait for the script(rootdir/etc/init.qcom.rc) to create socket folder
locUtilWaitForDir(SOCKET_DIR_EHUB);
EngineHubProxyBase* hubProxy = (*getter) (mMsgTask, mSystemStatus->getOsObserver(),
Expand Down
Loading

0 comments on commit 74b2a9d

Please sign in to comment.