Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox: [147] gearbox/trunk/submitted/gbxgarminacfr
|
From: <rus...@us...> - 2008-06-03 13:53:41
|
Revision: 147
http://gearbox.svn.sourceforge.net/gearbox/?rev=147&view=rev
Author: russo2503v
Date: 2008-06-03 06:53:43 -0700 (Tue, 03 Jun 2008)
Log Message:
-----------
big cleanup. removed frame assembly. untested on hardware
Modified Paths:
--------------
gearbox/trunk/submitted/gbxgarminacfr/doc.dox
gearbox/trunk/submitted/gbxgarminacfr/driver.cpp
gearbox/trunk/submitted/gbxgarminacfr/driver.h
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h
gearbox/trunk/submitted/gbxgarminacfr/test/test.cpp
Modified: gearbox/trunk/submitted/gbxgarminacfr/doc.dox
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/doc.dox 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/doc.dox 2008-06-03 13:53:43 UTC (rev 147)
@@ -56,6 +56,151 @@
$ cmake -DICEUTIL_HOME=/home/myuser/install .
@endverbatim
+@section gbx_library_gbxgarminacfr What is EPE?
+
+Bits of information assembled by Duncan Mercer.
+
+A quote from Garmin Engineering:
+
+The EPE is an estimation based upon the information the receiver
+can determine. SA consists of artificial clock errors and
+artificial ephemeral errors. Both of these effects, as well as
+atmospheric effects, can result in a positional area of uncer-
+tainty, which can be measured and will add to the receivers EPE.
+Bias errors cannot be measured and will typically not be detected
+in the EPE calculation.
+
+The 12XL will typically have a better EPE than other units due to
+the 12 channel correlator and the use of all tracked satellites
+in the positional computation.
+
+EPE is an estimation, rather than a measurement, but all
+measurable factors are used in the estimation algorithm. We
+consider the details of our EPE and FOM calculations proprietary.
+
+We calculate EPE our own way. URE and HDOP are definitely
+significant factors in the calculation. We calculate an over-
+determined solution, and fully understand the characteristics of
+SA, and are able (in our opinion) to provide for a better
+estimate of current position error than the simplistic calcula-
+tions will indicate.
+
+Many folks have and will demand to know our specific calcula-
+tions, but we consider these to be proprietary and we do not
+release the specific formulas. This is similar in our FOM
+calculation, we use a lot of finesse in our software which other
+manufacturers have not been able to duplicate. This is further
+evidenced by Dr. Wilson's reports on our accuracy compared to
+other receivers. If the tests were performed, I believe you would
+see closer correlation between our EPE values and actual errors,
+as compared to other manufacturers units.
+@endverbatim
+
+From Joe Mehaffey:
+
+@verbatim
+One experimenter has found that about 2/3 of the time, the
+ACTUAL error is less than the EPE readout on a Garmin 45 GPS unit
+tested. Your figures may vary. So.. EPE really is an estimate
+not a guarantee or a measurement.
+@endverbatim
+
+Sam Wormley offers the following additional information on EPE:
+
+@verbatim
+Garmin does not give an explanation for the EPE calculation
+displayed on a number of their handheld receivers. EPE is an
+acronym for "Estimated Position Error". See page 18 of the GPS
+12XL Owner's Manual and Reference. The behavior of EPE is not
+identical on all of Garmin's receivers... some reflect the dgps
+mode and others do not.
+
+A number of sources indicate the error in position can be
+estimated by the simple calculation DOP * URA with the former
+calculated in the GPS receiver from the geometry of the satel-
+lites used in the position solution and the later derived and
+computed from the GPS message transmitted by each satellite.
+
+DOP - Dilution of Precision - TDOP, PDOP, HDOP, VDOP, etc. are,
+in effect, multipliers that, when used with some measure of
+positioning statistic, can give you a realistic expectation of
+probability of error.
+
+URA - User Range Accuracy - this is a quantity that is transmit-
+ted in the navigation message that is the predicted (not
+measured) statistical ranging accuracy. Since it is defined for
+SPS (Standard Positioning Service), it includes SA.
+@endverbatim
+
+More from Sam Wormley (http://www.cnde.iastate.edu/gps.html)
+
+@verbatim
+The NMEA sentence $GPGGA recorded periodically over at least a
+twenty-four-hour period may provide enough data to draw a
+conclusion. This is a good experiment for all to try who have
+the capability to capture data.... you will learn much.
+
+Many of you may have better approaches than mine to gather and
+analyse data to resolve the EPE issue.... and, of course it goes
+without saying, we would hold Garmin in high[er] esteem if Garmin
+would give a complete technical discription of the calculation.
+And... if that technical language is over the typical readership
+heads, I'm sure there are many qualified to translate.
+@endverbatim
+
+Bill Stone wrote [March 1997]:
+
+@verbatim
+What does Garmin mean by EPE?
+
+EPE = HDOP * URA (1-sigma) would give a measure of the 68%
+confidence circle, i.e., 68% of your position fixes would fall
+within and 32% would fall outside.
+
+EPE = HDOP * URA (1-sigma) * 2 gives a measure of the 95%
+confidence circle, i.e., 95% of your position fixes would fall
+within and 5% would fall outside.
+
+EPE = HDOP * URA (1-sigma) * 0.73 gives a measure of the 50%
+confidence circle, i.e., 50% of your position fixes would fall
+within and 50% would fall outside. This is also known in the
+literature as CEP (Circular Error Probable).
+
+This latter expression is what several of you have indicated that
+you think Garmin's EPE calculation is for the Garmin 12XL. There
+may also be an "EPE filter" to minimize the EPE figure from
+jumping about to much.
+
+There should be a "reasonable correlation" between HDOP and outer
+radial position from a known geodetic point (could be determined
+over a long period of time by statisical averaging). I use the
+word "reasonable" because the comparison involves the points
+wandering around mostly within a circle of confidence, whereas
+HDOP will relate to the radius of that circle of confidence.
+@endverbatim
+
+To further complicate the analysis, witout an absolutely clear
+view of the sky, you can expect the HDOP to take jumps as
+satellites are obstructed from view. This might not be a bad
+idea to collect data under conditions less than desireable. John
+Franke reports:
+
+@verbatim
+It is interesting to see what happens under certain
+alinements. I recently had a case where the receiver (6
+channel, parallel) was tracking 4 satellites but was doing
+2D NAV and the HDOP was 32. A quick look at the satellite
+position screen showed the 4 tracked satellites to be in a
+straight line. There were other satellites available but
+the view to them was blocked by buildings. So, knowing the
+orbits repeat but are several minutes earlier each day, I
+watched the next day to see the alinement develop and the
+PDOP change. In less than twenty minutes the receiver went
+from 3D NAV with a PDOP of 2.3 to 2D NAV and a HDOP of 30
+back to 3D NAV and PDOP of 2.4! Quite impressive and
+educational to observe.
+@endverbatim
+
*/
Modified: gearbox/trunk/submitted/gbxgarminacfr/driver.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/driver.cpp 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/driver.cpp 2008-06-03 13:53:43 UTC (rev 147)
@@ -11,8 +11,7 @@
#include <iostream>
#include <sstream>
#include <gbxsickacfr/gbxutilacfr/gbxutilacfr.h>
-// #include <IceUtil/IceUtil.h>
-// #include <gbxsickacfr/gbxiceutilacfr/gbxiceutilacfr.h>
+#include <gbxgarminacfr/gbxgpsutilacfr/nmea.h>
#include <cstdlib>
#include <sys/time.h>
#include <time.h>
@@ -23,6 +22,133 @@
using namespace std;
using namespace gbxgarminacfr;
+///////////////////////////////////////
+
+namespace {
+
+// Get the useful bits from a GGA message
+GenericData* extractGgaData( gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
+{
+ GgaData* data = new GgaData;
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the tokens in the GGA message
+ enum GgaTokens{MsgType=0,UTC,Lat,LatDir,Lon,LonDir,FixType,
+ NSatsUsed,HDOP,Hgt,M1,GeoidHgt,M2,DiffAge,DiffId};
+
+ // fix type
+ switch (msg.getDataToken(FixType)[0])
+ {
+ case '0':
+ data->fixType = Invalid;
+ // NOTE: not processing the rest!
+ return data;
+ case '1':
+ data->fixType = Autonomous;
+ break;
+ case '2':
+ data->fixType = Differential;
+ break;
+ }
+
+ //UTC time
+ sscanf(msg.getDataToken(UTC).c_str(),"%02d%02d%lf",
+ &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
+ //position
+ int deg;
+ double min;
+ double dir;
+
+ //latitude
+ sscanf(msg.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
+ dir = (*msg.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
+ data->latitude=dir*(deg+(min/60.0));
+ //longitude
+ sscanf(msg.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
+ dir = (*msg.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
+ data->longitude=dir*(deg+(min/60.0));
+
+ //number of satellites in use
+ data->satellites = atoi(msg.getDataToken(NSatsUsed).c_str());
+
+ //altitude
+ data->altitude=atof(msg.getDataToken(Hgt).c_str());
+
+ //geoidal Separation
+ data->geoidalSeparation=atof(msg.getDataToken(GeoidHgt).c_str());
+
+ return data;
+}
+
+// VTG provides velocity and heading information
+GenericData* extractVtgData( gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
+{
+ VtgData* data = new VtgData;
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the VTG message items
+ enum VtgTokens{MsgType=0,HeadingTrue,T,HeadingMag,M,SpeedKnots,
+ N,SpeedKPH,K,ModeInd};
+
+ //Check for an empty string. Means that we are not moving
+ //When the message has empty fields tokeniser skips so we get the next field inline.
+ if(msg.getDataToken(HeadingTrue)[0] == 'T' ) {
+ data->headingTrue=0.0;
+ data->headingMagnetic=0.0;
+ data->speed=0.0;
+ data->climbRate=0.0;
+ // NOTE: not processing the rest!
+ return data;
+ }
+
+ // true heading
+ double headingRad = DEG2RAD(atof(msg.getDataToken(HeadingTrue).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingTrue=headingRad;
+
+ // magnetic heading
+ headingRad = DEG2RAD(atof(msg.getDataToken(HeadingMag).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingMagnetic=headingRad;
+
+ //speed - converted to m/s
+ data->speed=atof(msg.getDataToken(SpeedKPH).c_str());
+ data->speed*=(1000/3600.0);
+
+ //set to zero
+ data->climbRate=0.0;
+
+ return data;
+}
+
+// RME message. This one is garmin specific... Give position error estimates
+// See doc.dox for a discussion of the position errors as reported here.
+// Essentially the EPE reported by the garmin is a 1 sigma error (RMS) or a
+// 68% confidence bounds.
+GenericData* extractRmeData( gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
+{
+ RmeData* data = new RmeData;
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the RME message items
+ enum RmeTokens{MsgType=0,HError,M1,VError,M2,EPE,M3};
+
+ data->horizontalPositionError = atof(msg.getDataToken(HError).c_str());
+ data->verticalPositionError = atof(msg.getDataToken(VError).c_str());
+
+ return data;
+}
+
+}
+
+///////////////////////////////////////
+
bool
Config::isValid() const
{
@@ -35,7 +161,9 @@
Config::toString() const
{
std::stringstream ss;
- ss << "Garmin driver config: device="<<device;
+ ss << "Garmin driver config: " <<
+ "\tdevice="<<device <<
+ "\twill read sentences: GPGGA="<<readGga<<" GPVTG="<<readVtg<<" PGRME="<<readRme;
return ss.str();
}
@@ -51,7 +179,7 @@
if ( !config_.isValid() )
{
stringstream ss;
- ss << __func__ << "(): Invalid config: " << config_.toString();
+ ss << "Invalid config: " << config_.toString();
throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO, ss.str() );
}
@@ -59,12 +187,10 @@
ssDebug << "Connecting to GPS on serial port " << config_.device;
tracer_.debug( ssDebug.str() );
- // there's no need to make this configurable
-// int baud = context_.properties().getPropertyAsIntWidDefault( prefix+"Baud", 4800 );
+ // BAUD rate is dictated by the NMEA standard.
int baud = 4800;
- // according to Duncan, the first 5 initialization messages come in 1 sec.
- // 2 secs should be conservative.
+ // the first 5 initialization messages come in 1 sec. Therefore, 2 secs should be conservative.
serial_.reset( new gbxserialacfr::Serial( config_.device, baud, gbxserialacfr::Serial::Timeout(2,0) ) );
init();
@@ -75,23 +201,12 @@
disableDevice();
}
-void
-Driver::read( Data &data )
-{
- return readFrame( data );
-}
-
void
Driver::init()
{
- //Make sure that we clear our internal data structures
- memset((void*)(&nmeaMessage_) , 0 , sizeof(nmeaMessage_));
- memset((void*)(&gpsData_) , 0 , sizeof(gpsData_));
-
try {
enableDevice();
//TODO Need to check here that we have been successful.
- clearFrame();
}
catch ( const gbxserialacfr::SerialException &e )
{
@@ -102,12 +217,9 @@
}
}
-//*****************************************************************************
-
void
Driver::enableDevice()
{
-
//Create the messages that we are going to send and add the checksums
//Note that the checksum field is filled with 'x's before we start
gbxgpsutilacfr::NmeaMessage DisableAllMsg("$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum);
@@ -128,277 +240,131 @@
sleep(1);
}
-
-
-//***********************************************************************
void
Driver::disableDevice()
{
-
//Simply send the no messages command!
gbxgpsutilacfr::NmeaMessage DisableAllMsg("$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum);
serial_->writeString(DisableAllMsg.sentence());
}
+std::auto_ptr<GenericData>
+Driver::read()
+{
+ std::auto_ptr<GenericData> genericData;
+ gbxgpsutilacfr::NmeaMessage nmeaMessage;
+ // Make sure that we clear our internal data structures
+ // alexm: is this necessary? should NmeaMessage do it for itself?
+ memset((void*)(&nmeaMessage) , 0 , sizeof(nmeaMessage));
+ int nmeaExceptionCount = 0;
+ int nmeaFailChecksumCount = 0;
-//****************************************************************************
-// Read one complete frame of data. IE all the messages that we need before returning the data.
-
-void
-Driver::readFrame(Data& GpsData)
-{
+ while ( true )
+ {
+ string serialData;
- string serial_data;
- int gpsMsgNotYetGotFrameCount = 0;
-
- //How many messages are we looking for to make our frame
- const int N_MSGS_IN_FRAME = 3;
-
- //Clear our data before we start trying to assemble the frame
- clearFrame();
-
-
- while(! haveCompleteFrame() ){
-
+ //
// This will block up to the timeout
+ //
tracer_.debug( "Driver::read(): calling serial_->readLine()", 10 );
- int ret = serial_->readLine(serial_data);
- tracer_.debug( serial_data, 10 );
-
-// timeOfRead_ = IceUtil::Time::now();
-// gbxsickacfr::gbxiceutilacfr::now( timeOfReadSec_, timeOfReadUsec_ );
+ int ret = serial_->readLine(serialData);
+
+ // get time stamp right away (Linux only!)
timeval now;
if ( gettimeofday( &now, 0 ) != 0 ) {
stringstream ss;
ss << "Pproblem getting timeofday: " << strerror(errno) << endl;
throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,ss.str() );
}
-
- if ( ret<0 ) {
- throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO, "Driver: Timeout reading from serial port" );
- }
- if(ret==0) {
+ // zero tolerance to serial errors
+ if ( ret<0 )
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO, "Driver: Timeout reading from serial port" );
+ if ( ret==0 )
throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: Read 0 bytes from serial port");
- }
//
// We successfully read something from the serial port
//
+ tracer_.debug( serialData, 10 );
-
//Put it into the message object and checksum the data
- static int nmeaExceptionCount =0;
- try{
- //This throws if it cannot find the * to deliminate the checksum field
- nmeaMessage_.setSentence(serial_data.c_str(),gbxgpsutilacfr::TestChecksum);
+ try {
+ // This throws if it cannot find the * to deliminate the checksum field
+ nmeaMessage.setSentence( serialData.c_str(), gbxgpsutilacfr::TestChecksum );
}
- catch (gbxgpsutilacfr::NmeaException &e){
- //Don't throw if only occasional messages are missing the checksums
- if(nmeaExceptionCount++ < 3) {return;}
+ catch ( const gbxgpsutilacfr::NmeaException& e ) {
+ //Don't throw on isolated checksum problems
+ if ( nmeaExceptionCount++ < 3 ) {
+ continue;
+ }
stringstream ss;
- ss << "MainThread: Problem reading from GPS: " << e.what();
+ ss << "Problem reading from GPS: " << e.what();
tracer_.error( ss.str() );
throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,ss.str());
}
nmeaExceptionCount = 0;
- //Only populate the data structures if our message passes the checksum!
- static int nmeaFailChecksumCount =0;
- if(nmeaMessage_.haveValidChecksum()){
- nmeaFailChecksumCount = 0;
- addDataToFrame();
- }else{
- if(nmeaFailChecksumCount++ >= 3){ //Dont throw an exception on the first failed checksum.
- throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: more than 3 sequential messages failed the checksum\n");
- }else{
- tracer_.error("Driver: Single message failed checksum. Not throwing an exception yet!\n" );
+ // Only populate the data structures if our message passes the checksum!
+ if ( !nmeaMessage.haveValidChecksum() ) {
+ // Dont throw an exception on the first failed checksum.
+ if ( nmeaFailChecksumCount++ < 3 ) {
+ tracer_.warning("Gps driver: Single message failed checksum. Not throwing an exception yet!" );
+ continue;
}
+ else {
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"More than 3 sequential messages failed the checksum");
+ }
}
+ nmeaFailChecksumCount = 0;
- //Make sure that we do not wait for ever trying to get a frame of data
- //Note that we might need to skip the N * $PGRMO messages echoed back from receiver when starting
- //As well as the N * messages that we are looking for
- if(gpsMsgNotYetGotFrameCount++ >= (N_MSGS_IN_FRAME * 3)){
- throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: Not able to assemble a complete data frame\n");
+ //First split up the data fields in the string we have read.
+ nmeaMessage.parseTokens();
+
+ //We should not get any messages with failed checksums, but just in case
+ if( nmeaMessage.haveTestedChecksum() && (!nmeaMessage.haveValidChecksum()) ) {
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: Message fails checksum");
}
+
+ //And then find out which type of messge we have recieved...
+ string MsgType = nmeaMessage.getDataToken(0);
+
+ if ( MsgType == "$GPGGA" ) {
+ tracer_.debug("got GGA message",4);
+ if ( !config_.readGga )
+ continue;
+ genericData.reset( extractGgaData( nmeaMessage, now.tv_sec, now.tv_usec ) );
+ break;
+ }
+ else if ( MsgType == "$GPVTG" ) {
+ tracer_.debug("got VTG message",4);
+ if ( !config_.readVtg )
+ continue;
+ genericData.reset( extractVtgData( nmeaMessage, now.tv_sec, now.tv_usec ) );
+ break;
+ }
+ else if ( MsgType == "$PGRME" ) {
+ tracer_.debug("got RME message",4);
+ if ( !config_.readRme )
+ continue;
+ genericData.reset( extractRmeData( nmeaMessage, now.tv_sec, now.tv_usec ) );
+ break;
+ }
+ else if ( MsgType == "$PGRMO" ) {
+ //This message is sent by us to control msg transmission and then echoed by GPS
+ //So we can just ignore it
+ continue;
+ }
+ else {
+ // if we get here the msg is unknown
+ stringstream ErrMsg;
+ ErrMsg << "Message type unknown " << MsgType <<endl;
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,ErrMsg.str());
+ }
}
- tracer_.debug("GPS got a complete frame\n", 10 );
-
- // Hand the data back to the outside world
- GpsData=gpsData_;
+ return genericData;
}
-
-
-
-
-//**********************************************************************************
-
-void
-Driver::addDataToFrame()
-{
- //First split up the data fields in the string we have read.
- nmeaMessage_.parseTokens();
-
- //We should not be being passed any messages with failed checksums, but just in case
- if(nmeaMessage_.haveTestedChecksum() && (!nmeaMessage_.haveValidChecksum())){
- throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: Message fails checksum");
- }
-
- //And then find out which type of messge we have recieved...
- string MsgType = nmeaMessage_.getDataToken(0);
-
- if(MsgType == "$GPGGA"){
- tracer_.debug("got GGA message\n",4);
- extractGGAData();
- haveGGA_ = true;
- return;
- }else if(MsgType == "$GPVTG"){
- tracer_.debug("got VTG message\n",4);
- extractVTGData();
- haveVTG_ = true;
- return;
- }else if(MsgType == "$PGRME"){
- tracer_.debug("got RME message\n",4);
- extractRMEData();
- haveRME_ = true;
- return;
- }else if(MsgType == "$PGRMO"){
- //This message is sent by us to control msg transmission and then echoed by GPS
- //So we can just ignore it
- return;
- }else{
- // if we get here the msg is unknown
- stringstream ErrMsg;
- ErrMsg << "Message type unknown " << MsgType <<endl;
- throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,ErrMsg.str());
- }
-}
-
-
-//**************************************************************************************
-// Get the useful bits from a GGA message
-
-void
-Driver::extractGGAData(void){
-
- //Names for the tokens in the GGA message
- enum GGATokens{MsgType=0,UTC,Lat,LatDir,Lon,LonDir,FixType,
- NSatsUsed,HDOP,Hgt,M1,GeoidHgt,M2,DiffAge,DiffId};
-
- //cout << nmeaMessage_.sentence()<<endl;
-
- //position fix type
- switch (nmeaMessage_.getDataToken(FixType)[0])
- {
- case '0':
- gpsData_.positionType = GpsPositionTypeNotAvailable;
- return;
- case '1':
- gpsData_.positionType = GpsPositionTypeAutonomous;
- break;
- case '2':
- gpsData_.positionType = GpsPositionTypeDifferential;
- break;
- }
-
-// gpsData_.timeStamp = orcaice::toOrcaTime (timeOfRead_);
-// gbxiceutil::timeFromIceUtil( timeOfRead_, gpsData_.timeStampSec, gpsData_.timeStampUsec );
- gpsData_.timeStampSec = timeOfReadSec_;
- gpsData_.timeStampUsec = timeOfReadUsec_;
-
- //UTC time
- sscanf(nmeaMessage_.getDataToken(UTC).c_str(),"%02d%02d%lf",
- &gpsData_.utcTimeHrs, &gpsData_.utcTimeMin, &gpsData_.utcTimeSec );
- //position
- int deg;
- double min;
- double dir;
-
- //latitude
- sscanf(nmeaMessage_.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
- dir = (*nmeaMessage_.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
- gpsData_.latitude=dir*(deg+(min/60.0));
- //longitude
- sscanf(nmeaMessage_.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
- dir = (*nmeaMessage_.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
- gpsData_.longitude=dir*(deg+(min/60.0));
-
- //number of satellites in use
- gpsData_.satellites = atoi(nmeaMessage_.getDataToken(NSatsUsed).c_str());
-
- //altitude
- gpsData_.altitude=atof(nmeaMessage_.getDataToken(Hgt).c_str());
-
- //geoidal Separation
- gpsData_.geoidalSeparation=atof(nmeaMessage_.getDataToken(GeoidHgt).c_str());
-
-
- //cout << "Lat " << GpsData_.latitude << " Long " << GpsData_.longitude ;
- //cout << " Hght "<< GpsData_.altitude << " Geoid "<< GpsData_.geoidalSeparation << endl;
-
- // Set flag
-
- return;
-}
-
-
-//********************************************************************
-// VTG provides velocity and heading information
-void
-Driver::extractVTGData(void){
-
- //Names for the VTG message items
- enum VTGTokens{MsgType=0,HeadingTrue,T,HeadingMag,M,SpeedKnots,
- N,SpeedKPH,K,ModeInd};
-
- //Check for an empty string. Means that we are not moving
- //When the message has empty fields tokeniser skips so we get the next field inline.
- if(nmeaMessage_.getDataToken(HeadingTrue)[0] == 'T' ){
- gpsData_.speed=0.0;
- gpsData_.climbRate=0.0;
- gpsData_.heading=0.0;
- return;
- }
-
- //heading
- double headingRad = DEG2RAD(atof(nmeaMessage_.getDataToken(HeadingTrue).c_str()));
- NORMALISE_ANGLE( headingRad );
- gpsData_.heading=headingRad;
- //speed - converted to m/s
- gpsData_.speed=atof(nmeaMessage_.getDataToken(SpeedKPH).c_str());
- gpsData_.speed*=(1000/3600.0);
- //set to zero
- gpsData_.climbRate=0.0;
-
- //cout << nmeaMessage_.sentence() << endl;
- // cout << "head "<< RAD2DEG(GpsData_.heading) << " speed " << GpsData_.speed << endl;
-
- return;
-}
-
-
-//*********************************************************************************************
-// RME message. This one is garmin specific... Give position error estimates
-// See the file garminErrorPositionEstimate.txt for a discussion of the position errors as
-// reported here. Essentially the EPE reported by the garmin is a 1 sigma error (RMS) or a
-// 68% confidence bounds.
-
-void
-Driver::extractRMEData(void){
- //Names for the RME message items
- enum VTGTokens{MsgType=0,HError,M1,VError,M2,EPE,M3};
-
- gpsData_.horizontalPositionError = atof(nmeaMessage_.getDataToken(HError).c_str());
- gpsData_.verticalPositionError = atof(nmeaMessage_.getDataToken(VError).c_str());
-
- //cout << nmeaMessage_.sentence() << endl;
- //cout << "Herr " << GpsData_.horizontalPositionError << " Verr " << GpsData_.verticalPositionError<<endl;
-
- return;
-
-}
Modified: gearbox/trunk/submitted/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/driver.h 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/driver.h 2008-06-03 13:53:43 UTC (rev 147)
@@ -14,7 +14,6 @@
#include <gbxserialacfr/serial.h>
#include <gbxsickacfr/gbxutilacfr/tracer.h>
#include <gbxsickacfr/gbxutilacfr/status.h>
-#include <gbxgarminacfr/gbxgpsutilacfr/nmea.h>
#include <memory>
namespace gbxgarminacfr {
@@ -23,106 +22,211 @@
class Config
{
public:
- Config() {};
+ Config() :
+ readGga(true),
+ readVtg(true),
+ readRme(true) {};
+ //! Returns true if the configuration is sane.
bool isValid() const;
+ //! Returns human-readable configuration description.
std::string toString() const;
//! Serial device. e.g. "/dev/ttyS0"
std::string device;
+
+ //! Read PGGGA sentence
+ bool readGga;
+ //! Read PGVTG sentence
+ bool readVtg;
+ //! Read GPRME sentence
+ bool readRme;
};
+//! Possible types GenericData can contain
+enum DataType
+{
+ //! Contents of PGGGA message.
+ GpGga,
+ //! Contents of PGVTG message.
+ GpVtg,
+ //! Contents of PGRME message.
+ PgRme
+};
-//! Gps position types.
-//! Using Novatel codes here is probably not the best thing. With more
-//! thought it's probably possible to categorize these position types
-//! into more generic categories. For now, non-Novatel receivers should
-//! use the generic types listed first.
-enum PositionType {
+//! Possible Status Messages GenericData can contain
+enum StatusMessageType
+{
+ //! Nothing new, no message
+ NoMsg,
+ //! All good, but something to say
+ Ok,
+ //! Problem, likely to go away
+ Warning,
+ //! Problem, probably fatal
+ Fault
+};
+
+//! Generic data type returned by a read
+class GenericData
+{
+public:
+ virtual ~GenericData(){};
+ //! Returns data type.
+ virtual DataType type() const=0;
+ //! Status message type.
+ StatusMessageType statusMessageType;
+ //! Status message
+ std::string statusMessage;
+
+private:
+};
+
+//! GPS fix types.
+enum FixType
+{
//! Invalid or not available
- GpsPositionTypeNotAvailable,
+ Invalid,
//! Autonomous position
//! (This is the normal case for non-differential GPS)
- GpsPositionTypeAutonomous,
+ Autonomous,
//! Differentially corrected
- GpsPositionTypeDifferential,
- NovatelNone,
- NovatelFixedPos,
- NovatelFixedHeigth,
- NovatelFloatConv,
- NovatelWideLane,
- NovatelNarrowLane,
- NovatelDopplerVelocity,
- NovatelSingle,
- NovatelPsrDiff,
- NovatelWAAS,
- NovatelPropagated,
- NovatelOmnistar,
- NovatelL1Float,
- NovatelIonFreeFloat,
- NovatelNarrowFloat,
- NovatelL1Int,
- NovatelWideInt,
- NovatelNarrowInt,
- NovatelRTKDirectINS,
- NovatelINS,
- NovatelINSPSRSP,
- NovatelINSPSRFLOAT,
- NovatelINSRTKFLOAT,
- NovatelINSRTKFIXED,
- NovatelOmnistarHP,
- NovatelUnknown
+ Differential
};
-//! Gps data structure
-struct Data
+//! Fix data structure
+struct GgaData : public GenericData
{
+public:
+ DataType type() const { return GpGga; }
+
//! Time (according to the computer clock) when data was measured.
//! Number of seconds
int timeStampSec;
//! Time (according to the computer clock) when data was measured.
//! Number of microseconds
int timeStampUsec;
- //! UTC time (according to GPS device), reference is Greenwich.
+
+ //! UTC time (according to the GPS device), reference is Greenwich.
//! Hour [0..23]
int utcTimeHrs;
- //! UTC time (according to GPS device), reference is Greenwich.
+ //! UTC time (according to the GPS device), reference is Greenwich.
//! Minutes [0..59]
int utcTimeMin;
- //! UTC time (according to GPS device), reference is Greenwich.
+ //! UTC time (according to the GPS device), reference is Greenwich.
//! Seconds [0.0..59.9999(9)]
double utcTimeSec;
- //! Latitude (degrees)
+ //! Latitude [degrees]
double latitude;
- //! Longitude (degrees)
+ //! Longitude [degrees]
double longitude;
- //! Altitude (metres above ellipsoid)
+ //! Altitude [metres above ellipsoid]
double altitude;
- //! Horizontal position error: one standard deviation (metres)
- double horizontalPositionError;
- //! Vertical position error: one standard deviation (metres)
- double verticalPositionError;
-
- //! Heading/track/course with respect to true north (rad)
- double heading;
- //! Horizontal velocity (metres/second)
- double speed;
- //! Vertical velocity (metres/second)
- double climbRate;
-
+ //! Fix type.
+ FixType fixType;
+
//! Number of satellites
int satellites;
- int observationCountOnL1;
- int observationCountOnL2;
- //! Position type (see above)
- PositionType positionType;
- //! Geoidal Separation (metres)
+
+ //! Horizontal dilution of position [metres]
+ double horizontalDilutionOfPosition;
+
+ //! Height of geoid (mean sea level) above WGS84 ellipsoid [metres]
double geoidalSeparation;
};
+//! Vector track and speed over ground data structure.
+class VtgData : public GenericData
+{
+public:
+ DataType type() const { return GpVtg; }
-//! Garmin driver
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! Heading/track/course with respect to true North [rad]
+ double headingTrue;
+ //! Heading/track/course with respect to magnetic North [rad]
+ double headingMagnetic;
+ //! Horizontal velocity [metres/second]
+ double speed;
+ //! Vertical velocity [metres/second]
+ double climbRate;
+};
+
+ enum VTGTokens{MsgType=0,HError,M1,VError,M2,EPE,M3};
+
+//! Gps data structure
+class RmeData : public GenericData
+{
+public:
+ DataType type() const { return PgRme; }
+
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! Horizontal position error: one standard deviation [metres)]
+ double horizontalPositionError;
+ //! Vertical position error: one standard deviation [metres]
+ double verticalPositionError;
+};
+
+/*!
+
+Garmin GPS driver
+
+
+Referennces:
+- http://en.wikipedia.org/wiki/NMEA
+- http://www.gpsinformation.org/dale/interface.htm
+
+All Garmin receivers understand the latest standard which is called: 0183 version 2.0.
+
+This standard dictates a transfer rate of 4800 baud.
+
+Out of all the messages below, this driver can read only the following messages (sentences):
+- GPGGA
+- GPVTG
+- PGRME
+
+The sentences sent by Garmin receivers include:
+NMEA 2.0
+- GPBOD bearing, origin to destination - earlier G-12's do not transmit this
+- GPGGA fix data
+- GPGLL Lat/Lon data - earlier G-12's do not transmit this
+- GPGSA overall satellite reception data
+- GPGSV detailed satellite data
+- GPRMB minimum recommended data when following a route
+- GPRMC minimum recommended data
+- GPRTE route data
+- GPWPL waypoint data (this is bidirectional)
+
+NMEA 1.5 - some units do not support version 1.5
+- GPBOD bearing origin to destination - earlier G-12's do not send this
+- GPBWC bearing to waypoint using great circle route.
+- GPGLL lat/lon - earlier G-12's do not send this
+- GPRMC minimum recommend data
+- GPRMB minimum recommended data when following a route
+- GPVTG vector track and speed over ground
+- GPWPL waypoint data (only when active goto)
+- GPXTE cross track error
+
+In addition Garmin receivers send the following Proprietary Sentences:
+- PGRME (estimated error) - not sent if set to 0183 1.5
+- PGRMM (map datum)
+- PGRMZ (altitude)
+- PSLIB (beacon receiver control)
+
+*/
class Driver
{
@@ -133,44 +237,23 @@
//! gbxutilacfr::Tracer and gbxutilacfr::Status allow
//! (human-readable and machine-readable respectively) external
//! monitorining of the driver's internal state.
- Driver( const Config &config,
- gbxsickacfr::gbxutilacfr::Tracer &tracer,
- gbxsickacfr::gbxutilacfr::Status &status );
+ Driver( const Config& config,
+ gbxsickacfr::gbxutilacfr::Tracer& tracer,
+ gbxsickacfr::gbxutilacfr::Status& status );
~Driver();
-
//! Blocks till new data is available
- void read( Data &data );
+ std::auto_ptr<GenericData> read();
private:
void init();
- void addDataToFrame();
void enableDevice();
void disableDevice();
- int resetDevice();
- void extractGGAData();
- void extractVTGData();
- void extractRMEData();
- void clearFrame(){haveGGA_ = false; haveVTG_ = false; haveRME_ =false;};
- bool haveCompleteFrame(){return (haveGGA_ & haveVTG_ & haveRME_);};
- void readFrame( Data &data);
-
std::auto_ptr<gbxserialacfr::Serial> serial_;
- Data gpsData_;
- gbxgpsutilacfr::NmeaMessage nmeaMessage_;
- int timeOfReadSec_;
- int timeOfReadUsec_;
-
- //*** NOTE:- if we change the number of messages in the frame need to change
- //The N_MSGS_IN_FRAME in the readFrame fn...
- bool haveGGA_;
- bool haveVTG_;
- bool haveRME_;
-
Config config_;
gbxsickacfr::gbxutilacfr::Tracer& tracer_;
gbxsickacfr::gbxutilacfr::Status& status_;
Modified: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp 2008-06-03 13:53:43 UTC (rev 147)
@@ -103,7 +103,6 @@
default:
assert(true);
}
-
}
@@ -145,7 +144,6 @@
//failed the checksum!
return false;
-
}
@@ -188,7 +186,6 @@
//Put the byte values as upper case HEX back into the message
sprintf(sentence_ + loopCount + 1,"%02X",chkRunning);
-
}
@@ -208,5 +205,4 @@
//keep track of what we have done.
haveTokens_ = true;
-
}
Modified: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h 2008-06-03 13:53:43 UTC (rev 147)
@@ -68,54 +68,55 @@
// When using class to send data, need to add checksum, when reciving data need to test checksum
// Checksums are usually optional
- enum{TestChecksum, AddChecksum, DontTestOrAddChecksum};
+enum{TestChecksum, AddChecksum, DontTestOrAddChecksum};
// class SOEXPORT NmeaMessage{
- class NmeaMessage{
- public:
- NmeaMessage();
- NmeaMessage(const char *sentence, int testCheckSum = DontTestOrAddChecksum);
+class NmeaMessage
+{
+public:
+ NmeaMessage();
+ NmeaMessage(const char *sentence, int testCheckSum = DontTestOrAddChecksum);
- // Do we only have the raw string ?
- bool haveSentence(){return haveSentence_;};
- // Set up the internal data for a sentence
- void setSentence(const char *data, int testCheckSum = DontTestOrAddChecksum);
- // Have we parsed fields ?
- bool haveTokens(){return haveTokens_;};
- // have we a valid checksum ?
- bool haveValidChecksum(){return checkSumOK_;};
- // have we checked the checksum?
- bool haveTestedChecksum(){return haveCheckSum_;};
- // calculate the checksum from sentence
- // Note that this function may throw NMEA_Exception...
- bool testChecksumOk();
- // Return the raw sentence string
- const char * sentence(){return sentence_;};
- // Return a single data token as a string
- std::string& getDataToken(int i){return dataTokens_[i];};
+ // Set up the internal data for a sentence
+ void setSentence(const char *data, int testCheckSum = DontTestOrAddChecksum);
- // Return the number of fields
- int numDataTokens(){return dataTokens_.size();};
- //Tokenise the string that we received
- void parseTokens();
+ // Do we only have the raw string ?
+ bool haveSentence() const { return haveSentence_; };
+ // Have we parsed fields ?
+ bool haveTokens() const { return haveTokens_; };
+ // have we a valid checksum ?
+ bool haveValidChecksum() const { return checkSumOK_; };
+ // have we checked the checksum?
+ bool haveTestedChecksum()const { return haveCheckSum_; };
+ // calculate the checksum from sentence
+ // Note that this function may throw NMEA_Exception...
+ bool testChecksumOk();
+ // Return the raw sentence string
+ const char * sentence() { return sentence_; };
+ // Return a single data token as a string
+ std::string& getDataToken(int i) { return dataTokens_[i]; };
- private:
- void init();
- void addCheckSum();
- // Do we only have the raw string ?
- bool haveSentence_;
- // Have we parsed data into tokens ?
- bool haveTokens_;
- // Have we a checksum and is it valid?
- bool haveCheckSum_;
- bool checkSumOK_;
- // The raw sentence, allow for terminator
- char sentence_[MAX_SENTENCE_LEN+1];
- // The tokenised data
- std::vector<std::string> dataTokens_;
-
- };
+ // Return the number of fields
+ int numDataTokens() const { return dataTokens_.size(); };
+ //Tokenise the string that we received
+ void parseTokens();
+private:
+ void init();
+ void addCheckSum();
+ // Do we only have the raw string ?
+ bool haveSentence_;
+ // Have we parsed data into tokens ?
+ bool haveTokens_;
+ // Have we a checksum and is it valid?
+ bool haveCheckSum_;
+ bool checkSumOK_;
+ // The raw sentence, allow for terminator
+ char sentence_[MAX_SENTENCE_LEN+1];
+ // The tokenised data
+ std::vector<std::string> dataTokens_;
+};
+
}
#endif
Modified: gearbox/trunk/submitted/gbxgarminacfr/test/test.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/test/test.cpp 2008-06-02 03:04:54 UTC (rev 146)
+++ gearbox/trunk/submitted/gbxgarminacfr/test/test.cpp 2008-06-03 13:53:43 UTC (rev 147)
@@ -24,7 +24,7 @@
using namespace std;
//
-// Instantiates the laser driver, reads a few scans
+// Instantiates the driver, reads a messages
//
int main( int argc, char **argv )
{
@@ -33,7 +33,7 @@
string port = "/dev/ttyS0";
// Get some options from the command line
- while ((opt = getopt(argc, argv, "p:b:")) != -1)
+ while ((opt = getopt(argc, argv, "p:")) != -1)
{
switch ( opt )
{
@@ -42,14 +42,17 @@
break;
default:
cout << "Usage: " << argv[0] << " [-p port]" << endl << endl
- << "-p port\tPort the laser scanner is connected to. E.g. /dev/ttyS0" << endl;
+ << "-p port\tPort the device is connected to. E.g. /dev/ttyS0" << endl;
return 1;
}
}
- // Set up the laser's configuration
+ // Set up the device configuration
gbxgarminacfr::Config config;
config.device = port;
+ config.readGga = true;
+ config.readVtg = true;
+ config.readRme = true;
if ( !config.isValid() ) {
cout << "Test: Invalid device configuration structure: " << config.toString() << endl;
exit(1);
@@ -57,7 +60,7 @@
cout << "Using configuration: " << config.toString() << endl;
// Instantiate objects to handle messages from the driver
- const bool debug=false;
+ const bool debug = false;
gbxsickacfr::gbxutilacfr::TrivialTracer tracer( debug );
gbxsickacfr::gbxutilacfr::TrivialStatus status( tracer );
@@ -74,7 +77,7 @@
}
// Create data structure to store sensor data
- gbxgarminacfr::Data data;
+ std::auto_ptr<gbxgarminacfr::GenericData> data;
// Read a few times
const int numReads = 3;
@@ -82,7 +85,7 @@
{
try
{
- device->read( data );
+ data = device->read();
cout<<"Test: Got data "<<i+1<<" of "<<numReads<<endl;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|