diff --git a/core/SystemStatus.cpp b/core/SystemStatus.cpp index 3310b8bb..1adeb66a 100644 --- a/core/SystemStatus.cpp +++ b/core/SystemStatus.cpp @@ -47,7 +47,6 @@ class SystemStatusNmeaBase { protected: std::vector mField; - timespec setUtcTime(std::string sutctime); SystemStatusNmeaBase(const char *str_in, uint32_t len_in) { @@ -86,31 +85,12 @@ public: static const uint32_t NMEA_MAXSIZE = DEBUG_NMEA_MAXSIZE; }; -timespec SystemStatusNmeaBase::setUtcTime(std::string sutctime) -{ - timespec ts = { 0ULL, 0ULL }; - uint64_t utctime_ns = atof(sutctime.c_str()) * 1000000000ULL; - ts.tv_nsec = utctime_ns % 1000000000ULL; - uint64_t utctime_s = utctime_ns / 1000000000ULL; - - uint64_t hour = utctime_s / 10000ULL; - uint64_t min = (utctime_s / 100LL) % 100ULL; - uint64_t sec = utctime_s % 100ULL; - ts.tv_sec = hour * 3600ULL + min * 60ULL + sec; - - timeval tv; - gettimeofday(&tv, NULL); - ts.tv_sec += (uint64_t(tv.tv_sec / (24ULL * 60ULL * 60ULL))) * (24ULL * 60ULL * 60ULL); - return ts; -} - /****************************************************************************** SystemStatusPQWM1 ******************************************************************************/ class SystemStatusPQWM1 { public: - timespec mUtcTime; uint16_t mGpsWeek; // x1 uint32_t mGpsTowMs; // x2 uint8_t mTimeValid; // x3 @@ -199,11 +179,6 @@ public: return; } memset(&mM1, 0, sizeof(mM1)); - - timeval tv; - gettimeofday(&tv, NULL); - mM1.mUtcTime.tv_sec = tv.tv_sec; - mM1.mUtcTime.tv_nsec = tv.tv_usec * 1000UL; mM1.mGpsWeek = atoi(mField[eGpsWeek].c_str()); mM1.mGpsTowMs = atoi(mField[eGpsTowMs].c_str()); mM1.mTimeValid = atoi(mField[eTimeValid].c_str()); @@ -237,7 +212,6 @@ public: class SystemStatusPQWP1 { public: - timespec mUtcTime; uint8_t mEpiValidity; // x4 float mEpiLat; // x5 float mEpiLon; // x6 @@ -266,7 +240,6 @@ private: SystemStatusPQWP1 mP1; public: - inline timespec getUtcTime() { return mP1.mUtcTime; } inline uint8_t getEpiValidity() { return mP1.mEpiValidity; } inline float getEpiLat() { return mP1.mEpiLat; } inline float getEpiLon() { return mP1.mEpiLon; } @@ -282,7 +255,6 @@ public: return; } memset(&mP1, 0, sizeof(mP1)); - mP1.mUtcTime = setUtcTime(mField[eUtcTime]); mP1.mEpiValidity = strtol(mField[eEpiValidity].c_str(), NULL, 16); mP1.mEpiLat = atof(mField[eEpiLat].c_str()); mP1.mEpiLon = atof(mField[eEpiLon].c_str()); @@ -301,7 +273,6 @@ public: class SystemStatusPQWP2 { public: - timespec mUtcTime; float mBestLat; // x4 float mBestLon; // x5 float mBestAlt; // x6 @@ -339,7 +310,6 @@ public: return; } memset(&mP2, 0, sizeof(mP2)); - mP2.mUtcTime = setUtcTime(mField[eUtcTime]); mP2.mBestLat = atof(mField[eBestLat].c_str()); mP2.mBestLon = atof(mField[eBestLon].c_str()); mP2.mBestAlt = atof(mField[eBestAlt].c_str()); @@ -356,7 +326,6 @@ public: class SystemStatusPQWP3 { public: - timespec mUtcTime; uint8_t mXtraValidMask; uint32_t mGpsXtraAge; uint32_t mGloXtraAge; @@ -412,7 +381,6 @@ public: return; } memset(&mP3, 0, sizeof(mP3)); - mP3.mUtcTime = setUtcTime(mField[eUtcTime]); mP3.mXtraValidMask = strtol(mField[eXtraValidMask].c_str(), NULL, 16); mP3.mGpsXtraAge = atoi(mField[eGpsXtraAge].c_str()); mP3.mGloXtraAge = atoi(mField[eGloXtraAge].c_str()); @@ -435,7 +403,6 @@ public: class SystemStatusPQWP4 { public: - timespec mUtcTime; uint32_t mGpsEpheValid; uint32_t mGloEpheValid; uint64_t mBdsEpheValid; @@ -473,7 +440,6 @@ public: return; } memset(&mP4, 0, sizeof(mP4)); - mP4.mUtcTime = setUtcTime(mField[eUtcTime]); mP4.mGpsEpheValid = strtol(mField[eGpsEpheValid].c_str(), NULL, 16); mP4.mGloEpheValid = strtol(mField[eGloEpheValid].c_str(), NULL, 16); mP4.mBdsEpheValid = strtol(mField[eBdsEpheValid].c_str(), NULL, 16); @@ -490,7 +456,6 @@ public: class SystemStatusPQWP5 { public: - timespec mUtcTime; uint32_t mGpsUnknownMask; uint32_t mGloUnknownMask; uint64_t mBdsUnknownMask; @@ -558,7 +523,6 @@ public: return; } memset(&mP5, 0, sizeof(mP5)); - mP5.mUtcTime = setUtcTime(mField[eUtcTime]); mP5.mGpsUnknownMask = strtol(mField[eGpsUnknownMask].c_str(), NULL, 16); mP5.mGloUnknownMask = strtol(mField[eGloUnknownMask].c_str(), NULL, 16); mP5.mBdsUnknownMask = strtol(mField[eBdsUnknownMask].c_str(), NULL, 16); @@ -585,7 +549,6 @@ public: class SystemStatusPQWP6 { public: - timespec mUtcTime; uint32_t mFixInfoMask; }; @@ -611,7 +574,6 @@ public: return; } memset(&mP6, 0, sizeof(mP6)); - mP6.mUtcTime = setUtcTime(mField[eUtcTime]); mP6.mFixInfoMask = strtol(mField[eFixInfoMask].c_str(), NULL, 16); } @@ -624,7 +586,6 @@ public: class SystemStatusPQWS1 { public: - timespec mUtcTime; uint32_t mFixInfoMask; uint32_t mHepeLimit; }; @@ -653,7 +614,6 @@ public: return; } memset(&mS1, 0, sizeof(mS1)); - mS1.mUtcTime = setUtcTime(mField[eUtcTime]); mS1.mFixInfoMask = atoi(mField[eFixInfoMask].c_str()); mS1.mHepeLimit = atoi(mField[eHepeLimit].c_str()); } @@ -665,7 +625,6 @@ public: SystemStatusTimeAndClock ******************************************************************************/ SystemStatusTimeAndClock::SystemStatusTimeAndClock(const SystemStatusPQWM1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mGpsWeek(nmea.mGpsWeek), mGpsTowMs(nmea.mGpsTowMs), mTimeValid(nmea.mTimeValid), @@ -708,7 +667,6 @@ void SystemStatusTimeAndClock::dump() SystemStatusXoState ******************************************************************************/ SystemStatusXoState::SystemStatusXoState(const SystemStatusPQWM1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mXoState(nmea.mXoState) { } @@ -733,7 +691,6 @@ void SystemStatusXoState::dump() SystemStatusRfAndParams ******************************************************************************/ SystemStatusRfAndParams::SystemStatusRfAndParams(const SystemStatusPQWM1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mPgaGain(nmea.mPgaGain), mGpsBpAmpI(nmea.mGpsBpAmpI), mGpsBpAmpQ(nmea.mGpsBpAmpQ), @@ -796,7 +753,6 @@ void SystemStatusRfAndParams::dump() SystemStatusErrRecovery ******************************************************************************/ SystemStatusErrRecovery::SystemStatusErrRecovery(const SystemStatusPQWM1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mRecErrorRecovery(nmea.mRecErrorRecovery) { } @@ -821,7 +777,6 @@ void SystemStatusErrRecovery::dump() SystemStatusInjectedPosition ******************************************************************************/ SystemStatusInjectedPosition::SystemStatusInjectedPosition(const SystemStatusPQWP1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mEpiValidity(nmea.mEpiValidity), mEpiLat(nmea.mEpiLat), mEpiLon(nmea.mEpiLon), @@ -864,7 +819,6 @@ void SystemStatusInjectedPosition::dump() SystemStatusBestPosition ******************************************************************************/ SystemStatusBestPosition::SystemStatusBestPosition(const SystemStatusPQWP2& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mBestLat(nmea.mBestLat), mBestLon(nmea.mBestLon), mBestAlt(nmea.mBestAlt), @@ -901,7 +855,6 @@ void SystemStatusBestPosition::dump() SystemStatusXtra ******************************************************************************/ SystemStatusXtra::SystemStatusXtra(const SystemStatusPQWP3& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mXtraValidMask(nmea.mXtraValidMask), mGpsXtraAge(nmea.mGpsXtraAge), mGloXtraAge(nmea.mGloXtraAge), @@ -956,7 +909,6 @@ void SystemStatusXtra::dump() SystemStatusEphemeris ******************************************************************************/ SystemStatusEphemeris::SystemStatusEphemeris(const SystemStatusPQWP4& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mGpsEpheValid(nmea.mGpsEpheValid), mGloEpheValid(nmea.mGloEpheValid), mBdsEpheValid(nmea.mBdsEpheValid), @@ -993,7 +945,6 @@ void SystemStatusEphemeris::dump() SystemStatusSvHealth ******************************************************************************/ SystemStatusSvHealth::SystemStatusSvHealth(const SystemStatusPQWP5& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mGpsUnknownMask(nmea.mGpsUnknownMask), mGloUnknownMask(nmea.mGloUnknownMask), mBdsUnknownMask(nmea.mBdsUnknownMask), @@ -1060,7 +1011,6 @@ void SystemStatusSvHealth::dump() SystemStatusPdr ******************************************************************************/ SystemStatusPdr::SystemStatusPdr(const SystemStatusPQWP6& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mFixInfoMask(nmea.mFixInfoMask) { } @@ -1085,7 +1035,6 @@ void SystemStatusPdr::dump() SystemStatusPositionFailure ******************************************************************************/ SystemStatusPositionFailure::SystemStatusPositionFailure(const SystemStatusPQWS1& nmea) : - SystemStatusItemBase(nmea.mUtcTime), mFixInfoMask(nmea.mFixInfoMask), mHepeLimit(nmea.mHepeLimit) { @@ -1161,7 +1110,9 @@ SystemStatus::SystemStatus() bool SystemStatus::setTimeAndCLock(const SystemStatusPQWM1& nmea) { SystemStatusTimeAndClock s(nmea); - if (mCache.mTimeAndClock.empty() || !mCache.mTimeAndClock.back().equals(s)) { + if (!mCache.mTimeAndClock.empty() && mCache.mTimeAndClock.back().equals(s)) { + mCache.mTimeAndClock.back().mUtcReported = s.mUtcReported; + } else { mCache.mTimeAndClock.push_back(s); if (mCache.mTimeAndClock.size() > maxTimeAndClock) { mCache.mTimeAndClock.erase(mCache.mTimeAndClock.begin()); @@ -1173,7 +1124,9 @@ bool SystemStatus::setTimeAndCLock(const SystemStatusPQWM1& nmea) bool SystemStatus::setXoState(const SystemStatusPQWM1& nmea) { SystemStatusXoState s(nmea); - if (mCache.mXoState.empty() || !mCache.mXoState.back().equals(s)) { + if (!mCache.mXoState.empty() && mCache.mXoState.back().equals(s)) { + mCache.mXoState.back().mUtcReported = s.mUtcReported; + } else { mCache.mXoState.push_back(s); if (mCache.mXoState.size() > maxXoState) { mCache.mXoState.erase(mCache.mXoState.begin()); @@ -1185,7 +1138,9 @@ bool SystemStatus::setXoState(const SystemStatusPQWM1& nmea) bool SystemStatus::setRfAndParams(const SystemStatusPQWM1& nmea) { SystemStatusRfAndParams s(nmea); - if (mCache.mRfAndParams.empty() || !mCache.mRfAndParams.back().equals(s)) { + if (!mCache.mRfAndParams.empty() && mCache.mRfAndParams.back().equals(s)) { + mCache.mRfAndParams.back().mUtcReported = s.mUtcReported; + } else { mCache.mRfAndParams.push_back(s); if (mCache.mRfAndParams.size() > maxRfAndParams) { mCache.mRfAndParams.erase(mCache.mRfAndParams.begin()); @@ -1197,7 +1152,9 @@ bool SystemStatus::setRfAndParams(const SystemStatusPQWM1& nmea) bool SystemStatus::setErrRecovery(const SystemStatusPQWM1& nmea) { SystemStatusErrRecovery s(nmea); - if (mCache.mErrRecovery.empty() || !mCache.mErrRecovery.back().equals(s)) { + if (!mCache.mErrRecovery.empty() && mCache.mErrRecovery.back().equals(s)) { + mCache.mErrRecovery.back().mUtcReported = s.mUtcReported; + } else { mCache.mErrRecovery.push_back(s); if (mCache.mErrRecovery.size() > maxErrRecovery) { mCache.mErrRecovery.erase(mCache.mErrRecovery.begin()); @@ -1212,7 +1169,9 @@ bool SystemStatus::setErrRecovery(const SystemStatusPQWM1& nmea) bool SystemStatus::setInjectedPosition(const SystemStatusPQWP1& nmea) { SystemStatusInjectedPosition s(nmea); - if (mCache.mInjectedPosition.empty() || !mCache.mInjectedPosition.back().equals(s)) { + if (!mCache.mInjectedPosition.empty() && mCache.mInjectedPosition.back().equals(s)) { + mCache.mInjectedPosition.back().mUtcReported = s.mUtcReported; + } else { mCache.mInjectedPosition.push_back(s); if (mCache.mInjectedPosition.size() > maxInjectedPosition) { mCache.mInjectedPosition.erase(mCache.mInjectedPosition.begin()); @@ -1224,7 +1183,9 @@ bool SystemStatus::setInjectedPosition(const SystemStatusPQWP1& nmea) bool SystemStatus::setBestPosition(const SystemStatusPQWP2& nmea) { SystemStatusBestPosition s(nmea); - if (mCache.mBestPosition.empty() || !mCache.mBestPosition.back().equals(s)) { + if (!mCache.mBestPosition.empty() && mCache.mBestPosition.back().equals(s)) { + mCache.mBestPosition.back().mUtcReported = s.mUtcReported; + } else { mCache.mBestPosition.push_back(s); if (mCache.mBestPosition.size() > maxBestPosition) { mCache.mBestPosition.erase(mCache.mBestPosition.begin()); @@ -1236,7 +1197,9 @@ bool SystemStatus::setBestPosition(const SystemStatusPQWP2& nmea) bool SystemStatus::setXtra(const SystemStatusPQWP3& nmea) { SystemStatusXtra s(nmea); - if (mCache.mXtra.empty() || !mCache.mXtra.back().equals(s)) { + if (!mCache.mXtra.empty() && mCache.mXtra.back().equals(s)) { + mCache.mXtra.back().mUtcReported = s.mUtcReported; + } else { mCache.mXtra.push_back(s); if (mCache.mXtra.size() > maxXtra) { mCache.mXtra.erase(mCache.mXtra.begin()); @@ -1248,7 +1211,9 @@ bool SystemStatus::setXtra(const SystemStatusPQWP3& nmea) bool SystemStatus::setEphemeris(const SystemStatusPQWP4& nmea) { SystemStatusEphemeris s(nmea); - if (mCache.mEphemeris.empty() || !mCache.mEphemeris.back().equals(s)) { + if (!mCache.mEphemeris.empty() && mCache.mEphemeris.back().equals(s)) { + mCache.mEphemeris.back().mUtcReported = s.mUtcReported; + } else { mCache.mEphemeris.push_back(s); if (mCache.mEphemeris.size() > maxEphemeris) { mCache.mEphemeris.erase(mCache.mEphemeris.begin()); @@ -1260,7 +1225,9 @@ bool SystemStatus::setEphemeris(const SystemStatusPQWP4& nmea) bool SystemStatus::setSvHealth(const SystemStatusPQWP5& nmea) { SystemStatusSvHealth s(nmea); - if (mCache.mSvHealth.empty() || !mCache.mSvHealth.back().equals(s)) { + if (!mCache.mSvHealth.empty() && mCache.mSvHealth.back().equals(s)) { + mCache.mSvHealth.back().mUtcReported = s.mUtcReported; + } else { mCache.mSvHealth.push_back(s); if (mCache.mSvHealth.size() > maxSvHealth) { mCache.mSvHealth.erase(mCache.mSvHealth.begin()); @@ -1272,7 +1239,9 @@ bool SystemStatus::setSvHealth(const SystemStatusPQWP5& nmea) bool SystemStatus::setPdr(const SystemStatusPQWP6& nmea) { SystemStatusPdr s(nmea); - if (mCache.mPdr.empty() || !mCache.mPdr.back().equals(s)) { + if (!mCache.mPdr.empty() && mCache.mPdr.back().equals(s)) { + mCache.mPdr.back().mUtcReported = s.mUtcReported; + } else { mCache.mPdr.push_back(s); if (mCache.mPdr.size() > maxPdr) { mCache.mPdr.erase(mCache.mPdr.begin()); @@ -1287,7 +1256,9 @@ bool SystemStatus::setPdr(const SystemStatusPQWP6& nmea) bool SystemStatus::setPositionFailure(const SystemStatusPQWS1& nmea) { SystemStatusPositionFailure s(nmea); - if (mCache.mPositionFailure.empty() || !mCache.mPositionFailure.back().equals(s)) { + if (!mCache.mPositionFailure.empty() && mCache.mPositionFailure.back().equals(s)) { + mCache.mPositionFailure.back().mUtcReported = s.mUtcReported; + } else { mCache.mPositionFailure.push_back(s); if (mCache.mPositionFailure.size() > maxPositionFailure) { mCache.mPositionFailure.erase(mCache.mPositionFailure.begin()); @@ -1322,8 +1293,7 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) } char buf[SystemStatusNmeaBase::NMEA_MAXSIZE + 1] = { 0 }; - strncpy(buf, data, len); - LOC_LOGI("setNmeaString-0321a: nmea=%s", buf); + strlcpy(buf, data, sizeof(buf)); pthread_mutex_lock(&mMutexSystemStatus); @@ -1393,10 +1363,7 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) bool SystemStatus::eventPosition(const UlpLocation& location, const GpsLocationExtended& locationEx) { - timespec ts; - ts.tv_sec = location.gpsLocation.timestamp / 1000ULL; - ts.tv_nsec = (location.gpsLocation.timestamp % 1000ULL) * 1000000ULL; - SystemStatusLocation s(location, locationEx, ts); + SystemStatusLocation s(location, locationEx); if ((mCache.mLocation.empty()) || !mCache.mLocation.back().equals(s)) { mCache.mLocation.push_back(s); if (mCache.mLocation.size() > maxLocation) { diff --git a/core/SystemStatus.h b/core/SystemStatus.h index f001f610..e8cb197b 100644 --- a/core/SystemStatus.h +++ b/core/SystemStatus.h @@ -49,8 +49,16 @@ namespace loc_core class SystemStatusItemBase { public: - timespec mUtcTime; - SystemStatusItemBase(timespec utctime) : mUtcTime(utctime) { }; + timespec mUtcTime; // UTC timestamp when this info was last updated + timespec mUtcReported; // UTC timestamp when this info was reported + + SystemStatusItemBase() { + timeval tv; + gettimeofday(&tv, NULL); + mUtcTime.tv_sec = tv.tv_sec; + mUtcTime.tv_nsec = tv.tv_usec *1000ULL; + mUtcReported = mUtcTime; + }; virtual ~SystemStatusItemBase() { }; virtual void dump(void) { }; }; @@ -61,9 +69,7 @@ public: UlpLocation mLocation; GpsLocationExtended mLocationEx; SystemStatusLocation(const UlpLocation& location, - const GpsLocationExtended& locationEx, - const timespec& ts) : - SystemStatusItemBase(ts), + const GpsLocationExtended& locationEx) : mLocation(location), mLocationEx(locationEx){ }; bool equals(SystemStatusLocation& peer);