Commit 61e4edb6 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[nmea] Added NMEA 0183 parsing basics

parent 85607121
......@@ -36,6 +36,7 @@ list(APPEND rtac_base_headers
include/rtac_base/geometry.h
include/rtac_base/interpolation.h
include/rtac_base/cuda_defines.h
include/rtac_base/nmea_utils.h
)
add_library(rtac_base SHARED
......
#ifndef _DEF_RTAC_BASE_NMEA_CODEC_H_
#define _DEF_RTAC_BASE_NMEA_CODEC_H_
#include <string>
#include <iostream>
#include <iomanip>
#include <rtac_base/types/common.h>
namespace rtac { namespace nmea {
struct NmeaError : public std::runtime_error {
enum Code : uint8_t {
NoError = 0x0,
InvalidDelimiter = 0x1,
InvalidCharacter = 0x2,
NoChecksum = 0x4,
ChecksumMismatch = 0x8,
};
static std::string code_to_string(const Code& code) {
switch(code) {
default: return "Unknown NmeaError"; break;
case NoError : return "NoError"; break;
case InvalidDelimiter: return "InvalidDelimiter"; break;
case InvalidCharacter: return "InvalidCharacter"; break;
case NoChecksum : return "NoChecksum"; break;
case ChecksumMismatch: return "ChecksumMismatch"; break;
}
}
NmeaError(const Code& code) :
std::runtime_error(code_to_string(code))
{}
NmeaError(const std::string& what_arg) :
std::runtime_error(what_arg)
{}
};
inline NmeaError::Code nmea_invalid(const std::string& msg)
{
if(msg[0] != '$' || msg[msg.size() - 2] != '\r' || msg[msg.size() - 1] != '\n') {
return NmeaError::InvalidDelimiter;
}
uint8_t checksum = 0;
unsigned int i = 1;
for(; i < msg.size(); i++) {
if(msg[i] == '*') {
i++;
break;
}
if(msg[i] < 32 || msg[i] > 126) {
if(msg[i] == '\r') {
// End of message reached without finding a checksum delimiter.
return NmeaError::NoChecksum;
}
return NmeaError::InvalidCharacter;
}
checksum ^= msg[i];
}
auto transmittedChecksum = std::stoul(msg.c_str() + i, nullptr, 16);
if(transmittedChecksum != checksum) {
return NmeaError::ChecksumMismatch;
}
return NmeaError::NoError;
}
inline std::string nmea_type(const std::string& msg)
{
return msg.substr(1,5);
}
template <typename T = uint64_t>
inline T millis_from_gps_time(const std::string& date)
{
return 1000*(3600*std::stoul(date.substr(0,2)) // hours
+ 60*std::stoul(date.substr(2,2)) // minutes
+ std::stoul(date.substr(4,2))) // seconds
+ std::stoul(date.substr(7)); // millis
}
}; //namespace nmea
}; //namespace rtac
inline std::ostream& operator<<(std::ostream& os, const rtac::nmea::NmeaError::Code& code)
{
os << rtac::nmea::NmeaError::code_to_string(code);
return os;
}
#endif //_DEF_RTAC_BASE_NMEA_CODEC_H_
......@@ -15,6 +15,7 @@ list(APPEND test_names
buildtarget_test.cpp
ppmformat_test.cpp
nmea_utils.cpp
)
list(APPEND test_deps
......
#include <iostream>
#include <list>
using namespace std;
#include <rtac_base/nmea_utils.h>
using namespace rtac::nmea;
int main()
{
// All messages are valid except for the last ones which were edited to
// exibit various errors.
std::list<std::string> msgs;
msgs.push_back("$GPGGA,142042.70,4811.68519184,N,00300.99147341,W,4,15,0.0,121.352,M,49.938,M,1.6,0153*5B\r\n");
msgs.push_back("$GPVTG,343.49,T,343.22,M,2.810,N,5.204,K,R*35\r\n");
msgs.push_back("$GPGST,142042.70,,0.024,0.023,0.00,0.024,0.023,0.022*4F\r\n");
msgs.push_back("$PASHR,142042.70,337.97,T,-001.67,-001.23,-000.00,0.014,0.014,0.065,2,0,*2F\r\n");
msgs.push_back("$GPGGA,142042.75,4811.68522913,N,00300.99148992,W,4,15,0.0,121.352,M,49.938,M,1.6,0153*5B\r\n");
msgs.push_back("$GPVTG,343.35,T,343.07,M,2.800,N,5.185,K,R*32\r\n");
msgs.push_back("$GPGST,142042.75,,0.024,0.023,0.00,0.024,0.023,0.022*4A\r\n");
msgs.push_back("$PASHR,142042.75,337.89,T,-001.66,-001.24,-000.00,0.014,0.014,0.065,2,0,*23\r\n");
// First character was deleted. Should trigger an InvalidDelimiter error.
msgs.push_back("GPGGA,142042.80,4811.68526652,N,00300.99150646,W,4,15,0.0,121.352,M,49.938,M,1.6,0153*50\r\n");
// Last character was deleted. Should trigger an InvalidDelimiter error.
msgs.push_back("$GPVTG,343.51,T,343.24,M,2.805,N,5.195,K,R*35\r");
// Checksum delimiter was deleted. Should trigger a NoChecksum error.
msgs.push_back("$GPGST,142042.80,,0.024,0.023,0.00,0.024,0.023,0.02240\r\n");
// First character of the first field was deleted (first field should be
// 142042.80). This should trigger a ChecksumMismatch error.
msgs.push_back("$PASHR,42042.80,337.82,T,-001.65,-001.24,-000.00,0.014,0.014,0.065,2,0,*21\r\n");
for(auto m : msgs) {
//cout << m;
if(auto err = nmea_invalid(m)) {
cout << "NmeaError " << err << " : " << m << endl;
continue;
}
}
return 0;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment