Fix NMEA framing errors

main.cpp

#include <iostream>
#include <sstream>
#include <vector>

using namespace std;

/** Accepts NMEA data with framing errors and returns valid NMEA sentences if available */
class cNMEAFramer
{
public:

    /** Constructor
        @param[in] s the number of bytes without a valid sentence
                        we are willing to accept before giving up
    */
    cNMEAFramer( int s);

    /** Add some data as it arrives
        @param[in] newdata pointer to start of new data
        @param[in] length number of bytes in new data
        Throws exception if buffer overflows
    */
    void Add( unsigned char* newdata, int length );

    /** Get first valid NMEA sentence received
        @return vector of bytes, size 0 if no sentence available
    */
    vector<unsigned char> Sentence();

    string Dump();

private:
    int mySize;
    vector<unsigned char> myBuf;
    int myFront;

    int FindFirstSentenceStart();
    int FindFirstSentenceEnd( int start );
};

cNMEAFramer::cNMEAFramer( int s )
    : mySize( s )
    , myBuf(mySize)
    , myFront( 0 )
{

}

void cNMEAFramer::Add( unsigned char* newdata, int length )
{
    if( myFront + length > mySize )
        throw std::runtime_error("cNMEAFramer buffer overflow");
    for( int k = 0; k < length; k++ )
    {
        myBuf[ myFront++ ] = *newdata++;
    }
}

vector<unsigned char> cNMEAFramer::Sentence()
{
    vector<unsigned char> sentence;
    int start = FindFirstSentenceStart();
    if( start < 0 )
        return sentence;
    int end = FindFirstSentenceEnd( start );
    if( end < 0 )
        return sentence;
    sentence.insert(
        sentence.begin(),
        myBuf.begin()+start,
        myBuf.begin()+end );
    myBuf.erase(
        myBuf.begin(),
        myBuf.begin()+end-1);
    return sentence;
}

int cNMEAFramer::FindFirstSentenceStart()
{
    for( int t = 0; t < myFront; t++ )
    {
        if( myBuf[t] == '$' &&
                myBuf[t+1] == 'G' &&
                myBuf[t+2] == 'P' )
            return t;
    }
    return -1;

}

int cNMEAFramer::FindFirstSentenceEnd( int start )
{
    for( int e = start+3; e < myFront; e++ )
    {
        if( myBuf[e] == '*' )
            return e+3;
    }
    return -1;
}
string cNMEAFramer::Dump()
{
    stringstream ss;
    for( auto c : myBuf )
        ss << (char) c;
    return ss.str();
}

int main()
{
    cNMEAFramer Framer( 250 );

    vector<string> vmsg;

    // a perfect sentence
    vmsg.push_back("$GPRMC,,V,,,,,,,,,N*53");

    // a perfect sentence
    vmsg.push_back("$GPVTG,,,,,,,,N*30");

    // two sentences received in one read
    vmsg.push_back("$GPRMC,,V,,,,,,,,,N*53$GPVTG,,,,,,,,N*30");

    // a sentence received in fragments
    vmsg.push_back("$GPRMC,,V,,,");
    vmsg.push_back(",,,,,,N*53");

    std::cout << "\t received\t\t\t\t\toutput\n";
    vector<unsigned char> vs;
    for( auto& msg : vmsg )
    {
        cout << msg << "\n";

        // simulate read
        Framer.Add(
            (unsigned char*)msg.c_str(),     // simulated message read
            msg.length() );

        // loop over sentences that might have been read
        int count = 0;
        for( ; ; )
        {
            // extract next valid sentence
            vs = Framer.Sentence();

            if( ! vs.size() )
            {
                if( ! count )
                    // there was no valid sentence
                    cout << "\t\t\t\t\t\t----\n";
                break;
            }
            //display valid sentence
            cout << "\t\t\t\t\t\t";
            for( auto c : vs )
                cout << c;
            cout << "\n";
            count++;
        }
    }
    return 0;
}