From 6228498147cc83891788a32da8ec22ff01c4f962 Mon Sep 17 00:00:00 2001 From: Noel Eck Date: Fri, 25 May 2018 15:51:13 -0700 Subject: [PATCH] nmea_gps: Added parsing TXT and bw calculations Added code to parse GPTXT nmea sentences. Added methods to calculate sentences/second and raw bytes/second. Signed-off-by: Noel Eck --- src/nmea_gps/nmea_gps.cxx | 160 +++++++++++++++++++++++-- src/nmea_gps/nmea_gps.hpp | 77 +++++++++++- src/nmea_gps/nmea_gps.i | 1 + tests/unit/nmea_gps/nmea_gps_tests.cxx | 31 +++++ 4 files changed, 256 insertions(+), 13 deletions(-) diff --git a/src/nmea_gps/nmea_gps.cxx b/src/nmea_gps/nmea_gps.cxx index aeaf9f29..aad7116b 100644 --- a/src/nmea_gps/nmea_gps.cxx +++ b/src/nmea_gps/nmea_gps.cxx @@ -22,6 +22,7 @@ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +#include #include #include #include @@ -39,7 +40,10 @@ NMEAGPS::NMEAGPS(unsigned int uart, unsigned int baudrate, int enable_pin) : m_nmea_gps(nmea_gps_init(uart, baudrate, enable_pin)), _running(false), - _maxQueueDepth(10) + _maxQueueDepth(10), + _sentences_since_start(0), + _bytes_since_start(0), + _seconds_since_start(0.0) { if (!m_nmea_gps) throw std::runtime_error(string(__FUNCTION__) @@ -49,7 +53,10 @@ NMEAGPS::NMEAGPS(unsigned int uart, unsigned int baudrate, NMEAGPS::NMEAGPS(const std::string& uart, unsigned int baudrate) : m_nmea_gps(nmea_gps_init_raw(uart.c_str(), baudrate)), _running(false), - _maxQueueDepth(10) + _maxQueueDepth(10), + _sentences_since_start(0), + _bytes_since_start(0), + _seconds_since_start(0.0) { if (!m_nmea_gps) throw std::runtime_error(string(__FUNCTION__) @@ -59,7 +66,10 @@ NMEAGPS::NMEAGPS(const std::string& uart, unsigned int baudrate) : NMEAGPS::NMEAGPS(unsigned int bus, uint8_t addr) : m_nmea_gps(nmea_gps_init_ublox_i2c(bus, addr)), _running(false), - _maxQueueDepth(10) + _maxQueueDepth(10), + _sentences_since_start(0), + _bytes_since_start(0), + _seconds_since_start(0.0) { if (!m_nmea_gps) throw std::runtime_error(string(__FUNCTION__) @@ -84,14 +94,18 @@ std::string NMEAGPS::readStr(size_t size) throw std::runtime_error(string(__FUNCTION__) + ": nmea_gps_read() failed"); - return string(buffer, rv); + /* Keep track of bytes read */ + _bytes_since_start += rv; + return buffer; } int NMEAGPS::writeStr(const std::string& buffer) { int rv; - if ((rv = nmea_gps_write(m_nmea_gps, (char*)buffer.data(), + /* The write takes a char*. This should be OK since it's known that the mraa + * write call does not change buffer */ + if ((rv = nmea_gps_write(m_nmea_gps, const_cast(buffer.c_str()), buffer.size())) < 0) throw std::runtime_error(string(__FUNCTION__) + ": nmea_gps_write() failed"); @@ -285,12 +299,41 @@ void NMEAGPS::_parse_gpgll(const std::string& sentence) /* Throw away oldest if full, push to queue */ _mtx_fix.lock(); - if (_queue_fix.size() == _maxQueueDepth) - _queue_fix.pop(); + if (_queue_fix.size() == _maxQueueDepth) _queue_fix.pop(); _queue_fix.push(fix); _mtx_fix.unlock(); } +/* + * Regex for matching NMEA TXT messages + * Grab-bag of messages coming from a GPS device. Can basically be any + * additional information that the manufacture wants to send out. + * + * For example, the ublox GPS compass writes out data about itself: + * $GPTXT,01,01,02,u-blox ag - www.u-blox.com*50 + * $GPTXT,01,01,02,HW UBX-G60xx 00040007 *52 + * $GPTXT,01,01,02,EXT CORE 7.03 (45970) Mar 17 2011 16:26:24*44 + * $GPTXT,01,01,02,ROM BASE 6.02 (36023) Oct 15 2009 16:52:08*58 + * $GPTXT,01,01,02,MOD LEA-6H-0*2D + * $GPTXT,01,01,02,ANTSUPERV=AC SD PDoS SR*20 + * $GPTXT,01,01,02,ANTSTATUS=OK*3B + */ +static std::regex rex_txt(R"(^\$GPTXT,(\d{2}),(\d{2}),(\d{2}),(.*)[*]([A-Z0-9]{2}))"); +void NMEAGPS::_parse_gptxt(const std::string& sentence) +{ + /* Parse the GLL message */ + std::smatch m; + if (!std::regex_search(sentence, m, rex_txt) || + (std::stoi(m[5], nullptr, 16) != checksum(sentence)) ) + return; + + /* Throw away oldest if full, push to queue */ + _mtx_txt.lock(); + if (_queue_txt.size() == _maxQueueDepth) _queue_txt.pop(); + _queue_txt.push({std::stoi(m[3]), m[4]}); + _mtx_txt.unlock(); +} + void NMEAGPS::parseNMEASentence(const std::string& sentence) { /* Needs to start with $GP... and be (at least 6 characters @@ -307,6 +350,9 @@ void NMEAGPS::parseNMEASentence(const std::string& sentence) /* Call the corresponding parser */ (this->*parser)(sentence); } + + /* Keep track of total number of sentences */ + _sentences_since_start++; } /* Throw away oldest if full, push to raw sentence queue */ @@ -353,12 +399,31 @@ void NMEAGPS::_parse_thread() } } +static double getTimeSinceEpoch_s() +{ + auto now = std::chrono::system_clock::now(); + auto now_s = std::chrono::time_point_cast(now); + auto epoch = now_s.time_since_epoch(); + auto value = std::chrono::duration_cast(epoch); + return value.count()/1000.0; +} + void NMEAGPS::parseStart() { /* Don't create multiple running threads */ if (_running) return; + /* Set running flag */ _running = true; + + /* Reset total number of bytes/sentences parsed */ + _sentences_since_start = 0; + _bytes_since_start = 0; + + /* Reset the total number of seconds */ + _seconds_since_start = getTimeSinceEpoch_s(); + + /* Start the thread */ _parser = std::thread(&NMEAGPS::_parse_thread, this); } @@ -370,6 +435,9 @@ void NMEAGPS::parseStop() _running = false; if (_parser.joinable()) _parser.join(); + + /* Zero number of bytes/sentences parsed */ + _sentences_since_start = 0; } gps_fix NMEAGPS::getFix() @@ -400,6 +468,20 @@ std::string NMEAGPS::getRawSentence() return ret; } +nmeatxt NMEAGPS::getTxtMessage() +{ + nmeatxt ret; + _mtx_txt.lock(); + if (!_queue_txt.empty()) + { + /* Get a copy of the sentence, pop an element */ + ret = _queue_txt.front(); + _queue_txt.pop(); + } + _mtx_txt.unlock(); + return ret; +} + size_t NMEAGPS::fixQueueSize() { _mtx_fix.lock(); @@ -416,6 +498,14 @@ size_t NMEAGPS::rawSentenceQueueSize() return x; } +size_t NMEAGPS::txtMessageQueueSize() +{ + _mtx_txt.lock(); + size_t x =_queue_txt.size(); + _mtx_txt.unlock(); + return x; +} + std::string gps_fix::__str__() { std::ostringstream oss; @@ -456,18 +546,72 @@ std::string satellite::__str__() return oss.str(); } +std::string nmeatxt::__str__() +{ + /* Return an emty string */ + if (!severity && message.empty()) return ""; + + std::ostringstream oss; + oss << "["; + switch (severity) + { + case 0: + oss << "ERROR"; + break; + case 1: + oss << "WARNING"; + break; + case 2: + oss << "NOTICE"; + break; + case 7: + oss << "USER"; + break; + default: + oss << "UNKNOWN"; + break; + } + oss << "] " << message; + + return oss.str(); +} + +double NMEAGPS::sentencesPerSecond() +{ + double now_s = getTimeSinceEpoch_s(); + return _sentences_since_start/(now_s - _seconds_since_start); +} + +double NMEAGPS::bytesPerSecond() +{ + double now_s = getTimeSinceEpoch_s(); + return _bytes_since_start/(now_s - _seconds_since_start); +} + std::string NMEAGPS::__str__() { std::ostringstream oss; std::vector sats = satellites(); + size_t msgs = txtMessageQueueSize(); size_t qsz = getMaxQueueDepth(); oss << "NMEA GPS Instance" << std::endl << " Parsing: " << (isRunning() ? "T" : "F") << std::endl + << " NMEA sentences/second: " << std::fixed << std::setprecision(2) + << sentencesPerSecond() + << " (" << bytesPerSecond() << " bps)" << std::endl << " Available satellites: " << sats.size() << std::endl; for(auto sat : sats) oss << " " << sat.__str__() << std::endl; oss << " Queues" << std::endl << " Raw sentence Q: " << std::setw(4) << rawSentenceQueueSize() << "/" << qsz << std::endl - << " GPS fix Q: " << std::setw(4) << fixQueueSize() << "/" << qsz << std::endl; + << " GPS fix Q: " << std::setw(4) << fixQueueSize() << "/" << qsz << std::endl + << " Messages Q: " << std::setw(4) << msgs << "/" << qsz; + if (msgs > 0) + { + oss << std::endl << " Messages" << std::endl; + for (size_t i = 0; i < msgs; i++) + oss << " " << getTxtMessage().__str__() << std::endl; + } + return oss.str(); } diff --git a/src/nmea_gps/nmea_gps.hpp b/src/nmea_gps/nmea_gps.hpp index 9b49ff2f..6696c775 100644 --- a/src/nmea_gps/nmea_gps.hpp +++ b/src/nmea_gps/nmea_gps.hpp @@ -84,15 +84,15 @@ namespace upm { /** Satellite structure definition */ struct satellite { /** PRN pseudo-random-noise value which identifies a satellite */ - std::string prn = std::string(""); + std::string prn; /** Satellite elevation angle in degrees */ - int elevation_deg = 0; + int elevation_deg; /** Satellite azimuth angle in degrees */ - int azimuth_deg = 0; + int azimuth_deg; /** Satellite signal-to-noise ratio */ - int snr = 0; + int snr; /** Default constructor */ - satellite() = default; + satellite():satellite("", 0, 0, 0){} /** * Create a satellite from arguments constructor * @param sprn Target PRN string @@ -110,6 +110,28 @@ namespace upm { std::string __str__(); }; + /** Text structure definition */ + struct nmeatxt { + /** Message severity */ + int severity; + /** Message text */ + std::string message; + /** Default constructor */ + nmeatxt():nmeatxt(0, "") {} + /** + * Create a nmeatxt from arguments constructor + * @param severity Message severity + * @param message Target message + */ + nmeatxt(int severity, const std::string& message): + severity(severity), message(message){} + /** + * Provide a string representation of this structure. + * @return String representing a nmeatxt + */ + std::string __str__(); + }; + /** GPS fix quality values */ enum class gps_fix_quality { /** No fix available or invalid */ @@ -298,6 +320,13 @@ namespace upm { */ std::string getRawSentence(); + /** + * Pop and return a message from the message queue (GPTXT sentences) + * If the queue contains no elements, an empty string is returned + * @return NMEA text message + */ + nmeatxt getTxtMessage(); + /** * Get the number of elements in the GPS fix queue. * @return Number of fixes in the GPS fix queue @@ -310,6 +339,12 @@ namespace upm { */ size_t rawSentenceQueueSize(); + /** + * Get the number of messages in the NMEA message queue. + * @return Number of messages in the message queue + */ + size_t txtMessageQueueSize(); + /** * Parse NMEA sentences. * Raw sentence is placed into sentence queue. Additional structures are @@ -324,6 +359,21 @@ namespace upm { */ std::vector satellites(); + /** + * Get the number of sentences parsed per second + * @return Sentences parsed per second + */ + double sentencesPerSecond(); + + /** + * Get the throughput of the device. This number shows a rough + * data-rate as well as provides a bit of debug since it will show + * bps even if sentences are not getting parsed correctly. + + * @return Total bytes read from the device/second (bps) + */ + double bytesPerSecond(); + /** * Provide a string representation of this class * @return String representing this instance @@ -353,6 +403,8 @@ namespace upm { void _parse_gpgsv(const std::string& string); /** Parse GPGLL sentences, place in satellite collection */ void _parse_gpgll(const std::string& string); + /** Parse GPTXT sentences, place in text collection */ + void _parse_gptxt(const std::string& string); /** Provide function pointer typedef for handling NMEA chunks */ using fp = void (NMEAGPS::*)(const std::string &); @@ -362,18 +414,33 @@ namespace upm { {"GPGGA", &NMEAGPS::_parse_gpgga}, {"GPGSV", &NMEAGPS::_parse_gpgsv}, {"GPGLL", &NMEAGPS::_parse_gpgll}, + {"GPTXT", &NMEAGPS::_parse_gptxt}, }; /** Raw NMEA sentence fix queue */ std::queue _queue_nmea_sentence; std::mutex _mtx_nmea_sentence; + /** GPS fix queue */ std::queue _queue_fix; std::mutex _mtx_fix; + /** Message queue */ + std::queue _queue_txt; + std::mutex _mtx_txt; + /** Specify a queue size for parsed objects */ std::atomic _maxQueueDepth; + /** Count # of parsed sentences each time thread is started */ + std::atomic _sentences_since_start; + + /** Count # oharacters read each time thread is started */ + std::atomic _bytes_since_start; + + /** Count # of parsed sentences each time thread is started */ + std::atomic _seconds_since_start; + /** Set of current satellites */ std::list _satlist; std::mutex _mtx_satlist; diff --git a/src/nmea_gps/nmea_gps.i b/src/nmea_gps/nmea_gps.i index 6e10f50f..b5308cf8 100644 --- a/src/nmea_gps/nmea_gps.i +++ b/src/nmea_gps/nmea_gps.i @@ -11,6 +11,7 @@ JAVA_JNI_LOADLIBRARY(javaupm_nmea_gps) /* Attach pythons __str__ method to a similar method in C++ */ %feature("python:slot", "tp_str", functype="reprfunc") upm::gps_fix::__str__; %feature("python:slot", "tp_str", functype="reprfunc") upm::satellite::__str__; +%feature("python:slot", "tp_str", functype="reprfunc") upm::nmeatxt::__str__; %feature("python:slot", "tp_str", functype="reprfunc") upm::NMEAGPS::__str__; #endif /* END Python syntax */ diff --git a/tests/unit/nmea_gps/nmea_gps_tests.cxx b/tests/unit/nmea_gps/nmea_gps_tests.cxx index dacacb57..022ebb40 100644 --- a/tests/unit/nmea_gps/nmea_gps_tests.cxx +++ b/tests/unit/nmea_gps/nmea_gps_tests.cxx @@ -244,3 +244,34 @@ TEST_F(nmea_gps_unit, parse_gll_valid) /* Should have 5 GPS fixes */ ASSERT_EQ(gps.fixQueueSize(), 5); } + +/* Parse example txt sentences */ +TEST_F(nmea_gps_unit, parse_txt_valid) +{ + upm::NMEAGPS gps(0, 115200, -1); + std::vector snts = + { + "$GPTXT,01,01,02,u-blox ag - www.u-blox.com*50", + "$GPTXT,01,01,02,HW UBX-G60xx 00040007 *52", + "$GPTXT,01,01,02,EXT CORE 7.03 (45970) Mar 17 2011 16:26:24*44", + "$GPTXT,01,01,02,ROM BASE 6.02 (36023) Oct 15 2009 16:52:08*58", + "$GPTXT,01,01,02,MOD LEA-6H-0*2D", + "$GPTXT,01,01,02,ANTSUPERV=AC SD PDoS SR*20", + "$GPTXT,01,01,02,ANTSTATUS=OK*3B" + }; + + /* Parse the first sentence */ + gps.parseNMEASentence(snts.front()); + + ASSERT_EQ(gps.txtMessageQueueSize(), 1); + /* Get the message */ + upm::nmeatxt msg = gps.getTxtMessage(); + ASSERT_EQ(msg.severity, 2); + + /* Parse the rest */ + for(const auto& sentence : snts) + gps.parseNMEASentence(sentence); + + /* Should have 5 GPS fixes */ + ASSERT_EQ(gps.txtMessageQueueSize(), 7); +}