Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[488] gearbox/trunk
|
From: <bo...@us...> - 2010-03-18 07:54:25
|
Revision: 488
http://gearbox.svn.sourceforge.net/gearbox/?rev=488&view=rev
Author: borax00
Date: 2010-03-18 07:54:14 +0000 (Thu, 18 Mar 2010)
Log Message:
-----------
MR Changes
Modified Paths:
--------------
gearbox/trunk/doc/history.dox
gearbox/trunk/src/gbxgarminacfr/driver.cpp
gearbox/trunk/src/gbxgarminacfr/driver.h
gearbox/trunk/src/gbxgarminacfr/test/test.cpp
gearbox/trunk/src/gbxsickacfr/driver.cpp
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/buffer.h
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/test/buffertest.cpp
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/thread.cpp
gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserver.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserver.h
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserverhealthchecks.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserversystem.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserversystem.h
gearbox/trunk/src/gbxsmartbatteryacfr/smartbattery.h
gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/test/shorttest.cpp
gearbox/trunk/src/gbxutilacfr/exceptions.cpp
gearbox/trunk/src/gbxutilacfr/status.cpp
gearbox/trunk/src/gbxutilacfr/status.h
gearbox/trunk/src/gbxutilacfr/subhealth.h
gearbox/trunk/src/gbxutilacfr/substatus.h
gearbox/trunk/src/gbxutilacfr/test/trivialstatustest.cpp
gearbox/trunk/src/gbxutilacfr/trivialstatus.cpp
gearbox/trunk/src/gbxutilacfr/trivialstatus.h
Added Paths:
-----------
gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
gearbox/trunk/src/gbxgarminacfr/nmeamessages.h
gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp
gearbox/trunk/src/gbxgarminacfr/nmeasentence.h
Removed Paths:
-------------
gearbox/trunk/src/gbxgarminacfr/nmea.cpp
gearbox/trunk/src/gbxgarminacfr/nmea.h
Modified: gearbox/trunk/doc/history.dox
===================================================================
--- gearbox/trunk/doc/history.dox 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/doc/history.dox 2010-03-18 07:54:14 UTC (rev 488)
@@ -21,15 +21,6 @@
Developers: as you make substantial changes in the code (add features, fix bugs, etc.), add an item in the section on changes. When the next version is released, this list will be copied into the release notes.
-POST 9.11 CHANGES
-
-@par Project wide
-
-- Build system
- - GBX_REQUIRE_LIB macro now uses a direct check for target existence.
- - new GBX_ADD_HEADER_ONLY_LIBRARY macro for handling dependencies on header-only libraries.
-
-
@section gbx_doc_history_todo To-Do List for Next Release
None.
@@ -38,10 +29,24 @@
@par Project wide
+- Build system
+ - GBX_REQUIRE_LIB macro now uses a direct check for target existence.
+ - new GBX_ADD_HEADER_ONLY_LIBRARY macro for handling dependencies on header-only libraries.
+
@par New libraries
@par Updated libraries
+- libGbxUtilAcfr
+ - Status class:
+ - functions for changing the state, initialising(), working(), finalising(), no longer
+ change the message. A separate call to one of the health-related function is needed if a change is
+ required.
+ - Renamed health SubsystemFault to SubsystemCritical, and function fault() to critical().
+ - defined enums for component state and health.
+ - added a function to access the component status.
+ - removed functions to specify "infrastructure state" which is now expected to be an optional subsystem.
+
@par Retired libraries
@section gbx_doc_history_911 Changes in Release 9.11
@@ -55,6 +60,9 @@
@par Updated libraries
+- libGbxSmartBatteryAcfr (tobi)
+ - Oceanserver class now has its own thread which reads continuously from the hardware
+
- libGbxNovatelAcfr:
- Bugfix: matched GPS status/solution enums to Novatel's internal types, there were gaps due to reserved values before.
Note: this will create mismatches with old log-files (not data, status/solution-type only)! (MichaelM, patch by Ian Mahon)
Modified: gearbox/trunk/src/gbxgarminacfr/driver.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.cpp 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/driver.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -12,14 +12,13 @@
#include <iostream>
#include <sstream>
#include <gbxutilacfr/gbxutilacfr.h>
-#include "nmea.h"
+#include "nmeasentence.h"
#include <cstdlib>
#include <cstring>
#include <sys/time.h>
#include <time.h>
#include <errno.h>
#include <stdio.h>
-
#include "driver.h"
using namespace std;
@@ -28,243 +27,27 @@
namespace gbxgarminacfr {
-namespace {
-
-// Get the useful bits from a GGA message
-GenericData* extractGgaData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
+bool
+Config::isValid( std::string &reason ) const
{
- std::auto_ptr<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};
-
- //UTC time
- sscanf(msg.getDataToken(UTC).c_str(),"%02d%02d%lf",
- &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
-
- //number of satellites in use
- data->satellites = atoi(msg.getDataToken(NSatsUsed).c_str());
-
- // fix type
- switch ( msg.getDataToken(FixType)[0] )
+ if ( device.empty() )
{
- case '0':
- data->fixType = Invalid;
- // NOTE: not processing the rest!
- data->latitude = 0.0;
- data->longitude = 0.0;
- data->altitude = 0.0;
- data->geoidalSeparation = 0.0;
- return data.release();
- case '1':
- data->fixType = Autonomous;
- break;
- case '2':
- data->fixType = Differential;
- break;
- default :
- throw gbxutilacfr::Exception( ERROR_INFO, "GGA sentence contains unknown GPS fix type: '"+msg.getDataToken(FixType)+"'" );
+ reason = "device not set";
+ return false;
}
-
- //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));
-
- //altitude
- data->isAltitudeKnown = !msg.isDataTokenEmpty(Hgt);
- if ( data->isAltitudeKnown )
- data->altitude=atof(msg.getDataToken(Hgt).c_str());
-
- //geoidal Separation
- data->geoidalSeparation=atof(msg.getDataToken(GeoidHgt).c_str());
- return data.release();
-}
-
-// VTG provides velocity and heading information
-GenericData* extractVtgData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<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 can't tell anything useful.
- if ( msg.isDataTokenEmpty(HeadingTrue) )
+ if ( !( readGga || readVtg || readRme || readRmc ) )
{
- data->isValid = false;
- data->headingTrue = 0.0;
- data->headingMagnetic = 0.0;
- data->speed = 0.0;
- return data.release();
+ reason = "not configured to read any message types";
+ return false;
}
- data->isValid = true;
- // 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);
-
- return data.release();
-}
-
-// 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( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<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};
-
- if ( msg.isDataTokenEmpty(HError) )
+ if ( protocol != "Garmin" && protocol != "NMEA" )
{
- // No valid information
- data->isValid = false;
- data->isVerticalPositionErrorValid = false;
- data->horizontalPositionError = 0.0;
- data->verticalPositionError = 0.0;
- data->estimatedPositionError = 0.0;
- return data.release();
+ reason = "invalid protocol";
+ return false;
}
- data->isValid = true;
- data->horizontalPositionError = atof(msg.getDataToken(HError).c_str());
-
- if ( msg.isDataTokenEmpty(VError) )
- {
- data->isVerticalPositionErrorValid = false;
- data->verticalPositionError = -1;
- }
- else
- {
- data->isVerticalPositionErrorValid = true;
- data->verticalPositionError = atof(msg.getDataToken(VError).c_str());
- }
-
- data->estimatedPositionError = atof(msg.getDataToken(EPE).c_str());
- return data.release();
-}
-
-// RMC provides a combination of GGA and VTG data
-GenericData* extractRmcData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<RmcData> data( new RmcData );
-
- data->timeStampSec = timeSec;
- data->timeStampUsec = timeUsec;
-
- //Names for the RMC message items
- enum RmcTokens{MsgType=0,UTC,Status,Lat,LatDir,Lon,LonDir,SpeedKnots,
- HeadingTrue,DiffAge,MagneticVar,MagneticDir,ModeInd};
-
- //Check for an empty string. Means that we can't tell anything useful.
- if ( msg.isDataTokenEmpty(HeadingTrue) )
- {
- data->isValid = false;
- data->headingTrue = 0.0;
- data->headingMagnetic = 0.0;
- data->speed = 0.0;
- return data.release();
- }
- data->isValid = true;
-
- //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));
-
- // true heading
- double headingRad = DEG2RAD(atof(msg.getDataToken(HeadingTrue).c_str()));
- NORMALISE_ANGLE( headingRad );
- data->headingTrue=headingRad;
-
- // magnetic heading
- double headingVar;
- try {
- // Magnetic deviation from true
- headingVar = DEG2RAD(atof(msg.getDataToken(MagneticVar).c_str()));
- // Direction of magnetic deviation
- if (*msg.getDataToken(MagneticDir).c_str() == 'E')
- headingRad -= headingVar;
- else
- headingRad += headingVar;
- }
- catch ( const gbxgpsutilacfr::NmeaException& e ) {
- // If this occurs, cannot get magnetic heading. Set it to 0.
- headingRad = 0.0;
- }
- NORMALISE_ANGLE( headingRad );
- data->headingMagnetic=headingRad;
-
- //speed - converted from knots to m/s
- data->speed=atof(msg.getDataToken(SpeedKnots).c_str());
- // knots to kph
- data->speed*=1.852;
- // kph to m/s
- data->speed*=(1000/3600.0);
-
- return data.release();
-}
-
-}
-
-///////////////////////////////////////
-
-bool
-Config::isValid() const
-{
- if ( device.empty() )
- return false;
-
- if ( !( readGga || readVtg || readRme ) )
- return false;
-
return true;
}
@@ -274,23 +57,26 @@
std::stringstream ss;
ss << "Garmin driver config: " << endl
<< "\tdevice="<<device << endl
+ << "\tprotocol="<<protocol << endl
<< "\twill read sentences: GPGGA="<<readGga<<" GPVTG="<<readVtg<<" PGRME="<<readRme<<" GPRMC="<<readRmc;
return ss.str();
}
///////////////////////////////////////
-Driver::Driver( const Config &config,
- gbxutilacfr::Tracer &tracer,
- gbxutilacfr::Status &status ) :
+Driver::Driver( const Config &config,
+ gbxutilacfr::Tracer &tracer,
+ gbxutilacfr::Status &status,
+ int serialDebugLevel ) :
config_(config),
tracer_(tracer),
status_(status)
{
- if ( !config_.isValid() )
+ std::string reason;
+ if ( !config_.isValid(reason) )
{
stringstream ss;
- ss << "Invalid config: " << config_.toString();
+ ss << "Invalid config: " << reason << endl << config_.toString();
throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
}
@@ -302,8 +88,10 @@
int baud = 4800;
// 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) ) );
-
+ serial_.reset( new gbxserialacfr::Serial( config_.device,
+ baud,
+ gbxserialacfr::Serial::Timeout(2, 0),
+ serialDebugLevel ) );
init();
}
@@ -346,37 +134,37 @@
// Non-Garmin devices do not support message enabling and disabling
if (config_.protocol != "Garmin") return;
- //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
+ // 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
//First disables all output messages then enable selected ones only.
tracer_.debug( "Driver::enableDevice(): calling disableAllMsg", 10 );
- gbxgpsutilacfr::NmeaMessage disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( disableAllMsg.sentence() );
// alexb: what is this sleep for?
- sleep(1);
+ //sleep(1);
if ( config_.readGga ) {
tracer_.debug( "Driver::enableDevice(): calling enableGgaMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableGgaMsg( "$PGRMO,GPGGA,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableGgaMsg( "$PGRMO,GPGGA,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableGgaMsg.sentence() );
}
if ( config_.readVtg ) {
tracer_.debug( "Driver::enableDevice(): calling enableVtgMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableVtgMsg( "$PGRMO,GPVTG,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableVtgMsg( "$PGRMO,GPVTG,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableVtgMsg.sentence() );
}
if ( config_.readRme ) {
tracer_.debug( "Driver::enableDevice(): calling enableRmeMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableRmeMsg( "$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableRmeMsg( "$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableRmeMsg.sentence() );
}
if ( config_.readRmc ) {
tracer_.debug( "Driver::enableDevice(): calling enableRmcMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableRmcMsg( "$PGRMO,GPRMC,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableRmcMsg( "$PGRMO,GPRMC,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableRmcMsg.sentence() );
}
@@ -388,7 +176,7 @@
Driver::disableDevice()
{
// Simply send the no messages command!
- gbxgpsutilacfr::NmeaMessage disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( disableAllMsg.sentence() );
}
@@ -396,7 +184,7 @@
Driver::read()
{
std::auto_ptr<GenericData> genericData;
- gbxgpsutilacfr::NmeaMessage nmeaMessage;
+ gbxgpsutilacfr::NmeaSentence nmeaMessage;
int nmeaExceptionCount = 0;
int nmeaFailChecksumCount = 0;
@@ -434,18 +222,6 @@
assert( *(serialData.end()-1) == '\n' && "Driver: Serial data did not end in '\n'" );
serialData.erase( serialData.end()-1 );
- // dealing with unexplained cntrl characters in the message
-/*
- for ( string::iterator it=serialData.begin(); it!=serialData.end(); ++it ) {
- if ( iscntrl( *it ) ) {
- // debug
- cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<" before='"<<serialData<<"'"<<endl;
- cout<<now.tv_sec<<" "<<now.tv_usec<<" removing control char="<<std::hex<<(unsigned int)*it<<std::dec<<endl;
- it = serialData.erase( it );
- cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<" after='"<<serialData<<"'"<<endl;
- }
- }
-*/
//Put it into the message object and checksum the data
try {
// This throws if it cannot find the * to deliminate the checksum field
@@ -463,19 +239,13 @@
}
nmeaExceptionCount = 0;
- // debug
-// if ( nmeaFailChecksumCount>0 )
-// cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<nmeaFailChecksumCount<<" the sentence AFTER, raw='"<<serialData<<"'"<<endl<<endl;
-
// Only populate the data structures if our message passes the checksum!
const int nmeaFailChecksumMaxCount = 10;
if ( !nmeaMessage.haveValidChecksum() ) {
// Dont throw an exception on the first failed checksum.
if ( nmeaFailChecksumCount++ < nmeaFailChecksumMaxCount ) {
-// tracer_.warning("Gps driver: Single message failed checksum. Not throwing an exception yet!" );
// debug
cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<nmeaFailChecksumCount<<endl;
-// <<" sentence='"<<nmeaMessage.sentence()<<"' raw='"<<serialData<<"'"<<endl;
continue;
}
else {
@@ -486,7 +256,7 @@
}
nmeaFailChecksumCount = 0;
- //First split up the data fields in the string we have read.
+ // 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
@@ -548,7 +318,7 @@
}
else {
stringstream ss; ss << "Message type unknown " << MsgType <<endl;
- // if we get here the msg is unknown
+ // if we get here the MsgType is unknown
if ( config_.ignoreUnknown )
tracer_.debug( ss.str() );
else
Modified: gearbox/trunk/src/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.h 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/driver.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -15,6 +15,7 @@
#include <gbxutilacfr/tracer.h>
#include <gbxutilacfr/status.h>
#include <memory>
+#include <gbxgarminacfr/nmeamessages.h>
namespace gbxgarminacfr {
@@ -23,6 +24,7 @@
{
public:
Config() :
+ protocol("Garmin"),
readGga(true),
readVtg(true),
readRme(true),
@@ -30,9 +32,7 @@
ignoreUnknown(false) {};
//! Returns true if the configuration is sane. Checks include:
- //! - a non-empty device name
- //! - at least one read-sentence flag set to TRUE
- bool isValid() const;
+ bool isValid( std::string &reason ) const;
//! Returns human-readable configuration description.
std::string toString() const;
@@ -58,200 +58,6 @@
bool ignoreUnknown;
};
-//! Possible types GenericData can contain
-enum DataType
-{
- //! Contents of PGGGA message.
- GpGga,
- //! Contents of PGVTG message.
- GpVtg,
- //! Contents of PGRME message.
- PgRme,
- //! Contents of GPRMC message.
- GpRmc
-};
-
-//! Generic data type returned by a read
-class GenericData
-{
-public:
- virtual ~GenericData() {};
- //! Returns data type.
- virtual DataType type() const=0;
-
-private:
-};
-
-//! GPS fix types.
-enum FixType
-{
- //! Invalid or not available
- Invalid,
- //! Autonomous position
- //! (This is the normal case for non-differential GPS)
- Autonomous,
- //! Differentially corrected
- Differential
-};
-std::string toString( const FixType &f );
-
-//! Fix data structure. Note that when fixType is Invalid, all other data except the time stamps
-//! are meaningless.
-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 the GPS device), reference is Greenwich.
- //! Hour [0..23]
- int utcTimeHrs;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Minutes [0..59]
- int utcTimeMin;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Seconds [0.0..59.9999(9)]
- double utcTimeSec;
-
- //! Latitude [degrees]
- double latitude;
- //! Longitude [degrees]
- double longitude;
- //! Altitude is meaningful if and only if isAltitudeKnown
- bool isAltitudeKnown;
- //! Altitude [metres above ellipsoid] (only meaningful if isAltitudeKnown)
- double altitude;
-
- //! Fix type. When fixType is Invalid, all other data except the time stamps
- //! are meaningless.
- FixType fixType;
-
- //! Number of satellites
- int satellites;
-
- //! Horizontal dilution of position [metres]
- double horizontalDilutionOfPosition;
-
- //! Height of geoid (mean sea level) above WGS84 ellipsoid [metres]
- double geoidalSeparation;
-};
-std::string toString( const GgaData &d );
-inline std::ostream &operator<<( std::ostream &s, const GgaData &d )
-{ return s << toString(d); }
-
-//! Vector track and speed over ground data structure.
-class VtgData : public GenericData
-{
-public:
- DataType type() const { return GpVtg; }
-
- //! 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;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! 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;
-};
-std::string toString( const VtgData &d );
-inline std::ostream &operator<<( std::ostream &s, const VtgData &d )
-{ return s << toString(d); }
-
-//! Gps data structure
-//! (This one is Garmin-specific)
-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;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! When false, means that the GPS unit can't tell us anything
- //! about our vertical error
- bool isVerticalPositionErrorValid;
-
- //! Horizontal position error: one standard deviation [metres)]
- double horizontalPositionError;
- //! Vertical position error: one standard deviation [metres]
- double verticalPositionError;
-
- //! Estimated position error.
- double estimatedPositionError;
-};
-std::string toString( const RmeData &d );
-inline std::ostream &operator<<( std::ostream &s, const RmeData &d )
-{ return s << toString(d); }
-
-//! Gps data structure
-class RmcData : public GenericData
-{
-public:
- DataType type() const { return GpRmc; }
-
- //! 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 the GPS device), reference is Greenwich.
- //! Hour [0..23]
- int utcTimeHrs;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Minutes [0..59]
- int utcTimeMin;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Seconds [0.0..59.9999(9)]
- double utcTimeSec;
-
- //! Latitude [degrees]
- double latitude;
- //! Longitude [degrees]
- double longitude;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! 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;
-};
-std::string toString( const RmcData &d );
-inline std::ostream &operator<<( std::ostream &s, const RmcData &d )
-{ return s << toString(d); }
-
-
-
/*!
Garmin GPS driver.
@@ -260,10 +66,14 @@
This standard dictates a transfer rate of 4800 baud.
This driver can read only the following messages (sentences):
-- GPGGA fix data
-- PGRME (estimated error) - not sent if set to 0183 1.5 (garmin-specific)
-- GPVTG vector track and speed over ground
-- GPRMC
+- GPGGA: fix data
+- PGRME: (estimated error) - not sent if set to 0183 1.5 (garmin-specific)
+- GPVTG: vector track and speed over ground
+- GPRMC: known as the "Recommended Minimum" sentence, is the most
+ common sentence transmitted by GPS devices. This one sentence
+ contains nearly everything a GPS application needs: latitude,
+ longitude, speed, bearing, satellite-derived time, fix status
+ and magnetic variation.
Processing of individual messages can be disabled in the Config structure.
@@ -284,9 +94,10 @@
//! gbxutilacfr::Tracer and gbxutilacfr::Status allow
//! (human-readable and machine-readable respectively) external
//! monitorining of the driver's internal state.
- Driver( const Config& config,
- gbxutilacfr::Tracer& tracer,
- gbxutilacfr::Status& status );
+ Driver( const Config &config,
+ gbxutilacfr::Tracer &tracer,
+ gbxutilacfr::Status &status,
+ int serialDebugLevel = 0 );
~Driver();
Deleted: gearbox/trunk/src/gbxgarminacfr/nmea.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmea.cpp 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/nmea.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -1,254 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2004-2008 Mathew Ridley, Alex Brooks, Alexei Makarenko, Tobias Kaupp
- *
- * This distribution is licensed to you under the terms described in
- * the LICENSE file included in this distribution.
- *
- */
-
-#include <stdio.h>
-#include <string>
-#include <iostream>
-#include <assert.h>
-#include <sstream>
-#include <gbxutilacfr/tokenise.h>
-
-// //////////////////////////////
-
-// // Ensure we have strnlen
-// // eg. Solaris doesn't define strnlen in string.h, so define it here.
-// #if !HAVE_STRNLEN
-
-// #include <cstring>
-
-// // inline the fucker to guard against multiple inclusion, without the
-// // hassle of a special lib.
-// inline size_t strnlen(const char *s, size_t maxlen)
-// {
-// char *p;
-// if (s == NULL) {
-// return maxlen;
-// }
-// p = (char *)memchr(s, 0, maxlen);
-// if (p == NULL) {
-// return maxlen;
-// }
-// return ((p - s) + 1);
-// }
-// #endif
-
-// //////////////////////////////
-
-#include "nmea.h"
-
-using namespace std;
-using namespace gbxgpsutilacfr;
-
-const char NMEAStartOfSentence = '$';
-const char NMEAChecksumDelim = '*';
-
-
-//The blank constructor
-NmeaMessage::NmeaMessage()
-{
- init();
-}
-
-void NmeaMessage::init()
-{
- haveCheckSum_ = false;
- checkSumOK_ = false;
-
- // Now clear the internal data store
- sentence_.clear();
- dataTokens_.clear();
-}
-
-NmeaMessage::NmeaMessage(const std::string &sentence, NmeaMessageOptions addOrTestCheckSum)
-{
- init();
- setSentence(sentence,addOrTestCheckSum);
-}
-
-
-//Load the data as requested and test the checksum if we are asked to.
-void
-NmeaMessage::setSentence(const std::string &data, NmeaMessageOptions addOrTestCheckSum)
-{
- init();
-
- sentence_ = data;
-
- switch ( addOrTestCheckSum )
- {
- case TestChecksum: {
- // This is for Rx'd data that we need to test for correct reception
- // (internally it will also call addCheckSum())
- testChecksumOk();
- break;
- }
- case AddChecksum: {
- // This is for Tx data that needs to checksummed before sending
- addCheckSum();
- checkSumOK_ = true;
- break;
- }
- case DontTestOrAddChecksum:
- break;
- default:
- assert( false && "unrecognized message option" );
- }
-}
-
-bool
-NmeaMessage::testChecksumOk()
-{
- haveCheckSum_ = true;
- checkSumOK_ = false;
-
- //First save the existing two checksum chars from the message
- //These are straight after the '*' character
- const size_t starPos = sentence_.find( NMEAChecksumDelim );
- if ( starPos == std::string::npos )
- {
- // cout<<"device: no checksum delimiter"<<endl;
- return false;
- }
-
- if ( starPos+2 >= sentence_.size() )
- {
- // cout<<"device: no checksum after delimiter"<<endl;
- return false;
- }
-
- //save the high and low bytes of the checksum
- //Make sure they are in upper case!
- const int checksumPos = starPos+1;
- const char chksum_HIB = (char)toupper(sentence_[checksumPos]);
- const char chksum_LOB = (char)toupper(sentence_[checksumPos+1]);
-
- //invalidate the existing checksum
- sentence_[checksumPos] = 'x';
- sentence_[checksumPos+1] = 'x';
-
- //Re-calculate our own copy of the checksum
- addCheckSum();
-
- //Now compare our saved version with our new ones
- if( (chksum_HIB == sentence_[checksumPos]) && (chksum_LOB == sentence_[checksumPos+1]) ) {
- //all looked good!
- checkSumOK_ = true;
- return true;
- }
-
- //failed the checksum!
-// cout<<"device: '"<<chksum_HIB<<chksum_LOB <<"' ("<<std::hex<<(unsigned int)chksum_HIB<<","<<(unsigned int)chksum_LOB<<std::dec<<") "
-// <<"driver: '"<<*ptr<<*(ptr+1)<<"'" <<"' ("<<std::hex<<(unsigned int)*ptr<<","<<(unsigned int)*(ptr+1)<<std::dec<<") "<<endl;
- return false;
-}
-
-// Add the checksum chars to an existing message
-// NOTE: this assumes that there is allready space in the message for
-// the checksum, and that the checksum delimiter is there
-void
-NmeaMessage::addCheckSum()
-{
- assert( haveSentence() && "calling addCheckSum() without a sentence" );
-
- haveCheckSum_ = true;
-
- //check that we have the '$' at the start
- if ( sentence_[0]!= NMEAStartOfSentence ) {
- throw NmeaException("cannot calculate checksum, missing leading '$'");
- }
-
- unsigned char chkRunning = 0;
-
- // we start from 1 to skip the leading '$'
- int loopCount;
- for ( loopCount = 1; loopCount < (int)(sentence_.size()); loopCount++ )
- {
- unsigned char nextChar = static_cast<unsigned char>(sentence_[loopCount]);
-
- // no delimiter uh oh
- if( (nextChar=='\r') || (nextChar=='\n') || (nextChar=='\0') ) {
- throw NmeaException("cannot calculate checksum, missing final '*'");
- }
-
- // goodie we found it (the '*' is not included into the checksum)
- if ( nextChar==NMEAChecksumDelim ) {
- break;
- }
-
- // alexm: gcc-4.3 is very strict with the next line.
- // not sure what to do about it yet.
- // http://gcc.gnu.org/ml/gcc/2008-05/msg00363.html
-
- // Keep the running XOR total
- chkRunning ^= nextChar;
- }
-
- if ( loopCount+2 >= (int)(sentence_.size()) )
- {
- throw NmeaException("addCheckSum(): no space for checksum of '*' not found.");
- }
-
- //Put the byte values as upper case HEX back into the message
- sprintf( &(sentence_[loopCount + 1]),"%02X", chkRunning );
-}
-
-// Parse the data fields of our message...
-void
-NmeaMessage::parseTokens()
-{
- //We should not attempt to be parsing a message twice...
- assert( numDataTokens()==0 && "calling parseTokens() with tokens" );
-
- //Split the message at the commas
- //TODO cope with missing fields
- dataTokens_ = gbxutilacfr::tokenise(sentence_, ",");
-
- //Now discard the $ and the * from the first and last tokens...
- //TODO : - dataTokens_[0] =
-
- //keep track of what we have done.
- haveTokens_ = true;
-}
-
-bool
-NmeaMessage::isDataTokenEmpty(int i) const
-{
- if ( i >= (int)(dataTokens_.size()) )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but only " << dataTokens_.size() << " exist in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- return dataTokens_[i].empty();
-}
-
-const std::string &
-NmeaMessage::getDataToken(int i) const
-{
- if ( i >= (int)(dataTokens_.size()) )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but only " << dataTokens_.size() << " exist in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- if ( dataTokens_[i].empty() )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but this token is empty in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- return dataTokens_[i];
-}
Deleted: gearbox/trunk/src/gbxgarminacfr/nmea.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmea.h 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/nmea.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -1,136 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, Duncan Mercer
- *
- * This distribution is licensed to you under the terms described in
- * the LICENSE file included in this distribution.
- *
- */
-
-#ifndef GBXGPSUTILACFR_NMEA_H
-#define GBXGPSUTILACFR_NMEA_H
-
-#include <vector>
-#include <string>
-
-/*
-
-for further info:
-
-http://www.kh-gps.de/nmea-faq.htm
-http://vancouver-webpages.com/peter/nmeafaq.txt
-
-NMEA-0183 sentence
-
-$aaccc,c--c*hh<CR><LF>
-|| || || |
-|| || || \________ <CR><LF> - End of sentence (0xOD 0xOA)
-|| || |\__________ hh - Checksum field hexadecimal [optional]
-|| || \___________ * - Checksum delimiter (0x2A) [optional]
-|| |\_______________ c--c - Data sentence block
-|| \________________ , - Field delimiter (0x2c)
-|\_____________________ aaccc - Address field/Command
-\______________________ $ - Start of sentence
-
- The optional checksum field consists of a "*" and two hex digits
- representing the exclusive OR of all characters between, but not
- including, the "$" and "*". A checksum is required on some
- sentences.
-
-*/
-
-namespace gbxgpsutilacfr {
-
-
-// class SOEXPORT NmeaException : public std::exception
-class NmeaException : public std::exception
-{
-public:
- NmeaException(const char *message)
- : message_(message) {}
-
- NmeaException(const std::string &message)
- : message_(message) {}
-
- virtual ~NmeaException() throw() {}
-
- virtual const char* what() const throw() { return message_.c_str(); }
-
-protected:
- std::string message_;
-};
-
-
-#define MAX_SENTENCE_LEN 256
-
-// When using class to send data, need to add checksum, when reciving data need to test checksum
-// Checksums are usually optional
-enum NmeaMessageOptions
-{
- TestChecksum,
- AddChecksum,
- DontTestOrAddChecksum
-};
-
-// class SOEXPORT NmeaMessage{
-class NmeaMessage
-{
-public:
- NmeaMessage();
- NmeaMessage(const std::string &sentence, NmeaMessageOptions addOrTestCheckSum=DontTestOrAddChecksum );
-
- // Set up the internal data for a sentence.
- // May throw NmeaException if TestChecksum is specified.
- void setSentence(const std::string &data, NmeaMessageOptions addOrTestCheckSum=DontTestOrAddChecksum );
-
- // Do we have the raw string?
- bool haveSentence() const { return !sentence_.empty(); };
-
- // Do we have parsed fields?
- bool haveTokens() const { return haveTokens_; };
-
- // Do we have a valid checksum?
- bool haveValidChecksum() const { return checkSumOK_; };
-
- // Have we checked the checksum?
- bool haveTestedChecksum()const { return haveCheckSum_; };
-
- // calculate the checksum from sentence
- // May throw NmeaException.
- bool testChecksumOk();
-
- // Return the raw sentence string
- const std::string &sentence() const { return sentence_; };
-
- // Return a single data token as a string.
- // Throws an exception if that token is empty (see 'isDataTokenEmpty()')
- const std::string &getDataToken(int i) const;
-
- bool isDataTokenEmpty(int i) const;
-
- // Return the number of fields
- int numDataTokens() const { return dataTokens_.size(); };
-
- //Tokenise the string that we received
- void parseTokens();
-
-private:
- void init();
- // May throw NmeaException.
- void addCheckSum();
- // 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];
- std::string sentence_;
- // The tokenised data
- std::vector<std::string> dataTokens_;
-};
-
-}
-
-#endif
Added: gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,236 @@
+#include "nmeamessages.h"
+#include <iostream>
+#include "nmeasentence.h"
+#include <memory>
+#include <cstdlib>
+#include <gbxutilacfr/exceptions.h>
+#include <gbxutilacfr/mathdefs.h>
+
+using namespace std;
+
+namespace gbxgarminacfr {
+
+// Get the useful bits from a GGA message
+GenericData* extractGgaData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<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};
+
+ //UTC time
+ sscanf(sentence.getDataToken(UTC).c_str(),"%02d%02d%lf",
+ &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
+
+ //number of satellites in use
+ data->satellites = atoi(sentence.getDataToken(NSatsUsed).c_str());
+
+ // fix type
+ switch ( sentence.getDataToken(FixType)[0] )
+ {
+ case '0':
+ data->fixType = Invalid;
+ // NOTE: not processing the rest!
+ data->latitude = 0.0;
+ data->longitude = 0.0;
+ data->altitude = 0.0;
+ data->geoidalSeparation = 0.0;
+ return data.release();
+ case '1':
+ data->fixType = Autonomous;
+ break;
+ case '2':
+ data->fixType = Differential;
+ break;
+ default :
+ throw gbxutilacfr::Exception( ERROR_INFO, "GGA sentence contains unknown GPS fix type: '"+sentence.getDataToken(FixType)+"'" );
+ }
+
+ //position
+ int deg;
+ double min;
+ double dir;
+
+ //latitude
+ sscanf(sentence.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
+ dir = (*sentence.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
+ data->latitude=dir*(deg+(min/60.0));
+ //longitude
+ sscanf(sentence.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
+ dir = (*sentence.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
+ data->longitude=dir*(deg+(min/60.0));
+
+ //altitude
+ data->isAltitudeKnown = !sentence.isDataTokenEmpty(Hgt);
+ if ( data->isAltitudeKnown )
+ data->altitude=atof(sentence.getDataToken(Hgt).c_str());
+
+ //geoidal Separation
+ data->geoidalSeparation=atof(sentence.getDataToken(GeoidHgt).c_str());
+
+ return data.release();
+}
+
+// VTG provides velocity and heading information
+GenericData* extractVtgData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<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 can't tell anything useful.
+ if ( sentence.isDataTokenEmpty(HeadingTrue) )
+ {
+ data->isValid = false;
+ data->headingTrue = 0.0;
+ data->headingMagnetic = 0.0;
+ data->speed = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ // true heading
+ double headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingTrue).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingTrue=headingRad;
+
+ // magnetic heading
+ headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingMag).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingMagnetic=headingRad;
+
+ //speed - converted to m/s
+ data->speed=atof(sentence.getDataToken(SpeedKPH).c_str());
+ data->speed*=(1000/3600.0);
+
+ return data.release();
+}
+
+// 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( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<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};
+
+ if ( sentence.isDataTokenEmpty(HError) )
+ {
+ // No valid information
+ data->isValid = false;
+ data->isVerticalPositionErrorValid = false;
+ data->horizontalPositionError = 0.0;
+ data->verticalPositionError = 0.0;
+ data->estimatedPositionError = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ data->horizontalPositionError = atof(sentence.getDataToken(HError).c_str());
+
+ if ( sentence.isDataTokenEmpty(VError) )
+ {
+ data->isVerticalPositionErrorValid = false;
+ data->verticalPositionError = -1;
+ }
+ else
+ {
+ data->isVerticalPositionErrorValid = true;
+ data->verticalPositionError = atof(sentence.getDataToken(VError).c_str());
+ }
+
+ data->estimatedPositionError = atof(sentence.getDataToken(EPE).c_str());
+ return data.release();
+}
+
+// RMC provides a combination of GGA and VTG data
+GenericData* extractRmcData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<RmcData> data( new RmcData );
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the RMC message items
+ enum RmcTokens{MsgType=0,UTC,Status,Lat,LatDir,Lon,LonDir,SpeedKnots,
+ HeadingTrue,DiffAge,MagneticVar,MagneticDir,ModeInd};
+
+ //Check for an empty string. Means that we can't tell anything useful.
+ if ( sentence.isDataTokenEmpty(HeadingTrue) )
+ {
+ data->isValid = false;
+ data->headingTrue = 0.0;
+ data->headingMagnetic = 0.0;
+ data->speed = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ //UTC time
+ sscanf(sentence.getDataToken(UTC).c_str(),"%02d%02d%lf",
+ &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
+
+ //position
+ int deg;
+ double min;
+ double dir;
+
+ //latitude
+ sscanf(sentence.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
+ dir = (*sentence.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
+ data->latitude=dir*(deg+(min/60.0));
+ //longitude
+ sscanf(sentence.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
+ dir = (*sentence.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
+ data->longitude=dir*(deg+(min/60.0));
+
+ // true heading
+ double headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingTrue).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingTrue=headingRad;
+
+ // magnetic heading
+ double headingVar;
+ try {
+ // Magnetic deviation from true
+ headingVar = DEG2RAD(atof(sentence.getDataToken(MagneticVar).c_str()));
+ // Direction of magnetic deviation
+ if (*sentence.getDataToken(MagneticDir).c_str() == 'E')
+ headingRad -= headingVar;
+ else
+ headingRad += headingVar;
+ }
+ catch ( const gbxgpsutilacfr::NmeaException& e ) {
+ // If this occurs, cannot get magnetic heading. Set it to 0.
+ headingRad = 0.0;
+ }
+ NORMALISE_ANGLE( headingRad );
+ data->headingMagnetic=headingRad;
+
+ //speed - converted from knots to m/s
+ data->speed=atof(sentence.getDataToken(SpeedKnots).c_str());
+ // knots to kph
+ data->speed*=1.852;
+ // kph to m/s
+ data->speed*=(1000/3600.0);
+
+ return data.release();
+}
+
+}
+
Added: gearbox/trunk/src/gbxgarminacfr/nmeamessages.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeamessages.h (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeamessages.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,216 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, Duncan Mercer
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+#ifndef GBXGPSUTILACFR_NMEAMESSAGES_H
+#define GBXGPSUTILACFR_NMEAMESSAGES_H
+
+#include <string>
+#include <gbxgarminacfr/nmeasentence.h>
+
+namespace gbxgarminacfr {
+
+//! Possible types GenericData can contain
+enum DataType
+{
+ //! Contents of PGGGA message.
+ GpGga,
+ //! Contents of PGVTG message.
+ GpVtg,
+ //! Contents of PGRME message.
+ PgRme,
+ //! Contents of GPRMC message.
+ GpRmc
+};
+
+//! Generic data type returned by a read
+class GenericData
+{
+public:
+ virtual ~GenericData() {};
+ //! Returns data type.
+ virtual DataType type() const=0;
+
+private:
+};
+
+//! GPS fix types.
+enum FixType
+{
+ //! Invalid or not available
+ Invalid,
+ //! Autonomous position
+ //! (This is the normal case for non-differential GPS)
+ Autonomous,
+ //! Differentially corrected
+ Differential
+};
+std::string toString( const FixType &f );
+
+//! Fix data structure. Note that when fixType is Invalid, all other data except the time stamps
+//! are meaningless.
+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 the GPS device), reference is Greenwich.
+ //! Hour [0..23]
+ int utcTimeHrs;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Minutes [0..59]
+ int utcTimeMin;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Seconds [0.0..59.9999(9)]
+ double utcTimeSec;
+
+ //! Latitude [degrees]
+ double latitude;
+ //! Longitude [degrees]
+ double longitude;
+ //! Altitude is meaningful if and only if isAltitudeKnown
+ bool isAltitudeKnown;
+ //! Altitude [metres above ellipsoid] (only meaningful if isAltitudeKnown)
+ double altitude;
+
+ //! Fix type. When fixType is Invalid, all other data except the time stamps
+ //! are meaningless.
+ FixType fixType;
+
+ //! Number of satellites
+ int satellites;
+
+ //! Horizontal dilution of position [metres]
+ double horizontalDilutionOfPosition;
+
+ //! Height of geoid (mean sea level) above WGS84 ellipsoid [metres]
+ double geoidalSeparation;
+};
+std::string toString( const GgaData &d );
+inline std::ostream &operator<<( std::ostream &s, const GgaData &d )
+{ return s << toString(d); }
+GenericData* extractGgaData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Vector track and speed over ground data structure.
+class VtgData : public GenericData
+{
+public:
+ DataType type() const { return GpVtg; }
+
+ //! 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;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! 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;
+};
+std::string toString( const VtgData &d );
+inline std::ostream &operator<<( std::ostream &s, const VtgData &d )
+{ return s << toString(d); }
+GenericData* extractVtgData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Gps data structure
+//! (This one is Garmin-specific)
+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;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! When false, means that the GPS unit can't tell us anything
+ //! about our vertical error
+ bool isVerticalPositionErrorValid;
+
+ //! Horizontal position error: one standard deviation [metres)]
+ double horizontalPositionError;
+ //! Vertical position error: one standard deviation [metres]
+ double verticalPositionError;
+
+ //! Estimated position error.
+ double estimatedPositionError;
+};
+std::string toString( const RmeData &d );
+inline std::ostream &operator<<( std::ostream &s, const RmeData &d )
+{ return s << toString(d); }
+GenericData* extractRmeData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Gps data structure
+class RmcData : public GenericData
+{
+public:
+ DataType type() const { return GpRmc; }
+
+ //! 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 the GPS device), reference is Greenwich.
+ //! Hour [0..23]
+ int utcTimeHrs;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Minutes [0..59]
+ int utcTimeMin;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Seconds [0.0..59.9999(9)]
+ double utcTimeSec;
+
+ //! Latitude [degrees]
+ double latitude;
+ //! Longitude [degrees]
+ double longitude;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! 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;
+};
+std::string toString( const RmcData &d );
+inline std::ostream &operator<<( std::ostream &s, const RmcData &d )
+{ return s << toString(d); }
+GenericData* extractRmcData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+}
+
+#endif
Added: gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,254 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Mathew Ridley, Alex Brooks, Alexei Makarenko, Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <stdio.h>
+#include <string>
+#include <iostream>
+#include <assert.h>
+#include <sstream>
+#include <gbxutilacfr/tokenise.h>
+
+// //////////////////////////////
+
+// // Ensure we have strnlen
+// // eg. Solaris doesn't define strnlen in string.h, so define it here.
+// #if !HAVE_STRNLEN
+
+// #include <cstring>
+
+// // inline the fucker to guard against multiple inclusion, without the
+// // hassle of a special lib.
+// inline size_t strnlen(const char *s, size_t maxlen)
+// {
+// char *p;
+// if (s == NULL) {
+// return maxlen;
+// }
+// p = (char *)memchr(s, 0, maxlen);
+// if (p == NULL) {
+// return maxlen;
+// }
+// return ((p - s) + 1);
+// }
+// #endif
+
+// //////////////////////////////
+
+#include "nmeasentence.h"
+
+using namespace std;
+using namespace gbxgpsutilacfr;
+
+const char NMEAStartOfSentence = '$';
+const char NMEAChecksumDelim = '*';
+
+
+//The blank constructor
+NmeaSentence::NmeaSentence()
+{
+ init();
+}
+
+void NmeaSentence::init()
+{
+ haveCheckSum_ = false;
+ checkSumOK_ = false;
+
+ // Now clear the internal data store
+ sentence_.clear();
+ dataTokens_.clear();
+}
+
+NmeaSentence::NmeaSentence(const std::string &sentence, NmeaSentenceOptions addOrTestCheckSum)
+{
+ init();
+ setSentence(sentence,addOrTestCheckSum);
+}
+
+
+//Load the data as requested and test the checksum if we are asked to.
+void
+NmeaSentence::setSentence(const std::string &data, NmeaSentenceOptions addOrTestCheckSum)
+{
+ init();
+
+ sentence_ = data;
+
+ switch ( addOrTestCheckSum )
+ {
+ case TestChecksum: {
+ // This is for Rx'd data that we need to test for correct reception
+ // (internally it will also call addCheckSum())
+ testChecksumOk();
+ break;
+ }
+ case AddChecksum: {
+ // This is for Tx data that needs to checksummed before sending
+ addCheckSum();
+ checkSumOK_ = true;
+ break;
+ }
+ case DontTestOrAddChecksum:
+ break;
+ default:
+ assert( false && "unrecognized message option" );
+ }
+}
+
+bool
+NmeaSentence::testChecksumOk()
+{
+ haveCheckSum_ = true;
+ checkSumOK_ = false;
+
+ //First save the existing two checksum chars from the message
+ //These are straight after the '*' character
+ const size_t starPos = sentence_.find( NMEAChecksumDelim );
+ if ( starPos == std::string::npos )
+ {
+ // cout<<"device: no checksum delimiter"<<endl;
+ return false;
+ }
+
+ if ( starPos+2 >= sentence_.size() )
+ {
+ // cout<<"device: no checksum after delimiter"<<endl;
+ return false;
+ }
+
+ //save the high and low bytes of the checksum
+ //Make sure they are in upper case!
+ const int checksumPos = starPos+1;
+ const char chksum_HIB = (char)toupper(sentence_[checksumPos]);
+ const char chksum_LOB = (char)toupper(sentence_[checksumPos+1]);
+
+ //invalidate the existing checksum
+ sentence_[checksumPos] = 'x';
+ sentence_[checksumPos+1] = 'x';
+
+ //Re-calculate our own copy of the checksum
+ addCheckSum();
+
+ //Now compare our saved version with our new ones
+ if( (chksum_HIB == sentence_[checksumPos]) && (chksum_LOB == sentence_[checksumPos+1]) ) {
+ //all looked good!
+ checkSumOK_ = true;
+ return true;
+ }
+
+ //failed the checksum!
+// cout<<"device: '"<<chksum_HIB<<chksum_LOB <<"' ("<<std::hex<<(unsigned int)chksum_HIB<<","<<(unsigned int)chksum_LOB<<std::dec<<") "
+// <<"driver: '"<<*ptr<<*(ptr+1)<<"'" <<"' ("<<std::hex<<(unsigned int)*ptr<<","<<(unsigned int)*(ptr+1)<<std::dec<<") "<<endl;
+ return false;
+}
+
+// Add the checksum chars to an existing message
+// NOTE: this assumes that there is allready space in the message for
+// the checksum, and that the checksum delimiter is there
+void
+NmeaSentence::addCheckSum()
+{
+ assert( haveSentence() && "calling addCheckSum() without a sentence" );
+
+ haveCheckSum_ = true;
+
+ //check that we have the '$' at the start
+ if ( sentence_[0]!= NMEAStartOfSentence ) {
+ throw NmeaException("cannot calculate checksum, missing leading '$'");
+ }
+
+ unsigned char chkRunning = 0;
+
+ // we start from 1 to skip the leading '$'
+ int loopCount;
+ for ( loopCount = 1; loopCount < (int)(sentence_.size()); loopCount++ )
+ {
+ un...
[truncated message content] |