mirror of
https://github.com/eclipse/upm.git
synced 2025-03-14 20:47:30 +03:00
nmea_gps: Update to handle parsing into structures
NMEA GPS class now uses threads to handle parsing NMEA sentences.
Currently only handles GGA, GSV, and GLL types. Added queues for
holding parsed data.
* Parsing thread
* Parse GPS Fix from GGA and GLL
* Parse satellite info from GSV
* Provide a queue for GPS fix data as well as raw sentences
(for debugging)
* Target structures and NMEAGPS class have a __str__() method
which summarizes the objects into a std::string. These are
tied to the __str__ method in python
* Added google unit test for the above functionality
Signed-off-by: Noel Eck <noel.eck@intel.com>
This commit is contained in:
Noel Eck
parent
1b5087105b
commit
3a077df5f6
@ -6,4 +6,4 @@ upm_mixed_module_init (NAME nmea_gps
|
||||
CPP_SRC nmea_gps.cxx
|
||||
FTI_SRC nmea_gps_fti.c
|
||||
CPP_WRAPS_C
|
||||
REQUIRES mraa utilities-c)
|
||||
REQUIRES mraa utilities-c ${CMAKE_THREAD_LIBS_INIT})
|
||||
|
||||
@ -67,6 +67,51 @@ static int readRegs(const nmea_gps_context dev, uint8_t reg,
|
||||
return rv;
|
||||
}
|
||||
|
||||
nmea_gps_context nmea_gps_init_raw(const char* uart, unsigned int baudrate)
|
||||
{
|
||||
// make sure MRAA is initialized
|
||||
int mraa_rv;
|
||||
if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
|
||||
{
|
||||
printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
nmea_gps_context dev =
|
||||
(nmea_gps_context)malloc(sizeof(struct _nmea_gps_context));
|
||||
|
||||
if (!dev)
|
||||
return NULL;
|
||||
|
||||
// zero out context
|
||||
memset((void *)dev, 0, sizeof(struct _nmea_gps_context));
|
||||
|
||||
dev->uart = NULL;
|
||||
dev->i2c = NULL;
|
||||
dev->gpio_en = NULL;
|
||||
|
||||
// initialize the MRAA contexts
|
||||
|
||||
// uart, default should be 8N1
|
||||
if (!(dev->uart = mraa_uart_init_raw(uart)))
|
||||
{
|
||||
printf("%s: mraa_uart_init_raw() failed.\n", __FUNCTION__);
|
||||
nmea_gps_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (nmea_gps_set_baudrate(dev, baudrate))
|
||||
{
|
||||
printf("%s: nmea_gps_set_baudrate() failed.\n", __FUNCTION__);
|
||||
nmea_gps_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
mraa_uart_set_flowcontrol(dev->uart, false, false);
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
// uart init
|
||||
nmea_gps_context nmea_gps_init(unsigned int uart, unsigned int baudrate,
|
||||
int enable_pin)
|
||||
@ -87,7 +132,7 @@ nmea_gps_context nmea_gps_init(unsigned int uart, unsigned int baudrate,
|
||||
|
||||
// zero out context
|
||||
memset((void *)dev, 0, sizeof(struct _nmea_gps_context));
|
||||
|
||||
|
||||
dev->uart = NULL;
|
||||
dev->i2c = NULL;
|
||||
dev->gpio_en = NULL;
|
||||
@ -107,7 +152,7 @@ nmea_gps_context nmea_gps_init(unsigned int uart, unsigned int baudrate,
|
||||
printf("%s: nmea_gps_set_baudrate() failed.\n", __FUNCTION__);
|
||||
nmea_gps_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
mraa_uart_set_flowcontrol(dev->uart, false, false);
|
||||
|
||||
@ -294,7 +339,7 @@ upm_result_t nmea_gps_set_baudrate(const nmea_gps_context dev,
|
||||
{
|
||||
printf("%s: mraa_uart_set_baudrate() failed.\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
@ -22,9 +22,14 @@
|
||||
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <regex>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "upm_utilities.h"
|
||||
#include "nmea_gps.hpp"
|
||||
|
||||
using namespace upm;
|
||||
@ -32,7 +37,19 @@ using namespace std;
|
||||
|
||||
NMEAGPS::NMEAGPS(unsigned int uart, unsigned int baudrate,
|
||||
int enable_pin) :
|
||||
m_nmea_gps(nmea_gps_init(uart, baudrate, enable_pin))
|
||||
m_nmea_gps(nmea_gps_init(uart, baudrate, enable_pin)),
|
||||
_running(false),
|
||||
_maxQueueDepth(10)
|
||||
{
|
||||
if (!m_nmea_gps)
|
||||
throw std::runtime_error(string(__FUNCTION__)
|
||||
+ ": nmea_gps_init() failed");
|
||||
}
|
||||
|
||||
NMEAGPS::NMEAGPS(const std::string& uart, unsigned int baudrate) :
|
||||
m_nmea_gps(nmea_gps_init_raw(uart.c_str(), baudrate)),
|
||||
_running(false),
|
||||
_maxQueueDepth(10)
|
||||
{
|
||||
if (!m_nmea_gps)
|
||||
throw std::runtime_error(string(__FUNCTION__)
|
||||
@ -40,7 +57,9 @@ NMEAGPS::NMEAGPS(unsigned int uart, unsigned int baudrate,
|
||||
}
|
||||
|
||||
NMEAGPS::NMEAGPS(unsigned int bus, uint8_t addr) :
|
||||
m_nmea_gps(nmea_gps_init_ublox_i2c(bus, addr))
|
||||
m_nmea_gps(nmea_gps_init_ublox_i2c(bus, addr)),
|
||||
_running(false),
|
||||
_maxQueueDepth(10)
|
||||
{
|
||||
if (!m_nmea_gps)
|
||||
throw std::runtime_error(string(__FUNCTION__)
|
||||
@ -49,6 +68,9 @@ NMEAGPS::NMEAGPS(unsigned int bus, uint8_t addr) :
|
||||
|
||||
NMEAGPS::~NMEAGPS()
|
||||
{
|
||||
_running = false;
|
||||
if (_parser.joinable())
|
||||
_parser.join();
|
||||
nmea_gps_close(m_nmea_gps);
|
||||
}
|
||||
|
||||
@ -65,7 +87,7 @@ std::string NMEAGPS::readStr(size_t size)
|
||||
return string(buffer, rv);
|
||||
}
|
||||
|
||||
int NMEAGPS::writeStr(std::string buffer)
|
||||
int NMEAGPS::writeStr(const std::string& buffer)
|
||||
{
|
||||
int rv;
|
||||
|
||||
@ -74,7 +96,7 @@ int NMEAGPS::writeStr(std::string buffer)
|
||||
throw std::runtime_error(string(__FUNCTION__)
|
||||
+ ": nmea_gps_write() failed");
|
||||
|
||||
return rv;
|
||||
return rv;
|
||||
}
|
||||
|
||||
void NMEAGPS::enable(bool enable)
|
||||
@ -95,3 +117,356 @@ bool NMEAGPS::dataAvailable(unsigned int millis)
|
||||
{
|
||||
return nmea_gps_data_available(m_nmea_gps, millis);
|
||||
}
|
||||
|
||||
size_t NMEAGPS::getMaxQueueDepth()
|
||||
{
|
||||
return _maxQueueDepth;
|
||||
}
|
||||
|
||||
size_t NMEAGPS::setMaxQueueDepth(size_t depth)
|
||||
{
|
||||
/* 1 <= depth <= 1000 */
|
||||
if (depth > 1000) depth = 1000;
|
||||
if (depth == 0) depth = 1;
|
||||
|
||||
_maxQueueDepth = depth;
|
||||
return _maxQueueDepth;
|
||||
}
|
||||
|
||||
/* Given a NMEA sentence, return a checksum which is calculated on
|
||||
* all characters between the '$' and the '*' */
|
||||
static uint8_t checksum(const std::string& sentence)
|
||||
{
|
||||
uint8_t chksum = 0;
|
||||
std::string::const_iterator it = sentence.begin();
|
||||
/* Skip the '$' */
|
||||
if (*it == '$') ++it;
|
||||
|
||||
/* Calculate the checksum on all characters */
|
||||
while ((*it != '*') && (it != sentence.end()))
|
||||
chksum ^= *it++;
|
||||
return chksum;
|
||||
}
|
||||
|
||||
/* Regex for matching NMEA GGA coordinates
|
||||
* Unfortunately these sentences appear-non standard between the devices tested
|
||||
* so it can be expected that these would need updating to match additional
|
||||
* devices.
|
||||
*/
|
||||
static std::regex rex_gga(R"(^\$GPGGA,(\d+\.\d+),(\d+)(\d{2}\.\d+),([NS]),(\d+)(\d{2}.\d+),([WE]),(\d+),(\d+),(\d+\.\d+),(\d+\.\d+),M,([+-]?\d+\.\d+),M,([+-]?\d+\.\d+),(\S+)[*]([A-Z0-9]{2}))");
|
||||
void NMEAGPS::_parse_gpgga(const std::string& sentence)
|
||||
{
|
||||
/* Parse the GGA message */
|
||||
std::smatch m;
|
||||
if (!std::regex_search(sentence, m, rex_gga))
|
||||
return;
|
||||
|
||||
gps_fix fix;
|
||||
fix.valid = true;
|
||||
fix.time_utc = m[1];
|
||||
int deg = std::stoi(m[2]);
|
||||
fix.coordinates.latitude = (deg + std::stof(m[3])/60.0) *
|
||||
(m[4] == "N" ? 1.0 : m[4] == "S" ? -1.0 : fix.valid = false );
|
||||
deg = std::stoi(m[5]);
|
||||
fix.coordinates.longitude = (deg + std::stof(m[6])/60.0) *
|
||||
(m[7] == "E" ? 1.0 : m[7] == "W" ? -1.0 : fix.valid = false );
|
||||
fix.quality = static_cast<gps_fix_quality>(std::stoi(m[8]));
|
||||
fix.satellites = std::stoi(m[9]);
|
||||
fix.hdop = std::stof(m[10]);
|
||||
fix.altitude_meters = std::stof(m[11]);
|
||||
fix.geoid_height_meters = std::stof(m[12]);
|
||||
fix.age_seconds = std::stof(m[13]);
|
||||
fix.station_id = m[14];
|
||||
fix.chksum_match = std::stoi(m[15], nullptr, 16) == checksum(sentence);
|
||||
fix.valid &= fix.chksum_match;
|
||||
|
||||
/* Throw away oldest if full, push to queue */
|
||||
_mtx_fix.lock();
|
||||
if (_queue_fix.size() == _maxQueueDepth)
|
||||
_queue_fix.pop();
|
||||
_queue_fix.push(fix);
|
||||
_mtx_fix.unlock();
|
||||
}
|
||||
|
||||
/* Regex for matching NMEA GSV satellite sentences
|
||||
* Unfortunately these sentences appear-non standard between the devices tested
|
||||
* so it can be expected that these would need updating to match additional
|
||||
* devices.
|
||||
*
|
||||
* Example sentence:
|
||||
*
|
||||
* $GPGSV,3,3,12,28,75,028,20,30,55,116,28,48,37,194,41,51,35,159,32*7A
|
||||
*/
|
||||
static std::regex rex_gsv_hdr(R"(^\$GPGSV,(\d+),(\d+),(\d\d),)");
|
||||
static std::regex rex_gsv_sat(R"((\d{2}),(\d{2}),(\d{3}),(\d+)?,?)");
|
||||
static std::regex rex_gsv_ftr("[*]([A-Z0-9]{2})$");
|
||||
void NMEAGPS::_parse_gpgsv(const std::string& sentence)
|
||||
{
|
||||
/* Parse the GSV header message */
|
||||
std::smatch mhdr;
|
||||
std::smatch mftr;
|
||||
/* No further parsing if this message doesn't match the header
|
||||
* or footer or the checksum is bad */
|
||||
if (!std::regex_search(sentence, mhdr, rex_gsv_hdr) ||
|
||||
!std::regex_search(sentence, mftr, rex_gsv_ftr) ||
|
||||
(std::stoi(mftr[1], nullptr, 16) != checksum(sentence)))
|
||||
return;
|
||||
|
||||
size_t total_svs = std::stoi(mhdr[3]);
|
||||
|
||||
/* Match each satellite */
|
||||
std::sregex_iterator next(sentence.begin(), sentence.end(), rex_gsv_sat);
|
||||
std::sregex_iterator end;
|
||||
while (next != end)
|
||||
{
|
||||
std::smatch satmatches = *next++;
|
||||
|
||||
/* Add these satellites. Only keep a max total_svs satellites at any
|
||||
* one time. The latest are the most current */
|
||||
satellite sat = {
|
||||
satmatches[1].str(),
|
||||
std::stoi(satmatches[2].str()),
|
||||
std::stoi(satmatches[3].str()),
|
||||
satmatches[4].str().empty() ? 0 :
|
||||
std::stoi(satmatches[4].str())
|
||||
};
|
||||
|
||||
/* Add to the back of satmap, remove any matching prn */
|
||||
_mtx_satlist.lock();
|
||||
auto sit = _satlist.begin();
|
||||
do
|
||||
{
|
||||
/* Remove */
|
||||
if ((*sit).prn == sat.prn)
|
||||
{
|
||||
sit = _satlist.erase(sit);
|
||||
break;
|
||||
}
|
||||
} while(++sit != _satlist.end());
|
||||
/* Add satellite to the end */
|
||||
_satlist.push_back(sat);
|
||||
|
||||
/* If more sats exist than current sat count, remove them */
|
||||
while (_satlist.size() > total_svs) _satlist.pop_front();
|
||||
_mtx_satlist.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Regex for matching NMEA GLL coordinates
|
||||
* Unfortunately these sentences appear-non standard between the devices tested
|
||||
* so it can be expected that these would need updating to match additional
|
||||
* devices.
|
||||
*
|
||||
* For example, the HJ GPS compass returned GLL sentences
|
||||
* with a duplicate ,A,A at the end :(
|
||||
* "$GPGLL,4532.55107,N,12257.68422,W,170004.20,A,A*74"
|
||||
*/
|
||||
static std::regex rex_gll(R"(^\$GPGLL,(\d+)(\d{2}\.\d+),([NS]),(\d+)(\d{2}.\d+),([WE]),(\d+\.\d+)(,A)?,A[*]([A-Z0-9]{2}))");
|
||||
void NMEAGPS::_parse_gpgll(const std::string& sentence)
|
||||
{
|
||||
/* Parse the GLL message */
|
||||
std::smatch m;
|
||||
if (!std::regex_search(sentence, m, rex_gll))
|
||||
return;
|
||||
|
||||
gps_fix fix;
|
||||
fix.valid = true;
|
||||
fix.time_utc = m[7];
|
||||
int deg = std::stoi(m[1]);
|
||||
fix.coordinates.latitude = (deg + std::stof(m[2])/60.0) *
|
||||
(m[3] == "N" ? 1.0 : m[3] == "S" ? -1.0 : fix.valid = false );
|
||||
deg = std::stoi(m[4]);
|
||||
fix.coordinates.longitude = (deg + std::stof(m[5])/60.0) *
|
||||
(m[6] == "E" ? 1.0 : m[6] == "W" ? -1.0 : fix.valid = false );
|
||||
fix.chksum_match = std::stoi(m[9], nullptr, 16) == checksum(sentence);
|
||||
fix.valid &= fix.chksum_match;
|
||||
|
||||
/* Throw away oldest if full, push to queue */
|
||||
_mtx_fix.lock();
|
||||
if (_queue_fix.size() == _maxQueueDepth)
|
||||
_queue_fix.pop();
|
||||
_queue_fix.push(fix);
|
||||
_mtx_fix.unlock();
|
||||
}
|
||||
|
||||
void NMEAGPS::parseNMEASentence(const std::string& sentence)
|
||||
{
|
||||
/* Needs to start with $GP... and be (at least 6 characters
|
||||
* long to call a parser. Otherwise skip parsing and put into
|
||||
* raw sentence queue for debug */
|
||||
size_t msgsz = sentence.size();
|
||||
if ((sentence.find("$GP") == 0) &&
|
||||
(msgsz >= 5) && (msgsz <=100))
|
||||
{
|
||||
auto cit = nmea_2_parser.find(sentence.substr(1, 5));
|
||||
if (cit != nmea_2_parser.end())
|
||||
{
|
||||
fp parser = cit->second;
|
||||
/* Call the corresponding parser */
|
||||
(this->*parser)(sentence);
|
||||
}
|
||||
}
|
||||
|
||||
/* Throw away oldest if full, push to raw sentence queue */
|
||||
_mtx_nmea_sentence.lock();
|
||||
if (_queue_nmea_sentence.size() == _maxQueueDepth)
|
||||
_queue_nmea_sentence.pop();
|
||||
_queue_nmea_sentence.push(sentence);
|
||||
_mtx_nmea_sentence.unlock();
|
||||
}
|
||||
|
||||
void NMEAGPS::_parse_thread()
|
||||
{
|
||||
/* NMEA 0183 max sentence length is 82 characters. There seems to be
|
||||
* varying specs out there. Using 94 characters between the $GP and
|
||||
* the checksum as a max length for a basic max length sanity check.
|
||||
* $GP(94 chars max)*XX length = 100 characters total
|
||||
*/
|
||||
std::regex rex(R"((\$GP.{5,94}\*[a-fA-F0-9][a-fA-F0-9])\r\n)");
|
||||
while (_running)
|
||||
{
|
||||
/* While data is available, read from the GPS. A 5s
|
||||
* timeout appears long, but UARTS can be slow with minimal
|
||||
* data getting returned, possible slow UART speeds, and it's
|
||||
* better to maximize the UART buffer. This currently
|
||||
* assumes whole sentences are returned each read.
|
||||
* TODO: Handle leftover uart data
|
||||
*/
|
||||
if (dataAvailable(5000))
|
||||
{
|
||||
/* Read a block */
|
||||
std::string buf = readStr(4095);
|
||||
|
||||
std::sregex_iterator next(buf.begin(), buf.end(), rex);
|
||||
std::sregex_iterator end;
|
||||
while (next != end)
|
||||
{
|
||||
std::smatch matches = *next++;
|
||||
parseNMEASentence(matches[1].str());
|
||||
}
|
||||
|
||||
/* Let this thread do other stuff */
|
||||
upm_delay_us(100);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NMEAGPS::parseStart()
|
||||
{
|
||||
/* Don't create multiple running threads */
|
||||
if (_running) return;
|
||||
|
||||
_running = true;
|
||||
_parser = std::thread(&NMEAGPS::_parse_thread, this);
|
||||
}
|
||||
|
||||
void NMEAGPS::parseStop()
|
||||
{
|
||||
/* Only stop if running */
|
||||
if (!_running) return;
|
||||
|
||||
_running = false;
|
||||
if (_parser.joinable())
|
||||
_parser.join();
|
||||
}
|
||||
|
||||
gps_fix NMEAGPS::getFix()
|
||||
{
|
||||
gps_fix x;
|
||||
_mtx_fix.lock();
|
||||
if (!_queue_fix.empty())
|
||||
{
|
||||
/* Get a copy of the structure, pop an element */
|
||||
x = _queue_fix.front();
|
||||
_queue_fix.pop();
|
||||
}
|
||||
_mtx_fix.unlock();
|
||||
return x;
|
||||
}
|
||||
|
||||
std::string NMEAGPS::getRawSentence()
|
||||
{
|
||||
std::string ret;
|
||||
_mtx_nmea_sentence.lock();
|
||||
if (!_queue_nmea_sentence.empty())
|
||||
{
|
||||
/* Get a copy of the sentence, pop an element */
|
||||
ret = _queue_nmea_sentence.front();
|
||||
_queue_nmea_sentence.pop();
|
||||
}
|
||||
_mtx_nmea_sentence.unlock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
size_t NMEAGPS::fixQueueSize()
|
||||
{
|
||||
_mtx_fix.lock();
|
||||
size_t x =_queue_fix.size();
|
||||
_mtx_fix.unlock();
|
||||
return x;
|
||||
}
|
||||
|
||||
size_t NMEAGPS::rawSentenceQueueSize()
|
||||
{
|
||||
_mtx_nmea_sentence.lock();
|
||||
size_t x =_queue_nmea_sentence.size();
|
||||
_mtx_nmea_sentence.unlock();
|
||||
return x;
|
||||
}
|
||||
|
||||
std::string gps_fix::__str__()
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "valid:" << (valid ? "T" : "F") << ", ";
|
||||
if (time_utc.size() < 6) oss << "UNKNOWN UTC, ";
|
||||
else
|
||||
oss << time_utc.substr(0, 2) << ":" << time_utc.substr(2,2) << ":"
|
||||
<< time_utc.substr(4,2) << time_utc.substr(6) << " UTC, ";
|
||||
oss << coordinates.latitude << ", " << coordinates.longitude << ", "
|
||||
<< "quality: " << static_cast<int>(quality) << ", "
|
||||
<< "sats: " << static_cast<int>(satellites) << ", "
|
||||
<< "hdop: " << hdop << ", "
|
||||
<< "alt (m): " << altitude_meters << ", "
|
||||
<< "geoid_ht (m): " << geoid_height_meters << ", "
|
||||
<< "age (s): " << age_seconds << ", "
|
||||
<< "dgps sid: " << station_id << ", "
|
||||
<< "chksum match: " << (chksum_match ? "T" : "F");
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::vector<satellite> NMEAGPS::satellites()
|
||||
{
|
||||
/* Create a new set for now */
|
||||
_mtx_satlist.lock();
|
||||
std::vector<satellite> sats(_satlist.begin(), _satlist.end());
|
||||
_mtx_satlist.unlock();
|
||||
|
||||
return sats;
|
||||
}
|
||||
|
||||
std::string satellite::__str__()
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "id:" << std::setw(3) << prn << ", "
|
||||
<< "elevation (d):" << std::setw(3) << elevation_deg
|
||||
<< ", " << "azimuth (d):" << std::setw(3) << azimuth_deg
|
||||
<< ", " << "snr:" << std::setw(3) << snr;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::string NMEAGPS::__str__()
|
||||
{
|
||||
std::ostringstream oss;
|
||||
auto sats = satellites();
|
||||
size_t qsz = getMaxQueueDepth();
|
||||
oss << "NMEA GPS Instance" << std::endl
|
||||
<< " Parsing: " << (isRunning() ? "T" : "F") << 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;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
@ -44,7 +44,7 @@ extern "C" {
|
||||
* An example using I2C.
|
||||
* @include nmea_gps_i2c.c
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Device context
|
||||
*/
|
||||
@ -53,11 +53,11 @@ extern "C" {
|
||||
mraa_gpio_context gpio_en;
|
||||
mraa_i2c_context i2c;
|
||||
} *nmea_gps_context;
|
||||
|
||||
|
||||
/**
|
||||
* NMEA_GPS Initializer for generic UART operation
|
||||
*
|
||||
* @param uart Specify which uart to use.
|
||||
* @param uart Specify which mraa uart index to use.
|
||||
* @param baudrate Specify the baudrate to use. The device defaults
|
||||
* to 9600 baud.
|
||||
* @param enable_pin Specify the GPIO pin to use for the enable pin,
|
||||
@ -67,6 +67,16 @@ extern "C" {
|
||||
nmea_gps_context nmea_gps_init(unsigned int uart, unsigned int baudrate,
|
||||
int enable_pin);
|
||||
|
||||
/**
|
||||
* NMEA_GPS Initializer for generic UART operation
|
||||
*
|
||||
* @param uart Specify which uart (fs device path) to use.
|
||||
* @param baudrate Specify the baudrate to use. The device defaults
|
||||
* to 9600 baud.
|
||||
* @return an initialized device context on success, NULL on error.
|
||||
*/
|
||||
nmea_gps_context nmea_gps_init_raw(const char* uart, unsigned int baudrate);
|
||||
|
||||
/**
|
||||
* NMEA_GPS Initializer for UBLOX I2C operation
|
||||
*
|
||||
|
||||
@ -23,133 +23,359 @@
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <iostream>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
#include <cstdlib>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "nmea_gps.h"
|
||||
|
||||
namespace upm {
|
||||
/**
|
||||
* @brief Generic NMEA GPS Serial Device Library
|
||||
* @defgroup nmea_gps libupm-nmea_gps
|
||||
* @ingroup uart gpio gps
|
||||
*/
|
||||
|
||||
/**
|
||||
* @library nmea_gps
|
||||
* @sensor nmea_gps
|
||||
* @comname Generic Serial Interface for GPS NMEA Devices
|
||||
* @type gps
|
||||
* @man dfrobot seeed
|
||||
* @con uart gpio
|
||||
* @altname VK2828u7 ublox LEA-6H
|
||||
*
|
||||
* @brief API for the NMEA GPS Module
|
||||
*
|
||||
* This driver was tested with a number of GPS devices that emit
|
||||
* NMEA data via a serial interface of some sort (typically a UART).
|
||||
*
|
||||
* The I2C capablity was tested with a UBLOX LEA-6H based GPS shield
|
||||
* from DFRobot. Currently, the I2C capability is only supported
|
||||
* for UBLOX devices (or compatibles) that conform to the
|
||||
* specifications outlined in the u-blox6 Receiver Description
|
||||
* Protocol Specification, Chapter 4, DDC Port.
|
||||
*
|
||||
* An example using the UART.
|
||||
* @snippet nmea_gps.cxx Interesting
|
||||
* An example using I2C.
|
||||
* @snippet nmea_gps-i2c.cxx Interesting
|
||||
*/
|
||||
|
||||
class NMEAGPS {
|
||||
public:
|
||||
/**
|
||||
* @brief Generic NMEA GPS Serial Device Library
|
||||
* @defgroup nmea_gps libupm-nmea_gps
|
||||
* @ingroup uart gpio gps
|
||||
*/
|
||||
|
||||
/**
|
||||
* NMEAGPS object constructor for a UART
|
||||
* @file nmea_gps.hpp
|
||||
* @library nmea_gps
|
||||
* @sensor nmea_gps
|
||||
* @comname Generic Serial Interface for GPS NMEA Devices
|
||||
* @type gps
|
||||
* @man dfrobot seeed
|
||||
* @con uart gpio
|
||||
* @altname VK2828u7 ublox LEA-6H
|
||||
*
|
||||
* @param uart Specify which uart to use.
|
||||
* @param baudrate Specify the baudrate to use. The device defaults
|
||||
* to 9600 baud.
|
||||
* @param enable_pin Specify the GPIO pin to use for the enable pin,
|
||||
* -1 to not use an enable pin.
|
||||
*/
|
||||
NMEAGPS(unsigned int uart, unsigned int baudrate,
|
||||
int enable_pin);
|
||||
|
||||
/**
|
||||
* NMEAGPS object constructor for a UBLOX I2C interface
|
||||
* @brief API for the NMEA GPS Module
|
||||
*
|
||||
* @param bus Specify which the I2C bus to use.
|
||||
* @param addr Specify the I2C address to use. For UBLOX devices,
|
||||
* this typically defaults to 0x42.
|
||||
*/
|
||||
NMEAGPS(unsigned int bus, uint8_t addr);
|
||||
|
||||
/**
|
||||
* NMEAGPS object destructor
|
||||
*/
|
||||
~NMEAGPS();
|
||||
|
||||
/**
|
||||
* Read character data from the device.
|
||||
* This driver was tested with a number of GPS devices that emit
|
||||
* NMEA data via a serial interface of some sort (typically a UART).
|
||||
*
|
||||
* @param size The maximum number of characters to read.
|
||||
* @return string containing the data read.
|
||||
*/
|
||||
std::string readStr(size_t size);
|
||||
|
||||
/**
|
||||
* Write character data to the device. This is only valid for a
|
||||
* UART device.
|
||||
* The I2C capablity was tested with a UBLOX LEA-6H based GPS shield
|
||||
* from DFRobot. Currently, the I2C capability is only supported
|
||||
* for UBLOX devices (or compatibles) that conform to the
|
||||
* specifications outlined in the u-blox6 Receiver Description
|
||||
* Protocol Specification, Chapter 4, DDC Port.
|
||||
*
|
||||
* @param buffer The string containing the data to write.
|
||||
* @return The number of bytes written.
|
||||
* An example using the UART.
|
||||
* @snippet nmea_gps.cxx Interesting
|
||||
* An example using I2C.
|
||||
* @snippet nmea_gps-i2c.cxx Interesting
|
||||
*/
|
||||
int writeStr(std::string buffer);
|
||||
class NMEAGPS;
|
||||
|
||||
/**
|
||||
* Enable or disable the device. When disabled, the device enters a
|
||||
* low power mode and does not emit NMEA data. It will still
|
||||
* maintain location data however.
|
||||
*
|
||||
* @param enable true to enable the device, false otherwise.
|
||||
/** Coordinates for lat/long as decimal degrees (DD) */
|
||||
struct coord_DD {
|
||||
/** Latitude in decimal degrees */
|
||||
double latitude = 0.0;
|
||||
/** Longitude in decimal degrees */
|
||||
double longitude = 0.0;
|
||||
};
|
||||
|
||||
/** Satellite structure definition */
|
||||
struct satellite {
|
||||
/** PRN pseudo-random-noise value which identifies a satellite */
|
||||
std::string prn = std::string("");
|
||||
/** Satellite elevation angle in degrees */
|
||||
int elevation_deg = 0;
|
||||
/** Satellite azimuth angle in degrees */
|
||||
int azimuth_deg = 0;
|
||||
/** Satellite signal-to-noise ratio */
|
||||
int snr = 0;
|
||||
/** Default constructor */
|
||||
satellite() = default;
|
||||
/**
|
||||
* Create a satellite from arguments constructor
|
||||
* @param sprn Target PRN string
|
||||
* @param elevation Target elevation angle in degrees
|
||||
* @param azimuth Target azimuth angle in degrees
|
||||
* @param snr Target signal to noise ratio (usually in dB,
|
||||
* unfortunately non-standard)
|
||||
*/
|
||||
satellite(const std::string& sprn, int elevation, int azimuth, int snr):
|
||||
prn(sprn), elevation_deg(elevation), azimuth_deg(azimuth), snr(snr) {}
|
||||
/**
|
||||
* Provide a string representation of this structure.
|
||||
* @return String representing a satellite
|
||||
*/
|
||||
std::string __str__();
|
||||
};
|
||||
|
||||
/** GPS fix quality values */
|
||||
enum class gps_fix_quality {
|
||||
/** No fix available or invalid */
|
||||
no_fix = 0,
|
||||
/** Fix - single point */
|
||||
fix_sp,
|
||||
/** Fix - differential point */
|
||||
fix_dp,
|
||||
/** Fix - pulse per second */
|
||||
fix_pps,
|
||||
/** Fix - real time kinematic */
|
||||
fix_rtk,
|
||||
/** Fix - float real time kinematic */
|
||||
fix_frtk,
|
||||
/** Fix - dead reckoning */
|
||||
fix_dr,
|
||||
/** Fix - manual input */
|
||||
fix_manual,
|
||||
/** Fix - simulation mode */
|
||||
fix_simulation
|
||||
};
|
||||
|
||||
/** GPS fix definition. A GPS fix structure should only be used if
|
||||
* valid == true
|
||||
*/
|
||||
void enable(bool enable);
|
||||
struct gps_fix {
|
||||
/** Fix coordinates */
|
||||
coord_DD coordinates;
|
||||
/** UTC time string as HHMMSS.mS */
|
||||
std::string time_utc = std::string("");
|
||||
/** GPS fix signal quality */
|
||||
gps_fix_quality quality = gps_fix_quality::no_fix;
|
||||
/** Number of satellites in use */
|
||||
uint8_t satellites = 0;
|
||||
/** Horizontal dilution of precision, unitless, lower is better */
|
||||
float hdop = 0.0;
|
||||
/** Altitude above mean sea level in meters */
|
||||
float altitude_meters = 0.0;
|
||||
/** Difference between the WGS-84 earth ellipsoid and mean-sea-level */
|
||||
float geoid_height_meters = 0.0;
|
||||
/** Time in seconds since last differential GPS fix */
|
||||
float age_seconds = 0.0;
|
||||
/** Differential GPS station ID */
|
||||
std::string station_id = std::string("");
|
||||
/** True if this gps_fix structure is valid to use */
|
||||
bool valid = false;
|
||||
/** True if the checksum matched, valid is set to false on mismatch */
|
||||
bool chksum_match = false;
|
||||
/**
|
||||
* Provide a string representation of this structure.
|
||||
* @return String representing a GPS Fix
|
||||
*/
|
||||
std::string __str__();
|
||||
};
|
||||
|
||||
/**
|
||||
* Set the baudrate of the device. By default, the constructor
|
||||
* will set the baudrate to 9600. This is only valid for UART
|
||||
* devices.
|
||||
*
|
||||
* @param baudrate The baud rate to set for the device.
|
||||
*/
|
||||
void setBaudrate(unsigned int baudrate);
|
||||
class NMEAGPS {
|
||||
public:
|
||||
|
||||
/**
|
||||
* Determine whether there is data available to be read. In the
|
||||
* case of a UART, this function will wait up to "millis"
|
||||
* milliseconds for data to become available. In the case of an I2C
|
||||
* device, the millis argument is ignored and the function will
|
||||
* return immediately, indicating whether data is available.
|
||||
*
|
||||
* @param millis The number of milliseconds to wait for data to
|
||||
* become available.
|
||||
* @return true if data is available to be read, false otherwise.
|
||||
*/
|
||||
bool dataAvailable(unsigned int millis);
|
||||
/**
|
||||
* NMEAGPS object constructor for a UART
|
||||
*
|
||||
* @param uart Specify which mraa uart index to use.
|
||||
* @param baudrate Specify the baudrate to use. The device defaults
|
||||
* to 9600 baud.
|
||||
* @param enable_pin Specify the GPIO pin to use for the enable pin,
|
||||
* -1 to not use an enable pin.
|
||||
*/
|
||||
NMEAGPS(unsigned int uart, unsigned int baudrate,
|
||||
int enable_pin);
|
||||
|
||||
protected:
|
||||
// nmeaGPS device context
|
||||
nmea_gps_context m_nmea_gps;
|
||||
/**
|
||||
* NMEAGPS object constructor for a UART
|
||||
*
|
||||
* @param uart Specify which uart to use (fs device path)
|
||||
* @param baudrate Specify the baudrate to use. The device defaults
|
||||
* to 9600 baud.
|
||||
*/
|
||||
NMEAGPS(const std::string& uart, unsigned int baudrate);
|
||||
|
||||
private:
|
||||
/* Disable implicit copy and assignment operators */
|
||||
NMEAGPS(const NMEAGPS&) = delete;
|
||||
NMEAGPS &operator=(const NMEAGPS&) = delete;
|
||||
};
|
||||
/**
|
||||
* NMEAGPS object constructor for a UBLOX I2C interface
|
||||
*
|
||||
* @param bus Specify which the I2C bus to use.
|
||||
* @param addr Specify the I2C address to use. For UBLOX devices,
|
||||
* this typically defaults to 0x42.
|
||||
*/
|
||||
NMEAGPS(unsigned int bus, uint8_t addr);
|
||||
|
||||
/**
|
||||
* NMEAGPS object destructor
|
||||
*/
|
||||
~NMEAGPS();
|
||||
|
||||
/**
|
||||
* Read character data from the device.
|
||||
*
|
||||
* @param size The maximum number of characters to read.
|
||||
* @return string containing the data read.
|
||||
*/
|
||||
std::string readStr(size_t size);
|
||||
|
||||
/**
|
||||
* Write character data to the device. This is only valid for a
|
||||
* UART device.
|
||||
*
|
||||
* @param buffer The string containing the data to write.
|
||||
* @return The number of bytes written.
|
||||
*/
|
||||
int writeStr(const std::string& buffer);
|
||||
|
||||
/**
|
||||
* Enable or disable the device. When disabled, the device enters a
|
||||
* low power mode and does not emit NMEA data. It will still
|
||||
* maintain location data however.
|
||||
*
|
||||
* @param enable true to enable the device, false otherwise.
|
||||
*/
|
||||
void enable(bool enable);
|
||||
|
||||
/**
|
||||
* Set the baudrate of the device. By default, the constructor
|
||||
* will set the baudrate to 9600. This is only valid for UART
|
||||
* devices.
|
||||
*
|
||||
* @param baudrate The baud rate to set for the device.
|
||||
*/
|
||||
void setBaudrate(unsigned int baudrate);
|
||||
|
||||
/**
|
||||
* Determine whether there is data available to be read. In the
|
||||
* case of a UART, this function will wait up to "millis"
|
||||
* milliseconds for data to become available. In the case of an I2C
|
||||
* device, the millis argument is ignored and the function will
|
||||
* return immediately, indicating whether data is available.
|
||||
*
|
||||
* @param millis The number of milliseconds to wait for data to
|
||||
* become available.
|
||||
* @return true if data is available to be read, false otherwise.
|
||||
*/
|
||||
bool dataAvailable(unsigned int millis);
|
||||
|
||||
/**
|
||||
* Return the current maximum queue depth.
|
||||
* @return Maximum queue depth
|
||||
*/
|
||||
size_t getMaxQueueDepth();
|
||||
|
||||
/**
|
||||
* Set the current maximum queue depth.
|
||||
* @param depth New target queue depth
|
||||
* 1 <= depth <= 1000
|
||||
* @return Actual maximum queue depth
|
||||
*/
|
||||
size_t setMaxQueueDepth(size_t depth);
|
||||
|
||||
/**
|
||||
* Start a NMEA parsing thread for reading/parsing NMEA sentences. The
|
||||
* thread calls the readStr method, parsing NMEA sentences as they are
|
||||
* encountered. Each sentence type is pushed into a corresponding queue
|
||||
* of size
|
||||
*/
|
||||
void parseStart();
|
||||
|
||||
/**
|
||||
* Stop a running NMEA parsing thread
|
||||
*/
|
||||
void parseStop();
|
||||
|
||||
/**
|
||||
* Is the parsing thread currently running?
|
||||
* @return True if parsing
|
||||
*/
|
||||
bool isRunning() {return _running;}
|
||||
|
||||
/**
|
||||
* Pop and return a GPS fix structure from the GPS fix queue.
|
||||
* A GPS fix should only be used if valid is true.
|
||||
* @return GPS fix structure
|
||||
*/
|
||||
gps_fix getFix();
|
||||
|
||||
/**
|
||||
* Pop and return a raw NMEA sentence from the NMEA sentence queue.
|
||||
* If the queue contains no elements, an empty string is returned
|
||||
* @return NMEA raw sentence
|
||||
*/
|
||||
std::string getRawSentence();
|
||||
|
||||
/**
|
||||
* Get the number of elements in the GPS fix queue.
|
||||
* @return Number of fixes in the GPS fix queue
|
||||
*/
|
||||
size_t fixQueueSize();
|
||||
|
||||
/**
|
||||
* Get the number of elements in the NMEA raw sentence queue.
|
||||
* @return Number of sentences in the raw NMEA sentence queue
|
||||
*/
|
||||
size_t rawSentenceQueueSize();
|
||||
|
||||
/**
|
||||
* Parse NMEA sentences.
|
||||
* Raw sentence is placed into sentence queue. Additional structures are
|
||||
* parsed depending on sentence type
|
||||
* @param sentence NMEA raw sentence ($...\r\n) inclusive
|
||||
*/
|
||||
void parseNMEASentence(const std::string& sentence);
|
||||
|
||||
/**
|
||||
* Return a vector of the current satellites
|
||||
* @return Current satellites
|
||||
*/
|
||||
std::vector<satellite> satellites();
|
||||
|
||||
/**
|
||||
* Provide a string representation of this class
|
||||
* @return String representing this instance
|
||||
*/
|
||||
std::string __str__();
|
||||
protected:
|
||||
/** nmeaGPS device context */
|
||||
nmea_gps_context m_nmea_gps;
|
||||
|
||||
private:
|
||||
/** Disable implicit copy and assignment operators */
|
||||
NMEAGPS(const NMEAGPS&) = delete;
|
||||
NMEAGPS &operator=(const NMEAGPS&) = delete;
|
||||
|
||||
/** Handle reading/parsing NMEA data */
|
||||
std::thread _parser;
|
||||
|
||||
/** Method runs in a spawned thread for parsing NMEA sentences */
|
||||
void _parse_thread();
|
||||
|
||||
/** Helper for thread syncronization */
|
||||
std::atomic<bool> _running;
|
||||
|
||||
/** Parse GPGGA sentences, place in GPS fix queue */
|
||||
void _parse_gpgga(const std::string& string);
|
||||
/** Parse GPGSV sentences, place in satellite collection */
|
||||
void _parse_gpgsv(const std::string& string);
|
||||
/** Parse GPGLL sentences, place in satellite collection */
|
||||
void _parse_gpgll(const std::string& string);
|
||||
|
||||
/** Provide function pointer typedef for handling NMEA chunks */
|
||||
using fp = void (NMEAGPS::*)(const std::string &);
|
||||
/** Map of NMEA type to parser method */
|
||||
const std::map<std::string, fp> nmea_2_parser =
|
||||
{
|
||||
{"GPGGA", &NMEAGPS::_parse_gpgga},
|
||||
{"GPGSV", &NMEAGPS::_parse_gpgsv},
|
||||
{"GPGLL", &NMEAGPS::_parse_gpgll},
|
||||
};
|
||||
|
||||
/** 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;
|
||||
|
||||
/** Specify a queue size for parsed objects */
|
||||
std::atomic<size_t> _maxQueueDepth;
|
||||
|
||||
/** Set of current satellites */
|
||||
std::list<satellite> _satlist;
|
||||
std::mutex _mtx_satlist;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -6,9 +6,21 @@ JAVA_JNI_LOADLIBRARY(javaupm_nmea_gps)
|
||||
#endif
|
||||
/* END Java syntax */
|
||||
|
||||
/* BEGIN Python syntax ------------------------------------------------------- */
|
||||
#ifdef SWIGPYTHON
|
||||
/* 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::NMEAGPS::__str__;
|
||||
#endif
|
||||
/* END Python syntax */
|
||||
|
||||
/* BEGIN Common SWIG syntax ------------------------------------------------- */
|
||||
%include "std_vector.i"
|
||||
|
||||
%{
|
||||
#include "nmea_gps.hpp"
|
||||
%}
|
||||
%template(satellitevec) std::vector<upm::satellite>;
|
||||
%include "nmea_gps.hpp"
|
||||
/* END Common SWIG syntax */
|
||||
|
||||
@ -18,6 +18,11 @@ target_link_libraries(json_tests GTest::GTest GTest::Main)
|
||||
target_include_directories(json_tests PRIVATE "${UPM_COMMON_HEADER_DIRS}/")
|
||||
gtest_add_tests(json_tests "" AUTO)
|
||||
|
||||
# Unit tests - nmea_gps library
|
||||
add_executable(nmea_gps_tests nmea_gps/nmea_gps_tests.cxx)
|
||||
target_link_libraries(nmea_gps_tests nmea_gps GTest::GTest GTest::Main)
|
||||
gtest_add_tests(nmea_gps_tests "" AUTO)
|
||||
|
||||
# Add a custom target for unit tests
|
||||
add_custom_target(tests-unit ALL
|
||||
DEPENDS
|
||||
|
||||
246
tests/unit/nmea_gps/nmea_gps_tests.cxx
Normal file
246
tests/unit/nmea_gps/nmea_gps_tests.cxx
Normal file
@ -0,0 +1,246 @@
|
||||
#include "gtest/gtest.h"
|
||||
#include "nmea_gps.hpp"
|
||||
#include "mraa.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <random>
|
||||
|
||||
/* NMEA GPS test fixture */
|
||||
class nmea_gps_unit : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
/* One-time setup logic if needed */
|
||||
nmea_gps_unit() = default;
|
||||
|
||||
/* One-time tear-down logic if needed */
|
||||
~nmea_gps_unit() override = default;
|
||||
|
||||
/* Fail if not MOCK platform */
|
||||
void SetUp() override
|
||||
{
|
||||
ASSERT_EQ(mraa::getPlatformType(), mraa::MOCK_PLATFORM) <<
|
||||
"NMEA_GPS test requires mraa compiled with mraa::MOCK_PLATFORM";
|
||||
}
|
||||
|
||||
/* Per-test tear-down logic if needed */
|
||||
void TearDown() override {}
|
||||
};
|
||||
|
||||
/* Basic tests */
|
||||
TEST_F(nmea_gps_unit, DOA)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Min queue size is 1 */
|
||||
gps.setMaxQueueDepth(0);
|
||||
ASSERT_EQ(gps.getMaxQueueDepth(), 1);
|
||||
|
||||
/* Max queue size is 1000 */
|
||||
gps.setMaxQueueDepth(1001);
|
||||
ASSERT_EQ(gps.getMaxQueueDepth(), 1000);
|
||||
|
||||
/* Queues should be empty */
|
||||
ASSERT_EQ(gps.fixQueueSize(), 0);
|
||||
ASSERT_EQ(gps.rawSentenceQueueSize(), 0);
|
||||
}
|
||||
|
||||
/* Check parsing varying length sentences */
|
||||
TEST_F(nmea_gps_unit, parse_max_size)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Parse an sentence that is too long to be NMEA */
|
||||
gps.parseNMEASentence("$GPGSV,2,1,08,07,64,079,,08,39,066,,09,25,159,,09,25,159,,09,25,159,,09,25,159,,09,25,159,,09,25,159,,09,25,159,,07,64,079,*73");
|
||||
ASSERT_EQ(gps.rawSentenceQueueSize(), 1);
|
||||
ASSERT_EQ(gps.satellites().size(), 0);
|
||||
}
|
||||
|
||||
/* Basic test */
|
||||
TEST_F(nmea_gps_unit, parse_basic)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Parse an invalid sentence */
|
||||
gps.parseNMEASentence("$GPGGA,182333.50,,,,,0,00,99.99,,,,,,*6B");
|
||||
}
|
||||
|
||||
/* Parse an invalid sentence */
|
||||
TEST_F(nmea_gps_unit, parse_gps_fix_invalid)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Parse an invalid sentence */
|
||||
gps.parseNMEASentence("$GPGGA,182333.50,,,,,0,00,99.99,,,,,,*6B");
|
||||
|
||||
/* Should be 1 entry in the raw queue */
|
||||
ASSERT_EQ(gps.rawSentenceQueueSize(), 1);
|
||||
/* Should be 0 in GPS fix queue */
|
||||
ASSERT_EQ(gps.fixQueueSize(), 0);
|
||||
|
||||
/* Get the GPS fix */
|
||||
upm::gps_fix f = gps.getFix();
|
||||
ASSERT_EQ(f.valid, false);
|
||||
ASSERT_EQ(f.quality, upm::gps_fix_quality::no_fix);
|
||||
/* Call the string method for coverage */
|
||||
f.__str__();
|
||||
|
||||
/* Get the 1 raw sentence */
|
||||
gps.getRawSentence();
|
||||
|
||||
/* Should be 0 entries in each queue */
|
||||
ASSERT_EQ(gps.rawSentenceQueueSize(), 0);
|
||||
ASSERT_EQ(gps.fixQueueSize(), 0);
|
||||
}
|
||||
|
||||
std::string randstr(size_t size)
|
||||
{
|
||||
std::string retstr(".", size);
|
||||
|
||||
std::random_device rd;
|
||||
std::default_random_engine eng(rd());
|
||||
std::uniform_int_distribution<char> distr(32, 126);
|
||||
for (size_t i = 0; i < size; i++)
|
||||
retstr[i] = distr(eng);
|
||||
return retstr;
|
||||
}
|
||||
|
||||
/* Parse bogus sentences */
|
||||
TEST_F(nmea_gps_unit, parse_gps_fix_bogus)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Parse some bogus sentences */
|
||||
std::string gga = "$GPGGA,182333.50,,,,,0,00,99.99,,,,,,*6B";
|
||||
gps.parseNMEASentence(gga);
|
||||
|
||||
for (size_t i = 0; i < gga.size(); i++)
|
||||
{
|
||||
std::string tmp = gga;
|
||||
gps.parseNMEASentence(tmp.replace(i, 1, "x"));
|
||||
}
|
||||
|
||||
for (int i = 0; i < 1000; i++)
|
||||
gps.parseNMEASentence(randstr(40));
|
||||
|
||||
/* Still no GPS fix */
|
||||
ASSERT_EQ(gps.fixQueueSize(), 0);
|
||||
}
|
||||
|
||||
/* Parse valid gga sentences */
|
||||
TEST_F(nmea_gps_unit, parse_gps_fix_valid)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
|
||||
/* Parse a valid sentence */
|
||||
gps.parseNMEASentence("$GPGGA,172814.0,3723.46587704,N,12202.26957864,W,"
|
||||
+ std::string("2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F"));
|
||||
|
||||
/* Should be 1 entry in GPS fix queue */
|
||||
ASSERT_EQ(gps.fixQueueSize(), 1);
|
||||
|
||||
upm::gps_fix f = gps.getFix();
|
||||
ASSERT_EQ(f.valid, true) << f.__str__();
|
||||
}
|
||||
|
||||
/* Parse valid gsv sentences */
|
||||
TEST_F(nmea_gps_unit, parse_gsv_valid)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
std::vector<std::string> snts =
|
||||
{
|
||||
"$GPGSV,2,1,08,07,64,079,,08,39,066,,09,25,159,,11,15,117,*7B",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,22,48,37,194,,51,35,158,*75",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,21,48,37,194,,51,35,158,*76",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,,48,37,194,,51,35,158,*75",
|
||||
"$GPGSV,2,1,08,07,64,079,,08,39,066,,09,25,159,,11,15,117,*7B",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,21,48,37,194,,51,35,158,*76",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,20,48,37,194,,51,35,158,*77",
|
||||
"$GPGSV,2,1,08,07,64,079,,08,39,066,,09,25,159,,11,15,117,*7B",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,18,48,37,194,,51,35,158,*7C",
|
||||
"$GPGSV,2,2,08,13,25,313,,30,78,336,17,48,37,194,,51,35,158,*73",
|
||||
"$GPGSV,2,1,08,07,64,080,,08,39,066,,09,25,159,,11,15,117,*7D",
|
||||
"$GPGSV,2,2,08,13,26,313,,30,78,336,13,48,37,194,,51,35,158,*74",
|
||||
"$GPGSV,2,1,08,07,64,080,,08,39,066,,09,25,159,,11,15,117,*7D",
|
||||
"$GPGSV,2,2,08,13,26,313,,30,78,336,09,48,37,194,,51,35,158,*7F",
|
||||
"$GPGSV,2,1,08,07,64,080,,08,39,066,,09,25,160,,11,15,117,*77",
|
||||
"$GPGSV,2,2,08,13,26,313,,30,78,336,23,48,37,194,,51,35,158,*77",
|
||||
"$GPGSV,3,3,09,51,35,158,*4E",
|
||||
"$GPGSV,2,1,08,07,64,080,22,08,39,066,,09,25,160,,11,15,117,*77",
|
||||
"$GPGSV,2,1,08,07,64,080,21,08,39,066,,09,25,160,,11,15,117,*74"
|
||||
};
|
||||
|
||||
/* Parse the first sentence */
|
||||
gps.parseNMEASentence(snts.front());
|
||||
|
||||
/* Should have 4 satellites */
|
||||
auto sats = gps.satellites();
|
||||
ASSERT_EQ(sats.size(), 4);
|
||||
|
||||
ASSERT_EQ(sats[0].prn, "07");
|
||||
ASSERT_EQ(sats[0].elevation_deg, 64);
|
||||
ASSERT_EQ(sats[0].azimuth_deg, 79);
|
||||
ASSERT_EQ(sats[0].snr, 0);
|
||||
|
||||
ASSERT_EQ(sats[1].prn, "08");
|
||||
ASSERT_EQ(sats[1].elevation_deg, 39);
|
||||
ASSERT_EQ(sats[1].azimuth_deg, 66);
|
||||
ASSERT_EQ(sats[1].snr, 0);
|
||||
|
||||
ASSERT_EQ(sats[2].prn, "09");
|
||||
ASSERT_EQ(sats[2].elevation_deg, 25);
|
||||
ASSERT_EQ(sats[2].azimuth_deg, 159);
|
||||
ASSERT_EQ(sats[2].snr, 0);
|
||||
|
||||
ASSERT_EQ(sats[3].prn, "11");
|
||||
ASSERT_EQ(sats[3].elevation_deg, 15);
|
||||
ASSERT_EQ(sats[3].azimuth_deg, 117);
|
||||
ASSERT_EQ(sats[3].snr, 0);
|
||||
|
||||
/* Parse the rest */
|
||||
for(const auto& sentence : snts)
|
||||
gps.parseNMEASentence(sentence);
|
||||
|
||||
/* Finish up with 8 satellites */
|
||||
sats = gps.satellites();
|
||||
ASSERT_EQ(sats.size(), 8);
|
||||
|
||||
/* Verify the last satellite */
|
||||
ASSERT_EQ(sats.back().prn, "11");
|
||||
ASSERT_EQ(sats.back().elevation_deg, 15);
|
||||
ASSERT_EQ(sats.back().azimuth_deg, 117);
|
||||
ASSERT_EQ(sats.back().snr, 0);
|
||||
|
||||
/* The 4th should have a non-zero snr */
|
||||
ASSERT_EQ(sats[4].snr, 21);
|
||||
}
|
||||
|
||||
/* Parse valid gll sentences */
|
||||
TEST_F(nmea_gps_unit, parse_gll_valid)
|
||||
{
|
||||
upm::NMEAGPS gps(0, 115200, -1);
|
||||
std::vector<std::string> snts =
|
||||
{
|
||||
"$GPGLL,4532.55107,N,12257.68422,W,170004.20,A,A*74",
|
||||
"$GPGLL,4532.55008,N,12257.68195,W,170005.00,A,A*70",
|
||||
"$GPGLL,4532.55027,N,12257.68252,W,170006.10,A,A*77",
|
||||
"$GPGLL,4532.54370,N,12257.65873,W,170006.90,A,A*7B",
|
||||
"$GPGLL,4532.54230,N,12257.65302,W,170008.00,A,A*74"
|
||||
};
|
||||
|
||||
/* Parse the first sentence */
|
||||
gps.parseNMEASentence(snts.front());
|
||||
|
||||
/* Get the fix */
|
||||
upm::gps_fix f = gps.getFix();
|
||||
ASSERT_EQ(f.valid, true) << f.__str__();
|
||||
ASSERT_FLOAT_EQ(f.coordinates.latitude, 45.542517833333335) << f.__str__();
|
||||
ASSERT_FLOAT_EQ(f.coordinates.longitude, -122.96140366666667) << f.__str__();
|
||||
ASSERT_EQ(f.time_utc, "170004.20") << f.__str__();
|
||||
|
||||
/* Parse the rest */
|
||||
for(const auto& sentence : snts)
|
||||
gps.parseNMEASentence(sentence);
|
||||
|
||||
/* Should have 5 GPS fixes */
|
||||
ASSERT_EQ(gps.fixQueueSize(), 5);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user