From 592b3f557edac9a1e25a8a22306573ee7f530ee9 Mon Sep 17 00:00:00 2001 From: Katz Yamada Date: Tue, 14 Mar 2017 11:13:23 -0700 Subject: [PATCH] SystemStatus - initial release Add GNSS SystemStatus component to handle debug NMEA messages from mpss. This component handles GNSS debug info sent through debug NMEA and then stores it in its cache and pass it to clients per requests. Also debug NMEA is turned on by default. Change-Id: Ia11a124ff43d27568f544a3a4742dd7a846869fe CRs-Fixed: 1099152 --- core/Android.mk | 6 +- core/LocDualContext.cpp | 12 + core/LocDualContext.h | 6 +- core/Makefile.am | 6 +- core/SystemStatus.cpp | 1425 +++++++++++++++++++++++++++++++++++++++ core/SystemStatus.h | 275 ++++++++ core/gps_extended_c.h | 7 +- gnss/GnssAdapter.cpp | 24 +- 8 files changed, 1749 insertions(+), 12 deletions(-) create mode 100644 core/SystemStatus.cpp create mode 100644 core/SystemStatus.h diff --git a/core/Android.mk b/core/Android.mk index b260b1dc..b8e25a96 100644 --- a/core/Android.mk +++ b/core/Android.mk @@ -30,7 +30,8 @@ LOCAL_SRC_FILES += \ LocAdapterBase.cpp \ ContextBase.cpp \ LocDualContext.cpp \ - loc_core_log.cpp + loc_core_log.cpp \ + SystemStatus.cpp LOCAL_CFLAGS += \ -fno-short-enums \ @@ -53,7 +54,8 @@ LOCAL_COPY_HEADERS:= \ gps_extended_c.h \ gps_extended.h \ loc_core_log.h \ - LocAdapterProxyBase.h + LocAdapterProxyBase.h \ + SystemStatus.h include $(BUILD_SHARED_LIBRARY) diff --git a/core/LocDualContext.cpp b/core/LocDualContext.cpp index fd465e9d..c6a88962 100644 --- a/core/LocDualContext.cpp +++ b/core/LocDualContext.cpp @@ -35,6 +35,7 @@ #include #include #include +#include namespace loc_core { @@ -56,6 +57,7 @@ const MsgTask* LocDualContext::mMsgTask = NULL; ContextBase* LocDualContext::mFgContext = NULL; ContextBase* LocDualContext::mBgContext = NULL; ContextBase* LocDualContext::mInjectContext = NULL; +SystemStatus* LocDualContext::mSystemStatus = NULL; // the name must be shorter than 15 chars const char* LocDualContext::mLocationHalName = "Loc_hal_worker"; #ifndef USE_GLIB @@ -147,4 +149,14 @@ LocDualContext::LocDualContext(const MsgTask* msgTask, { } +SystemStatus* LocDualContext::getSystemStatus(void) +{ + pthread_mutex_lock(&LocDualContext::mGetLocContextMutex); + if (NULL == mSystemStatus) { + mSystemStatus = new SystemStatus(); + } + pthread_mutex_unlock(&LocDualContext::mGetLocContextMutex); + return mSystemStatus; +} + } diff --git a/core/LocDualContext.h b/core/LocDualContext.h index ce77a1aa..76421525 100644 --- a/core/LocDualContext.h +++ b/core/LocDualContext.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2011-2014, The Linux Foundation. All rights reserved. +/* Copyright (c) 2011-2014, 2017 The Linux Foundation. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are @@ -36,6 +36,8 @@ namespace loc_core { +class SystemStatus; + class LocDualContext : public ContextBase { static const MsgTask* mMsgTask; static ContextBase* mFgContext; @@ -45,6 +47,7 @@ class LocDualContext : public ContextBase { const char* name, bool joinable = true); static const MsgTask* getMsgTask(const char* name, bool joinable = true); static pthread_mutex_t mGetLocContextMutex; + static SystemStatus* mSystemStatus; protected: LocDualContext(const MsgTask* msgTask, @@ -69,6 +72,7 @@ public: } static void injectFeatureConfig(ContextBase *context); + static SystemStatus* getSystemStatus(void); }; } diff --git a/core/Makefile.am b/core/Makefile.am index b3d833f6..6a4f3534 100644 --- a/core/Makefile.am +++ b/core/Makefile.am @@ -16,14 +16,16 @@ libloc_core_la_h_sources = \ gps_extended_c.h \ gps_extended.h \ loc_core_log.h \ - LocAdapterProxyBase.h + LocAdapterProxyBase.h \ + SystemStatus.h libloc_core_la_c_sources = \ LocApiBase.cpp \ LocAdapterBase.cpp \ ContextBase.cpp \ LocDualContext.cpp \ - loc_core_log.cpp + loc_core_log.cpp \ + SystemStatus.cpp library_includedir = $(pkgincludedir)/core diff --git a/core/SystemStatus.cpp b/core/SystemStatus.cpp new file mode 100644 index 00000000..592344bd --- /dev/null +++ b/core/SystemStatus.cpp @@ -0,0 +1,1425 @@ +/* Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation, nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +#define LOG_TAG "LocSvc_SystemStatus" + +#include +#include +#include +#include +#include +#include +#include + +namespace loc_core +{ + +/****************************************************************************** + SystemStatusNmeaBase - base class for all NMEA parsers +******************************************************************************/ +class SystemStatusNmeaBase +{ +protected: + std::vector mField; + timespec setUtcTime(std::string sutctime); + +public: + static const uint32_t NMEA_MINSIZE = 6; + static const uint32_t NMEA_MAXSIZE = 256; + + SystemStatusNmeaBase(const char *str_in, uint32_t len_in) + { + // check size and talker + if (len_in > NMEA_MAXSIZE || len_in < NMEA_MINSIZE || (str_in[0] != '$')) { + return; + } + + std::string parser(str_in); + std::string::size_type index = 0; + + // verify checksum field + index = parser.find("*"); + if (index == std::string::npos) { + return; + } + parser[index] = ','; + + // tokenize parser + while (1) { + std::string str; + index = parser.find(","); + if (index == std::string::npos) { + break; + } + str = parser.substr(0, index); + parser = parser.substr(index + 1); + mField.push_back(str); + } + } + + virtual ~SystemStatusNmeaBase() { } +}; + +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 + uint8_t mTimeSource; // x4 + int32_t mTimeUnc; // x5 + int32_t mClockFreqBias; // x6 + int32_t mClockFreqBiasUnc; // x7 + uint8_t mXoState; // x8 + int32_t mPgaGain; // x9 + uint32_t mGpsBpAmpI; // xA + uint32_t mGpsBpAmpQ; // xB + uint32_t mAdcI; // xC + uint32_t mAdcQ; // xD + uint32_t mJammerGps; // xE + uint32_t mJammerGlo; // xF + uint32_t mJammerBds; // x10 + uint32_t mJammerGal; // x11 + uint32_t mRecErrorRecovery; // x12 +}; + +// parser +class SystemStatusPQWM1parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eGpsWeek = 1, + eGpsTowMs = 2, + eTimeValid = 3, + eTimeSource = 4, + eTimeUnc = 5, + eClockFreqBias = 6, + eClockFreqBiasUnc = 7, + eXoState = 8, + ePgaGain = 9, + eGpsBpAmpI = 10, + eGpsBpAmpQ = 11, + eAdcI = 12, + eAdcQ = 13, + eJammerGps = 14, + eJammerGlo = 15, + eJammerBds = 16, + eJammerGal = 17, + eRecErrorRecovery = 18, + eMax = eRecErrorRecovery + }; + SystemStatusPQWM1 mM1; + +public: + inline uint16_t getGpsWeek() { return mM1.mGpsWeek; } + inline uint32_t getGpsTowMs() { return mM1.mGpsTowMs; } + inline uint8_t getTimeValid() { return mM1.mTimeValid; } + inline uint8_t getTimeSource() { return mM1.mTimeSource; } + inline int32_t getTimeUnc() { return mM1.mTimeUnc; } + inline int32_t getClockFreqBias() { return mM1.mClockFreqBias; } + inline int32_t getClockFreqBiasUnc() { return mM1.mClockFreqBiasUnc; } + inline uint8_t getXoState() { return mM1.mXoState;} + inline int32_t getPgaGain() { return mM1.mPgaGain; } + inline uint32_t getGpsBpAmpI() { return mM1.mGpsBpAmpI; } + inline uint32_t getGpsBpAmpQ() { return mM1.mGpsBpAmpQ; } + inline uint32_t getAdcI() { return mM1.mAdcI; } + inline uint32_t getAdcQ() { return mM1.mAdcQ; } + inline uint32_t getJammerGps() { return mM1.mJammerGps; } + inline uint32_t getJammerGlo() { return mM1.mJammerGlo; } + inline uint32_t getJammerBds() { return mM1.mJammerBds; } + inline uint32_t getJammerGal() { return mM1.mJammerGal; } + inline uint32_t getRecErrorRecovery() { return mM1.mRecErrorRecovery; } + + SystemStatusPQWM1parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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()); + mM1.mTimeSource = atoi(mField[eTimeSource].c_str()); + mM1.mTimeUnc = atoi(mField[eTimeUnc].c_str()); + mM1.mClockFreqBias = atoi(mField[eClockFreqBias].c_str()); + mM1.mClockFreqBiasUnc = atoi(mField[eClockFreqBiasUnc].c_str()); + mM1.mXoState = atoi(mField[eXoState].c_str()); + mM1.mPgaGain = atoi(mField[ePgaGain].c_str()); + mM1.mGpsBpAmpI = atoi(mField[eGpsBpAmpI].c_str()); + mM1.mGpsBpAmpQ = atoi(mField[eGpsBpAmpQ].c_str()); + mM1.mAdcI = atoi(mField[eAdcI].c_str()); + mM1.mAdcQ = atoi(mField[eAdcQ].c_str()); + mM1.mJammerGps = atoi(mField[eJammerGps].c_str()); + mM1.mJammerGlo = atoi(mField[eJammerGlo].c_str()); + mM1.mJammerBds = atoi(mField[eJammerBds].c_str()); + mM1.mJammerGal = atoi(mField[eJammerGal].c_str()); + mM1.mRecErrorRecovery = atoi(mField[eRecErrorRecovery].c_str()); + } + + inline SystemStatusPQWM1& get() { return mM1;} //getparser +}; + +/****************************************************************************** + SystemStatusPQWP1 +******************************************************************************/ +class SystemStatusPQWP1 +{ +public: + timespec mUtcTime; + uint8_t mEpiValidity; // x4 + float mEpiLat; // x5 + float mEpiLon; // x6 + float mEpiAlt; // x7 + float mEpiHepe; // x8 + float mEpiAltUnc; // x9 + uint8_t mEpiSrc; // x10 +}; + +class SystemStatusPQWP1parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eEpiValidity = 2, + eEpiLat = 3, + eEpiLon = 4, + eEpiAlt = 5, + eEpiHepe = 6, + eEpiAltUnc = 7, + eEpiSrc = 8, + eMax = eEpiSrc + }; + 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; } + inline float getEpiAlt() { return mP1.mEpiAlt; } + inline float getEpiHepe() { return mP1.mEpiHepe; } + inline float getEpiAltUnc() { return mP1.mEpiAltUnc; } + inline uint8_t getEpiSrc() { return mP1.mEpiSrc; } + + SystemStatusPQWP1parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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()); + mP1.mEpiAlt = atof(mField[eEpiAlt].c_str()); + mP1.mEpiHepe = atoi(mField[eEpiHepe].c_str()); + mP1.mEpiAltUnc = atof(mField[eEpiAltUnc].c_str()); + mP1.mEpiSrc = atoi(mField[eEpiSrc].c_str()); + } + + inline SystemStatusPQWP1& get() { return mP1;} +}; + +/****************************************************************************** + SystemStatusPQWP2 +******************************************************************************/ +class SystemStatusPQWP2 +{ +public: + timespec mUtcTime; + float mBestLat; // x4 + float mBestLon; // x5 + float mBestAlt; // x6 + float mBestHepe; // x7 + float mBestAltUnc; // x8 +}; + +class SystemStatusPQWP2parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eBestLat = 2, + eBestLon = 3, + eBestAlt = 4, + eBestHepe = 5, + eBestAltUnc = 6, + eMax = eBestAltUnc + }; + SystemStatusPQWP2 mP2; + +public: + inline float getBestLat() { return mP2.mBestLat; } + inline float getBestLon() { return mP2.mBestLon; } + inline float getBestAlt() { return mP2.mBestAlt; } + inline float getBestHepe() { return mP2.mBestHepe; } + inline float getBestAltUnc() { return mP2.mBestAltUnc; } + + SystemStatusPQWP2parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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()); + mP2.mBestHepe = atof(mField[eBestHepe].c_str()); + mP2.mBestAltUnc = atof(mField[eBestAltUnc].c_str()); + } + + inline SystemStatusPQWP2& get() { return mP2;} +}; + +/****************************************************************************** + SystemStatusPQWP3 +******************************************************************************/ +class SystemStatusPQWP3 +{ +public: + timespec mUtcTime; + uint8_t mXtraValidMask; + uint32_t mGpsXtraAge; + uint32_t mGloXtraAge; + uint32_t mBdsXtraAge; + uint32_t mGalXtraAge; + uint32_t mQzssXtraAge; + uint32_t mGpsXtraValid; + uint32_t mGloXtraValid; + uint64_t mBdsXtraValid; + uint64_t mGalXtraValid; + uint8_t mQzssXtraValid; +}; + +class SystemStatusPQWP3parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eXtraValidMask = 2, + eGpsXtraAge = 3, + eGloXtraAge = 4, + eBdsXtraAge = 5, + eGalXtraAge = 6, + eQzssXtraAge = 7, + eGpsXtraValid = 8, + eGloXtraValid = 9, + eBdsXtraValid = 10, + eGalXtraValid = 11, + eQzssXtraValid = 12, + eMax = eQzssXtraValid + }; + SystemStatusPQWP3 mP3; + +public: + inline uint8_t getXtraValid() { return mP3.mXtraValidMask; } + inline uint32_t getGpsXtraAge() { return mP3.mGpsXtraAge; } + inline uint32_t getGloXtraAge() { return mP3.mGloXtraAge; } + inline uint32_t getBdsXtraAge() { return mP3.mBdsXtraAge; } + inline uint32_t getGalXtraAge() { return mP3.mGalXtraAge; } + inline uint32_t getQzssXtraAge() { return mP3.mQzssXtraAge; } + inline uint32_t getGpsXtraValid() { return mP3.mGpsXtraValid; } + inline uint32_t getGloXtraValid() { return mP3.mGloXtraValid; } + inline uint64_t getBdsXtraValid() { return mP3.mBdsXtraValid; } + inline uint64_t getGalXtraValid() { return mP3.mGalXtraValid; } + inline uint8_t getQzssXtraValid() { return mP3.mQzssXtraValid; } + + SystemStatusPQWP3parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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()); + mP3.mBdsXtraAge = atoi(mField[eBdsXtraAge].c_str()); + mP3.mGalXtraAge = atoi(mField[eGalXtraAge].c_str()); + mP3.mQzssXtraAge = atoi(mField[eQzssXtraAge].c_str()); + mP3.mGpsXtraValid = strtol(mField[eGpsXtraValid].c_str(), NULL, 16); + mP3.mGloXtraValid = strtol(mField[eGloXtraValid].c_str(), NULL, 16); + mP3.mBdsXtraValid = strtol(mField[eBdsXtraValid].c_str(), NULL, 16); + mP3.mGalXtraValid = strtol(mField[eGalXtraValid].c_str(), NULL, 16); + mP3.mQzssXtraValid = strtol(mField[eQzssXtraValid].c_str(), NULL, 16); + } + + inline SystemStatusPQWP3& get() { return mP3;} +}; + +/****************************************************************************** + SystemStatusPQWP4 +******************************************************************************/ +class SystemStatusPQWP4 +{ +public: + timespec mUtcTime; + uint32_t mGpsEpheValid; + uint32_t mGloEpheValid; + uint64_t mBdsEpheValid; + uint64_t mGalEpheValid; + uint8_t mQzssEpheValid; +}; + +class SystemStatusPQWP4parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eGpsEpheValid = 2, + eGloEpheValid = 3, + eBdsEpheValid = 4, + eGalEpheValid = 5, + eQzssEpheValid = 6, + eMax = eQzssEpheValid + }; + SystemStatusPQWP4 mP4; + +public: + inline uint32_t getGpsEpheValid() { return mP4.mGpsEpheValid; } + inline uint32_t getGloEpheValid() { return mP4.mGloEpheValid; } + inline uint64_t getBdsEpheValid() { return mP4.mBdsEpheValid; } + inline uint64_t getGalEpheValid() { return mP4.mGalEpheValid; } + inline uint8_t getQzssEpheValid() { return mP4.mQzssEpheValid; } + + SystemStatusPQWP4parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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); + mP4.mGalEpheValid = strtol(mField[eGalEpheValid].c_str(), NULL, 16); + mP4.mQzssEpheValid = strtol(mField[eQzssEpheValid].c_str(), NULL, 16); + } + + inline SystemStatusPQWP4& get() { return mP4;} +}; + +/****************************************************************************** + SystemStatusPQWP5 +******************************************************************************/ +class SystemStatusPQWP5 +{ +public: + timespec mUtcTime; + uint32_t mGpsUnknownMask; + uint32_t mGloUnknownMask; + uint64_t mBdsUnknownMask; + uint64_t mGalUnknownMask; + uint8_t mQzssUnknownMask; + uint32_t mGpsGoodMask; + uint32_t mGloGoodMask; + uint64_t mBdsGoodMask; + uint64_t mGalGoodMask; + uint8_t mQzssGoodMask; + uint32_t mGpsBadMask; + uint32_t mGloBadMask; + uint64_t mBdsBadMask; + uint64_t mGalBadMask; + uint8_t mQzssBadMask; +}; + +class SystemStatusPQWP5parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eGpsUnknownMask = 2, + eGloUnknownMask = 3, + eBdsUnknownMask = 4, + eGalUnknownMask = 5, + eQzssUnknownMask = 6, + eGpsGoodMask = 7, + eGloGoodMask = 8, + eBdsGoodMask = 9, + eGalGoodMask = 10, + eQzssGoodMask = 11, + eGpsBadMask = 12, + eGloBadMask = 13, + eBdsBadMask = 14, + eGalBadMask = 15, + eQzssBadMask = 16, + eMax = eQzssBadMask + }; + SystemStatusPQWP5 mP5; + +public: + inline uint32_t getGpsUnknownMask() { return mP5.mGpsUnknownMask; } + inline uint32_t getGloUnknownMask() { return mP5.mGloUnknownMask; } + inline uint64_t getBdsUnknownMask() { return mP5.mBdsUnknownMask; } + inline uint64_t getGalUnknownMask() { return mP5.mGalUnknownMask; } + inline uint8_t getQzssUnknownMask() { return mP5.mQzssUnknownMask; } + inline uint32_t getGpsGoodMask() { return mP5.mGpsGoodMask; } + inline uint32_t getGloGoodMask() { return mP5.mGloGoodMask; } + inline uint64_t getBdsGoodMask() { return mP5.mBdsGoodMask; } + inline uint64_t getGalGoodMask() { return mP5.mGalGoodMask; } + inline uint8_t getQzssGoodMask() { return mP5.mQzssGoodMask; } + inline uint32_t getGpsBadMask() { return mP5.mGpsBadMask; } + inline uint32_t getGloBadMask() { return mP5.mGloBadMask; } + inline uint64_t getBdsBadMask() { return mP5.mBdsBadMask; } + inline uint64_t getGalBadMask() { return mP5.mGalBadMask; } + inline uint8_t getQzssBadMask() { return mP5.mQzssBadMask; } + + SystemStatusPQWP5parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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); + mP5.mGalUnknownMask = strtol(mField[eGalUnknownMask].c_str(), NULL, 16); + mP5.mQzssUnknownMask = strtol(mField[eQzssUnknownMask].c_str(), NULL, 16); + mP5.mGpsGoodMask = strtol(mField[eGpsGoodMask].c_str(), NULL, 16); + mP5.mGloGoodMask = strtol(mField[eGloGoodMask].c_str(), NULL, 16); + mP5.mBdsGoodMask = strtol(mField[eBdsGoodMask].c_str(), NULL, 16); + mP5.mGalGoodMask = strtol(mField[eGalGoodMask].c_str(), NULL, 16); + mP5.mQzssGoodMask = strtol(mField[eQzssGoodMask].c_str(), NULL, 16); + mP5.mGpsBadMask = strtol(mField[eGpsBadMask].c_str(), NULL, 16); + mP5.mGloBadMask = strtol(mField[eGloBadMask].c_str(), NULL, 16); + mP5.mBdsBadMask = strtol(mField[eBdsBadMask].c_str(), NULL, 16); + mP5.mGalBadMask = strtol(mField[eGalBadMask].c_str(), NULL, 16); + mP5.mQzssBadMask = strtol(mField[eQzssBadMask].c_str(), NULL, 16); + } + + inline SystemStatusPQWP5& get() { return mP5;} +}; + +/****************************************************************************** + SystemStatusPQWP6parser +******************************************************************************/ +class SystemStatusPQWP6 +{ +public: + timespec mUtcTime; + uint32_t mFixInfoMask; +}; + +class SystemStatusPQWP6parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eFixInfoMask = 2, + eMax = eFixInfoMask + }; + SystemStatusPQWP6 mP6; + +public: + inline uint32_t getFixInfoMask() { return mP6.mFixInfoMask; } + + SystemStatusPQWP6parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + return; + } + memset(&mP6, 0, sizeof(mP6)); + mP6.mUtcTime = setUtcTime(mField[eUtcTime]); + mP6.mFixInfoMask = strtol(mField[eFixInfoMask].c_str(), NULL, 16); + } + + inline SystemStatusPQWP6& get() { return mP6;} +}; + +/****************************************************************************** + SystemStatusPQWS1parser +******************************************************************************/ +class SystemStatusPQWS1 +{ +public: + timespec mUtcTime; + uint32_t mFixInfoMask; + uint32_t mHepeLimit; +}; + +class SystemStatusPQWS1parser : public SystemStatusNmeaBase +{ +private: + enum + { + eTalker = 0, + eUtcTime = 1, + eFixInfoMask = 2, + eHepeLimit = 3, + eMax = eHepeLimit + }; + SystemStatusPQWS1 mS1; + +public: + inline uint16_t getFixInfoMask() { return mS1.mFixInfoMask; } + inline uint32_t getHepeLimit() { return mS1.mHepeLimit; } + + SystemStatusPQWS1parser(const char *str_in, uint32_t len_in) + : SystemStatusNmeaBase(str_in, len_in) + { + if (mField.size() < eMax) { + 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()); + } + + inline SystemStatusPQWS1& get() { return mS1;} +}; + +/****************************************************************************** + SystemStatusTimeAndClock +******************************************************************************/ +SystemStatusTimeAndClock::SystemStatusTimeAndClock(const SystemStatusPQWM1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mGpsWeek(nmea.mGpsWeek), + mGpsTowMs(nmea.mGpsTowMs), + mTimeValid(nmea.mTimeValid), + mTimeSource(nmea.mTimeSource), + mTimeUnc(nmea.mTimeUnc), + mClockFreqBias(nmea.mClockFreqBias), + mClockFreqBiasUnc(nmea.mClockFreqBiasUnc) +{ +} + +bool SystemStatusTimeAndClock::equals(SystemStatusTimeAndClock& peer) +{ + if ((mGpsWeek != peer.mGpsWeek) || + (mGpsTowMs != peer.mGpsTowMs) || + (mTimeValid != peer.mTimeValid) || + (mTimeSource != peer.mTimeSource) || + (mTimeUnc != peer.mTimeUnc) || + (mClockFreqBias != peer.mClockFreqBias) || + (mClockFreqBiasUnc != peer.mClockFreqBiasUnc)) { + return false; + } + return true; +} + +void SystemStatusTimeAndClock::dump() +{ + LOC_LOGV("TimeAndClock: u=%ld:%ld g=%d:%d v=%d s=%d u=%d b=%d bu=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mGpsWeek, + mGpsTowMs, + mTimeValid, + mTimeSource, + mTimeUnc, + mClockFreqBias, + mClockFreqBiasUnc); + return; +} + +/****************************************************************************** + SystemStatusXoState +******************************************************************************/ +SystemStatusXoState::SystemStatusXoState(const SystemStatusPQWM1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mXoState(nmea.mXoState) +{ +} + +bool SystemStatusXoState::equals(SystemStatusXoState& peer) +{ + if (mXoState != peer.mXoState) { + return false; + } + return true; +} + +void SystemStatusXoState::dump() +{ + LOC_LOGV("XoState: u=%ld:%ld x=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mXoState); + return; +} + +/****************************************************************************** + SystemStatusRfAndParams +******************************************************************************/ +SystemStatusRfAndParams::SystemStatusRfAndParams(const SystemStatusPQWM1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mPgaGain(nmea.mPgaGain), + mGpsBpAmpI(nmea.mGpsBpAmpI), + mGpsBpAmpQ(nmea.mGpsBpAmpQ), + mAdcI(nmea.mAdcI), + mAdcQ(nmea.mAdcQ), + mJammerGps(nmea.mJammerGps), + mJammerGlo(nmea.mJammerGlo), + mJammerBds(nmea.mJammerBds), + mJammerGal(nmea.mJammerGal) +{ +} + +bool SystemStatusRfAndParams::equals(SystemStatusRfAndParams& peer) +{ + if ((mPgaGain != peer.mPgaGain) || + (mGpsBpAmpI != peer.mGpsBpAmpI) || + (mGpsBpAmpQ != peer.mGpsBpAmpQ) || + (mAdcI != peer.mAdcI) || + (mAdcQ != peer.mAdcQ) || + (mJammerGps != peer.mJammerGps) || + (mJammerGlo != peer.mJammerGlo) || + (mJammerBds != peer.mJammerBds) || + (mJammerGal != peer.mJammerGal)) { + return false; + } + return true; +} + +void SystemStatusRfAndParams::dump() +{ + LOC_LOGV("RfAndParams: u=%ld:%ld p=%d bi=%d bq=%d ai=%d aq=%d gp=%d gl=%d bd=%d ga=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mPgaGain, + mGpsBpAmpI, + mGpsBpAmpQ, + mAdcI, + mAdcQ, + mJammerGps, + mJammerGlo, + mJammerBds, + mJammerGal); + return; +} + +/****************************************************************************** + SystemStatusErrRecovery +******************************************************************************/ +SystemStatusErrRecovery::SystemStatusErrRecovery(const SystemStatusPQWM1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mRecErrorRecovery(nmea.mRecErrorRecovery) +{ +} + +bool SystemStatusErrRecovery::equals(SystemStatusErrRecovery& peer) +{ + if (mRecErrorRecovery != peer.mRecErrorRecovery) { + return false; + } + return true; +} + +void SystemStatusErrRecovery::dump() +{ + LOC_LOGV("ErrRecovery: u=%ld:%ld e=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mRecErrorRecovery); + return; +} + +/****************************************************************************** + SystemStatusInjectedPosition +******************************************************************************/ +SystemStatusInjectedPosition::SystemStatusInjectedPosition(const SystemStatusPQWP1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mEpiValidity(nmea.mEpiValidity), + mEpiLat(nmea.mEpiLat), + mEpiLon(nmea.mEpiLon), + mEpiAlt(nmea.mEpiAlt), + mEpiHepe(nmea.mEpiHepe), + mEpiAltUnc(nmea.mEpiAltUnc), + mEpiSrc(nmea.mEpiSrc) +{ +} + +bool SystemStatusInjectedPosition::equals(SystemStatusInjectedPosition& peer) +{ + if ((mEpiValidity != peer.mEpiValidity) || + (mEpiLat != peer.mEpiLat) || + (mEpiLon != peer.mEpiLon) || + (mEpiAlt != peer.mEpiAlt) || + (mEpiHepe != peer.mEpiHepe) || + (mEpiAltUnc != peer.mEpiAltUnc) || + (mEpiSrc != peer.mEpiSrc)) { + return false; + } + return true; +} + +void SystemStatusInjectedPosition::dump() +{ + LOC_LOGV("InjectedPosition: u=%ld:%ld v=%x la=%f lo=%f al=%f he=%f au=%f es=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mEpiValidity, + mEpiLat, + mEpiLon, + mEpiAlt, + mEpiHepe, + mEpiAltUnc, + mEpiSrc); + return; +} + +/****************************************************************************** + SystemStatusBestPosition +******************************************************************************/ +SystemStatusBestPosition::SystemStatusBestPosition(const SystemStatusPQWP2& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mBestLat(nmea.mBestLat), + mBestLon(nmea.mBestLon), + mBestAlt(nmea.mBestAlt), + mBestHepe(nmea.mBestHepe), + mBestAltUnc(nmea.mBestAltUnc) +{ +} + +bool SystemStatusBestPosition::equals(SystemStatusBestPosition& peer) +{ + if ((mBestLat != peer.mBestLat) || + (mBestLon != peer.mBestLon) || + (mBestAlt != peer.mBestAlt) || + (mBestHepe != peer.mBestHepe) || + (mBestAltUnc != peer.mBestAltUnc)) { + return false; + } + return true; +} + +void SystemStatusBestPosition::dump() +{ + LOC_LOGV("BestPosition: u=%ld:%ld la=%f lo=%f al=%f he=%f au=%f", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mBestLat, + mBestLon, + mBestAlt, + mBestHepe, + mBestAltUnc); + return; +} + +/****************************************************************************** + SystemStatusXtra +******************************************************************************/ +SystemStatusXtra::SystemStatusXtra(const SystemStatusPQWP3& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mXtraValidMask(nmea.mXtraValidMask), + mGpsXtraAge(nmea.mGpsXtraAge), + mGloXtraAge(nmea.mGloXtraAge), + mBdsXtraAge(nmea.mBdsXtraAge), + mGalXtraAge(nmea.mGalXtraAge), + mQzssXtraAge(nmea.mQzssXtraAge), + mGpsXtraValid(nmea.mGpsXtraValid), + mGloXtraValid(nmea.mGloXtraValid), + mBdsXtraValid(nmea.mBdsXtraValid), + mGalXtraValid(nmea.mGalXtraValid), + mQzssXtraValid(nmea.mQzssXtraValid) +{ +} + +bool SystemStatusXtra::equals(SystemStatusXtra& peer) +{ + if ((mXtraValidMask != peer.mXtraValidMask) || + (mGpsXtraAge != peer.mGpsXtraAge) || + (mGloXtraAge != peer.mGloXtraAge) || + (mBdsXtraAge != peer.mBdsXtraAge) || + (mGalXtraAge != peer.mGalXtraAge) || + (mQzssXtraAge != peer.mQzssXtraAge) || + (mGpsXtraValid != peer.mGpsXtraValid) || + (mGloXtraValid != peer.mGloXtraValid) || + (mBdsXtraValid != peer.mBdsXtraValid) || + (mGalXtraValid != peer.mGalXtraValid) || + (mQzssXtraValid != peer.mQzssXtraValid)) { + return false; + } + return true; +} + +void SystemStatusXtra::dump() +{ + LOC_LOGV("SystemStatusXtra: u=%ld:%ld m=%x a=%d:%d:%d:%d:%d v=%x:%x:%x:%x:%x", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mXtraValidMask, + mGpsXtraAge, + mGloXtraAge, + mBdsXtraAge, + mGalXtraAge, + mQzssXtraAge, + mGpsXtraValid, + mGloXtraValid, + mBdsXtraValid, + mGalXtraValid, + mQzssXtraValid); + return; +} + +/****************************************************************************** + SystemStatusEphemeris +******************************************************************************/ +SystemStatusEphemeris::SystemStatusEphemeris(const SystemStatusPQWP4& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mGpsEpheValid(nmea.mGpsEpheValid), + mGloEpheValid(nmea.mGloEpheValid), + mBdsEpheValid(nmea.mBdsEpheValid), + mGalEpheValid(nmea.mGalEpheValid), + mQzssEpheValid(nmea.mQzssEpheValid) +{ +} + +bool SystemStatusEphemeris::equals(SystemStatusEphemeris& peer) +{ + if ((mGpsEpheValid != peer.mGpsEpheValid) || + (mGloEpheValid != peer.mGloEpheValid) || + (mBdsEpheValid != peer.mBdsEpheValid) || + (mGalEpheValid != peer.mGalEpheValid) || + (mQzssEpheValid != peer.mQzssEpheValid)) { + return false; + } + return true; +} + +void SystemStatusEphemeris::dump() +{ + LOC_LOGV("Ephemeris: u=%ld:%ld ev=%x:%x:%x:%x:%x", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mGpsEpheValid, + mGloEpheValid, + mBdsEpheValid, + mGalEpheValid, + mQzssEpheValid); + return; +} + +/****************************************************************************** + SystemStatusSvHealth +******************************************************************************/ +SystemStatusSvHealth::SystemStatusSvHealth(const SystemStatusPQWP5& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mGpsUnknownMask(nmea.mGpsUnknownMask), + mGloUnknownMask(nmea.mGloUnknownMask), + mBdsUnknownMask(nmea.mBdsUnknownMask), + mGalUnknownMask(nmea.mGalUnknownMask), + mQzssUnknownMask(nmea.mQzssUnknownMask), + mGpsGoodMask(nmea.mGpsGoodMask), + mGloGoodMask(nmea.mGloGoodMask), + mBdsGoodMask(nmea.mBdsGoodMask), + mGalGoodMask(nmea.mGalGoodMask), + mQzssGoodMask(nmea.mQzssGoodMask), + mGpsBadMask(nmea.mGpsBadMask), + mGloBadMask(nmea.mGloBadMask), + mBdsBadMask(nmea.mBdsBadMask), + mGalBadMask(nmea.mGalBadMask), + mQzssBadMask(nmea.mQzssBadMask) +{ +} + +bool SystemStatusSvHealth::equals(SystemStatusSvHealth& peer) +{ + if ((mGpsUnknownMask != peer.mGpsUnknownMask) || + (mGloUnknownMask != peer.mGloUnknownMask) || + (mBdsUnknownMask != peer.mBdsUnknownMask) || + (mGalUnknownMask != peer.mGalUnknownMask) || + (mQzssUnknownMask != peer.mQzssUnknownMask) || + (mGpsGoodMask != peer.mGpsGoodMask) || + (mGloGoodMask != peer.mGloGoodMask) || + (mBdsGoodMask != peer.mBdsGoodMask) || + (mGalGoodMask != peer.mGalGoodMask) || + (mQzssGoodMask != peer.mQzssGoodMask) || + (mGpsBadMask != peer.mGpsBadMask) || + (mGloBadMask != peer.mGloBadMask) || + (mBdsBadMask != peer.mBdsBadMask) || + (mGalBadMask != peer.mGalBadMask) || + (mQzssBadMask != peer.mQzssBadMask)) { + return false; + } + return true; +} + +void SystemStatusSvHealth::dump() +{ + LOC_LOGV("SvHealth: u=%ld:%ld u=%x:%x:%x:%x:%x g=%x:%x:%x:%x:%x b=%x:%x:%x:%x:%x", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mGpsUnknownMask, + mGloUnknownMask, + mBdsUnknownMask, + mGalUnknownMask, + mQzssUnknownMask, + mGpsGoodMask, + mGloGoodMask, + mBdsGoodMask, + mGalGoodMask, + mQzssGoodMask, + mGpsBadMask, + mGloBadMask, + mBdsBadMask, + mGalBadMask, + mQzssBadMask); + return; +} + +/****************************************************************************** + SystemStatusPdr +******************************************************************************/ +SystemStatusPdr::SystemStatusPdr(const SystemStatusPQWP6& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mFixInfoMask(nmea.mFixInfoMask) +{ +} + +bool SystemStatusPdr::equals(SystemStatusPdr& peer) +{ + if (mFixInfoMask != peer.mFixInfoMask) { + return false; + } + return true; +} + +void SystemStatusPdr::dump() +{ + LOC_LOGV("Pdr: u=%ld:%ld m=%x", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mFixInfoMask); + return; +} + +/****************************************************************************** + SystemStatusPositionFailure +******************************************************************************/ +SystemStatusPositionFailure::SystemStatusPositionFailure(const SystemStatusPQWS1& nmea) : + SystemStatusItemBase(nmea.mUtcTime), + mFixInfoMask(nmea.mFixInfoMask), + mHepeLimit(nmea.mHepeLimit) +{ +} + +bool SystemStatusPositionFailure::equals(SystemStatusPositionFailure& peer) +{ + if ((mFixInfoMask != peer.mFixInfoMask) || + (mHepeLimit != peer.mHepeLimit)) { + return false; + } + return true; +} + +void SystemStatusPositionFailure::dump() +{ + LOC_LOGV("PositionFailure: u=%ld:%ld m=%d h=%d", + mUtcTime.tv_sec, mUtcTime.tv_nsec, + mFixInfoMask, + mHepeLimit); + return; +} + +/****************************************************************************** + SystemStatus +******************************************************************************/ +pthread_mutex_t SystemStatus::mMutexSystemStatus = PTHREAD_MUTEX_INITIALIZER; + +SystemStatus::SystemStatus() +{ + mCache.mTimeAndClock.clear(); + mCache.mXoState.clear(); + mCache.mRfAndParams.clear(); + mCache.mErrRecovery.clear(); + + mCache.mInjectedPosition.clear(); + mCache.mBestPosition.clear(); + mCache.mXtra.clear(); + mCache.mEphemeris.clear(); + mCache.mSvHealth.clear(); + mCache.mPdr.clear(); + mCache.mPositionFailure.clear(); +} + +/****************************************************************************** + SystemStatus - M1 functions +******************************************************************************/ +bool SystemStatus::setTimeAndCLock(const SystemStatusPQWM1& nmea) +{ + SystemStatusTimeAndClock s(nmea); + if (mCache.mTimeAndClock.empty() || !mCache.mTimeAndClock.back().equals(s)) { + mCache.mTimeAndClock.push_back(s); + if (mCache.mTimeAndClock.size() > maxTimeAndClock) { + mCache.mTimeAndClock.erase(mCache.mTimeAndClock.begin()); + } + } + return true; +} + +bool SystemStatus::setXoState(const SystemStatusPQWM1& nmea) +{ + SystemStatusXoState s(nmea); + if (mCache.mXoState.empty() || !mCache.mXoState.back().equals(s)) { + mCache.mXoState.push_back(s); + if (mCache.mXoState.size() > maxXoState) { + mCache.mXoState.erase(mCache.mXoState.begin()); + } + } + return true; +} + +bool SystemStatus::setRfAndParams(const SystemStatusPQWM1& nmea) +{ + SystemStatusRfAndParams s(nmea); + if (mCache.mRfAndParams.empty() || !mCache.mRfAndParams.back().equals(s)) { + mCache.mRfAndParams.push_back(s); + if (mCache.mRfAndParams.size() > maxRfAndParams) { + mCache.mRfAndParams.erase(mCache.mRfAndParams.begin()); + } + } + return true; +} + +bool SystemStatus::setErrRecovery(const SystemStatusPQWM1& nmea) +{ + SystemStatusErrRecovery s(nmea); + if (mCache.mErrRecovery.empty() || !mCache.mErrRecovery.back().equals(s)) { + mCache.mErrRecovery.push_back(s); + if (mCache.mErrRecovery.size() > maxErrRecovery) { + mCache.mErrRecovery.erase(mCache.mErrRecovery.begin()); + } + } + return true; +} + +/****************************************************************************** + SystemStatus - Px functions +******************************************************************************/ +bool SystemStatus::setInjectedPosition(const SystemStatusPQWP1& nmea) +{ + SystemStatusInjectedPosition s(nmea); + if (mCache.mInjectedPosition.empty() || !mCache.mInjectedPosition.back().equals(s)) { + mCache.mInjectedPosition.push_back(s); + if (mCache.mInjectedPosition.size() > maxInjectedPosition) { + mCache.mInjectedPosition.erase(mCache.mInjectedPosition.begin()); + } + } + return true; +} + +bool SystemStatus::setBestPosition(const SystemStatusPQWP2& nmea) +{ + SystemStatusBestPosition s(nmea); + if (mCache.mBestPosition.empty() || !mCache.mBestPosition.back().equals(s)) { + mCache.mBestPosition.push_back(s); + if (mCache.mBestPosition.size() > maxBestPosition) { + mCache.mBestPosition.erase(mCache.mBestPosition.begin()); + } + } + return true; +} + +bool SystemStatus::setXtra(const SystemStatusPQWP3& nmea) +{ + SystemStatusXtra s(nmea); + if (mCache.mXtra.empty() || !mCache.mXtra.back().equals(s)) { + mCache.mXtra.push_back(s); + if (mCache.mXtra.size() > maxXtra) { + mCache.mXtra.erase(mCache.mXtra.begin()); + } + } + return true; +} + +bool SystemStatus::setEphemeris(const SystemStatusPQWP4& nmea) +{ + SystemStatusEphemeris s(nmea); + if (mCache.mEphemeris.empty() || !mCache.mEphemeris.back().equals(s)) { + mCache.mEphemeris.push_back(s); + if (mCache.mEphemeris.size() > maxEphemeris) { + mCache.mEphemeris.erase(mCache.mEphemeris.begin()); + } + } + return true; +} + +bool SystemStatus::setSvHealth(const SystemStatusPQWP5& nmea) +{ + SystemStatusSvHealth s(nmea); + if (mCache.mSvHealth.empty() || !mCache.mSvHealth.back().equals(s)) { + mCache.mSvHealth.push_back(s); + if (mCache.mSvHealth.size() > maxSvHealth) { + mCache.mSvHealth.erase(mCache.mSvHealth.begin()); + } + } + return true; +} + +bool SystemStatus::setPdr(const SystemStatusPQWP6& nmea) +{ + SystemStatusPdr s(nmea); + if (mCache.mPdr.empty() || !mCache.mPdr.back().equals(s)) { + mCache.mPdr.push_back(s); + if (mCache.mPdr.size() > maxPdr) { + mCache.mPdr.erase(mCache.mPdr.begin()); + } + } + return true; +} + +/****************************************************************************** + SystemStatus - Sx functions +******************************************************************************/ +bool SystemStatus::setPositionFailure(const SystemStatusPQWS1& nmea) +{ + SystemStatusPositionFailure s(nmea); + if (mCache.mPositionFailure.empty() || !mCache.mPositionFailure.back().equals(s)) { + mCache.mPositionFailure.push_back(s); + if (mCache.mPositionFailure.size() > maxPositionFailure) { + mCache.mPositionFailure.erase(mCache.mPositionFailure.begin()); + } + } + return true; +} + +/****************************************************************************** +@brief API to set report data into internal buffer + +@param[In] data pointer to the NMEA string +@param[In] len length of the NMEA string + +@return true when successfully done +******************************************************************************/ +static uint32_t cnt = 0; +static uint32_t cnt_m1 = 0; +static uint32_t cnt_p1 = 0; +static uint32_t cnt_p2 = 0; +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_s1 = 0; + +bool SystemStatus::setNmeaString(const char *data, uint32_t len) +{ + bool ret = false; + if (NULL == data + || (len < SystemStatusNmeaBase::NMEA_MINSIZE) + || (len > SystemStatusNmeaBase::NMEA_MAXSIZE)) { + return false; + } + + char buf[SystemStatusNmeaBase::NMEA_MAXSIZE + 1] = { 0 }; + strncpy(buf, data, len); + LOC_LOGI("setNmeaString-0320a: len=%d str=%d nmea=%s", len, strlen(data), buf); + + pthread_mutex_lock(&mMutexSystemStatus); + + // parse the received nmea strings here + if (0 == strncmp(data, "$PQWM1", SystemStatusNmeaBase::NMEA_MINSIZE)) { + SystemStatusPQWM1 s = SystemStatusPQWM1parser(buf, len).get(); + ret = setTimeAndCLock(s); + ret |= setXoState(s); + ret |= setRfAndParams(s); + ret |= setErrRecovery(s); + cnt_m1++; + } + else if (0 == strncmp(data, "$PQWP1", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setInjectedPosition(SystemStatusPQWP1parser(buf, len).get()); + cnt_p1++; + } + else if (0 == strncmp(data, "$PQWP2", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setBestPosition(SystemStatusPQWP2parser(buf, len).get()); + cnt_p2++; + } + else if (0 == strncmp(data, "$PQWP3", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setXtra(SystemStatusPQWP3parser(buf, len).get()); + cnt_p3++; + } + else if (0 == strncmp(data, "$PQWP4", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setEphemeris(SystemStatusPQWP4parser(buf, len).get()); + cnt_p4++; + } + else if (0 == strncmp(data, "$PQWP5", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setSvHealth(SystemStatusPQWP5parser(buf, len).get()); + cnt_p5++; + } + else if (0 == strncmp(data, "$PQWP6", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setPdr(SystemStatusPQWP6parser(buf, len).get()); + cnt_p6++; + } + else if (0 == strncmp(data, "$PQWS1", SystemStatusNmeaBase::NMEA_MINSIZE)) { + ret = setPositionFailure(SystemStatusPQWS1parser(buf, len).get()); + cnt_s1++; + } + else { + // do nothing + } + cnt++; + LOC_LOGV("setNmeaString: cnt=%d M:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d S:%d", + cnt, + cnt_m1, + cnt_p1, + cnt_p2, + cnt_p3, + cnt_p4, + cnt_p5, + cnt_p6, + cnt_s1); + + pthread_mutex_unlock(&mMutexSystemStatus); + return ret; +} + +/****************************************************************************** +@brief API to get report data into a given buffer + +@param[In] reference to report buffer +@param[In] bool flag to identify latest only or entire buffer + +@return true when successfully done +******************************************************************************/ +bool SystemStatus::getReport(SystemStatusReports& report, bool isLatestOnly) const +{ + pthread_mutex_lock(&mMutexSystemStatus); + + if (isLatestOnly) { + // push back only the latest report and return it + report.mTimeAndClock.clear(); + if (mCache.mTimeAndClock.size() >= 1) { + report.mTimeAndClock.push_back(mCache.mTimeAndClock.back()); + report.mTimeAndClock.back().dump(); + } + report.mXoState.clear(); + if (mCache.mXoState.size() >= 1) { + report.mXoState.push_back(mCache.mXoState.back()); + report.mXoState.back().dump(); + } + report.mRfAndParams.clear(); + if (mCache.mRfAndParams.size() >= 1) { + report.mRfAndParams.push_back(mCache.mRfAndParams.back()); + report.mRfAndParams.back().dump(); + } + report.mErrRecovery.clear(); + if (mCache.mErrRecovery.size() >= 1) { + report.mErrRecovery.push_back(mCache.mErrRecovery.back()); + report.mErrRecovery.back().dump(); + } + + report.mInjectedPosition.clear(); + if (mCache.mInjectedPosition.size() >= 1) { + report.mInjectedPosition.push_back(mCache.mInjectedPosition.back()); + report.mInjectedPosition.back().dump(); + } + report.mBestPosition.clear(); + if (mCache.mBestPosition.size() >= 1) { + report.mBestPosition.push_back(mCache.mBestPosition.back()); + report.mBestPosition.back().dump(); + } + report.mXtra.clear(); + if (mCache.mXtra.size() >= 1) { + report.mXtra.push_back(mCache.mXtra.back()); + report.mXtra.back().dump(); + } + report.mEphemeris.clear(); + if (mCache.mEphemeris.size() >= 1) { + report.mEphemeris.push_back(mCache.mEphemeris.back()); + report.mEphemeris.back().dump(); + } + report.mSvHealth.clear(); + if (mCache.mSvHealth.size() >= 1) { + report.mSvHealth.push_back(mCache.mSvHealth.back()); + report.mSvHealth.back().dump(); + } + report.mPdr.clear(); + if (mCache.mPdr.size() >= 1) { + report.mPdr.push_back(mCache.mPdr.back()); + report.mPdr.back().dump(); + } + report.mPositionFailure.clear(); + if (mCache.mPositionFailure.size() >= 1) { + report.mPositionFailure.push_back(mCache.mPositionFailure.back()); + report.mPositionFailure.back().dump(); + } + } + else { + // copy entire reports and return them + report.mTimeAndClock.clear(); + report.mXoState.clear(); + report.mRfAndParams.clear(); + report.mErrRecovery.clear(); + + report.mInjectedPosition.clear(); + report.mBestPosition.clear(); + report.mXtra.clear(); + report.mEphemeris.clear(); + report.mSvHealth.clear(); + report.mPdr.clear(); + report.mPositionFailure.clear(); + report = mCache; + } + + pthread_mutex_unlock(&mMutexSystemStatus); + return true; +} + +} // namespace loc_core + diff --git a/core/SystemStatus.h b/core/SystemStatus.h new file mode 100644 index 00000000..95f82da5 --- /dev/null +++ b/core/SystemStatus.h @@ -0,0 +1,275 @@ +/* Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation, nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef __SYSTEM_STATUS__ +#define __SYSTEM_STATUS__ + +#include + +namespace loc_core +{ + +/****************************************************************************** + SystemStatus report data structure +******************************************************************************/ +class SystemStatusItemBase +{ +public: + timespec mUtcTime; + SystemStatusItemBase(timespec utctime) : mUtcTime(utctime) { }; + virtual ~SystemStatusItemBase() { }; + virtual void dump(void) { }; +}; + +class SystemStatusPQWM1; +class SystemStatusTimeAndClock : public SystemStatusItemBase +{ +public: + uint16_t mGpsWeek; + uint32_t mGpsTowMs; + uint8_t mTimeValid; + uint8_t mTimeSource; + int32_t mTimeUnc; + int32_t mClockFreqBias; + int32_t mClockFreqBiasUnc; + SystemStatusTimeAndClock(const SystemStatusPQWM1& nmea); + bool equals(SystemStatusTimeAndClock& peer); + void dump(void); +}; + +class SystemStatusXoState : public SystemStatusItemBase +{ +public: + uint8_t mXoState; + SystemStatusXoState(const SystemStatusPQWM1& nmea); + bool equals(SystemStatusXoState& peer); + void dump(void); +}; + +class SystemStatusRfAndParams : public SystemStatusItemBase +{ +public: + int32_t mPgaGain; + uint32_t mGpsBpAmpI; + uint32_t mGpsBpAmpQ; + uint32_t mAdcI; + uint32_t mAdcQ; + uint32_t mJammerGps; + uint32_t mJammerGlo; + uint32_t mJammerBds; + uint32_t mJammerGal; + SystemStatusRfAndParams(const SystemStatusPQWM1& nmea); + bool equals(SystemStatusRfAndParams& peer); + void dump(void); +}; + +class SystemStatusErrRecovery : public SystemStatusItemBase +{ +public: + uint32_t mRecErrorRecovery; + SystemStatusErrRecovery(const SystemStatusPQWM1& nmea); + bool equals(SystemStatusErrRecovery& peer); + void dump(void); +}; + +class SystemStatusPQWP1; +class SystemStatusInjectedPosition : public SystemStatusItemBase +{ +public: + uint8_t mEpiValidity; + float mEpiLat; + float mEpiLon; + float mEpiAlt; + float mEpiHepe; + float mEpiAltUnc; + uint8_t mEpiSrc; + SystemStatusInjectedPosition(const SystemStatusPQWP1& nmea); + bool equals(SystemStatusInjectedPosition& peer); + void dump(void); +}; + +class SystemStatusPQWP2; +class SystemStatusBestPosition : public SystemStatusItemBase +{ +public: + float mBestLat; + float mBestLon; + float mBestAlt; + float mBestHepe; + float mBestAltUnc; + SystemStatusBestPosition(const SystemStatusPQWP2& nmea); + bool equals(SystemStatusBestPosition& peer); + void dump(void); +}; + +class SystemStatusPQWP3; +class SystemStatusXtra : public SystemStatusItemBase +{ +public: + uint8_t mXtraValidMask; + uint32_t mGpsXtraAge; + uint32_t mGloXtraAge; + uint32_t mBdsXtraAge; + uint32_t mGalXtraAge; + uint32_t mQzssXtraAge; + uint32_t mGpsXtraValid; + uint32_t mGloXtraValid; + uint64_t mBdsXtraValid; + uint64_t mGalXtraValid; + uint8_t mQzssXtraValid; + SystemStatusXtra(const SystemStatusPQWP3& nmea); + bool equals(SystemStatusXtra& peer); + void dump(void); +}; + +class SystemStatusPQWP4; +class SystemStatusEphemeris : public SystemStatusItemBase +{ +public: + uint32_t mGpsEpheValid; + uint32_t mGloEpheValid; + uint64_t mBdsEpheValid; + uint64_t mGalEpheValid; + uint8_t mQzssEpheValid; + SystemStatusEphemeris(const SystemStatusPQWP4& nmea); + bool equals(SystemStatusEphemeris& peer); + void dump(void); +}; + +class SystemStatusPQWP5; +class SystemStatusSvHealth : public SystemStatusItemBase +{ +public: + uint32_t mGpsUnknownMask; + uint32_t mGloUnknownMask; + uint64_t mBdsUnknownMask; + uint64_t mGalUnknownMask; + uint8_t mQzssUnknownMask; + uint32_t mGpsGoodMask; + uint32_t mGloGoodMask; + uint64_t mBdsGoodMask; + uint64_t mGalGoodMask; + uint8_t mQzssGoodMask; + uint32_t mGpsBadMask; + uint32_t mGloBadMask; + uint64_t mBdsBadMask; + uint64_t mGalBadMask; + uint8_t mQzssBadMask; + SystemStatusSvHealth(const SystemStatusPQWP5& nmea); + bool equals(SystemStatusSvHealth& peer); + void dump(void); +}; + +class SystemStatusPQWP6; +class SystemStatusPdr : public SystemStatusItemBase +{ +public: + uint32_t mFixInfoMask; + SystemStatusPdr(const SystemStatusPQWP6& nmea); + bool equals(SystemStatusPdr& peer); + void dump(void); +}; + +class SystemStatusPQWS1; +class SystemStatusPositionFailure : public SystemStatusItemBase +{ +public: + uint32_t mFixInfoMask; + uint32_t mHepeLimit; + SystemStatusPositionFailure(const SystemStatusPQWS1& nmea); + bool equals(SystemStatusPositionFailure& peer); + void dump(void); +}; + +/****************************************************************************** + SystemStatusReports +******************************************************************************/ +class SystemStatusReports +{ +public: + std::vector mTimeAndClock; + std::vector mXoState; + std::vector mRfAndParams; + std::vector mErrRecovery; + + std::vector mInjectedPosition; + std::vector mBestPosition; + std::vector mXtra; + std::vector mEphemeris; + std::vector mSvHealth; + std::vector mPdr; + std::vector mPositionFailure; +}; + +/****************************************************************************** + SystemStatus +******************************************************************************/ +class SystemStatus +{ + static pthread_mutex_t mMutexSystemStatus; + + static const uint32_t maxTimeAndClock = 5; + static const uint32_t maxXoState = 5; + static const uint32_t maxRfAndParams = 5; + static const uint32_t maxErrRecovery = 5; + + static const uint32_t maxInjectedPosition = 5; + static const uint32_t maxBestPosition = 5; + static const uint32_t maxXtra = 5; + static const uint32_t maxEphemeris = 5; + static const uint32_t maxSvHealth = 5; + static const uint32_t maxPdr = 5; + static const uint32_t maxPositionFailure = 5; + + SystemStatusReports mCache; + + bool setTimeAndCLock(const SystemStatusPQWM1& nmea); + bool setXoState(const SystemStatusPQWM1& nmea); + bool setRfAndParams(const SystemStatusPQWM1& nmea); + bool setErrRecovery(const SystemStatusPQWM1& nmea); + + bool setInjectedPosition(const SystemStatusPQWP1& nmea); + bool setBestPosition(const SystemStatusPQWP2& nmea); + bool setXtra(const SystemStatusPQWP3& nmea); + bool setEphemeris(const SystemStatusPQWP4& nmea); + bool setSvHealth(const SystemStatusPQWP5& nmea); + bool setPdr(const SystemStatusPQWP6& nmea); + bool setPositionFailure(const SystemStatusPQWS1& nmea); + +public: + SystemStatus(); + ~SystemStatus() { } + + bool setNmeaString(const char *data, uint32_t len); + bool getReport(SystemStatusReports& reports, bool isLatestonly = false) const; +}; + +} // namespace loc_core + +#endif //__SYSTEM_STATUS__ + diff --git a/core/gps_extended_c.h b/core/gps_extended_c.h index 55fd03eb..965a2e6a 100644 --- a/core/gps_extended_c.h +++ b/core/gps_extended_c.h @@ -373,14 +373,15 @@ typedef uint32_t NmeaSentenceTypesMask; #define LOC_NMEA_MASK_GAGGA_V02 ((NmeaSentenceTypesMask)0x00004000) /**< Enable GAGGA type */ #define LOC_NMEA_MASK_PQGSA_V02 ((NmeaSentenceTypesMask)0x00008000) /**< Enable PQGSA type */ #define LOC_NMEA_MASK_PQGSV_V02 ((NmeaSentenceTypesMask)0x00010000) /**< Enable PQGSV type */ +#define LOC_NMEA_MASK_DEBUG_V02 ((NmeaSentenceTypesMask)0x00020000) /**< Enable DEBUG type */ + #define LOC_NMEA_ALL_SUPPORTED_MASK (LOC_NMEA_MASK_GGA_V02 | LOC_NMEA_MASK_RMC_V02 | \ LOC_NMEA_MASK_GSV_V02 | LOC_NMEA_MASK_GSA_V02 | LOC_NMEA_MASK_VTG_V02 | \ LOC_NMEA_MASK_PQXFI_V02 | LOC_NMEA_MASK_PSTIS_V02 | LOC_NMEA_MASK_GLGSV_V02 | \ LOC_NMEA_MASK_GNGSA_V02 | LOC_NMEA_MASK_GNGNS_V02 | LOC_NMEA_MASK_GARMC_V02 | \ LOC_NMEA_MASK_GAGSV_V02 | LOC_NMEA_MASK_GAGSA_V02 | LOC_NMEA_MASK_GAVTG_V02 | \ - LOC_NMEA_MASK_GAGGA_V02 | LOC_NMEA_MASK_PQGSA_V02 | LOC_NMEA_MASK_PQGSV_V02 ) - - + LOC_NMEA_MASK_GAGGA_V02 | LOC_NMEA_MASK_PQGSA_V02 | LOC_NMEA_MASK_PQGSV_V02 | \ + LOC_NMEA_MASK_DEBUG_V02 ) typedef enum { LOC_ENG_IF_REQUEST_SENDER_ID_QUIPC = 0, diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index e37a9923..5240a542 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -40,6 +40,7 @@ #include #include #include +#include using namespace loc_core; @@ -509,9 +510,16 @@ GnssAdapter::setConfigCommand() mAdapter.convertLppeCp(ContextBase::mGps_conf.LPPE_CP_TECHNOLOGY)); mApi.setLPPeProtocolUp( mAdapter.convertLppeUp(ContextBase::mGps_conf.LPPE_UP_TECHNOLOGY)); + + // set nmea mask type + uint32_t mask = 0; if (NMEA_PROVIDER_MP == ContextBase::mGps_conf.NMEA_PROVIDER) { - mApi.setNMEATypes(LOC_NMEA_ALL_SUPPORTED_MASK); + mask = LOC_NMEA_ALL_SUPPORTED_MASK; + } else { + mask = LOC_NMEA_MASK_DEBUG_V02; } + mApi.setNMEATypes(mask); + mApi.setXtraVersionCheck(ContextBase::mGps_conf.XTRA_VERSION_CHECK); if (ContextBase::mSap_conf.GYRO_BIAS_RANDOM_WALK_VALID || ContextBase::mSap_conf.ACCEL_RANDOM_WALK_SPECTRAL_DENSITY_VALID || @@ -999,8 +1007,7 @@ GnssAdapter::updateClientsEventMask() if (it->second.gnssSvCb != nullptr) { mask |= LOC_API_ADAPTER_BIT_SATELLITE_REPORT; } - if (it->second.gnssNmeaCb != nullptr && - NMEA_PROVIDER_MP == ContextBase::mGps_conf.NMEA_PROVIDER) { + if (it->second.gnssNmeaCb != nullptr) { mask |= LOC_API_ADAPTER_BIT_NMEA_1HZ_REPORT; } if (it->second.gnssMeasurementsCb != nullptr) { @@ -1994,7 +2001,16 @@ GnssAdapter::reportNmeaEvent(const char* nmea, size_t length, bool fromUlp) delete[] mNmea; } inline virtual void proc() const { - mAdapter.reportNmea(mNmea, mLength); + // extract bug report info - this returns true if consumed by systemstatus + bool ret = false; + SystemStatus* s = LocDualContext::getSystemStatus(); + if (nullptr != s) { + ret = s->setNmeaString(mNmea, mLength); + } + if (false == ret) { + // forward NMEA message to upper layer + mAdapter.reportNmea(mNmea, mLength); + } } };