Revised nmea and Garmin protocol classes.

The Garmin protocol implementation really is an NMEA protocol with a few
extra messages. Instead of duplicating the code, introduce the NMEA protocol
as a base class, which is reused for the Garmin class. The input/output
has not changed at all (and it maintains the FG-specific quirks, like our
NMEA class using LF-only linefeeds, while the Garmin protocol uses
CR-LF linefeeds.

src/Network/garmin.cxx

@@ -1,4 +1,4 @@
-// garmin.cxx -- Garmin protocal class
+// garmin.cxx -- Garmin protocol class
 //
 // Written by Curtis Olson, started November 1999.
 //
@@ -24,414 +24,80 @@
 #  include "config.h"
 #endif
 
-#include <iostream>
-#include <cstdlib>
-#include <cstring>
-#include <cstdio>
-
 #include <simgear/debug/logstream.hxx>
-#include <simgear/math/sg_geodesy.hxx>
-#include <simgear/io/iochannel.hxx>
-#include <simgear/timing/sg_time.hxx>
-
 #include <FDM/flightProperties.hxx>
-#include <Main/fg_props.hxx>
+#include <simgear/constants.h>
-#include <Main/globals.hxx>
-
 #include "garmin.hxx"
 
-using std::string;
+using namespace NMEA;
 
-FGGarmin::FGGarmin() {
+FGGarmin::FGGarmin() :
-  fdm = new FlightProperties;
+    FGNMEA(),
-}
+    mGarminMessages(GARMIN::PGRMZ),
-
+    mMetric(true) // use metric altitude reports
-FGGarmin::~FGGarmin() {
+    // In fact Garmin devices normally seem report barometric altitude in feet (not meters), but
-  delete fdm;
+    // the FG implementation has always used metric reports for years (and we keep it this way,
+    // to avoid complaints about changed implementation). The unit is also part of the NMEA message,
+    // so smart devices are probably ok with both anyway.
+{
+    // only enable the GPRMC standard NMEA message
+    mNmeaMessages = NMEA::GPRMC;
+    // Garmin uses CR-LF line feeds.
+    mLineFeed = "\r\n";
 }
 
 
-// calculate the garmin check sum
+FGGarmin::~FGGarmin()
-static char calc_nmea_cksum(char *sentence) {
+{
-    unsigned char sum = 0;
-    int i, len;
-
-    len = strlen(sentence);
-    sum = sentence[0];
-    for ( i = 1; i < len; i++ ) {
-        sum ^= sentence[i];
-    }
-
-    return sum;
 }
 
 
-// generate Garmin message
+// generate Garmin NMEA messages
-bool FGGarmin::gen_message() {
+bool FGGarmin::gen_message()
-    char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256], gsa[256];
+{
-    char dir;
+    (void) FGNMEA::gen_message();
-    int deg;
-    double min;
 
-    SGTime *t = globals->get_time_params();
+    char nmea[256];
 
-    char utc[10];
+    // RMZ sentence (Garmin proprietary)
-    sprintf( utc, "%02d%02d%02d", 
+    if (mGarminMessages & GARMIN::PGRMZ)
-	     t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
+    {
+        double altitude_ft = mFdm.get_Altitude();
 
-    char rmc_lat[20];
+        // $PGRMZ,AAAA.A,F,T*XX
-    double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES;
+        if (mMetric)
-    if ( latd < 0.0 ) {
+            sprintf( nmea, "$PGRMZ,%.1f,M,3", altitude_ft * SG_FEET_TO_METER );
-	latd = -latd;
+        else
-	dir = 'S';
+            sprintf( nmea, "$PGRMZ,%.1f,F,3", altitude_ft );
-    } else {
+        add_with_checksum(nmea, 256);
-	dir = 'N';
     }
-    deg = (int)(latd);
-    min = (latd - (double)deg) * 60.0;
-    sprintf( rmc_lat, "%02d%07.4f,%c", abs(deg), min, dir);
-
-    char rmc_lon[20];
-    double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES;
-    if ( lond < 0.0 ) {
-	lond = -lond;
-	dir = 'W';
-    } else {
-	dir = 'E';
-    }
-    deg = (int)(lond);
-    min = (lond - (double)deg) * 60.0;
-    sprintf( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir);
-
-    char speed[10];
-    sprintf( speed, "%05.1f", fdm->get_V_equiv_kts() );
-
-    char heading[10];
-    sprintf( heading, "%05.1f", fdm->get_Psi() * SGD_RADIANS_TO_DEGREES );
-
-    char altitude_m[10];
-    sprintf( altitude_m, "%02d", 
-	     (int)(fdm->get_Altitude() * SG_FEET_TO_METER) );
-
-    char date[10];
-    int year = t->getGmt()->tm_year;
-    while ( year >= 100 ) { year -= 100; }
-    sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday, 
-	     t->getGmt()->tm_mon+1, year );
-
-    char magvar[10];
-    float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
-    if ( magdeg < 0.0 ) {
-        magdeg = -magdeg;
-        dir = 'W';
-    } else {
-        dir = 'E';
-    }
-    sprintf( magvar, "%05.1f,%c", magdeg, dir );
-
-    // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
-    sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,%s",
-	     utc, rmc_lat, rmc_lon, speed, heading, date, magvar );
-    sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
-
-    // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
-    //          utc, lat, lon, altitude_m );
-
-    sprintf( rmz, "PGRMZ,%s,M,3", altitude_m );
-    sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
-
-    sprintf( gsa, "%s",
-             "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38" );
-
-    SG_LOG( SG_IO, SG_DEBUG, rmc );
-    SG_LOG( SG_IO, SG_DEBUG, rmz );
-    SG_LOG( SG_IO, SG_DEBUG, gsa );
-
-    string garmin_sentence;
-
-    // RMC sentence
-    garmin_sentence = "$";
-    garmin_sentence += rmc;
-    garmin_sentence += "*";
-    garmin_sentence += rmc_sum;
-    garmin_sentence += "\r\n";
-
-    // RMZ sentence (garmin proprietary)
-    garmin_sentence += "$";
-    garmin_sentence += rmz;
-    garmin_sentence += "*";
-    garmin_sentence += rmz_sum;
-    garmin_sentence += "\r\n";
-
-    // GSA sentence (totally faked)
-    garmin_sentence += gsa;
-    garmin_sentence += "\r\n";
-
-    length = garmin_sentence.length();
-    strncpy( buf, garmin_sentence.c_str(), length );
 
     return true;
 }
 
+// process a Garmin sentence
+void FGGarmin::parse_message(const std::vector<std::string>& tokens)
+{
+    if (tokens[0] == "PGRMZ")
+    {
+        if (tokens.size()<3)
+            return;
 
-// parse Garmin message
+        // #1: altitude
-bool FGGarmin::parse_message() {
+        double altitude = atof( tokens[1].c_str() );
-    SG_LOG( SG_IO, SG_INFO, "parse garmin message" );
 
-    string msg = buf;
+        // #2: altitude units
-    msg = msg.substr( 0, length );
+        const string& alt_units = tokens[2];
-    SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
+        if ( alt_units != "F" && alt_units != "f" )
-
-    string::size_type begin_line, end_line, begin, end;
-    begin_line = begin = 0;
-
-    // extract out each line
-    end_line = msg.find("\n", begin_line);
-    while ( end_line != string::npos ) {
-	string line = msg.substr(begin_line, end_line - begin_line);
-	begin_line = end_line + 1;
-	SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
-
-	// leading character
-	string start = msg.substr(begin, 1);
-	++begin;
-	SG_LOG( SG_IO, SG_INFO, "  start = " << start );
-
-	// sentence
-	end = msg.find(",", begin);
-	if ( end == string::npos ) {
-	    return false;
-	}
-    
-	string sentence = msg.substr(begin, end - begin);
-	begin = end + 1;
-	SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
-
-	double lon_deg, lon_min, lat_deg, lat_min;
-	double lon, lat, speed, heading, altitude;
-
-	if ( sentence == "GPRMC" ) {
-	    // time
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string utc = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
-
-	    // junk
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string junk = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
-
-	    // lat val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lat_deg = atof( lat_str.substr(0, 2).c_str() );
-	    lat_min = atof( lat_str.substr(2).c_str() );
-
-	    // lat dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lat = lat_deg + ( lat_min / 60.0 );
-	    if ( lat_dir == "S" ) {
-		lat *= -1;
-	    }
-
-	    fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
-
-	    // lon val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lon_deg = atof( lon_str.substr(0, 3).c_str() );
-	    lon_min = atof( lon_str.substr(3).c_str() );
-
-	    // lon dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lon = lon_deg + ( lon_min / 60.0 );
-	    if ( lon_dir == "W" ) {
-		lon *= -1;
-	    }
-
-	    fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
-
-#if 0
-	    double sl_radius, lat_geoc;
-	    sgGeodToGeoc( fdm->get_Latitude(), 
-			  fdm->get_Altitude(), 
-			  &sl_radius, &lat_geoc );
-	    fdm->set_Geocentric_Position( lat_geoc, 
-			   fdm->get_Longitude(), 
-	     	           sl_radius + fdm->get_Altitude() );
-#endif
-
-	    // speed
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string speed_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    speed = atof( speed_str.c_str() );
-	    fdm->set_V_calibrated_kts( speed );
-	    // fdm->set_V_ground_speed( speed );
-	    SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
-
-	    // heading
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string hdg_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    heading = atof( hdg_str.c_str() );
-	    fdm->set_Euler_Angles( fdm->get_Phi(), 
-					     fdm->get_Theta(), 
-					     heading * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
-	} else if ( sentence == "PGRMZ" ) {
-	    // altitude
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string alt_str = msg.substr(begin, end - begin);
-	    altitude = atof( alt_str.c_str() );
-	    begin = end + 1;
-
-	    // altitude units
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string alt_units = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    if ( alt_units != "F" && alt_units != "f" ) {
             altitude *= SG_METER_TO_FEET;
+
+        mFdm.set_Altitude( altitude );
+
+        SG_LOG( SG_IO, SG_DEBUG, " altitude  = " << altitude );
     }
-
+    else
-	    fdm->set_Altitude( altitude );
+    {
-    
+        // not a Garmin message. Maybe standard NMEA message.
- 	    SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
+        FGNMEA::parse_message(tokens);
-
-	}
-
-	begin = begin_line;
-	end_line = msg.find("\n", begin_line);
-    }
-
-    return true;
-}
-
-
-// open hailing frequencies
-bool FGGarmin::open() {
-    if ( is_enabled() ) {
-	SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " 
-		<< "is already in use, ignoring" );
-	return false;
-    }
-
-    SGIOChannel *io = get_io_channel();
-
-    if ( ! io->open( get_direction() ) ) {
-	SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
-	return false;
-    }
-
-    set_enabled( true );
-
-    return true;
-}
-
-
-// process work for this port
-bool FGGarmin::process() {
-    SGIOChannel *io = get_io_channel();
-
-    if ( get_direction() == SG_IO_OUT ) {
-	gen_message();
-	if ( ! io->write( buf, length ) ) {
-	    SG_LOG( SG_IO, SG_WARN, "Error writing data." );
-	    return false;
-	}
-    } else if ( get_direction() == SG_IO_IN ) {
-	if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
-	    SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
-	    if ( parse_message() ) {
-		SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
-	    } else {
-		SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
-	    }
-	} else {
-	    SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
-	    return false;
-	}
-	if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
-	    SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
-	    if ( parse_message() ) {
-		SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
-	    } else {
-		SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
-	    }
-	} else {
-	    SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
-	    return false;
     }
 }
-
-    return true;
-}
-
-
-// close the channel
-bool FGGarmin::close() {
-    SGIOChannel *io = get_io_channel();
-
-    set_enabled( false );
-
-    if ( ! io->close() ) {
-	return false;
-    }
-
-    return true;
-}

src/Network/garmin.hxx

@@ -1,4 +1,4 @@
-// garmin.hxx -- Garmin protocal class
+// garmin.hxx -- Garmin protocol class
 //
 // Written by Curtis Olson, started November 1999.
 //
@@ -20,44 +20,36 @@
 //
 // $Id$
 
-
 #ifndef _FG_GARMIN_HXX
 #define _FG_GARMIN_HXX
 
+#include "nmea.hxx"
 
-#include <simgear/compiler.h>
+namespace NMEA
+{
+    // Garmin proprietary messages
+    namespace GARMIN
+    {
+        const unsigned int PGRMZ = (1<<0);
+    }
+}
 
-#include <string>
 
-#include "protocol.hxx"
+class FGGarmin : public FGNMEA {
 
-class FlightProperties;
 
-class FGGarmin : public FGProtocol {
+protected:
+    unsigned int mGarminMessages;
+    bool mMetric;
 
-    char buf[ FG_MAX_MSG_SIZE ];
+    // process a Garmin sentence
-    int length;
+    virtual void parse_message(const std::vector<std::string>& tokens);
-    FlightProperties* fdm;
 
 public:
-
     FGGarmin();
     ~FGGarmin();
 
-    bool gen_message();
+    virtual bool gen_message();
-    bool parse_message();
- 
-    // open hailing frequencies
-    bool open();
-
-    // process work for this port
-    bool process();
-
-    // close the channel
-    bool close();
 };
 
-
 #endif // _FG_GARMIN_HXX
-
-

src/Network/nmea.cxx

@@ -1,4 +1,4 @@
-// nmea.cxx -- NMEA protocal class
+// nmea.cxx -- NMEA protocol class
 //
 // Written by Curtis Olson, started November 1999.
 //
@@ -39,44 +39,43 @@
 
 #include "nmea.hxx"
 
-FGNMEA::FGNMEA() {
+FGNMEA::FGNMEA() :
-  fdm = new FlightProperties();
+    mLength(0),
+    mNmeaMessages(NMEA::SET),
+    mLineFeed("\n")
+{
 }
 
+
 FGNMEA::~FGNMEA() {
-  delete fdm;
 }
 
 
-// calculate the nmea check sum
+// calculate the NMEA check sum
-static char calc_nmea_cksum(char *sentence) {
+void FGNMEA::add_with_checksum(char *sentence, unsigned int buf_size) {
+    unsigned int i;
     unsigned char sum = 0;
-    int i, len;
 
-    // cout << sentence << endl;
+    for (i = 1; sentence[i] != 0; i++ ) {
-
-    len = strlen(sentence);
-    sum = sentence[0];
-    for ( i = 1; i < len; i++ ) {
-        // cout << sentence[i];
         sum ^= sentence[i];
     }
-    // cout << endl;
 
-    // printf("sum = %02x\n", sum);
+    if (i + 6 < buf_size)
-    return sum;
+        snprintf( &sentence[i], 6, "*%02X%s", sum, mLineFeed);
+
+    SG_LOG( SG_IO, SG_DEBUG, sentence );
+
+    mNmeaSentence += sentence;
 }
 
 
 // generate NMEA message
-bool FGNMEA::gen_message() {
+bool FGNMEA::gen_message()
-    // cout << "generating nmea message" << endl;
+{
-
-    char rmc[256], gga[256], gsa[256];
-    char rmc_sum[10], gga_sum[10];
     char dir;
     int deg;
     double min;
+    char nmea[256];
 
     SGTime *t = globals->get_time_params();
 
@@ -84,8 +83,9 @@ bool FGNMEA::gen_message() {
     sprintf( utc, "%02d%02d%02d", 
              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
 
-    char gga_lat[20], rmc_lat[20];
+    char lat[20];
-    double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES;
+    {
+        double latd = mFdm.get_Latitude() * SGD_RADIANS_TO_DEGREES;
         if ( latd < 0.0 ) {
             latd = -latd;
             dir = 'S';
@@ -94,11 +94,12 @@ bool FGNMEA::gen_message() {
         }
         deg = (int)(latd);
         min = (latd - (double)deg) * 60.0;
-    sprintf( gga_lat, "%02d%07.4f,%c", abs(deg), min, dir);
+        sprintf( lat, "%02d%07.4f,%c", abs(deg), min, dir);
-    sprintf( rmc_lat, "%02d%07.4f,%c", abs(deg), min, dir);
+    }
 
-    char gga_lon[20], rmc_lon[20];
+    char lon[20];
-    double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES;
+    {
+        double lond = mFdm.get_Longitude() * SGD_RADIANS_TO_DEGREES;
         if ( lond < 0.0 ) {
             lond = -lond;
             dir = 'W';
@@ -107,35 +108,41 @@ bool FGNMEA::gen_message() {
         }
         deg = (int)(lond);
         min = (lond - (double)deg) * 60.0;
-    sprintf( gga_lon, "%03d%07.4f,%c", abs(deg), min, dir);
+        sprintf( lon, "%03d%07.4f,%c", abs(deg), min, dir);
-    sprintf( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir);
+    }
 
     double vn = fgGetDouble( "/velocities/speed-north-fps" );
     double ve = fgGetDouble( "/velocities/speed-east-fps" );
+
+    char speed[10];
+    {
         double fps = sqrt( vn*vn + ve*ve );
         double mps = fps * SG_FEET_TO_METER;
         double kts = mps * SG_METER_TO_NM * 3600;
-    char speed[10];
         sprintf( speed, "%.1f", kts );
+    }
 
+    char heading[10];
+    {
         double hdg_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
         if ( hdg_true < 0 ) {
           hdg_true += 360.0;
         }
-    char heading[10];
         sprintf( heading, "%.1f", hdg_true );
+    }
 
-    char altitude_m[10];
+    double altitude_ft = mFdm.get_Altitude();
-    sprintf( altitude_m, "%.1f", 
-	     fdm->get_Altitude() * SG_FEET_TO_METER );
 
     char date[10];
+    {
         int year = t->getGmt()->tm_year;
         while ( year >= 100 ) { year -= 100; }
         sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
              t->getGmt()->tm_mon+1, year );
+    }
 
     char magvar[10];
+    {
         float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
         if ( magdeg < 0.0 ) {
             magdeg = -magdeg;
@@ -144,343 +151,210 @@ bool FGNMEA::gen_message() {
             dir = 'E';
         }
         sprintf( magvar, "%.1f,%c", magdeg, dir );
- 
+    }
-    // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
-    sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,%s,A",
-	     utc, rmc_lat, rmc_lon, speed, heading, date, magvar );
-    sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
-
-    // $GPGGA,HHMMSS,DDMM.MMMM,N,DDDMM.MMMM,W,1,NN,H.H,AAAA.A,M,GG.G,M,,*XX
-    sprintf( gga, "GPGGA,%s,%s,%s,1,08,0.9,%s,M,0.0,M,,",
-	     utc, gga_lat, gga_lon, altitude_m );
-    sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
-    sprintf( gsa, "%s",
-             "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38" );
-
-    SG_LOG( SG_IO, SG_DEBUG, rmc );
-    SG_LOG( SG_IO, SG_DEBUG, gga );
-    SG_LOG( SG_IO, SG_DEBUG, gsa );
-
-    string nmea_sentence;
 
     // RMC sentence
-    nmea_sentence = "$";
+    if (mNmeaMessages & NMEA::GPRMC)
-    nmea_sentence += rmc;
+    {
-    nmea_sentence += "*";
+        // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
-    nmea_sentence += rmc_sum;
+        sprintf( nmea, "$GPRMC,%s,A,%s,%s,%s,%s,%s,%s,A",
-    nmea_sentence += "\n";
+                 utc, lat, lon, speed, heading, date, magvar );
+        add_with_checksum(nmea, 256);
+    }
 
     // GGA sentence
-    nmea_sentence += "$";
+    if (mNmeaMessages & NMEA::GPGGA)
-    nmea_sentence += gga;
+    {
-    nmea_sentence += "*";
+        // $GPGGA,HHMMSS,DDMM.MMMM,N,DDDMM.MMMM,W,1,NN,H.H,AAAA.A,M,GG.G,M,,*XX
-    nmea_sentence += gga_sum;
+        sprintf( nmea, "$GPGGA,%s,%s,%s,1,08,0.9,%.1f,M,0.0,M,,",
-    nmea_sentence += "\n";
+                 utc, lat, lon, altitude_ft * SG_FEET_TO_METER );
+        add_with_checksum(nmea, 256);
+    }
 
     // GSA sentence (totally faked)
-    nmea_sentence += gsa;
+    if (mNmeaMessages & NMEA::GPGSA)
-    nmea_sentence += "\n";
+    {
+        sprintf( nmea, "%s%s",
+                "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38", mLineFeed );
+        SG_LOG( SG_IO, SG_DEBUG, nmea );
 
-    // cout << nmea_sentence;
+        mNmeaSentence += nmea;
-
+    }
-    length = nmea_sentence.length();
-    strncpy( buf, nmea_sentence.c_str(), length );
 
     return true;
 }
 
-
 // parse NMEA message.  messages will look something like the
 // following:
 //
 // $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
 // $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
 
-bool FGNMEA::parse_message() {
+void FGNMEA::parse_line() {
-    SG_LOG( SG_IO, SG_INFO, "parse nmea message" );
+    SG_LOG( SG_IO, SG_DEBUG, "parse nmea message" );
 
-    string msg = buf;
+    if (mLength > FG_MAX_MSG_SIZE-1)
-    msg = msg.substr( 0, length );
+        mLength = FG_MAX_MSG_SIZE-1;
-    SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 
-    string::size_type begin_line, end_line, begin, end;
+    SG_LOG( SG_IO, SG_DEBUG, "entire message = " << mBuf );
-    begin_line = begin = 0;
 
-    // extract out each line
+    // test leading character
-    end_line = msg.find("\n", begin_line);
+    if (mBuf[0] != '$')
-    while ( end_line != string::npos ) {
+    {
-	string line = msg.substr(begin_line, end_line - begin_line);
+        SG_LOG( SG_IO, SG_DEBUG, "  invalid NMEA start character = " << mBuf[0]);
-	begin_line = end_line + 1;
+        return;
-	SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
-
-	// leading character
-	string start = msg.substr(begin, 1);
-	++begin;
-	SG_LOG( SG_IO, SG_INFO, "  start = " << start );
-
-	// sentence
-	end = msg.find(",", begin);
-	if ( end == string::npos ) {
-	    return false;
     }
 
-	string sentence = msg.substr(begin, end - begin);
+    // get rid of checksum and "*" delimiter
-	begin = end + 1;
+    while ((mLength > 0)&&(mBuf[mLength]!='*'))
-	SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
+    {
+        mLength--;
+    }
+    mBuf[mLength] = 0;
 
+    // split string to tokens
+    std::vector<std::string> tokens;
+    for (unsigned int pos=1;pos < mLength;pos++)
+    {
+        const char* pCurrent = &mBuf[pos];
+        while ((mBuf[pos]!=',')&&(pos<mLength))
+            pos++;
+        if (mBuf[pos]==',')
+            mBuf[pos] = 0;
+        tokens.push_back(pCurrent);
+    }
+
+    if (tokens.size() == 0)
+        return;
+
+    if (tokens.size()>1)
+    {
+        for (unsigned int i=0;i<tokens.size();i++)
+        {
+            SG_LOG( SG_IO, SG_DEBUG, "  NMEA token # " << i << ": " << tokens[i]);
+        }
+        parse_message(tokens);
+    }
+}
+
+void FGNMEA::parse_message(const std::vector<std::string>& tokens)
+{
     double lon_deg, lon_min, lat_deg, lat_min;
-	double lon, lat, speed, heading, altitude;
+    double lon, lat;
+    string::size_type begin = 0, end;
 
-	if ( sentence == "GPRMC" ) {
+    if (tokens[0] == "GPRMC" ) {
-	    // time
+        // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
-	    end = msg.find(",", begin);
+        if ( tokens.size()<9)
-	    if ( end == string::npos ) {
+            return;
-		return false;
-	    }
 
-	    string utc = msg.substr(begin, end - begin);
+        // #1: time
-	    begin = end + 1;
+        const string& utc = tokens[1];
-	    SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
+        SG_LOG( SG_IO, SG_DEBUG, "  utc = " << utc );
 
-	    // junk
+        // #2: junk
-	    end = msg.find(",", begin);
+        SG_LOG( SG_IO, SG_DEBUG, "  junk = " << tokens[2] );
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string junk = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
-
-	    // lat val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lat_deg = atof( lat_str.substr(0, 2).c_str() );
-	    lat_min = atof( lat_str.substr(2).c_str() );
-
-	    // lat dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
 
+        // #3: lat val
+        lat_deg = atof( tokens[3].substr(0, 2).c_str() );
+        lat_min = atof( tokens[3].substr(2).c_str() );
         lat = lat_deg + ( lat_min / 60.0 );
-	    if ( lat_dir == "S" ) {
+
+        // #4: lat dir
+        if ( tokens[4] == "S" )
             lat *= -1;
-	    }
 
-	    fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
+        mFdm.set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
-
-	    // lon val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lon_deg = atof( lon_str.substr(0, 3).c_str() );
-	    lon_min = atof( lon_str.substr(3).c_str() );
-
-	    // lon dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
 
+        // #5: lon val
+        lon_deg = atof( tokens[5].substr(0, 3).c_str() );
+        lon_min = atof( tokens[5].substr(3).c_str() );
         lon = lon_deg + ( lon_min / 60.0 );
-	    if ( lon_dir == "W" ) {
-		lon *= -1;
-	    }
 
-	    fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
+        // #6: lon dir
-	    SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
+        if ( tokens[6] == "W" )
+            lon *= -1;
+
+        mFdm.set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
+        SG_LOG( SG_IO, SG_DEBUG, "  lat = " << lat << ", lon = " << lon );
 
 #if 0
         double sl_radius, lat_geoc;
-	    sgGeodToGeoc( fdm->get_Latitude(), 
+        sgGeodToGeoc( mFdm.get_Latitude(),
-			  fdm->get_Altitude(), 
+              mFdm.get_Altitude(),
               &sl_radius, &lat_geoc );
-	    fdm->set_Geocentric_Position( lat_geoc, 
+        mFdm.set_Geocentric_Position( lat_geoc,
-			   fdm->get_Longitude(), 
+               mFdm.get_Longitude(),
-	     	           sl_radius + fdm->get_Altitude() );
+                       sl_radius + mFdm.get_Altitude() );
 #endif
 
-	    // speed
+        // #7: speed
-	    end = msg.find(",", begin);
+        double speed = atof( tokens[7].c_str() );
-	    if ( end == string::npos ) {
+        mFdm.set_V_calibrated_kts( speed );
-		return false;
+        // mFdm.set_V_ground_speed( speed );
-	    }
+        SG_LOG( SG_IO, SG_DEBUG, "  speed = " << speed );
 
-	    string speed_str = msg.substr(begin, end - begin);
+        // #8: heading
-	    begin = end + 1;
+        double heading = atof( tokens[8].c_str() );
-	    speed = atof( speed_str.c_str() );
+        mFdm.set_Euler_Angles( mFdm.get_Phi(),
-	    fdm->set_V_calibrated_kts( speed );
+                         mFdm.get_Theta(),
-	    // fdm->set_V_ground_speed( speed );
-	    SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
-
-	    // heading
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string hdg_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-	    heading = atof( hdg_str.c_str() );
-	    fdm->set_Euler_Angles( fdm->get_Phi(), 
-					     fdm->get_Theta(), 
                          heading * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
+        SG_LOG( SG_IO, SG_DEBUG, "  heading = " << heading );
-	} else if ( sentence == "GPGGA" ) {
+    } else
-	    // time
+    if (tokens[0] == "GPGGA" ) {
-	    end = msg.find(",", begin);
+        if ( tokens.size()<11)
-	    if ( end == string::npos ) {
+            return;
-		return false;
-	    }
 
-	    string utc = msg.substr(begin, end - begin);
+        // #1: time
-	    begin = end + 1;
+        const string& utc = tokens[1];
-	    SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
+        SG_LOG( SG_IO, SG_DEBUG, "  utc = " << utc );
-
-	    // lat val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lat_deg = atof( lat_str.substr(0, 2).c_str() );
-	    lat_min = atof( lat_str.substr(2).c_str() );
-
-	    // lat dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lat_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
 
+        // #2: lat val
+        lat_deg = atof( tokens[2].substr(0, 2).c_str() );
+        lat_min = atof( tokens[2].substr(2).c_str() );
         lat = lat_deg + ( lat_min / 60.0 );
-	    if ( lat_dir == "S" ) {
+
+        // #3: lat dir
+        if ( tokens[4] == "S" )
             lat *= -1;
-	    }
 
-	    // fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
+        mFdm.set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
-
-	    // lon val
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_str = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    lon_deg = atof( lon_str.substr(0, 3).c_str() );
-	    lon_min = atof( lon_str.substr(3).c_str() );
-
-	    // lon dir
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string lon_dir = msg.substr(begin, end - begin);
-	    begin = end + 1;
 
+        // #4: lon val
+        lon_deg = atof( tokens[4].substr(0, 3).c_str() );
+        lon_min = atof( tokens[4].substr(3).c_str() );
         lon = lon_deg + ( lon_min / 60.0 );
-	    if ( lon_dir == "W" ) {
+
+        // #5: lon dir
+        if ( tokens[5] == "W" )
             lon *= -1;
-	    }
 
-	    // fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
+        mFdm.set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
-	    SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
+        SG_LOG( SG_IO, SG_DEBUG, "  lat = " << lat << ", lon = " << lon );
 
-	    // junk
+        // #6: junk
-	    end = msg.find(",", begin);
+        SG_LOG( SG_IO, SG_DEBUG, "  junk = " << tokens[6] );
-	    if ( end == string::npos ) {
-		return false;
-	    }
 
-	    string junk = msg.substr(begin, end - begin);
+        // #7: junk
-	    begin = end + 1;
+        SG_LOG( SG_IO, SG_DEBUG, "  junk = " << tokens[7] );
-	    SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
-	    // junk
+        // #8: junk
-	    end = msg.find(",", begin);
+        SG_LOG( SG_IO, SG_DEBUG, "  junk = " << tokens[8] );
-	    if ( end == string::npos ) {
-		return false;
-	    }
 
-	    junk = msg.substr(begin, end - begin);
+        // #9: altitude
-	    begin = end + 1;
+        double altitude = atof( tokens[9].c_str() );
-	    SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
-	    // junk
+        // #10: altitude unit
-	    end = msg.find(",", begin);
+        const string& alt_units = tokens[10];
-	    if ( end == string::npos ) {
-		return false;
-	    }
 
-	    junk = msg.substr(begin, end - begin);
+        if ( alt_units != "F" && alt_units != "f" ) {
-	    begin = end + 1;
-	    SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
-
-	    // altitude
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string alt_str = msg.substr(begin, end - begin);
-	    altitude = atof( alt_str.c_str() );
-	    begin = end + 1;
-
-	    // altitude units
-	    end = msg.find(",", begin);
-	    if ( end == string::npos ) {
-		return false;
-	    }
-    
-	    string alt_units = msg.substr(begin, end - begin);
-	    begin = end + 1;
-
-	    if ( alt_units != "F" ) {
             altitude *= SG_METER_TO_FEET;
         }
 
-	    fdm->set_Altitude( altitude );
+        mFdm.set_Altitude( altitude );
-    
- 	    SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 
+        SG_LOG( SG_IO, SG_DEBUG, " altitude  = " << altitude );
     }
-
-	// printf("%.8f %.8f\n", lon, lat);
-
-	begin = begin_line;
-	end_line = msg.find("\n", begin_line);
-    }
-
-    return true;
 }
 
 
@@ -509,28 +383,33 @@ bool FGNMEA::open() {
 bool FGNMEA::process() {
     SGIOChannel *io = get_io_channel();
 
-    if ( get_direction() == SG_IO_OUT ) {
+    if ( get_direction() == SG_IO_OUT )
+    {
+        // process output
         gen_message();
-	if ( ! io->write( buf, length ) ) {
+        if ((!mNmeaSentence.empty())&&
+            (!io->write( mNmeaSentence.c_str(), mNmeaSentence.length() )))
+        {
             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
-	    return false;
         }
-    } else if ( get_direction() == SG_IO_IN ) {
+        mNmeaSentence = "";
-	if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
-	    parse_message();
-	} else {
-	    SG_LOG( SG_IO, SG_WARN, "Error reading data." );
-	    return false;
     }
-	if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
+    else
-	    parse_message();
+    if ( get_direction() == SG_IO_IN )
+    {
+        // process input lines (up to two lines per cycle)
+        for (int i=0;i<2;i++)
+        {
+            if ( (mLength = io->readline( mBuf, FG_MAX_MSG_SIZE )) > 0 ) {
+                parse_line();
             } else {
+                printf("Error reading data!\n");
                 SG_LOG( SG_IO, SG_WARN, "Error reading data." );
-	    return false;
+            }
         }
     }
 
-    return true;
+    return true; // return value is unused
 }
 
 
@@ -540,9 +419,5 @@ bool FGNMEA::close() {
 
     set_enabled( false );
 
-    if ( ! io->close() ) {
+    return io->close();
-	return false;
-    }
-
-    return true;
 }

src/Network/nmea.hxx

@@ -1,4 +1,4 @@
-// nmea.hxx -- NMEA protocal class
+// nmea.hxx -- NMEA protocol class
 //
 // Written by Curtis Olson, started November 1999.
 //
@@ -24,7 +24,6 @@
 #ifndef _FG_NMEA_HXX
 #define _FG_NMEA_HXX
 
-
 #include <simgear/compiler.h>
 
 #include <string>
@@ -33,30 +32,49 @@
 
 class FlightProperties;
 
+namespace NMEA
+{
+    // supported NMEA messages
+    const unsigned int GPRMC = (1<<0);
+    const unsigned int GPGGA = (1<<1);
+    const unsigned int GPGSA = (1<<2);
+
+    const unsigned int SET   = (GPRMC|GPGGA|GPGSA);
+}
+
 class FGNMEA : public FGProtocol {
 
-    char buf[ FG_MAX_MSG_SIZE ];
+protected:
-    int length;
+    char mBuf[FG_MAX_MSG_SIZE];
-    FlightProperties* fdm;
+    unsigned int mLength;
-public:
+    unsigned int mNmeaMessages;
+    FlightProperties mFdm;
+    const char* mLineFeed;
+    string mNmeaSentence;
 
+    void add_with_checksum(char *sentence, unsigned int buf_size);
+
+    // process a single NMEA line
+    void parse_line();
+
+    // generate output message(s)
+    virtual bool gen_message();
+
+    // process a single NMEA sentence
+    virtual void parse_message(const std::vector<std::string>& tokens);
+
+public:
             FGNMEA();
             ~FGNMEA();
 
-    bool gen_message();
-    bool parse_message();
-
     // open hailing frequencies
-    bool open();
+    virtual bool open();
 
     // process work for this port
-    bool process();
+    virtual bool process();
 
     // close the channel
-    bool close();
+    virtual bool close();
 };
 
-
 #endif // _FG_NMEA_HXX
-
-