diff --git a/android/GnssDebug.cpp b/android/GnssDebug.cpp index d7a53676..e6f59df2 100644 --- a/android/GnssDebug.cpp +++ b/android/GnssDebug.cpp @@ -30,6 +30,9 @@ namespace implementation { using ::android::hardware::hidl_vec; +#define GNSS_DEBUG_UNKNOWN_UTC_TIME (1483228800ULL) // 1/1/2017 00:00 GMT +#define GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC (1.57783680E17) // 5 years in ns + GnssDebug::GnssDebug(Gnss* gnss) : mGnss(gnss) { } @@ -42,7 +45,7 @@ GnssDebug::GnssDebug(Gnss* gnss) : mGnss(gnss) */ Return GnssDebug::getDebugData(getDebugData_cb _hidl_cb) { - LOC_LOGI("GnssDebug - 0317a"); + LOC_LOGI("GnssDebug - 0426a"); DebugData data = { }; @@ -57,35 +60,57 @@ Return GnssDebug::getDebugData(getDebugData_cb _hidl_cb) mGnss->getGnssInterface()->getDebugReport(reports); // location block - data.position.valid = true; - data.position.latitudeDegrees = reports.mLocation.mLocation.latitude; - data.position.longitudeDegrees = reports.mLocation.mLocation.longitude; - data.position.altitudeMeters = reports.mLocation.mLocation.altitude; - data.position.speedMetersPerSec = (double)(reports.mLocation.mLocation.speed); - data.position.bearingDegrees = (double)(reports.mLocation.mLocation.bearing); - data.position.horizontalAccuracyMeters = (double)(reports.mLocation.mLocation.accuracy); - data.position.verticalAccuracyMeters = reports.mLocation.verticalAccuracyMeters; - data.position.speedAccuracyMetersPerSecond = reports.mLocation.speedAccuracyMetersPerSecond; - data.position.bearingAccuracyDegrees = reports.mLocation.bearingAccuracyDegrees; + if (reports.mLocation.mValid) { + data.position.valid = true; + data.position.latitudeDegrees = reports.mLocation.mLocation.latitude; + data.position.longitudeDegrees = reports.mLocation.mLocation.longitude; + data.position.altitudeMeters = reports.mLocation.mLocation.altitude; - LOC_LOGV("GnssDebug - lat=%f lon=%f", data.position.latitudeDegrees, data.position.longitudeDegrees); + data.position.speedMetersPerSec = + (double)(reports.mLocation.mLocation.speed); + data.position.bearingDegrees = + (double)(reports.mLocation.mLocation.bearing); + data.position.horizontalAccuracyMeters = + (double)(reports.mLocation.mLocation.accuracy); + data.position.verticalAccuracyMeters = + reports.mLocation.verticalAccuracyMeters; + data.position.speedAccuracyMetersPerSecond = + reports.mLocation.speedAccuracyMetersPerSecond; + data.position.bearingAccuracyDegrees = + reports.mLocation.bearingAccuracyDegrees; + LOC_LOGI("GnssDebug - lat=%f lon=%f", + data.position.latitudeDegrees, data.position.longitudeDegrees); - timeval tv_now, tv_report; - tv_report.tv_sec = reports.mLocation.mLocation.timestamp / 1000ULL; - tv_report.tv_usec = (reports.mLocation.mLocation.timestamp % 1000ULL) * 1000ULL; - gettimeofday(&tv_now, NULL); - data.position.ageSeconds = - (tv_now.tv_sec - tv_report.tv_sec) + (float)((tv_now.tv_usec - tv_report.tv_usec)) / 1000000; + timeval tv_now, tv_report; + tv_report.tv_sec = reports.mLocation.mLocation.timestamp / 1000ULL; + tv_report.tv_usec = + (reports.mLocation.mLocation.timestamp % 1000ULL) * 1000ULL; + gettimeofday(&tv_now, NULL); + data.position.ageSeconds = + (tv_now.tv_sec - tv_report.tv_sec) + + (float)((tv_now.tv_usec - tv_report.tv_usec)) / 1000000; - LOC_LOGV("GnssDebug - time now=%lld:%lld", tv_now.tv_sec, tv_now.tv_usec); - LOC_LOGV("GnssDebug - time rep=%lld:%lld", tv_report.tv_sec, tv_report.tv_usec); - LOC_LOGV("GnssDebug - age=%f", data.position.ageSeconds); + LOC_LOGI("GnssDebug - time now=%lld:%lld", tv_now.tv_sec, tv_now.tv_usec); + LOC_LOGI("GnssDebug - time rep=%lld:%lld",tv_report.tv_sec, tv_report.tv_usec); + LOC_LOGI("GnssDebug - age=%f", data.position.ageSeconds); + } + else { + data.position.valid = false; + } // time block - data.time.timeEstimate = reports.mTime.timeEstimate; - data.time.timeUncertaintyNs = reports.mTime.timeUncertaintyNs; - - LOC_LOGV("GnssDebug - timeestimate=%lld", data.time.timeEstimate); + if (reports.mTime.mValid) { + data.time.timeEstimate = reports.mTime.timeEstimate; + data.time.timeUncertaintyNs = reports.mTime.timeUncertaintyNs; + data.time.frequencyUncertaintyNsPerSec = + reports.mTime.frequencyUncertaintyNsPerSec; + } + else { + data.time.timeEstimate = GNSS_DEBUG_UNKNOWN_UTC_TIME; + data.time.timeUncertaintyNs = (float)(GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC); + data.time.frequencyUncertaintyNsPerSec = 0; + } + LOC_LOGI("GnssDebug - timeestimate=%lld", data.time.timeEstimate); // satellite data block SatelliteData s = { }; @@ -94,18 +119,31 @@ Return GnssDebug::getDebugData(getDebugData_cb _hidl_cb) for (uint32_t i=0; i #include +#include namespace android { namespace hardware { @@ -41,6 +42,9 @@ namespace implementation { void convertGnssLocation(Location& in, GnssLocation& out); void convertGnssConstellationType(GnssSvType& in, GnssConstellationType& out); +void convertGnssEphemerisType(GnssEphemerisType& in, GnssDebug::SatelliteEphemerisType& out); +void convertGnssEphemerisSource(GnssEphemerisSource& in, GnssDebug::SatelliteEphemerisSource& out); +void convertGnssEphemerisHealth(GnssEphemerisHealth& in, GnssDebug::SatelliteEphemerisHealth& out); } // namespace implementation } // namespace V1_0 diff --git a/core/SystemStatus.cpp b/core/SystemStatus.cpp index 1adeb66a..12f97ddc 100644 --- a/core/SystemStatus.cpp +++ b/core/SystemStatus.cpp @@ -113,6 +113,8 @@ public: double mAgcGlo; // x14 double mAgcBds; // x15 double mAgcGal; // x16 + int32_t mLeapSeconds;// x17 + int32_t mLeapSecUnc; // x18 }; // parser @@ -144,7 +146,9 @@ private: eAgcGlo = 20, eAgcBds = 21, eAgcGal = 22, - eMax = eRecErrorRecovery + eLeapSeconds = 23, + eLeapSecUnc = 24, + eMax }; SystemStatusPQWM1 mM1; @@ -171,11 +175,14 @@ public: inline uint32_t getAgcBds() { return mM1.mAgcBds; } inline uint32_t getAgcGal() { return mM1.mAgcGal; } inline uint32_t getRecErrorRecovery() { return mM1.mRecErrorRecovery; } + inline int32_t getLeapSeconds(){ return mM1.mLeapSeconds; } + inline int32_t getLeapSecUnc() { return mM1.mLeapSecUnc; } SystemStatusPQWM1parser(const char *str_in, uint32_t len_in) : SystemStatusNmeaBase(str_in, len_in) { if (mField.size() < eMax) { + LOC_LOGE("PQWM1parser - invalid size=%d", mField.size()); return; } memset(&mM1, 0, sizeof(mM1)); @@ -201,6 +208,8 @@ public: mM1.mAgcGlo = atof(mField[eAgcGlo].c_str()); mM1.mAgcBds = atof(mField[eAgcBds].c_str()); mM1.mAgcGal = atof(mField[eAgcGal].c_str()); + mM1.mLeapSeconds = atoi(mField[eLeapSeconds].c_str()); + mM1.mLeapSecUnc = atoi(mField[eLeapSecUnc].c_str()); } inline SystemStatusPQWM1& get() { return mM1;} //getparser @@ -235,7 +244,7 @@ private: eEpiHepe = 6, eEpiAltUnc = 7, eEpiSrc = 8, - eMax = eEpiSrc + eMax }; SystemStatusPQWP1 mP1; @@ -292,7 +301,7 @@ private: eBestAlt = 4, eBestHepe = 5, eBestAltUnc = 6, - eMax = eBestAltUnc + eMax }; SystemStatusPQWP2 mP2; @@ -357,7 +366,7 @@ private: eBdsXtraValid = 10, eGalXtraValid = 11, eQzssXtraValid = 12, - eMax = eQzssXtraValid + eMax }; SystemStatusPQWP3 mP3; @@ -422,7 +431,7 @@ private: eBdsEpheValid = 4, eGalEpheValid = 5, eQzssEpheValid = 6, - eMax = eQzssEpheValid + eMax }; SystemStatusPQWP4 mP4; @@ -495,7 +504,7 @@ private: eBdsBadMask = 14, eGalBadMask = 15, eQzssBadMask = 16, - eMax = eQzssBadMask + eMax }; SystemStatusPQWP5 mP5; @@ -560,7 +569,7 @@ private: eTalker = 0, eUtcTime = 1, eFixInfoMask = 2, - eMax = eFixInfoMask + eMax }; SystemStatusPQWP6 mP6; @@ -580,6 +589,44 @@ public: inline SystemStatusPQWP6& get() { return mP6;} }; +/****************************************************************************** + SystemStatusPQWP7parser +******************************************************************************/ +class SystemStatusPQWP7 +{ +public: + SystemStatusNav mNav[SV_ALL_NUM]; +}; + +class SystemStatusPQWP7parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eMax = 2 + SV_ALL_NUM*3 + }; + SystemStatusPQWP7 mP7; + +public: + SystemStatusPQWP7parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + LOC_LOGE("PQWP7parser - invalid size=%d", mField.size()); + return; + } + for (uint32_t i=0; i maxNavData) { + mCache.mNavData.erase(mCache.mNavData.begin()); + } + } + return true; +} + /****************************************************************************** SystemStatus - Sx functions ******************************************************************************/ @@ -1283,6 +1385,7 @@ static uint32_t cnt_p3 = 0; static uint32_t cnt_p4 = 0; static uint32_t cnt_p5 = 0; static uint32_t cnt_p6 = 0; +static uint32_t cnt_p7 = 0; static uint32_t cnt_s1 = 0; bool SystemStatus::setNmeaString(const char *data, uint32_t len) @@ -1293,7 +1396,7 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) } char buf[SystemStatusNmeaBase::NMEA_MAXSIZE + 1] = { 0 }; - strlcpy(buf, data, sizeof(buf)); + strlcpy(buf, data, (len < strlen(data))? len : strlen(data)); pthread_mutex_lock(&mMutexSystemStatus); @@ -1330,6 +1433,10 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) ret = setPdr(SystemStatusPQWP6parser(buf, len).get()); cnt_p6++; } + else if (0 == strncmp(data, "$PQWP7", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setNavData(SystemStatusPQWP7parser(buf, len).get()); + cnt_p7++; + } else if (0 == strncmp(data, "$PQWS1", SystemStatusNmeaBase::NMEA_MINSIZE)) { ret = setPositionFailure(SystemStatusPQWS1parser(buf, len).get()); cnt_s1++; @@ -1338,7 +1445,7 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) // do nothing } cnt++; - LOC_LOGV("setNmeaString: cnt=%d M:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d S:%d", + LOC_LOGV("setNmeaString: cnt=%d M:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d S:%d", cnt, cnt_m1, cnt_p1, @@ -1347,6 +1454,7 @@ bool SystemStatus::setNmeaString(const char *data, uint32_t len) cnt_p4, cnt_p5, cnt_p6, + cnt_p7, cnt_s1); pthread_mutex_unlock(&mMutexSystemStatus); @@ -1364,19 +1472,20 @@ bool SystemStatus::eventPosition(const UlpLocation& location, const GpsLocationExtended& locationEx) { SystemStatusLocation s(location, locationEx); - if ((mCache.mLocation.empty()) || !mCache.mLocation.back().equals(s)) { + if (!mCache.mLocation.empty() && mCache.mLocation.back().equals(s)) { + mCache.mLocation.back().mUtcReported = s.mUtcReported; + } + else { mCache.mLocation.push_back(s); if (mCache.mLocation.size() > maxLocation) { mCache.mLocation.erase(mCache.mLocation.begin()); } - - LOC_LOGV("eventPosition - lat=%f lon=%f alt=%f speed=%f", - s.mLocation.gpsLocation.latitude, - s.mLocation.gpsLocation.longitude, - s.mLocation.gpsLocation.altitude, - s.mLocation.gpsLocation.speed); - } + LOC_LOGV("eventPosition - lat=%f lon=%f alt=%f speed=%f", + s.mLocation.gpsLocation.latitude, + s.mLocation.gpsLocation.longitude, + s.mLocation.gpsLocation.altitude, + s.mLocation.gpsLocation.speed); return true; } @@ -1451,6 +1560,12 @@ bool SystemStatus::getReport(SystemStatusReports& report, bool isLatestOnly) con report.mPdr.push_back(mCache.mPdr.back()); report.mPdr.back().dump(); } + report.mNavData.clear(); + if (mCache.mNavData.size() >= 1) { + report.mNavData.push_back(mCache.mNavData.back()); + report.mNavData.back().dump(); + } + report.mPositionFailure.clear(); if (mCache.mPositionFailure.size() >= 1) { report.mPositionFailure.push_back(mCache.mPositionFailure.back()); @@ -1472,6 +1587,8 @@ bool SystemStatus::getReport(SystemStatusReports& report, bool isLatestOnly) con report.mEphemeris.clear(); report.mSvHealth.clear(); report.mPdr.clear(); + report.mNavData.clear(); + report.mPositionFailure.clear(); report = mCache; } diff --git a/core/SystemStatus.h b/core/SystemStatus.h index e8cb197b..a3b38d34 100644 --- a/core/SystemStatus.h +++ b/core/SystemStatus.h @@ -33,12 +33,20 @@ #include #include -#define GPS_MIN (1) +#define GPS_MIN (1) //1-32 #define SBAS_MIN (33) -#define GLO_MIN (65) -#define BDS_MIN (201) -#define QZSS_MIN (193) -#define GAL_MIN (301) +#define GLO_MIN (65) //65-88 +#define QZSS_MIN (193) //193-197 +#define BDS_MIN (201) //201-237 +#define GAL_MIN (301) //301-336 + +#define GPS_NUM (32) +#define SBAS_NUM (32) +#define GLO_NUM (24) +#define QZSS_NUM (5) +#define BDS_NUM (37) +#define GAL_NUM (36) +#define SV_ALL_NUM (GPS_NUM+GLO_NUM+QZSS_NUM+BDS_NUM+GAL_NUM) //=134 namespace loc_core { @@ -87,6 +95,8 @@ public: int32_t mTimeUnc; int32_t mClockFreqBias; int32_t mClockFreqBiasUnc; + int32_t mLeapSeconds; + int32_t mLeapSecUnc; SystemStatusTimeAndClock(const SystemStatusPQWM1& nmea); bool equals(SystemStatusTimeAndClock& peer); void dump(void); @@ -229,6 +239,23 @@ public: void dump(void); }; +class SystemStatusPQWP7; +struct SystemStatusNav +{ + GnssEphemerisType mType; + GnssEphemerisSource mSource; + int32_t mAgeSec; +}; + +class SystemStatusNavData : public SystemStatusItemBase +{ +public: + SystemStatusNav mNav[SV_ALL_NUM]; + SystemStatusNavData(const SystemStatusPQWP7& nmea); + bool equals(SystemStatusNavData& peer); + void dump(void); +}; + class SystemStatusPQWS1; class SystemStatusPositionFailure : public SystemStatusItemBase { @@ -259,6 +286,8 @@ public: std::vector mEphemeris; std::vector mSvHealth; std::vector mPdr; + std::vector mNavData; + std::vector mPositionFailure; }; @@ -282,6 +311,8 @@ class SystemStatus static const uint32_t maxEphemeris = 5; static const uint32_t maxSvHealth = 5; static const uint32_t maxPdr = 5; + static const uint32_t maxNavData = 5; + static const uint32_t maxPositionFailure = 5; SystemStatusReports mCache; @@ -299,6 +330,8 @@ class SystemStatus bool setEphemeris(const SystemStatusPQWP4& nmea); bool setSvHealth(const SystemStatusPQWP5& nmea); bool setPdr(const SystemStatusPQWP6& nmea); + bool setNavData(const SystemStatusPQWP7& nmea); + bool setPositionFailure(const SystemStatusPQWS1& nmea); public: diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index 992ced85..125f24f7 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -2596,59 +2596,135 @@ void GnssAdapter::dataConnFailedCommand(AGpsExtType agpsType){ } void GnssAdapter::convertSatelliteInfo(std::vector& out, - const GnssSvType& in_constellation, - const SystemStatusReports& in) + const GnssSvType& in_constellation, + const SystemStatusReports& in) { - GnssDebugSatelliteInfo s = {}; - uint64_t mask = 0ULL; - float age = 0.0; + uint64_t sv_mask = 0ULL; uint32_t svid_min = 0; - uint32_t mask_size = 0; + uint32_t svid_num = 0; + uint32_t svid_idx = 0; + uint64_t eph_health_good_mask = 0ULL; + uint64_t eph_health_bad_mask = 0ULL; + uint64_t server_perdiction_available_mask = 0ULL; + float server_perdiction_age = 0.0f; + + // set constellationi based parameters switch (in_constellation) { case GNSS_SV_TYPE_GPS: svid_min = GPS_MIN; - mask_size = 32; + svid_num = GPS_NUM; + svid_idx = 0; + if (!in.mSvHealth.empty()) { + eph_health_good_mask = in.mSvHealth.back().mGpsGoodMask; + eph_health_bad_mask = in.mSvHealth.back().mGpsBadMask; + } + if (!in.mXtra.empty()) { + server_perdiction_available_mask = in.mXtra.back().mGpsXtraValid; + server_perdiction_age = (float)(in.mXtra.back().mGpsXtraAge); + } break; case GNSS_SV_TYPE_GLONASS: svid_min = GLO_MIN; - mask_size = 32; - break; - case GNSS_SV_TYPE_BEIDOU: - svid_min = BDS_MIN; - mask_size = 64; + svid_num = GLO_NUM; + svid_idx = GPS_NUM; + if (!in.mSvHealth.empty()) { + eph_health_good_mask = in.mSvHealth.back().mGloGoodMask; + eph_health_bad_mask = in.mSvHealth.back().mGloBadMask; + } + if (!in.mXtra.empty()) { + server_perdiction_available_mask = in.mXtra.back().mGloXtraValid; + server_perdiction_age = (float)(in.mXtra.back().mGloXtraAge); + } break; case GNSS_SV_TYPE_QZSS: svid_min = QZSS_MIN; - mask_size = 32; + svid_num = QZSS_NUM; + svid_idx = GPS_NUM+GLO_NUM; + if (!in.mSvHealth.empty()) { + eph_health_good_mask = in.mSvHealth.back().mQzssGoodMask; + eph_health_bad_mask = in.mSvHealth.back().mQzssBadMask; + } + if (!in.mXtra.empty()) { + server_perdiction_available_mask = in.mXtra.back().mQzssXtraValid; + server_perdiction_age = (float)(in.mXtra.back().mQzssXtraAge); + } + break; + case GNSS_SV_TYPE_BEIDOU: + svid_min = BDS_MIN; + svid_num = BDS_NUM; + svid_idx = GPS_NUM+GLO_NUM+QZSS_NUM; + if (!in.mSvHealth.empty()) { + eph_health_good_mask = in.mSvHealth.back().mBdsGoodMask; + eph_health_bad_mask = in.mSvHealth.back().mBdsBadMask; + } + if (!in.mXtra.empty()) { + server_perdiction_available_mask = in.mXtra.back().mBdsXtraValid; + server_perdiction_age = (float)(in.mXtra.back().mBdsXtraAge); + } break; case GNSS_SV_TYPE_GALILEO: svid_min = GAL_MIN; - mask_size = 64; + svid_num = GAL_NUM; + svid_idx = GPS_NUM+GLO_NUM+QZSS_NUM+BDS_NUM; + if (!in.mSvHealth.empty()) { + eph_health_good_mask = in.mSvHealth.back().mGalGoodMask; + eph_health_bad_mask = in.mSvHealth.back().mGalBadMask; + } + if (!in.mXtra.empty()) { + server_perdiction_available_mask = in.mXtra.back().mGalXtraValid; + server_perdiction_age = (float)(in.mXtra.back().mGalXtraAge); + } break; default: return; } - if(!in.mEphemeris.empty()) { - mask = in.mEphemeris.back().mGpsEpheValid; - if(!in.mXtra.empty()) { - age = (float)(in.mXtra.back().mGpsXtraAge); + // extract each sv info from systemstatus report + for(uint32_t i=0; i &nmeaArraystr); #define DEBUG_NMEA_MINSIZE 6 -#define DEBUG_NMEA_MAXSIZE 256 +#define DEBUG_NMEA_MAXSIZE 4096 inline bool loc_nmea_is_debug(const char* nmea, int length) { return ((nullptr != nmea) && (length >= DEBUG_NMEA_MINSIZE) && (length <= DEBUG_NMEA_MAXSIZE) &&