alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-10-31 23:24:20 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

nmea_gps: Added parsing TXT and bw calculations

Browse Source 
Added code to parse GPTXT nmea sentences.  Added methods to calculate
sentences/second and raw bytes/second.

Signed-off-by: Noel Eck <noel.eck@intel.com>
This commit is contained in:
Noel Eck
2018-05-25 15:51:13 -07:00
parent 8b4e1020f4
commit 6228498147
4 changed files with 256 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

160
src/nmea_gps/nmea_gps.cxx
View File

@@ -22,6 +22,7 @@
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <chrono>
#include <ctime>
#include <iomanip>
#include <iostream>
@@ -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<char*>(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<std::chrono::seconds>(now);
auto epoch = now_s.time_since_epoch();
auto value = std::chrono::duration_cast<std::chrono::milliseconds>(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<satellite> 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();
}

77
src/nmea_gps/nmea_gps.hpp
View File

@@ -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<satellite> 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<std::string> _queue_nmea_sentence;
std::mutex _mtx_nmea_sentence;
/** GPS fix queue */
std::queue<gps_fix> _queue_fix;
std::mutex _mtx_fix;
/** Message queue */
std::queue<nmeatxt> _queue_txt;
std::mutex _mtx_txt;
/** Specify a queue size for parsed objects */
std::atomic<size_t> _maxQueueDepth;
/** Count # of parsed sentences each time thread is started */
std::atomic<size_t> _sentences_since_start;
/** Count # oharacters read each time thread is started */
std::atomic<size_t> _bytes_since_start;
/** Count # of parsed sentences each time thread is started */
std::atomic<double> _seconds_since_start;
/** Set of current satellites */
std::list<satellite> _satlist;
std::mutex _mtx_satlist;

1
src/nmea_gps/nmea_gps.i
View File

@@ -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 */

31
tests/unit/nmea_gps/nmea_gps_tests.cxx
View File

@@ -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<std::string> 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);
}