diff --git a/src/nmea_gps/CMakeLists.txt b/src/nmea_gps/CMakeLists.txt index 82c15c58..b3fb6591 100644 --- a/src/nmea_gps/CMakeLists.txt +++ b/src/nmea_gps/CMakeLists.txt @@ -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}) diff --git a/src/nmea_gps/nmea_gps.c b/src/nmea_gps/nmea_gps.c index 53840884..2e9b902e 100644 --- a/src/nmea_gps/nmea_gps.c +++ b/src/nmea_gps/nmea_gps.c @@ -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; } diff --git a/src/nmea_gps/nmea_gps.cxx b/src/nmea_gps/nmea_gps.cxx index 578e4ee9..b6dbce58 100644 --- a/src/nmea_gps/nmea_gps.cxx +++ b/src/nmea_gps/nmea_gps.cxx @@ -22,9 +22,14 @@ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +#include +#include #include +#include +#include #include +#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(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(quality) << ", " + << "sats: " << static_cast(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 NMEAGPS::satellites() +{ + /* Create a new set for now */ + _mtx_satlist.lock(); + std::vector 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(); +} diff --git a/src/nmea_gps/nmea_gps.h b/src/nmea_gps/nmea_gps.h index 392cf499..a01c1f7e 100644 --- a/src/nmea_gps/nmea_gps.h +++ b/src/nmea_gps/nmea_gps.h @@ -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 * diff --git a/src/nmea_gps/nmea_gps.hpp b/src/nmea_gps/nmea_gps.hpp index c4a91dca..9b49ff2f 100644 --- a/src/nmea_gps/nmea_gps.hpp +++ b/src/nmea_gps/nmea_gps.hpp @@ -23,133 +23,359 @@ */ #pragma once -#include +#include #include - -#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include #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 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 _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 nmea_2_parser = + { + {"GPGGA", &NMEAGPS::_parse_gpgga}, + {"GPGSV", &NMEAGPS::_parse_gpgsv}, + {"GPGLL", &NMEAGPS::_parse_gpgll}, + }; + + /** 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; + + /** Specify a queue size for parsed objects */ + std::atomic _maxQueueDepth; + + /** 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 8d66ce98..6e10f50f 100644 --- a/src/nmea_gps/nmea_gps.i +++ b/src/nmea_gps/nmea_gps.i @@ -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; %include "nmea_gps.hpp" /* END Common SWIG syntax */ diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index e9878b1d..2b306e63 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -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 diff --git a/tests/unit/nmea_gps/nmea_gps_tests.cxx b/tests/unit/nmea_gps/nmea_gps_tests.cxx new file mode 100644 index 00000000..dacacb57 --- /dev/null +++ b/tests/unit/nmea_gps/nmea_gps_tests.cxx @@ -0,0 +1,246 @@ +#include "gtest/gtest.h" +#include "nmea_gps.hpp" +#include "mraa.hpp" + +#include +#include + +/* 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 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 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 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); +}