Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[476] gearbox/trunk/src/gbxgarminacfr
|
From: <bo...@us...> - 2009-12-13 17:37:14
|
Revision: 476
http://gearbox.svn.sourceforge.net/gearbox/?rev=476&view=rev
Author: borax00
Date: 2009-12-13 17:37:04 +0000 (Sun, 13 Dec 2009)
Log Message:
-----------
Applied patch from Michael Warren.
Modified Paths:
--------------
gearbox/trunk/src/gbxgarminacfr/doc.dox
gearbox/trunk/src/gbxgarminacfr/driver.cpp
gearbox/trunk/src/gbxgarminacfr/driver.h
Modified: gearbox/trunk/src/gbxgarminacfr/doc.dox
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/doc.dox 2009-12-06 04:25:41 UTC (rev 475)
+++ gearbox/trunk/src/gbxgarminacfr/doc.dox 2009-12-13 17:37:04 UTC (rev 476)
@@ -16,7 +16,14 @@
@defgroup gbx_library_gbxgarminacfr GbxGarminAcfr
@brief Garmin GPS receiver.
-Uses Garmin serial implementation. Written for Garmin-GPS15L.
+Talks to a GPS unit over serial.
+
+Capable of talking to Garmin GPS units (which use special Garmin-only
+messages) or NMEA-only GPS units.
+
+Tested on:
+ - Garmin-GPS15L.
+
Tries to establish communication at 4800 baud (non-configurable).
For a full list of classes and functions, see @ref gbxgarminacfr.
Modified: gearbox/trunk/src/gbxgarminacfr/driver.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.cpp 2009-12-06 04:25:41 UTC (rev 475)
+++ gearbox/trunk/src/gbxgarminacfr/driver.cpp 2009-12-13 17:37:04 UTC (rev 476)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2004-2008 Duncan Mercer, Alex Brooks, Alexei Makarenko, Tobias Kaupp
+ * Copyright (c) 2004-2009 Duncan Mercer, Alex Brooks, Alexei Makarenko, Tobias Kaupp, Michael Warren
*
* This distribution is licensed to you under the terms described in
* the LICENSE file included in this distribution.
@@ -178,8 +178,82 @@
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
@@ -200,7 +274,7 @@
std::stringstream ss;
ss << "Garmin driver config: " << endl
<< "\tdevice="<<device << endl
- << "\twill read sentences: GPGGA="<<readGga<<" GPVTG="<<readVtg<<" PGRME="<<readRme;
+ << "\twill read sentences: GPGGA="<<readGga<<" GPVTG="<<readVtg<<" PGRME="<<readRme<<" GPRMC="<<readRmc;
return ss.str();
}
@@ -265,7 +339,7 @@
void
Driver::enableDevice()
{
- tracer_.info("Configure Garmin GPS device");
+ tracer_.info("Configure GPS device");
//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
@@ -291,6 +365,10 @@
gbxgpsutilacfr::NmeaMessage enableRmeMsg( "$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableRmeMsg.sentence() );
}
+ if ( config_.readRmc ) {
+ gbxgpsutilacfr::NmeaMessage enableRmcMsg( "$PGRMO,GPRMC,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ serial_->writeString( enableRmcMsg.sentence() );
+ }
// alexb: what is this sleep for?
sleep(1);
@@ -327,7 +405,7 @@
timeval now;
if ( gettimeofday( &now, 0 ) != 0 ) {
stringstream ss;
- ss << "Pproblem getting timeofday: " << strerror(errno) << endl;
+ ss << "Problem getting timeofday: " << strerror(errno) << endl;
throw gbxutilacfr::Exception( ERROR_INFO,ss.str() );
}
@@ -442,6 +520,17 @@
else
continue;
}
+ else if ( MsgType == "$GPRMC" ) {
+ if ( config_.readRmc )
+ tracer_.debug("got GPRMC message",4);
+ else
+ throw gbxutilacfr::Exception( ERROR_INFO, "got unexpected PGRMC message" );
+ genericData.reset( extractRmcData( nmeaMessage, now.tv_sec, now.tv_usec ) );
+ if ( genericData.get() )
+ break;
+ else
+ continue;
+ }
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 and wait for the next one.
@@ -515,4 +604,22 @@
return ss.str();
}
+std::string toString( const RmcData &d )
+{
+ stringstream ss;
+ ss << endl;
+ ss << " timeStampSec : " << d.timeStampSec << endl
+ << " timeStampUsec : " << d.timeStampUsec << endl
+ << " utcTimeHrs : " << d.utcTimeHrs << endl
+ << " utcTimeMin : " << d.utcTimeMin << endl
+ << " utcTimeSec : " << d.utcTimeSec << endl
+ << " latitude : " << d.latitude << endl
+ << " longitude : " << d.longitude << endl
+ << " isValid : " << d.isValid << endl
+ << " headingTrue : " << d.headingTrue << endl
+ << " headingMagnetic : " << d.headingMagnetic << endl
+ << " speed : " << d.speed;
+ return ss.str();
}
+
+}
Modified: gearbox/trunk/src/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.h 2009-12-06 04:25:41 UTC (rev 475)
+++ gearbox/trunk/src/gbxgarminacfr/driver.h 2009-12-13 17:37:04 UTC (rev 476)
@@ -44,7 +44,9 @@
//! Read GPVTG sentence
bool readVtg;
//! Read PGRME sentence
- bool readRme;
+ bool readRme;
+ //! Read GPRMC sentence
+ bool readRmc;
//! Ignore unknown messages. This driver tries to turn off all messages and then explicitely enables
//! the ones it understands. But with some devices this does not work and many messages types are received.
@@ -60,7 +62,9 @@
//! Contents of PGVTG message.
GpVtg,
//! Contents of PGRME message.
- PgRme
+ PgRme,
+ //! Contents of GPRMC message.
+ GpRmc
};
//! Generic data type returned by a read
@@ -166,6 +170,7 @@
{ return s << toString(d); }
//! Gps data structure
+//! (This one is Garmin-specific)
class RmeData : public GenericData
{
public:
@@ -198,6 +203,51 @@
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.
@@ -207,8 +257,9 @@
This driver can read only the following messages (sentences):
- GPGGA fix data
-- PGRME (estimated error) - not sent if set to 0183 1.5
+- PGRME (estimated error) - not sent if set to 0183 1.5 (garmin-specific)
- GPVTG vector track and speed over ground
+- GPRMC
Processing of individual messages can be disabled in the Config structure.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|