mirror of
https://github.com/eclipse/upm.git
synced 2025-07-09 13:21:22 +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
@ -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;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user