Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox: [127] gearbox/trunk/submitted
|
From: <rus...@us...> - 2008-04-26 06:41:59
|
Revision: 127
http://gearbox.svn.sourceforge.net/gearbox/?rev=127&view=rev
Author: russo2503v
Date: 2008-04-25 23:42:07 -0700 (Fri, 25 Apr 2008)
Log Message:
-----------
submitted garmin driver
Modified Paths:
--------------
gearbox/trunk/submitted/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/submitted/gbxgarminacfr/
gearbox/trunk/submitted/gbxgarminacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxgarminacfr/doc.dox
gearbox/trunk/submitted/gbxgarminacfr/driver.cpp
gearbox/trunk/submitted/gbxgarminacfr/driver.h
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.cpp
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.h
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/testmga.cpp
Modified: gearbox/trunk/submitted/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/CMakeLists.txt 2008-04-26 06:41:26 UTC (rev 126)
+++ gearbox/trunk/submitted/CMakeLists.txt 2008-04-26 06:42:07 UTC (rev 127)
@@ -9,5 +9,8 @@
# When adding new directories, please maintain order of inter-dependencies.
# Otherwise, maintain alphabetical order.
+ # E.g. ADD_SUBDIRECTORY( mydir )
+ ADD_SUBDIRECTORY( gbxgarminacfr )
+
ENDIF( GBX_BUILD_SUBMITTED )
\ No newline at end of file
Added: gearbox/trunk/submitted/gbxgarminacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/CMakeLists.txt 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,32 @@
+SET( lib_name GbxGarminAcfr )
+GBX_ADD_LICENSE( LGPL )
+
+SET( build TRUE )
+GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "only Linux OS is supported" )
+
+SET( int_libs GbxUtilAcfr GbxSerialAcfr )
+# SET( int_libs GbxUtilAcfr GbxSerialAcfr GbxGpsUtilAcfr )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
+
+IF( build )
+
+ ADD_SUBDIRECTORY( gbxgpsutilacfr )
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+ FILE( GLOB hdrs *.h )
+ FILE( GLOB srcs *.cpp )
+ SET( dep_libs ${int_libs} )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+ TARGET_LINK_LIBRARIES( ${lib_name} ${dep_libs} )
+# GBX_ADD_PKGCONFIG( ${lib_name} "Garmin GPS driver" "" dep_libs "" "" )
+
+ GBX_ADD_HEADERS( gbxgarminacfr ${hdrs} )
+
+# IF( GBX_BUILD_TESTS )
+# ADD_SUBDIRECTORY( test )
+# ENDIF( GBX_BUILD_TESTS )
+
+ENDIF( build )
Added: gearbox/trunk/submitted/gbxgarminacfr/doc.dox
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/doc.dox (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/doc.dox 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,70 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+/*!
+@ingroup gbx_libs
+@ingroup gbx_cpp
+@ingroup gbx_linux
+@defgroup gbx_library_gbxgarminacfr GbxGarminAcfr
+@brief Garmin GPS receiver.
+
+Uses Garmin serial implementation. Written for Garmin-GPS15L.
+Tries to establish communication at 4800 baud (non-configurable).
+
+@par Header file
+
+@verbatim
+#include <gbxgarminacfr/driver.h>
+@endverbatim
+
+@par Example
+ See test/test.cpp
+
+@par Style
+ See http://orca-robotics.sourceforge.net/orca/orca_doc_style.html
+
+@par Units and Coordinate System
+ See http://orca-robotics.sourceforge.net/orca/orca_doc_units.html
+
+@par Copyright
+ Duncan Mercer, Alex Brooks, Alexei Makarenko, Tobias Kaupp
+
+@par Responsible Developer
+ Alex Brooks
+
+@par License
+ LGPL
+
+@par Dependencies
+
+- libIceUtil (for timing), v.3.2 or newer (latest tested 3.3).
+- @ref gbx_library_gbxserialacfr
+
+@par Installation
+
+- Finding libIceUtil
+ - IceUtil will be found automatically if installed in one of several standard locations.
+ - You can specify it's installation point with @c ICEUTIL_HOME CMake variable (or an environment variable with the same name). For example:
+@verbatim
+$ cmake -DICEUTIL_HOME=/home/myuser/install .
+@endverbatim
+
+*/
+
+
+/*!
+@brief Garmin GPS driver
+@namespace gbxgarminacfr
+
+This namespace is part of a Garmin GPS driver.
+
+@see @ref gbx_library_gbxgarminacfr
+
+*/
Added: gearbox/trunk/submitted/gbxgarminacfr/driver.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/driver.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/driver.cpp 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,404 @@
+/*
+ * 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
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <iostream>
+#include <sstream>
+#include <gbxsickacfr/gbxutilacfr/gbxutilacfr.h>
+// #include <IceUtil/IceUtil.h>
+// #include <gbxsickacfr/gbxiceutilacfr/gbxiceutilacfr.h>
+#include <cstdlib>
+#include <sys/time.h>
+#include <time.h>
+#include <errno.h>
+
+#include "driver.h"
+
+using namespace std;
+using namespace gbxgarminacfr;
+
+bool
+Config::isValid() const
+{
+ if ( device.empty() ) return false;
+
+ return true;
+}
+
+std::string
+Config::toString() const
+{
+ std::stringstream ss;
+ ss << "Garmin driver config: device="<<device;
+ return ss.str();
+}
+
+/////////////////////
+
+Driver::Driver( const Config &config,
+ gbxsickacfr::gbxutilacfr::Tracer &tracer,
+ gbxsickacfr::gbxutilacfr::Status &status ) :
+ config_(config),
+ tracer_(tracer),
+ status_(status)
+{
+ if ( !config_.isValid() )
+ {
+ stringstream ss;
+ ss << __func__ << "(): Invalid config: " << config_.toString();
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO, ss.str() );
+ }
+
+ stringstream ssDebug;
+ 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 );
+ int baud = 4800;
+
+ // according to Duncan, the first 5 initialization messages come in 1 sec.
+ // 2 secs should be conservative.
+ serial_.reset( new gbxserialacfr::Serial( config_.device, baud, gbxserialacfr::Serial::Timeout(2,0) ) );
+
+ init();
+}
+
+Driver::~Driver()
+{
+ 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 )
+ {
+ stringstream ss;
+ ss << "Driver: Caught SerialException: " << e.what();
+ tracer_.error( ss.str() );
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO, ss.str() );
+ }
+}
+
+//*****************************************************************************
+
+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);
+ gbxgpsutilacfr::NmeaMessage Start_GGA_Msg("$PGRMO,GPGGA,1*xx\r\n",gbxgpsutilacfr::AddChecksum);
+ gbxgpsutilacfr::NmeaMessage Start_VTG_Msg("$PGRMO,GPVTG,1*xx\r\n",gbxgpsutilacfr::AddChecksum);
+ gbxgpsutilacfr::NmeaMessage Start_RME_Msg("$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum);
+
+ tracer_.info("Configure Garmin GPS device");
+
+
+ //First disables all output messages then enable selected ones only.
+ serial_->writeString(DisableAllMsg.sentence());
+ sleep(1);
+
+ serial_->writeString(Start_GGA_Msg.sentence());
+ serial_->writeString(Start_VTG_Msg.sentence());
+ serial_->writeString(Start_RME_Msg.sentence());
+ 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());
+}
+
+
+
+
+//****************************************************************************
+// Read one complete frame of data. IE all the messages that we need before returning the data.
+
+void
+Driver::readFrame(Data& GpsData)
+{
+
+ char serial_data[1024];
+ 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,1024);
+ tracer_.debug( serial_data, 10 );
+
+// timeOfRead_ = IceUtil::Time::now();
+// gbxsickacfr::gbxiceutilacfr::now( timeOfReadSec_, timeOfReadUsec_ );
+ 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) {
+ throw gbxsickacfr::gbxutilacfr::Exception( ERROR_INFO,"Driver: Read 0 bytes from serial port");
+ }
+
+ //
+ // We successfully read something from the serial port
+ //
+
+
+ //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,gbxgpsutilacfr::TestChecksum);
+ }
+ catch (gbxgpsutilacfr::NmeaException &e){
+ //Don't throw if only occasional messages are missing the checksums
+ if(nmeaExceptionCount++ < 3) {return;}
+ stringstream ss;
+ ss << "MainThread: 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" );
+ }
+ }
+
+ //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");
+ }
+
+ }
+
+ tracer_.debug("GPS got a complete frame\n", 10 );
+
+ // Hand the data back to the outside world
+ GpsData=gpsData_;
+}
+
+
+
+
+//**********************************************************************************
+
+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;
+
+}
Added: gearbox/trunk/submitted/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/driver.h (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/driver.h 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,179 @@
+/*
+ * 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
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef GBXGARMINACFR_DRIVER_H
+#define GBXGARMINACFR_DRIVER_H
+
+#include <gbxserialacfr/serial.h>
+#include <gbxgarminacfr/gbxgpsutilacfr/nmea.h>
+#include <memory>
+
+namespace gbxgarminacfr {
+
+//! Configuration structure
+class Config
+{
+public:
+ Config() {};
+ bool isValid() const;
+ std::string toString() const;
+
+ //! Serial device. e.g. "/dev/ttyS0"
+ std::string device;
+};
+
+
+//! 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 {
+ //! Invalid or not available
+ GpsPositionTypeNotAvailable,
+ //! Autonomous position
+ //! (This is the normal case for non-differential GPS)
+ GpsPositionTypeAutonomous,
+ //! 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
+};
+
+//! Gps data structure
+struct Data
+{
+ //! 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.
+ //! Hour [0..23]
+ int utcTimeHrs;
+ //! UTC time (according to GPS device), reference is Greenwich.
+ //! Minutes [0..59]
+ int utcTimeMin;
+ //! UTC time (according to GPS device), reference is Greenwich.
+ //! Seconds [0.0..59.9999(9)]
+ double utcTimeSec;
+
+ //! Latitude (degrees)
+ double latitude;
+ //! Longitude (degrees)
+ double longitude;
+ //! 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;
+
+ //! Number of satellites
+ int satellites;
+ int observationCountOnL1;
+ int observationCountOnL2;
+ //! Position type (see above)
+ PositionType positionType;
+ //! Geoidal Separation (metres)
+ double geoidalSeparation;
+};
+
+
+//! Garmin driver
+class Driver
+{
+
+public:
+
+ //! Constructor
+ //!
+ //! 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();
+
+
+ //! Blocks till new data is available
+ void read( Data &data );
+
+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_;
+};
+
+} // namespace
+
+#endif
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/CMakeLists.txt 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,29 @@
+SET( lib_name GbxGpsUtilAcfr )
+GBX_ADD_LICENSE( LGPL )
+
+SET( build TRUE )
+# don't give user an option (while it's an internal library)
+# GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+
+SET( int_libs GbxUtilAcfr )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
+
+IF( build )
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+ FILE( GLOB hdrs *.h )
+ FILE( GLOB srcs *.cpp )
+ SET( dep_libs ${int_libs} ${ext_libs} )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+
+ TARGET_LINK_LIBRARIES( ${lib_name} ${dep_libs} )
+
+ GBX_ADD_HEADERS( gbxgarminacfr/gbxgpsutilacfr ${hdrs} )
+
+ IF( GBX_BUILD_TESTS )
+ ADD_SUBDIRECTORY( test )
+ ENDIF( GBX_BUILD_TESTS )
+
+ENDIF( build )
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.cpp 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,349 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Matthew Ridley
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <cmath>
+using std::sqrt;
+using std::floor;
+using std::cos;
+using std::sin;
+using std::tan;
+using std::pow;
+
+#include <iostream>
+// #include <hydroportability/windows.h>
+
+#include "latlon2mga.h"
+
+namespace gbxgpsutilacfr{
+
+// LatLon_2_MGA convert (lat,lon) in degrees to (Northing, Easting) in meters
+void LatLon2MGA( double lat,
+ double lon,
+ double& Northing,
+ double& Easting,
+ int& Zone,
+ EGeodModel geodmodel) {
+ const double Lon_WE_Z0 = -186.0; // [], Longitude of western edge of zone zero
+ const double ZoneWidth = 6.0; // [], inverse zone width
+ const double inv_ZoneWidth = 1.0/ZoneWidth; // [1/], inverse zone width
+ const double Lon_cM_Z0 = Lon_WE_Z0 + 0.5*ZoneWidth; // [], Longitude of the central meridian of zone 0
+ const double False_Easting = 500000.0; // [m], False Easting
+ const double False_Northing = 10000000.0; // [m], False Northing
+ const double K0 = 0.9996; // Central Scale Factor
+
+ const double inv_6 = 1.0/6.0;
+ const double inv_24 = 1.0/24.0;
+ const double inv_120 = 1.0/120.0;
+ const double inv_720 = 1.0/720.0;
+ const double inv_5040 = 1.0/5040.0;
+ const double inv_40320 = 1.0/40320.0;
+
+ const double D2R = M_PI/180.0; // degrees -> radians conversion factor
+
+ static EGeodModel GeodModel = GM_UNDEFINED; // Geod model
+
+ static double a; // [m], Semi major axis
+ static double b; // [m], Semi minor axis
+ static double f; // flatening
+ static double e; // Eccentricity
+ static double e2, e4, e6; // Eccentricity powers
+ static double A0, A2, A4, A6; // Meridian Distance calculation parameters
+
+ double LonCM; // Longitude of the central meridian of calculation zone
+ double cLat, cLat2, cLat4, cLat6;
+ double sLat, sLat2;
+ double tLat, tLat2, tLat4, tLat6;
+ double omega, omega2, omega4, omega6, omega8;
+ double m, nu, Lat, Lon;
+ // double rho;
+ double t1, t2, t3, t4;
+ double psi, psi2, psi3, psi4;
+
+ if (GeodModel != geodmodel) {
+ GeodModel = geodmodel;
+
+ // Get Elipsoid Parameters, a = Semi major axis, f = flatening
+ switch(GeodModel) {
+ case GM_WGS84:
+ a = 6378137.0;
+ f = 1.0/298.257223563;
+ break;
+
+ case GM_GDA94:
+ a = 6378137.0;
+ f = 1.0/298.257222101;
+ break;
+
+ case GM_AGD84:
+ a = 6378160.0;
+ f = 1.0/298.25;
+ break;
+
+ case GM_WGS72:
+ a = 6378135.0;
+ f = 1.0/298.26;
+ break;
+
+ case GM_NSWC_9Z2:
+ a = 6378145.0;
+ f = 1.0/298.25;
+ break;
+
+ case GM_Clarke:
+ a = 20926348.0*0.3048;
+ f = 1.0/294.26;
+ break;
+ default:
+ std::cout << "The parameters for this GeoModel type have not been implemented" << std::endl;
+ break;
+ }
+
+ // Semi minor axis (m)
+ b = a*(1.0-f);
+
+ // Eccentricity
+ e = sqrt(2.0*f - f*f);
+ e2 = e*e;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ // Coeficients
+ A0 = 1.0 - (e2/4.0) - (3.0*e4/64.0) - (5.0*e6/256.0);
+ A2 = (3.0/8.0)*(e2 + e4/4.0 + 15*e6/128.0);
+ A4 = (15.0/256.0)*(e4 + 3.0*e6/4.0);
+ A6 = 35.0*e6/3072.0;
+ }
+
+ // calculate zone parameters
+ Zone = (int)floor((lon - Lon_WE_Z0)*inv_ZoneWidth);
+ LonCM = (Lon_cM_Z0 + ZoneWidth*(double)Zone)*D2R;
+
+ // Convert to radian
+ Lat = lat*D2R;
+ Lon = lon*D2R;
+
+ // Convert to MGA
+ cLat = cos(Lat);
+ sLat = sin(Lat);
+ tLat = tan(Lat);
+
+ omega = Lon - LonCM;
+
+ // Powers
+ cLat2 = cLat*cLat;
+ cLat4 = cLat2*cLat2;
+ cLat6 = cLat4*cLat2;
+ sLat2 = sLat*sLat;
+ tLat2 = tLat*tLat;
+ tLat4 = tLat2*tLat2;
+ tLat6 = tLat4*tLat2;
+ omega2 = omega*omega;
+ omega4 = omega2*omega2;
+ omega6 = omega4*omega2;
+ omega8 = omega4*omega4;
+
+ // Meridian Radius
+ m = a*(A0*Lat - A2*sin(2.0*Lat) + A4*sin(4.0*Lat) - A6*sin(6.0*Lat));
+
+ // Radius of Curvature
+ t1 = 1.0 - e2*sLat2;
+// rho = a*(1.0 - e2)/(pow((1.0 - e2*sLat2),1.5));
+ nu = a/(pow(t1,0.5)); // = a/(pow((1.0 - e2*sLat2),0.5));
+ psi = t1/(1.0 - e2); // = nu/rho;
+ psi2 = psi*psi;
+ psi3 = psi2*psi;
+ psi4 = psi2*psi2;
+
+ // Easting
+ t1 = (inv_6*omega2)*cLat2*(psi - tLat2);
+ t2 = (inv_120*omega4)*cLat4*(4.0*psi3*(1.0 - 6.0*tLat2) + psi2*(1.0 + 8.0*tLat2) - 2.0*psi*tLat2 + tLat4);
+ t3 = (inv_5040*omega6)*cLat6*(61.0 - 479.0*tLat2 + 179.0*tLat4 - tLat6);
+
+// double Ehat = (K0*nu*omega*cLat)*(1.0 + t1 + t2 + t3);
+// Easting = Ehat + False_Easting;
+ Easting = (K0*nu*omega*cLat)*(1.0 + t1 + t2 + t3) + False_Easting;
+
+ // Northing
+ t1 = (0.5*omega2);
+ t2 = (inv_24*omega4)*cLat2*(4.0*psi2 + psi - tLat2);
+ t3 = (inv_720*omega6)*cLat4*(8.0*psi4*(11.0 - 24.0*tLat2) - 28.0*psi3*(1.0-6.0*tLat2) + psi2*(1.0-32.0*tLat2) - psi*(2.0*tLat2) + tLat4);
+ t4 = (inv_40320*omega8)*cLat6*(1385.0 - 3111.0*tLat2 + 543.0*tLat4 - tLat6);
+
+// double Nhat = K0*(m + nu*sLat*cLat*(t1 + t2 + t3 + t4));
+// Northing = Nhat + False_Northing;
+ Northing = K0*(m + nu*sLat*cLat*(t1 + t2 + t3 + t4)) + False_Northing;
+}
+
+// MGA_2_LatLon convert (Northing, Easting) in meters to (lat,lon) in degrees
+void MGA2LatLon( double Northing,
+ double Easting,
+ int Zone,
+ double& lat,
+ double& lon,
+ EGeodModel geodmodel) {
+ const double Lon_WE_Z0 = -186.0; // [], Longitude of western edge of zone zero
+ const double ZoneWidth = 6.0; // [], inverse zone width
+// const double inv_ZoneWidth = 1.0/ZoneWidth; // [1/], inverse zone width
+ const double Lon_cM_Z0 = Lon_WE_Z0 + 0.5*ZoneWidth; // [], Longitude of the central meridian of zone 0
+ const double False_Easting = 500000.0; // [m], False Easting
+ const double False_Northing = 10000000.0; // [m], False Northing
+ const double K0 = 0.9996; // Central Scale Factor
+
+ const double c9_4 = 9.0/4.0;
+ const double c225_64 = 225.0/64.0;
+ const double c27_32 = 27.0/32.0;
+ const double c21_16 = 21.0/16.0;
+ const double c55_32 = 55.0/32.0;
+ const double c151_96 = 151.0/96.0;
+ const double c1097_512 = 1097.0/512.0;
+
+ const double inv_6 = 1.0/6.0;
+ const double inv_24 = 1.0/24.0;
+ const double inv_120 = 1.0/120.0;
+ const double inv_720 = 1.0/720.0;
+ const double inv_5040 = 1.0/5040.0;
+ const double inv_40320 = 1.0/40320.0;
+
+ const double D2R = M_PI/180.0;
+ const double R2D = 180.0/M_PI;
+
+ static EGeodModel GeodModel = GM_UNDEFINED; // Geod model
+
+ static double a; // [m], Semi major axis
+ static double b; // [m], Semi minor axis
+ static double f; // flatening
+ static double e; // Eccentricity
+ static double e2, e4, e6; // Eccentricity powers
+ static double n, n2, n3, n4, G;
+
+ double LonCM; // Longitude of the central meridian of calculation zone
+ double psip, psip2, psip3, psip4;
+ double tp, tp2, tp4, tp6;
+ double m, sigma, phip, sphip2, rhop, nup, Ep, EtKr, Secphip;
+ // double sphip;
+ double x, x3, x5, x7;
+ double t1, t2, t3, t4;
+ double Lat, Lon;
+
+ if (GeodModel != geodmodel) {
+ GeodModel = geodmodel;
+
+ // Get Elipsoid Parameters, a = Semi major axis, f = flatening
+ switch(GeodModel) {
+ case GM_WGS84:
+ a = 6378137.0;
+ f = 1.0/298.257223563;
+ break;
+
+ case GM_GDA94:
+ a = 6378137.0;
+ f = 1.0/298.257222101;
+ break;
+
+ case GM_AGD84:
+ a = 6378160.0;
+ f = 1.0/298.25;
+ break;
+
+ case GM_WGS72:
+ a = 6378135.0;
+ f = 1.0/298.26;
+ break;
+
+ case GM_NSWC_9Z2:
+ a = 6378145.0;
+ f = 1.0/298.25;
+ break;
+
+ case GM_Clarke:
+ a = 20926348.0*0.3048;
+ f = 1.0/294.26;
+ break;
+ default:
+ std::cout << "The parameters for this GeoModel type have not been implemented" << std::endl;
+ break;
+ }
+
+ // Semi minor axis (m)
+ b = a*(1.0-f);
+
+ // Eccentricity
+ e = sqrt(2.0*f - f*f);
+ e2 = e*e;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ n = f/(2.0 - f); // = (a-b)/(a+b)
+ n2 = n*n;
+ n3 = n*n2;
+ n4 = n2*n2;
+ G = a*(1.0 - n)*(1.0 - n2)*(1.0 + c9_4*n2 + c225_64*n4)*D2R;
+ }
+
+ // calculate zone parameters
+ LonCM = (Lon_cM_Z0 + ZoneWidth*(double)Zone)*D2R;
+
+ // Meridian Radius
+// Np = Northing - False_Northing;
+ m = (Northing - False_Northing)/K0;
+
+ // Foot-point Latitude
+ sigma = m/G*D2R;
+ phip = sigma + (1.5*n - c27_32*n3)*sin(2.0*sigma) + (c21_16*n2 - c55_32*n4)*sin(4.0*sigma) + c151_96*n3*sin(6.0*sigma) + c1097_512*n4*sin(8.0*sigma);
+
+ // Radius of Curvature
+ sphip2 = sin(phip);
+ sphip2 *= sphip2;
+ rhop = a*(1.0 - e2)/(pow((1.0 - e2*sphip2),1.5));
+ nup = a/(pow((1.0 - e2*sphip2),0.5));
+ psip = nup/rhop;
+ psip2 = psip*psip;
+ psip3 = psip2*psip;
+ psip4 = psip2*psip2;
+ tp = tan(phip);
+ tp2 = tp*tp;
+ tp4 = tp2*tp2;
+ tp6 = tp2*tp4;
+
+ // Latitude
+ Ep = Easting - False_Easting;
+ EtKr = Ep*tp/(K0*rhop);
+ x = Ep/(K0*nup);
+ x7 = x*x; // used as temp, x2
+ x3 = x7*x;
+ x5 = x3*x7;
+ x7 *= x5;
+
+ t1 = 0.5*x*EtKr;
+ t2 = inv_24*x3*EtKr*(-4.0*psip2 + 9.0*psip*(1.0 - tp2) + 12.0*tp2);
+ t3 = inv_720*x5*EtKr*(8.0*psip4*(11.0 - 24.0*tp2) - 12.0*psip3*(21.0 - 71.0*tp2) + 15.0*psip2*(15.0 - 98.0*tp2 + 15.0*tp4) + 180.0*psip*(5.0*tp2 - 3.0*tp4) + 360.0*tp4);
+ t4 = inv_40320*x7*EtKr*(1385.0 + 3633.0*tp2 + 4095.0*tp4 + 1575.0*tp6);
+ // The following line use to be:
+ // lat = phip - t1 + t2 - t3 + t4;
+ // which didn't make sense as lat was assigned two different values twice. Changed it so
+ // that compiler warnings were removed but hasn't been tested
+ Lat = phip - t1 + t2 - t3 + t4;
+ lat = Lat*R2D;
+
+ // Longitude
+ Secphip = 1.0/cos(phip);
+ t1 = x*Secphip;
+ t2 = inv_6*x3*Secphip*(psip + 2.0*tp2);
+ t3 = inv_120*x5*Secphip*(psip3*(-4.0 + 24.0*tp2) + psip2*(9.0 - 68.0*tp2) + 72.0*psip*tp2 + 24.0*tp4);
+ t4 = inv_5040*x7*Secphip*(61.0 + 662.0*tp2 + 1320.0*tp4 + 720.0*tp6);
+ // The following line use to be:
+ // lon = LonCM + t1 - t2 + t3 - t4;
+ // which didn't make sense as lon was assigned two different values twice. Changed it so
+ // that compiler warnings were removed but hasn't been tested
+ Lon = LonCM + t1 - t2 + t3 - t4;
+ lon = Lon*R2D;
+}
+
+} //namespace
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.h
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.h (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/latlon2mga.h 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,65 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Matthew Ridley
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+// #include <hydroportability/sharedlib.h>
+
+#ifndef LATLON2MGA_H
+#define LATLON2MGA_H
+
+namespace gbxgpsutilacfr {
+
+ // enumeration of geod models
+// SOEXPORT enum EGeodModel {
+ enum EGeodModel {
+ GM_UNDEFINED=-1,
+ GM_WGS84 =0,
+ GM_GDA94,
+ GM_WGS72,
+ GM_AGD84,
+ GM_NSWC_9Z2,
+ GM_Clarke,
+ GM_NoOptions,
+ GM_GRS80=GM_GDA94,
+ GM_ANS=GM_AGD84 };
+
+ // Geod model
+// SOEXPORT typedef struct {
+ typedef struct {
+ double a; // Semi major axis (m)
+ double b; // Semi minor axis (m)
+ double f; // flatening
+ double e; // Eccentricity
+ double e2; // Eccentricity^2
+ double A0, A2, A4, A6; // Meridian Distance calculation parameters
+ } TGeoModelData;
+
+ // LatLon_2_MGA convert (lat,lon) in degrees to (Northing, Easting) in meters
+// SOEXPORT void LatLon2MGA(
+ void LatLon2MGA(
+ double lat,
+ double lon,
+ double& Northing,
+ double& Easting,
+ int& Zone,
+ EGeodModel geodmodel = GM_WGS84);
+
+ // MGA_2_LatLon convert (Northing, Easting) in meters to (lat,lon) in degrees
+// SOEXPORT void MGA2LatLon(
+ void MGA2LatLon(
+ double Northing,
+ double Easting,
+ int Zone,
+ double& lat,
+ double& lon,
+ EGeodModel geodmodel = GM_WGS84);
+
+} //namespace
+
+#endif
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.cpp 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,212 @@
+/*
+ * 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 <gbxsickacfr/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()
+{
+ haveSentence_ = false;
+ haveTokens_ = false;
+ haveCheckSum_ = false;
+ checkSumOK_ = false;
+
+ // Now clear the internal data store
+ sentence_[0] = 0;
+ dataTokens_.clear();
+}
+
+
+//****************************************************************
+NmeaMessage::NmeaMessage(const char *sentence, int testCheckSum)
+{
+ init();
+ setSentence(sentence,testCheckSum);
+}
+
+
+//**********************************************************************
+//Load the data as requested and test the checksum if we are asked to.
+void NmeaMessage::setSentence(const char *data, int AddOrTestCheckSum)
+{
+ init();
+
+ strncpy(sentence_,data, MAX_SENTENCE_LEN);
+
+ //terminate just in case, Note that we have a buffer which is
+ //MAX_SENTENCE_LEN + 1 long!
+
+ sentence_[MAX_SENTENCE_LEN] = '\0';
+ haveSentence_ = true;
+
+ switch(AddOrTestCheckSum)
+ {
+ case TestChecksum: //This is for Rx'd data that we need to test for correct reception
+ testChecksumOk(); break;
+ case AddChecksum: //This is for Tx data that needs to checksummed before sending
+ addCheckSum(); checkSumOK_ = true; break;
+ case DontTestOrAddChecksum:
+ break;
+ default:
+ assert(true);
+ }
+
+}
+
+
+//*****************************************************************
+bool NmeaMessage::testChecksumOk()
+{
+ haveCheckSum_ = true;
+ checkSumOK_ = false;
+
+ //First save the checksum chars from the existing message
+ char* ptr;
+ char chksum_HIB,chksum_LOB;
+
+ //First save the existing two checksum chars from the message
+ //These are straight after the '*' character
+ ptr = strchr(sentence_, NMEAChecksumDelim);
+ if(!ptr){return false;}
+
+ //save the high and low bytes of the checksum
+ //Make sure they are in upper case!
+ chksum_HIB = toupper(*(++ptr));
+ chksum_LOB = toupper(*(ptr + 1));
+
+
+ //invalidate the existing checksum
+ *ptr = *(ptr+1) = 'x';
+
+ //****NOTE** We leave the ptr pointing at the first chksum byte
+
+ //Re-calculate our own copy of the checksum
+ addCheckSum();
+
+ //Now compare our saved version with our new ones
+ if((chksum_HIB == *ptr) && (chksum_LOB == *(ptr+1))){
+ //all looked good!
+ checkSumOK_ = true;
+ return true;
+ }
+
+ //failed the checksum!
+ 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_ );
+
+ haveCheckSum_ = true;
+
+ //check that we have the '$' at the start
+ if(sentence_[0]!= NMEAStartOfSentence)
+ {return;}
+
+ unsigned char chkRunning = 0;
+
+ int loopCount;
+ unsigned char nextChar;
+ for( loopCount =1; loopCount < MAX_SENTENCE_LEN; loopCount++){
+
+ nextChar = static_cast<unsigned char>(sentence_[loopCount]);
+
+ // no delimiter uh oh
+ if((nextChar=='\r')||(nextChar=='\n')||(nextChar=='\0')){
+ throw NmeaException("nmea: cannot calculate checksum, missing '*'\n");
+ return;
+ }
+
+ // goodie we found it
+ if(nextChar==NMEAChecksumDelim)
+ {break;}
+
+ //Keep the running total going
+ chkRunning ^= nextChar;
+ }
+
+ //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);
+
+ //Split the message at the commas
+ //TODO cope with missing fields
+ dataTokens_ = gbxsickacfr::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;
+
+}
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/nmea.h 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,121 @@
+/*
+ * 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.
+ *
+ */
+
+#include <vector>
+#include <string>
+
+// #include <hydroportability/sharedlib.h>
+
+#ifndef GBXGPSUTILACFR_NMEA_H
+#define GBXGPSUTILACFR_NMEA_H
+
+
+
+/*
+
+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{TestChecksum, AddChecksum, DontTestOrAddChecksum};
+
+// class SOEXPORT NmeaMessage{
+ 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];};
+
+ // Return the number of fields
+ int numDataTokens(){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
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/CMakeLists.txt 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,6 @@
+INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+LINK_LIBRARIES( GbxGpsUtilAcfr )
+
+ADD_EXECUTABLE( testmga testmga.cpp )
+GBX_ADD_TEST( GbxGpsUtilAcfrTestMga testmga )
Added: gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/testmga.cpp
===================================================================
--- gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/testmga.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxgarminacfr/gbxgpsutilacfr/test/testmga.cpp 2008-04-26 06:42:07 UTC (rev 127)
@@ -0,0 +1,54 @@
+/*
+ * 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 <gbxgarminacfr/gbxgpsutilacfr/latlon2mga.h>
+#include <iostream>
+#include <cmath>
+#include <cstdlib>
+
+using namespace std;
+using namespace gbxgpsutilacfr;
+
+bool close( double a, double b )
+{
+ if ( fabs(a-b) > 1e-3 )
+ {
+ cout<<"TRACE(testmga.cpp): diff: " << a-b << endl;
+ return false;
+ }
+ return true;
+}
+
+int main()
+{
+ double northing, easting;
+
+ double lat = -33.8895;
+ double lon = 151.193;
+
+ cout<<"TRACE(testmga.cpp): lat, lon: " << lat << ", " << lon << endl;
+
+ int zone;
+ LatLon2MGA( lat, lon, northing, easting, zone );
+ cout<<"TRACE(testmga.cpp): Using LatLon2MGA: northing,easting = " << northing << ", " << easting << endl;
+
+ double backlat, backlon;
+ MGA2LatLon( northing, easting, zone, backlat, backlon );
+ cout<<"TRACE(testmga.cpp): Converting back: lat,lon = " << backlat << ", " << backlon << endl;
+
+ if ( ! ( close(lat,backlat) && close(lon,backlon) ) )
+ {
+ cout << "ERROR(testmga.cpp): latlon->mga->latlon is broken." << endl;
+ exit(1);
+ }
+
+ cout<<"TRACE(testmga.cpp): test PASSED" << endl;
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|