[nmea] Added NMEA 0183 parsing basics

CMakeLists.txt

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

include/rtac_base/nmea_utils.h

+#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_
+

tests/CMakeLists.txt

@@ -15,6 +15,7 @@ list(APPEND test_names
     buildtarget_test.cpp
 
     ppmformat_test.cpp
+    nmea_utils.cpp
 )
 
 list(APPEND test_deps

tests/src/nmea_utils.cpp

+#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;
+}