Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox: [183] gearbox/trunk
|
From: <rum...@us...> - 2008-06-18 07:53:39
|
Revision: 183
http://gearbox.svn.sourceforge.net/gearbox/?rev=183&view=rev
Author: rumataxyz
Date: 2008-06-18 00:53:46 -0700 (Wed, 18 Jun 2008)
Log Message:
-----------
First commit of the novatel driver:
-compiles
-missing some functionality:
Config isValid() toString()
various data classes toString()
internal to external data types missing some conversions
-driver.h probably needs to be split into a config part and a data part
-NOT tested with hardware
Modified Paths:
--------------
gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/CMakeLists.txt
gearbox/trunk/submitted/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/submitted/gbxnovatelacfr/
gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
gearbox/trunk/submitted/gbxnovatelacfr/driver.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/crc32.cpp
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/crc32.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.cpp
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/novatelmessages.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.cpp
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/crc32test.cpp
gearbox/trunk/submitted/gbxnovatelacfr/novatel.dox
gearbox/trunk/submitted/gbxnovatelacfr/test/
gearbox/trunk/submitted/gbxnovatelacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxnovatelacfr/test/example.cmake.in
gearbox/trunk/submitted/gbxnovatelacfr/test/example.readme
gearbox/trunk/submitted/gbxnovatelacfr/test/test.cpp
Modified: gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2008-06-18 07:12:28 UTC (rev 182)
+++ gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2008-06-18 07:53:46 UTC (rev 183)
@@ -5,18 +5,16 @@
GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "only Linux OS is supported" )
-INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
-GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+ADD_SUBDIRECTORY( gbxiceutilacfr )
-SET( int_libs GbxUtilAcfr GbxSerialAcfr )
+SET( int_libs GbxUtilAcfr GbxIceUtilAcfr GbxSerialAcfr )
GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
# these are built internally
-SET( private_libs GbxIceUtilAcfr GbxSerialDeviceAcfr )
+SET( private_libs GbxSerialDeviceAcfr )
IF( build )
- ADD_SUBDIRECTORY( gbxiceutilacfr )
ADD_SUBDIRECTORY( gbxserialdeviceacfr )
INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
Modified: gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/CMakeLists.txt 2008-06-18 07:12:28 UTC (rev 182)
+++ gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/CMakeLists.txt 2008-06-18 07:53:46 UTC (rev 183)
@@ -2,19 +2,14 @@
GBX_ADD_LICENSE( LGPL )
SET( build TRUE )
-# don't give user an option
-# GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
-# this was already tested in the dir above
-# GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "only Linux OS is supported" )
+GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
-# this was already tested in the dir above
-# INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
-# GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
+GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
-# this lib is currently internal
SET( proj_libs GbxUtilAcfr )
-# GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${proj_libs} )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${proj_libs} )
IF( build )
@@ -22,7 +17,7 @@
INCLUDE( ${GBX_CMAKE_DIR}/UseIceUtil.cmake )
FILE( GLOB hdrs *.h )
- FILE( GLOB srcs *.cpp ) # detail/*.cpp )
+ FILE( GLOB srcs *.cpp )
SET( dep_libs ${proj_libs} )
# for config.h
Modified: gearbox/trunk/submitted/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/CMakeLists.txt 2008-06-18 07:12:28 UTC (rev 182)
+++ gearbox/trunk/submitted/CMakeLists.txt 2008-06-18 07:53:46 UTC (rev 183)
@@ -11,5 +11,6 @@
# Otherwise, maintain alphabetical order.
# E.g. ADD_SUBDIRECTORY( mydir )
+ ADD_SUBDIRECTORY( gbxnovatelacfr )
-ENDIF( GBX_BUILD_SUBMITTED )
\ No newline at end of file
+ENDIF( GBX_BUILD_SUBMITTED )
Added: gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt 2008-06-18 07:53:46 UTC (rev 183)
@@ -0,0 +1,38 @@
+SET( lib_name GbxNovatelAcfr )
+GBX_ADD_LICENSE( LGPL )
+
+SET( build TRUE )
+GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+
+INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
+GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+
+SET( int_libs GbxUtilAcfr GbxSerialAcfr )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
+
+# built internally
+SET( private_libs GbxNovatelUtilAcfr )
+
+IF( build )
+
+ ADD_SUBDIRECTORY( gbxnovatelutilacfr)
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+ INCLUDE( ${GBX_CMAKE_DIR}/UseIceUtil.cmake )
+
+ FILE( GLOB hdrs *.h )
+ FILE( GLOB srcs *.cpp )
+ SET( dep_libs ${int_libs} ${private_libs} )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+ TARGET_LINK_LIBRARIES( ${lib_name} ${dep_libs} )
+ GBX_ADD_PKGCONFIG( ${lib_name} "Drives NOVATEL hardware, directly connected to the computer." proj_libs int_libs "" "" )
+
+ GBX_ADD_HEADERS( gbxnovatelacfr ${hdrs} )
+
+ IF( GBX_BUILD_TESTS )
+ ADD_SUBDIRECTORY( test )
+ ENDIF( GBX_BUILD_TESTS )
+
+
+ENDIF( build )
Added: gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp 2008-06-18 07:53:46 UTC (rev 183)
@@ -0,0 +1,813 @@
+/*
+ * Orca-Robotics Project: Components for robotics
+ * http://orca-robotics.sf.net/
+ * Copyright (c) 2004-2008 Matthew Ridley, Ben Upcroft
+ *
+ * This copy of Orca is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <gbxnovatelacfr/driver.h>
+#include <gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h>
+#include <gbxnovatelacfr/gbxnovatelutilacfr/novatelmessages.h>
+#include <gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.h>
+#include <gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h>
+#include <gbxnovatelacfr/gbxnovatelutilacfr/crc32.h>
+
+#include <gbxserialacfr/gbxserialacfr.h>
+#include <gbxutilacfr/gbxutilacfr.h>
+
+#include <iostream>
+#include <sstream>
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include <math.h>
+#include <errno.h>
+#include <vector>
+
+#include <sys/time.h>
+#include <time.h>
+
+using namespace std;
+using namespace gbxserialacfr;
+
+namespace gnua = gbxnovatelutilacfr;
+namespace gna = gbxnovatelacfr;
+
+namespace {
+// binary messages defined by novatel
+#pragma pack(push,1)
+ union novatelMessage{
+ struct{
+ gnua::Oem4BinaryHeader header;
+ char data[484];
+ };
+ struct{
+ gnua::Oem4ShortBinaryHeader shortHeader;
+ char shortData[500];
+ };
+ unsigned char rawMessage[512];
+
+ // these guys are used to directly decode messages;
+ // obviously fails on endian mismatch, any sort of size mismatch and is rather nasty in general;
+ // feel free to implement something better
+ gnua::BestGpsPosLogB bestGpsPos;
+ gnua::BestGpsVelLogB bestGpsVel;
+ gnua::InsPvaLogSB insPva;
+ gnua::RawImuLogSB rawImu;
+ };
+#pragma pack(pop)
+ int readNovatelMessage(union novatelMessage &msg, struct timeval &timeStamp, gbxserialacfr::Serial *serial);
+ std::auto_ptr<gna::GenericData> createExternalMsg(gnua::InsPvaLogSB &insPva, struct timeval &timeStamp);
+ std::auto_ptr<gna::GenericData> createExternalMsg(gnua::BestGpsPosLogB &bestGpsPos, struct timeval &timeStamp);
+ std::auto_ptr<gna::GenericData> createExternalMsg(gnua::BestGpsVelLogB &bestGpsVel, struct timeval &timeStamp);
+ std::auto_ptr<gna::GenericData> createExternalMsg(gnua::RawImuLogSB &rawImu, struct timeval &timeStamp, gnua::ImuDecoder *imuDecoder);
+}
+
+namespace gbxnovatelacfr
+{
+Driver::Driver( const Config& cfg,
+ gbxutilacfr::Tracer &tracer) :
+ serial_(0),
+ baud_(115200),
+ config_(cfg),
+ tracer_(tracer)
+{
+ if(false == config_.isValid()){
+ throw (std::string("Invalid Configuration!"));
+ }
+
+ // configure serial port
+ baud_ = config_.baudRate_;
+ std::string serialDevice = config_.serialDevice_;
+ serial_.reset(new Serial( serialDevice, baud_, Serial::Timeout(1,0) ));
+ serial_->setDebugLevel(0);
+ if(0 != connectToHardware() ){
+ throw (std::string("failed to connect to receiver!"));
+ }
+
+ // just in case something is running... stops the novatel logging any messages
+ serial_->writeString( "unlogall\r\n" );
+ serial_->drain();
+ configureImu();
+ configureIns();
+ configureGps();
+ requestData();
+ serial_->flush();
+ tracer_.info("Setup done, starting normal operation!");
+}
+
+Driver::~Driver() {
+ // just in case something is running... stops the novatel logging any messages
+ try{
+ tracer_.info("Stopping NovatelSpan driver!");
+ serial_->flush();
+ serial_->writeString( "unlogall\r\n" );
+ serial_->drain();
+ tracer_.info("NovatelSpan driver stopped!");
+ }
+ catch(...){
+ //no throwing from destructors
+ }
+}
+
+int
+Driver::connectToHardware() {
+ // baudrates we test for; this is
+ // _not_ all the baudrates the receiver
+ // can possible be set to
+ int baudrates[]={
+ 9600,
+ 19200,
+ 38400,
+ 57600,
+ 115200,
+ 230400
+ };
+ int currentBaudrate = 0;
+ bool correctBaudrate = false;
+
+ std::cout << "Trying to hook up to receiver at different Baudrates\n";
+ int maxTry = 4;
+ int successThresh = 4;
+ int timeOutMsec = 150;
+ std::string challenge("unlogall\r\n");
+ std::string ack("<OK");
+ size_t i=0;
+ while(false == correctBaudrate && i<sizeof baudrates/sizeof baudrates[0]){
+ currentBaudrate = baudrates[i];
+ correctBaudrate = gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, currentBaudrate);
+ i++;
+ }
+ if(false == correctBaudrate){
+ std::cout << "\n!Failed to establish a connection to the receiver!\n";
+ std::cout << "Check physical connections; Check manually (minicom) for Baudrates < 9600kb/s.\n\n";
+ return -1;
+ }
+ char str[256];
+ sprintf( str,"com com1 %d n 8 1 n off on\r\n", baud_ );
+ serial_->writeString( str );
+ std::cout << "*******************************\n"
+ << "** Current Speed " << currentBaudrate << "\n"
+ << "** Resetting to " << baud_ << "\n"
+ << "*******************************\n";
+ std::cout << "** Testing new setting\n** ";
+ if(true == gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, baud_)){
+ std::cout << "*******************************\n";
+ return 0;
+ }else{
+ std::cout << "*******************************\n";
+ return -1;
+ }
+}
+
+void
+Driver::configureImu() {
+ int put;
+
+ if(config_.enableImu_){
+ imuDecoder_.reset(gnua::createImuDecoder(config_.imuType_));
+ std::stringstream ss;
+ // tell the novatel what serial port the imu is attached to (com3 == aux)
+ put = serial_->writeString( "interfacemode com3 imu imu on\r\n" );
+ // the type of imu being used
+ ss << "setimutype "
+ << config_.imuType_
+ << "\r\n";
+ put = serial_->writeString( ss.str().c_str() );
+ //force the IMU to re-align at every startup
+ //put = serial_->writeString( "inscommand reset\r\n" );
+ //tracer_.info("Reset IMU; Waiting 5 seconds before continuing!");
+ //sleep(5);
+ }else{
+ // no IMU --> disable INS
+ put = serial_->writeString( "inscommand disable\r\n" );
+ }
+ return;
+}
+
+void
+Driver::configureIns() {
+ int put;
+ if(config_.enableSetImuOrientation_ && config_.enableImu_){
+ std::stringstream ss;
+ // imu orientation constant
+ // this tells the imu where its z axis (up) is pointing. constants defined in manual.
+ // with imu mounted upside down, constant is 6 and axes are remapped: x = y, y = x, -z = z
+ ss << "setimuorientation " << config_.setImuOrientation_ << "\r\n";
+ put = serial_->writeString( ss.str().c_str() );
+ }
+
+ if(config_.enableVehicleBodyRotation_ && config_.enableImu_){
+ std::stringstream ss;
+ // vehicle to imu body rotation
+ // angular offset from the vehicle to the imu body. unclear how this relates to imu orientation command
+ // the novatel docs are not especially clear on this stuff; It's highly recommended to mount the IMU
+ // exactly as advised by novatel and just ignore this
+ ss << "vehiclebodyrotation "
+ << config_.vehicleBodyRotation_[0]
+ << config_.vehicleBodyRotation_[1]
+ << config_.vehicleBodyRotation_[2];
+ if(3 == config_.vehicleBodyRotationUncertainty_.size()){
+ // optional, vehicle to imu body rotation uncertainty
+ ss << config_.vehicleBodyRotationUncertainty_[0]
+ << config_.vehicleBodyRotationUncertainty_[1]
+ << config_.vehicleBodyRotationUncertainty_[2];
+ }
+ ss << "\r\n";
+ put = serial_->writeString( ss.str().c_str() );
+ }
+
+ if(config_.enableImu_){
+ std::stringstream ss;
+ // The span system kalman fiter needs this info; make _sure_ you do this right
+ ss << "setimutoantoffset "
+ << config_.imuToGpsOffset_[0]
+ << config_.imuToGpsOffset_[1]
+ << config_.imuToGpsOffset_[2];
+
+ if( 3 == config_.imuToGpsOffsetUncertainty_.size() ){
+ ss << config_.imuToGpsOffsetUncertainty_[0]
+ << config_.imuToGpsOffsetUncertainty_[1]
+ << config_.imuToGpsOffsetUncertainty_[2];
+ }
+ ss << "\r\n";
+ put = serial_->writeString( ss.str().c_str() );
+ }
+ return;
+}
+
+void
+Driver::configureGps() {
+ // hardcoded settings first
+
+ // turn off posave as this command implements position averaging for base stations.
+ int put = serial_->writeString( "posave off\r\n" );
+ // make sure that fixposition has not been set
+ put = serial_->writeString( "fix none\r\n" );
+ // select the geodetic datum for operation of the receiver (wgs84 = default)
+ put = serial_->writeString( "datum wgs84\r\n" );
+ //Let the receiver figure out which range corrections are best
+ put = serial_->writeString( "PSRDIFFSOURCE AUTO\r\n" );
+
+ // CDGPS
+ if(config_.enableCDGPS_){
+ tracer_.info("Turning on CDGPS!");
+ put = serial_->writeString( "ASSIGNLBAND CDGPS 1547547 4800\r\n" );
+ }
+
+ // turn SBAS on/off (essentially global DGPS)
+ if(config_.enableSBAS_){
+ tracer_.info("Turning on SBAS!");
+ put = serial_->writeString( "SBASCONTROL ENABLE Auto 0 ZEROTOTWO\r\n");
+ //we try to use WAAS satellites even below the horizon
+ put = serial_->writeString( "WAASECUTOFF -5.0\r\n");
+ }
+ else{
+ tracer_.info("Turning off SBAS!");
+ put = serial_->writeString( "SBASCONTROL DISABLE Auto 0 NONE\r\n");
+ }
+
+ // rtk
+ if(config_.enableRTK_){
+ tracer_.info("Turning on RTK!");
+ put = serial_->writeString( "com com2,9600,n,8,1,n,off,on\r\n" );
+ put = serial_->writeString( "interfacemode com2 rtca none\r\n" );
+ }
+
+ if(config_.enableUseOfOmniStarCarrier_){
+ //Let the receiver figure out which rtk corrections are best
+ put = serial_->writeString( "RTKSOURCE AUTO\r\n" );
+ }else{
+ //We only use our own rtk corrections; _not_ OmniSTAR HP/XP
+ put = serial_->writeString( "RTKSOURCE RTCA ANY\r\n" );
+ }
+ return;
+}
+
+void
+Driver::requestData() {
+ //we assume that the config_ has been checked at this point (isValid())
+ //so we don't need to check that the rates make sense
+ int put;
+
+ // GPS messages
+
+ // gps position without ins
+ if(config_.enableGpsPos_){
+ std::stringstream ss;
+ ss << "log bestgpsposb ontime " << config_.dtGpsPos_ << "\r\n";
+ put = serial_->writeString(ss.str().c_str());
+ ss.str("");
+ ss << "Turning on GPS position at " << 1.0/config_.dtGpsPos_ << "Hz!";
+ tracer_.info(ss.str().c_str());
+ }
+
+ // gps velocity without ins
+ if(config_.enableGpsVel_){
+ std::stringstream ss;
+ ss << "log bestgpsvelb ontime " << config_.dtGpsVel_ << "\r\n";
+ put = serial_->writeString(ss.str().c_str());
+ ss.str("");
+ ss << "Turning on GPS velocity at " << 1.0/config_.dtGpsVel_ << "Hz!";
+ tracer_.info(ss.str().c_str());
+ }
+
+
+ // INS messages
+
+ // pva data in wgs84 coordinates
+ if(config_.enableInsPva_){
+ std::stringstream ss;
+ ss << "log inspvasb ontime " << config_.dtInsPva_ << "\r\n";
+ put = serial_->writeString(ss.str().c_str());
+ ss.str("");
+ ss << "Turning on INS position/velocity/orientation at " << 1.0/config_.dtInsPva_ << "Hz!";
+ tracer_.info(ss.str().c_str());
+ }
+
+
+ // IMU messages
+
+ // raw accelerometer and gyro data
+ if(config_.enableRawImu_){
+ put = serial_->writeString( "log rawimusb onnew\r\n" );
+ tracer_.info("Turning on raw imu data!");
+ }
+
+ return;
+}
+
+std::auto_ptr<GenericData>
+Driver::read(){
+ union novatelMessage msg;
+ std::auto_ptr<GenericData> data;
+ data.reset(0);
+ struct timeval timeStamp = {0,0};
+
+ // read msg from hardware
+ do{
+ // Timeouts are not adjusted once a serial call returns;
+ // So we could be stuck here for longer than the set timeout.
+ int ret = serial_->bytesAvailableWait();
+ if ( ret >= 0 ) {
+ switch(readNovatelMessage(msg, timeStamp, serial_.get())){
+ case gnua::InsPvaSBLogType:
+ data = createExternalMsg(msg.insPva, timeStamp);
+ break;
+ case gnua::BestGpsVelBLogType:
+ data = createExternalMsg(msg.bestGpsVel, timeStamp);
+ break;
+ case gnua::BestGpsPosBLogType:
+ data = createExternalMsg(msg.bestGpsPos, timeStamp);
+ break;
+ case gnua::RawImuSBLogType:
+ data = createExternalMsg(msg.rawImu, timeStamp, imuDecoder_.get());
+ break;
+ default:
+ {
+ std::stringstream ss;
+ ss << "Warning("<<__FILE__<<":"<< __LINE__
+ <<" : got unexpected message from receiver; id: " << msg.header.msgId << std::endl;
+ if(config_.ignoreUnknownMessages_){
+ tracer_.warning(ss.str());
+ }else{
+ throw( ss.str() );
+ }
+ }
+ break;
+ }
+ }
+ else {
+ std::stringstream ss;
+ ss << "Warning("<<__FILE__<<":"<< __LINE__
+ << "Timed out while waiting for data";
+ throw (ss.str());
+ }
+ }while(NULL == data.get()); // repeat till we get valid data
+
+ return data;
+}
+
+Config::Config(const SimpleConfig &simpleCfg) :
+ serialDevice_(simpleCfg.serialDevice_),
+ baudRate_(simpleCfg.baudRate_),
+ enableImu_(true),
+ imuType_(simpleCfg.imuType_),
+ enableInsPva_(true),
+ enableGpsPos_(true),
+ enableGpsVel_(true),
+ enableRawImu_(true),
+ ignoreUnknownMessages_(false),
+ dtInsPva_(0.02),
+ dtGpsPos_(0.2),
+ dtGpsVel_(0.2),
+ fixInvalidRateSettings_(false),
+ imuToGpsOffset_(simpleCfg.imuToGpsOffset_),
+ enableInsOffset_(false),
+ enableInsPhaseUpdate_(true),
+ enableCDGPS_(true),
+ enableSBAS_(true),
+ enableRTK_(true),
+ enableUseOfOmniStarCarrier_(false),
+ enableSetImuOrientation_(false),
+ enableVehicleBodyRotation_(false) {
+}
+Config::Config(const GpsOnlyConfig &gpsOnlyCfg) :
+ serialDevice_(gpsOnlyCfg.serialDevice_),
+ baudRate_(gpsOnlyCfg.baudRate_),
+ enableImu_(false),
+ enableInsPva_(false),
+ enableGpsPos_(true),
+ enableGpsVel_(true),
+ enableRawImu_(false),
+ ignoreUnknownMessages_(false),
+ dtGpsPos_(0.05),
+ dtGpsVel_(0.05),
+ fixInvalidRateSettings_(false),
+ enableCDGPS_(true),
+ enableSBAS_(true),
+ enableRTK_(true),
+ enableUseOfOmniStarCarrier_(false) {
+}
+Config::Config() :
+ serialDevice_(""),
+ baudRate_(0),
+ enableImu_(false),
+ imuType_(""),
+ enableInsPva_(false),
+ enableGpsPos_(false),
+ enableGpsVel_(false),
+ enableRawImu_(false),
+ ignoreUnknownMessages_(false),
+ dtInsPva_(1.0),
+ dtGpsPos_(1.0),
+ dtGpsVel_(1.0),
+ fixInvalidRateSettings_(false),
+ imuToGpsOffset_(1,0.0),
+ imuToGpsOffsetUncertainty_(1,0.0),
+ enableInsOffset_(false),
+ insOffset_(1,0.0),
+ enableInsPhaseUpdate_(false),
+ enableCDGPS_(false),
+ enableSBAS_(false),
+ enableRTK_(false),
+ enableUseOfOmniStarCarrier_(false),
+ enableSetImuOrientation_(false),
+ setImuOrientation_(0),
+ enableVehicleBodyRotation_(false),
+ vehicleBodyRotation_(1,0.0),
+ vehicleBodyRotationUncertainty_(1,0.0) {
+}
+
+bool
+Config::isValid() const {
+ cout << __func__ << "implement me\n";
+ return true;
+}
+
+std::string
+Config::toString(){
+ cout << __func__ << "implement me\n";
+ return "";
+}
+
+bool
+SimpleConfig::isValid() const {
+ cout << __func__ << "implement me\n";
+ return true;
+}
+
+std::string
+SimpleConfig::toString(){
+ cout << __func__ << "implement me\n";
+ return "";
+}
+
+bool
+GpsOnlyConfig::isValid() const {
+ cout << __func__ << "implement me\n";
+ return true;
+}
+
+std::string
+GpsOnlyConfig::toString(){
+ cout << __func__ << "implement me\n";
+ return "";
+}
+
+} //namespace
+
+namespace{
+ int
+ readNovatelMessage(union novatelMessage &msg, struct timeval &timeStamp, gbxserialacfr::Serial *serial) {
+ // read the three sync bytes which are always at the start of the message header
+ unsigned short id;
+ unsigned long crc;
+ unsigned long in_crc;
+ msg.header.sb1 = 0;
+ int skip = -1;
+ int got;
+
+ // read the first sync byte
+ do{
+ got = serial->readFull( &msg.header.sb1, 1 );
+ if ( got <= 0 ) {
+ return got;
+ }
+ if( got>0 ) {
+ skip++;
+ }
+ }while( msg.header.sb1 != 0xaa );
+
+ // get timestamp after the first byte for accuracy
+ gettimeofday(&timeStamp, NULL);
+
+ // read the second sync byte
+ do {
+ got = serial->readFull( &msg.header.sb2, 1 );
+ if ( got <= 0 ) {
+ return got;
+ }
+ }while( got!=1 );
+
+ if( msg.header.sb2 != 0x44 ) {
+ return -1;
+ }
+
+ // read the third sync byte
+ do {
+ got = serial->readFull( &msg.header.sb3, 1 );
+ if ( got <= 0 ) {
+ return got;
+ }
+ }while( got != 1 );
+
+ switch( msg.header.sb3 ) {
+ case 0x12: //long packet
+ if( // how long is the header ?
+ -1 == serial->readFull( &msg.header.headerLength, 1 )
+ // read all of the header...
+ || -1 == serial->readFull( &msg.header.msgId, msg.header.headerLength-4 )
+ // read the message data
+ || -1 == serial->readFull( &msg.data, msg.header.msgLength )
+ || -1 == serial->readFull( &in_crc, 4 )
+ ){
+ return -1;
+ }
+
+ id = msg.header.msgId;
+
+ crc = gnua::crc( msg.rawMessage,
+ msg.header.msgLength+msg.header.headerLength );
+ break;
+
+ case 0x13: //short packet
+ if( // read rest of the header 12 bytes - 3 bytes already read, then the actual data, then the CRC
+ -1 == serial->readFull( &msg.shortHeader.msgLength, 9 )
+ || -1 == serial->readFull( &msg.shortData, msg.shortHeader.msgLength )
+ || -1 == serial->readFull( &in_crc, 4 )
+ ){
+ return -1;
+ }
+
+ id = msg.shortHeader.msgId;
+
+ crc = gnua::crc( msg.rawMessage,msg.shortHeader.msgLength + 12 );
+ break;
+
+ default: //bollocks
+ return -1;
+ }
+
+ if(in_crc != crc) {
+ fprintf( stderr,"CRC Error: 0x%lx, 0x%lx\n",in_crc,crc );
+ throw std::string( "CRC Error" );
+ return -1;
+ }
+
+ return id;
+ }
+
+
+ std::auto_ptr<gna::GenericData>
+ createExternalMsg(gnua::InsPvaLogSB &insPva, struct timeval &timeStamp){
+ //static int cnt;
+ //if(0 == cnt++ % 1000) cout << __func__ << " ins; implement me properly!\n";
+ gna::InsPvaData *data = new gna::InsPvaData;
+ std::auto_ptr<gna::GenericData> genericData( data );
+
+ //data
+ data->gpsWeekNr = insPva.data.gpsWeekNr;
+ data->secIntoWeek = insPva.data.secIntoWeek;
+ data->latitude = insPva.data.latitude;
+ data->longitude = insPva.data.longitude;
+ data->height = insPva.data.height;
+ data->northVelocity = insPva.data.northVelocity;
+ data->eastVelocity = insPva.data.eastVelocity;
+ data->upVelocity = insPva.data.upVelocity;
+ data->roll = insPva.data.roll;
+ data->pitch = insPva.data.pitch;
+ data->azimuth = insPva.data.azimuth;
+
+ //timestamp
+ data->timeStampSec = timeStamp.tv_sec;
+ data->timeStampUSec = timeStamp.tv_usec;
+
+ //status
+ switch( insPva.data.insStatus ) {
+ case 0:
+ data->statusMessage = "Ins is inactive";
+ data->statusMessageType = gna::Fault;
+ break;
+ case 1:
+ data->statusMessage = "Ins is aligning";
+ data->statusMessageType = gna::Warning;
+ break;
+ case 2:
+ data->statusMessage = "Ins solution is bad";
+ data->statusMessageType = gna::Warning;
+ break;
+ case 3:
+ data->statusMessage = "Ins solution is good";
+ data->statusMessageType = gna::Ok;
+ break;
+ case 4://fallthrough
+ case 5:
+ {
+ stringstream ss;
+ ss << "Reserved value?? Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
+ data->statusMessage = ss.str();
+ data->statusMessageType = gna::Warning;
+ }
+ break;
+ case 6:
+ data->statusMessage = "Bad Ins Gps agreement";
+ data->statusMessageType = gna::Warning;
+ break;
+ case 7:
+ data->statusMessage = "Ins alignment is complete but vehicle must perform maneuvers so that the attitude can converge";
+ data->statusMessageType = gna::Ok;
+ break;
+ default:
+ {
+ stringstream ss;
+ ss << "Unknown Ins Status. Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
+ data->statusMessage = ss.str();
+ data->statusMessageType = gna::Warning;
+ }
+ break;
+ }
+
+ return genericData;
+ }
+
+ std::auto_ptr<gna::GenericData>
+ createExternalMsg(gnua::BestGpsPosLogB &bestGpsPos, struct timeval &timeStamp){
+ static int cnt;
+ if(0 == cnt++ % 100) cout << __func__ << " gpspos; implement me properly!\n";
+ gna::BestGpsPosData *data = new gna::BestGpsPosData;
+ std::auto_ptr<gna::GenericData> genericData( data );
+
+ //data
+ data->gpsWeekNr = bestGpsPos.header.gpsWeekNr;
+ data->msIntoWeek = bestGpsPos.header.msIntoWeek;
+ //data->solutionStatus = bestGpsPos.data.solutionStatus;
+ //data->positionType = bestGpsPos.data.positionType;
+ data->latitude = bestGpsPos.data.latitude;
+ data->longitude = bestGpsPos.data.longitude;
+ data->heightAMSL = bestGpsPos.data.heightAMSL;
+ data->undulation = bestGpsPos.data.undulation;
+ data->datumId = bestGpsPos.data.datumId;
+ data->sigmaLatitude = bestGpsPos.data.sigmaLatitude;
+ data->sigmaLongitude = bestGpsPos.data.sigmaLongitude;
+ data->sigmaHeight = bestGpsPos.data.sigmaHeight;
+ data->baseStationId[4] = bestGpsPos.data.baseStationId[4];
+ data->diffAge = bestGpsPos.data.diffAge;
+ data->solutionAge = bestGpsPos.data.solutionAge;
+ data->numObservations = bestGpsPos.data.numObservations;
+ data->numL1Ranges = bestGpsPos.data.numL1Ranges;
+ data->numL1RangesRTK = bestGpsPos.data.numL1RangesRTK;
+ data->numL2RangesRTK = bestGpsPos.data.numL2RangesRTK;
+
+ //time
+ data->timeStampSec = timeStamp.tv_sec;
+ data->timeStampUSec = timeStamp.tv_usec;
+
+ //status
+ static bool lastStatusWasGood = false;
+ if(true == gnua::receiverStatusIsGood(bestGpsPos.header.receiverStatus)){
+ if (true == lastStatusWasGood){
+ // still all good, no need to be chatty
+ data->statusMessageType = gna::NoMsg;
+ data->statusMessage = "";
+ lastStatusWasGood = true;
+ }else{
+ // we are good now, report it
+ data->statusMessageType = gna::Ok;
+ data->statusMessage = "all is good";
+ lastStatusWasGood = true;
+ }
+ }else{
+ //whoops
+ data->statusMessageType = gna::Fault; // warning?
+ data->statusMessage = gnua::receiverStatusToString(bestGpsPos.header.receiverStatus);
+ lastStatusWasGood = false;
+ }
+ return genericData;
+
+ }
+
+ std::auto_ptr<gna::GenericData>
+ createExternalMsg(gnua::BestGpsVelLogB &bestGpsVel, struct timeval &timeStamp){
+ static int cnt;
+ if(0 == cnt++ % 100) cout << __func__ << " gpsvel; implement me properly!\n";
+ gna::BestGpsVelData *data = new gna::BestGpsVelData;
+ std::auto_ptr<gna::GenericData> genericData( data );
+
+ //data
+ data->gpsWeekNr = bestGpsVel.header.gpsWeekNr;
+ data->msIntoWeek = bestGpsVel.header.msIntoWeek;
+ //data->solutionStatus = bestGpsVel.data.solutionStatus;
+ //data->velocityType = bestGpsVel.data.velocityType;
+ data->latency = bestGpsVel.data.latency;
+ data->diffAge = bestGpsVel.data.diffAge;
+ data->horizontalSpeed = bestGpsVel.data.horizontalSpeed;
+ data->trackOverGround = bestGpsVel.data.trackOverGround;
+ data->verticalSpeed = bestGpsVel.data.verticalSpeed;
+
+ //time
+ data->timeStampSec = timeStamp.tv_sec;
+ data->timeStampUSec = timeStamp.tv_usec;
+
+ //status
+ static bool lastStatusWasGood = false;
+ if(true == gnua::receiverStatusIsGood(bestGpsVel.header.receiverStatus)){
+ if (true == lastStatusWasGood){
+ // still all good, no need to be chatty
+ data->statusMessageType = gna::NoMsg;
+ data->statusMessage = "";
+ lastStatusWasGood = true;
+ }else{
+ // we are good now, report it
+ data->statusMessageType = gna::Ok;
+ data->statusMessage = "all is good";
+ lastStatusWasGood = true;
+ }
+ }else{
+ //whoops
+ data->statusMessageType = gna::Fault; // warning?
+ data->statusMessage = gnua::receiverStatusToString(bestGpsVel.header.receiverStatus);
+ lastStatusWasGood = false;
+ }
+ return genericData;
+ }
+
+ std::auto_ptr<gna::GenericData>
+ createExternalMsg(gnua::RawImuLogSB &rawImu, struct timeval &timeStamp, gnua::ImuDecoder *imuDecoder){
+ //static int cnt;
+ //if(0 == cnt++ % 500) cout << __func__ << " imu; implement me properly!\n";
+ gna::RawImuData *data = new gna::RawImuData;
+ std::auto_ptr<gna::GenericData> genericData( data );
+
+ //data
+ data->gpsWeekNr = rawImu.data.gpsWeekNr;
+ data->secIntoWeek = rawImu.data.secIntoWeek;
+ //accels
+ data->zDeltaV = imuDecoder->accelCnt2MperSec(rawImu.data.zAccelCnt);
+ data->yDeltaV = -1.0*imuDecoder->accelCnt2MperSec(rawImu.data.yNegativAccelCnt);
+ data->xDeltaV = imuDecoder->accelCnt2MperSec(rawImu.data.xAccelCnt);
+ //gyros
+ data->zDeltaAng = imuDecoder->gyroCnt2Rad(rawImu.data.zGyroCnt);
+ data->yDeltaAng = -1.0*imuDecoder->gyroCnt2Rad(rawImu.data.yNegativGyroCnt);
+ data->xDeltaAng = imuDecoder->gyroCnt2Rad(rawImu.data.xGyroCnt);
+
+ //time
+ data->timeStampSec = timeStamp.tv_sec;
+ data->timeStampUSec = timeStamp.tv_usec;
+
+ //status
+ static bool lastStatusWasGood = false;
+ if(true == imuDecoder->statusIsGood(rawImu.data.imuStatus)){
+ if (true == lastStatusWasGood){
+ // still all good, no need to be chatty
+ data->statusMessageType = gna::NoMsg;
+ data->statusMessage = "";
+ lastStatusWasGood = true;
+ }else{
+ // we are good now, report it
+ data->statusMessageType = gna::Ok;
+ data->statusMessage = "all is good";
+ lastStatusWasGood = true;
+ }
+ }else{
+ //whoops
+ data->statusMessageType = gna::Fault; // warning?
+ data->statusMessage = imuDecoder->statusToString(rawImu.data.imuStatus);
+ lastStatusWasGood = false;
+ }
+ return genericData;
+ }
+}//namespace
Added: gearbox/trunk/submitted/gbxnovatelacfr/driver.h
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/driver.h (rev 0)
+++ gearbox/trunk/submitted/gbxnovatelacfr/driver.h 2008-06-18 07:53:46 UTC (rev 183)
@@ -0,0 +1,429 @@
+/*
+ * Orca-Robotics Project: Components for robotics
+ * http://orca-robotics.sf.net/
+ * Copyright (c) 2004-2008 Matthew Ridley, Ben Upcroft, Michael Moser
+ *
+ * This copy of Orca is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef GBXNOVATELACFR_DRIVER_H
+#define GBXNOVATELACFR_DRIVER_H
+
+#include <cstdlib>
+#include <string>
+#include <memory>
+#include <vector>
+
+// forward declarations
+// users don't need to know about serial devices or Tracers or imu decoders
+namespace gbxserialacfr{
+ class Serial;
+}
+namespace gbxutilacfr{
+ class Tracer;
+}
+namespace gbxnovatelutilacfr{
+ class ImuDecoder;
+}
+
+namespace gbxnovatelacfr{
+
+//! Minimum information to configure the receiver in INS mode
+class SimpleConfig{
+public:
+ //! @parameter imuToGpsOffset: vector (xyz [m]) from IMU center to Antenna Phase Center
+ //! in IMU coordinates, vital for INS performance, make sure you get this right!
+ //! @parameter imuType: as expected by the "SETIMUTYPE" command (SPAN Technology for OEMV User Manual Rev 3, Table 15, page 64)
+ SimpleConfig(std::string serialDevice, int baudRate, std::string imuType, std::vector<double > &imuToGpsOffset):
+ serialDevice_(serialDevice),
+ baudRate_(baudRate),
+ imuType_(imuType),
+ imuToGpsOffset_(imuToGpsOffset) {};
+
+ //! Returns true if the configuration is sane. Checks include:
+ //! - a non-empty device name
+ //! - baud rate is supported by device (9600, 19200, 38400, 115200, 230400)
+ //! - imuType refers to a known type
+ //! - offset has size 3
+ bool isValid() const;
+ //! Dumps the config in human readable form
+ std::string toString();
+
+ std::string serialDevice_;
+ int baudRate_;
+ std::string imuType_;
+ std::vector<double > imuToGpsOffset_;
+};
+
+//! Minimum information needed to configure the receiver in GPS only mode
+class GpsOnlyConfig{
+public:
+ GpsOnlyConfig(std::string serialDevice, int baudRate):
+ serialDevice_(serialDevice),
+ baudRate_(baudRate) {};
+
+ //! Returns true if the configuration is sane. Checks include:
+ //! - a non-empty device name
+ //! - baud rate is supported by device (9600, 19200, 38400, 115200, 230400)
+ bool isValid() const;
+ std::string toString();
+
+ std::string serialDevice_;
+ int baudRate_;
+};
+
+//! All the information needed to configure the driver. The device itself has even more options, consult your manual.
+//! If all these possibilities don't seem to be sufficient, consult your friendly developer for extension (better yet, send a patch)
+class Config{
+public:
+ Config(const SimpleConfig &simpleCfg); //!< yields a valid config, with reasonable defaults
+ Config(const GpsOnlyConfig &gpsOnlyCfg); //!< yields a valid config, for gps only operation
+ Config(); //!< disables everything, so you can set just the options you need
+
+ //! Returns true if the configuration is sane. Checks include:
+ //! - a non-empty device name
+ //! - baud rate is supported by device (9600, 19200, 38400, 115200, 230400)
+ //! - imuType refers to a known type
+ //! - offset has size 3
+ //! - message rates are consistent and don't exceed serial-data-rate
+ bool isValid() const;
+ //! Dumps the config in human readable form
+ std::string toString();
+
+ //!@name Serial settings
+ //
+ //!@{
+ std::string serialDevice_;
+ int baudRate_;
+ //!@}
+
+ //!@name IMU settings
+ //
+ //!@{
+ bool enableImu_;
+ std::string imuType_;
+ //!@}
+
+ //!@name Data settings
+ //
+ //!we disable all output in the ctor, and then enable the messages set to true here.
+ //!@{
+ bool enableInsPva_;
+ bool enableGpsPos_;
+ bool enableGpsVel_;
+ bool enableRawImu_;
+ bool ignoreUnknownMessages_; //!< normally we throw an exception, set this to "true" if you want to enable some other message permanently.
+ //!@}
+
+ //!@name Data rate settings
+ //
+ //!Time between messages in seconds (i.e. 0.01 == 100Hz). RawImu can only be reported at
+ //!the "natural" rate of the IMU (100Hz or 200Hz, depending on model).
+ //!We check in isValid() if any of these don't make sense
+ //!@{
+ double dtInsPva_; //!< 100Hz max, if RawImu is enabled 50Hz max
+ double dtGpsPos_; //!< 20Hz max, 5Hz max if RawImu or InsPva is enabled
+ double dtGpsVel_; //!< 20Hz max, 5Hz max if RawImu or InsPva is enabled
+ bool fixInvalidRateSettings_; //!< don't bitch about wrong rates, but change them to something sensible
+ //!@}
+
+ //!@name INS settings
+ //
+ //!@{
+ std::vector<double > imuToGpsOffset_;
+ std::vector<double > imuToGpsOffsetUncertainty_; //!< optional (size 3 or 0)
+ bool enableInsOffset_;
+ std::vector<double > insOffset_; //!< report INS position/velocity offset (xyz [m] in IMU coordinates) from the IMU center; useful e.g. to get data w.r. to robot's center of rotation
+ bool enableInsPhaseUpdate_; //!< tightly coupled (phase based vs position based) filter; Chance of better performance in adverse conditions
+ //!@}
+
+ //!@name GPS settings
+ //
+ //!@{
+ bool enableCDGPS_; //!< code-differential corrections over satellite (North America/Canada)
+ bool enableSBAS_; //!< code-differential corrections over satellite on GPS frequencies (WAAS/EGNOS)
+ bool enableRTK_; //!< carrier-differential corrections (you need to set up your own base-station and wireless link), assumes RTCA corrections on COM2, 9600bps, 8N1 (hardcoded)
+ bool enableUseOfOmniStarCarrier_; //!< carrier-differential corrections OMNIStarXP/HP (you need to get a subscription with them)
+ //!@}
+
+ //!@name INS settings for fast (dynamic) alignment
+ //! !I'd strongly recommend that you read the manual _very_ closely!
+ //! These guys enbale the Span system to do an alignment while moving, they also allow you to mount the IMU in weird ways (e.g. upside down).
+ //! It's worth to accept a fair amount of pain to mount the IMU in the recommended way. Otherwise you'll probably need a good amount of
+ //! experimentation/calibration to get a parameter-set that works.
+ //
+ //!@{
+ bool enableSetImuOrientation_;
+ int setImuOrientation_;
+ bool enableVehicleBodyRotation_;
+ std::vector<double > vehicleBodyRotation_;
+ std::vector<double > vehicleBodyRotationUncertainty_; //!< optional (size 3 or 0)
+ //!@}
+private:
+};
+
+//! possible Status Messages GenericData can contain
+enum StatusMessagetype {
+ //! Nothing new, no message
+ NoMsg,
+ //! All good, but something to say
+ Ok,
+ //! Problem, likely to go away
+ Warning,
+ //! Problem, probably fatal
+ Fault
+};
+
+//! Novatel's different solution status types
+enum GpsSolutionStatusType{
+ SolComputed, //!< Solution computed
+ InsufficientObs, //!< Insufficient observations
+ NoConvergence, //!< No convergence
+ Singularity, //!< Singularity at parameters matrix
+ CovTrace, //!< Covariance trace exceeds maximum (trace > 1000 m)
+ TestDist, //!< Test distance exceeded (maximum of 3 rejections if distance > 10 km)
+ ColdStart, //!< Not yet converged from cold start
+ VHLimit, //!< Height or velocity limits exceeded (in accordance with COCOM export licensing restrictions)
+ Variance, //!< Variance exceeds limits
+ Residuals, //!< Residuals are too large
+ DeltaPos, //!< Delta position is too large
+ NegativeVar, //!< Negative variance
+ IntegrityWarning, //!< Large residuals make position unreliable
+ InsInactive, //!< INS has not started yet
+ InsAligning, //!< INS doing its coarse alignment
+ InsBad, //!< INS position is bad
+ ImuUnplugged, //!< No IMU detected
+ Pending, //!< When a FIX POSITION command is entered, the receiver computes its own position and determines if the fixed position is valid
+ InvalidFix, //!< The fixed position, entered using the FIX POSITION command, is not valid
+ UnknownGpsSolutionStatusType
+};
+
+//! Novatel's different fix types; sadly mixed for position/velocity with some INS gear thrown in
+enum GpsPosVelType{
+ None, //!< No solution
+ FixedPos, //!< Position has been fixed by the FIX POSITION command or by position averaging
+ FixedHeight, //!< Position has been fixed by the FIX HEIGHT, or FIX AUTO, command or by position averaging
+ FloatConv, //!< Solution from floating point carrier phase ambiguities
+ WideLane, //!< Solution from wide-lane ambiguities
+ NarrowLane, //!< Solution from narrow-lane ambiguities
+ DopplerVelocity, //!< Velocity computed using instantaneous Doppler
+ Single, //!< Single point position
+ PsrDiff, //!< Pseudorange differential solution
+ Waas, //!< Solution calculated using corrections from an SBAS
+ Propagated, //!< Propagated by a Kalman filter without new observations
+ Omnistar, //!< OmniSTAR VBS position (L1 sub-meter) a
+ L1Float, //!< Floating L1 ambiguity solution
+ IonoFreeFloat, //!< Floating ionospheric-free ambiguity solution
+ NarrowFloat, //!< Floating narrow-lane ambiguity solution
+ L1Int, //!< Integer L1 ambiguity solution
+ WideInt, //!< Integer wide-lane ambiguity solution
+ NarrowInt, //!< Integer narrow-lane ambiguity solution
+ RtkDirectIns, //!< RTK status where the RTK filter is directly initialized from the INS filter. b
+ Ins, //!< INS calculated position corrected for the antenna b
+ InsPsrSp, //!< INS pseudorange single point solution - no DGPS corrections b
+ InsPsrDiff, //!< INS pseudorange differential solution b
+ InsRtkFloat, //!< INS RTK floating point ambiguities solution b
+ InsRtkFixed, //!< INS RTK fixed ambiguities solution b
+ OmniStarHp, //!< OmniSTAR high precision a
+ OmniStarXp, //!< OmniSTAR extra precision a
+ CdGps, //!< Position solution using CDGPS corrections
+ UnknownGpsPosVelType
+};
+
+
+//! possible types GenericData can contain
+enum DataType {
+ //! GenericData is really InsPvaData
+ InsPva,
+ //! GenericData is really BestGpsPosData
+ BestGpsPos,
+ //! GenericData is really BestGpsVelData
+ BestGpsVel,
+ //! GenericData is really RawImuData
+ RawImu
+};
+
+//! Generic (base) type returned by a read
+class GenericData
+{
+ public:
+ virtual ~GenericData(){};
+ virtual DataType type() const=0;
+ virtual std::string toString() const=0;
+ private:
+};
+
+//! INS position/velocity/attitude information
+class InsPvaData : public GenericData {
+ public:
+ DataType type() const {
+ return InsPva;
+ }
+ std::string toString() const{
+ return "implement me!";
+ }
+ int gpsWeekNr; //
+ double secIntoWeek; //
+ double latitude; //[deg] north positive WGS84
+ double longitude; //[deg] east positive WGS84
+ double height; //[m] above ellipsoid WGS84 (heigth_ellipsoid - undulation == height_geoid/AMSL)
+ double northVelocity; //[m/s] south is negative; true north?
+ double eastVelocity; //[m/s] west is negative; true east?
+ double upVelocity; //[m/s] down is negative; geoid/ellipsoid vertical?
+ //The default IMU axis definitions are:
+ // Y - forward
+ // Z - up
+ // X - right hand side
+ double roll; //[degree] right handed rotation from local level around y-axes
+ double pitch; //[degree] right handed rotation from local level around x-axes
+ double azimuth; //[degree] left handed around z-axes rotation from (true?) north clockwise
+
+ StatusMessagetype statusMessageType;
+ std::string statusMessage;
+
+ int timeStampSec; //!< in Computer time, beginning of message at serial port
+ int timeStampUSec; //!< in Computer time, beginning of message at serial port
+};
+
+//! Gps position information
+class BestGpsPosData : public GenericData {
+ public:
+ DataType type() const {
+ return BestGpsPos;
+ }
+ std::string toString() const{
+ return "implement me!";
+ }
+ int gpsWeekNr; //
+ unsigned int msIntoWeek; //milliseconds from beginning of week
+ GpsSolutionStatusType solutionStatus; //
+ GpsPosVelType positionType; //
+ double latitude; //[deg] north positive
+ double longitude; //[deg] east positive
+ double heightAMSL; //[m] AMSL == above mean sea level (geoid)
+ float undulation; //[m] aka geoidal seperation: undulation == heigth_ellipsoid - height_geoid/AMSL
+ unsigned int datumId; //
+ float sigmaLatitude; //[m? deg?] 1 standard deviation error estimate
+ float sigmaLongitude; //[m? deg?] 1 standard deviation error estimate
+ float sigmaHeight; //[m? deg?] 1 standard deviation error estimate
+ char baseStationId[4]; //
+ float diffAge; //[s]
+ float solutionAge; //[s]
+ int numObservations; //number of observations tracked (?) L1 code/carrier/doppler + L2 code/carrier/doppler?
+ int numL1Ranges; //number of L1 ranges used in computation (?)
+ int numL1RangesRTK; //number of L1 ranges above the RTK mask angle (??) number of L1 carrier ranges used?
+ int numL2RangesRTK; //number of L2 ranges above the RTK mask angle (??) number of L2 carrier ranges used?
+
+ StatusMessagetype statusMessageType;
+ std::string statusMessage;
+
+ int timeStampSec; //!< in Computer time, beginning of message at serial port
+ int timeStampUSec; //!< in Computer time, beginning of message at serial port
+};
+
+//! Gps velocity information
+class BestGpsVelData : public GenericData {
+ public:
+ DataType type() const {
+ return BestGpsVel;
+ }
+ std::string toString() const{
+ return "implement me!";
+ }
+ int gpsWeekNr; //
+ unsigned int msIntoWeek; //milliseconds from beginning of week
+ GpsSolutionStatusType solutionStatus; //
+ GpsPosVelType positionType; //
+ float latency; //[s]
+ float diffAge; //[s]
+ double horizontalSpeed; //[m/s]
+ double trackOverGround; //[deg] w respect to true North
+ double verticalSpeed; //[m/s]
+
+ StatusMessagetype statusMessageType;
+ std::string statusMessage;
+
+ int timeStampSec; //!< in Computer time, beginning of message at serial port
+ int timeStampUSec; //!< in Computer time, beginning of message at serial port
+};
+
+//! Raw IMU information
+class RawImuData : public GenericData {
+ public:
+ DataType type() const {
+ return RawImu;
+ }
+ std::string toString() const{
+ return "implement me!";
+ }
+ int gpsWeekNr;
+ double secIntoWeek;
+ double zDeltaV; //!< [m/s] change in speed, up positive
+ double yDeltaV; //!< [m/s] change in speed, forward positive
+ double xDeltaV; //!< [m/s] change in speed, right positive
+ double zDeltaAng; //!< [rad] change in angle, right handed around z
+ double yDeltaAng; //!< [rad] change in angle, right handed around y
+ double xDeltaAng; //!< [rad] change in angle, right handed around x
+
+ StatusMessagetype statusMessageType;
+ std::string statusMessage;
+
+ int timeStampSec; //!< in Computer time, beginning of message at serial port
+ int timeStampUSec; //!< in Computer time, beginning of message at serial port
+};
+
+class Driver {
+public:
+
+ Driver( const Config &cfg, gbxutilacfr::Tracer& tracer);
+ ~Driver();
+
+ //! Blocking read, returns one message
+ //! Throws gbxutilacfr::Exception when a problem is encountered.
+ //!
+ //
+ //! @verbatim
+ //! std::auto_ptr<gbxnovatelacfr::GenericData> data;
+ //!
+ //! try {
+ //! data = device->read();
+ //! }
+ //! catch ( const std::exception& e ) {
+ //! cout <<"Test: Failed to read data: "<<e.what()<<endl;
+ //! }
+ //! if(InsData == data.type()){
+ //! InsData *insData = dynamic_cast<InsData *>(data.get());
+ //! assert(insData);
+ //! //process insData
+ //! }
+ //! @endverbatim
+ //
+ std::auto_ptr<GenericData> read();
+
+private:
+
+ //! establish a serial connection to the receiver
+ int connectToHardware();
+ //! transfer configuration parameters related to the IMU
+ void configureImu();
+ //! transfer configuration parameters related to the INS
+ void configureIns();
+ //! transfer configuration parameters related to GPS
+ void configureGps();
+ //! turn on data messages we are interested in
+ void requestData();
+
+ std::auto_ptr<gbxnovatelutilacfr::ImuDecoder> imuDecoder_;
+
+ std::auto_ptr<gbxserialacfr::Serial> serial_;
+ int baud_;
+
+ Config config_;
+ gbxutilacfr::Tracer& tracer_;
+};
+
+
+} // namespace
+#endif
Added: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt 2008-06-18 07:53:46 UTC (rev 183)
@@ -0,0 +1,34 @@
+SET( lib_name GbxNovatelUtilAcfr )
+GBX_ADD_LICENSE( LGPL )
+
+SET( build TRUE )
+# don't give user an option
+# GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+
+# this was already tested in the dir above
+# GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "only Linux OS is supported" )
+
+# this was already tested in the dir above
+# INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
+# GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+
+IF( build )
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+ FILE( GLOB hdrs *.h )
+ FILE( GLOB srcs *.cpp )
+
+ # for config.h
+ INCLUDE_DIRECTORIES( ${PROJECT_BINARY_DIR} )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+ TARGET_LINK_LIBRARIES( ${lib_name} ${dep_libs} )
+
+ GBX_ADD_HEADERS( gbxnovatelacfr/gbxnovatelutilacfr ${hdrs} )
+
+ IF( GBX_BUILD_TESTS )
+ ADD_SUBDIRECTORY( test )
+ ENDIF( GBX_BUILD_TESTS )
+
+ENDIF( build )
Added: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/crc32.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/crc32.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/crc32.cpp 2008-06-18 07:53:46 UTC (rev 183)
@@ -0,0 +1,106 @@
+#include "crc32.h"
+
+////////////////////////////////////////////
+// I didn't use the implementation in the Novatel manuals. Reasons:
+// -makes assumptions that probably break for 64bit
+// -looks a bit akward
+// -potential copyright problem
+// -slow
+// Implementation adapted from here: http://www.ross.net/crc/download/crc_v3.txt (point 18)
+// This is also a good explanation of how crcs work.
+// Tested extensively against the Novatel implementation and for false positives/negatives.
+//
+// Code used to produce the crcLookup table:
+////////////////////////////////////////////
+//
+// CRC32Value() and CRC32_POLYNOMIAL are copied from:
+// OEMV Family Firmware Version 3.210 Reference Manual Rev 5, page 29
+// Types were adjusted to reflect actual size used (int -> uint8_t; unsigned long --> uint32_t).
+//
+// #define CRC32_POLYNOMIAL 0xEDB88320L
+// /* --------------------------------------------------------------------------
+// * Calculate a CRC value to be used by CRC calculation functions.
+// * -------------------------------------------------------------------------- */
+// uint32_t CRC32Value(uint8_t i)
+// {
+// int j;
+// uint32_t ulCRC;
+// ulCRC = i;
+// for ( j = 8 ; j > 0; j-- )
+// {
+// if ( ulCRC & 1 )
+// ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
+// else
+// ulCRC >>= 1;
+// }
+// return ulCRC;
+// }
+//
+// int main(void){
+// for (int i=0; i<256; i++){
+// uint32_t tmp = CRC32Value(i);
+// if(0==i%6){
+// printf("\n");
+// }else{
+// printf(" ");
+// }
+// printf("0x%08xL,", tmp);
+// }
+// return EXIT_SUCCESS;
+// }
+namespace gbxnovatelutilacfr {
+static const uint32_t crcTable[256] = {
+ 0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL, 0x076dc419L, 0x706af48fL,
+ 0xe963a535L, 0x9e6495a3L, 0x0edb8832L, 0x79dcb8a4L, 0xe0d5e91eL, 0x97d2d988L,
+ 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L, 0x90bf1d91L, 0x1db71064L, 0x6ab020f2L,
+ 0xf3b97148L, 0x84be41deL, 0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L,
+ 0x136c9856L, 0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL, 0x14015c4fL, 0x63066cd9L,
+ 0xfa0f3d63L, 0x8d080df5L, 0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L, 0xa2677172L,
+ 0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL, 0x35b5a8faL, 0x42b2986cL,
+ 0xdbbbc9d6L, 0xacbcf940L, 0x32d86ce3L, 0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L,
+ 0x26d930acL, 0x51de003aL, 0xc8d75180L, 0xbfd06116L, 0x21b4f4b5L, 0x56b3c423L,
+ 0xcfba9599L, 0xb8bda50fL, 0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
+ 0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL, 0x76dc4190L, 0x01db7106L,
+ 0x98d220bcL, 0xefd5102aL, 0x71b18589L, 0x06b6b51fL, 0x9fbfe4a5L, 0xe8b8d433L,
+ 0x7807c9a2L, 0x0f00f934L, 0x9609a88eL, 0xe10e9818L, 0x7f6a0dbbL, 0x086d3d2dL,
+ 0x91646c97L, 0xe6635c01L, 0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL,
+ 0x6c0695edL, 0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L, 0x65b0d9c6L, 0x12b7e950L,
+ 0x8bbeb8eaL, 0xfcb9887cL, 0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L, 0xfbd44c65L,
+ 0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L, 0x4adfa541L, 0x3dd895d7L,
+ 0xa4d1c46dL, 0xd3d6f4fbL, 0x4369e96aL, 0x346ed9fcL, 0xad678846L, 0xda60b8d0L,
+ 0x44042d73L, 0x33031de5L, 0xaa0a4c5fL, 0xdd0d7cc9L, 0x5005713cL, 0x270241aaL,
+ 0xbe0b1010L, 0xc90c2086L, 0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
+ 0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L, 0x59b33d17L, 0x2eb40d81L,
+ 0xb7bd5c3bL, 0xc0ba6cadL, 0xedb88320L, 0x9abfb3b6L, 0x03b6e20cL, 0x74b1d29aL,
+ 0xead54739L, 0x9dd277afL, 0x04db2615L, 0x73dc1683L, 0xe3630b12L, 0x94643b84L,
+ 0x0d6d6a3eL, 0x7a6a5aa8L, 0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L,
+ 0xf00f9344L, 0x8708a3d2L, 0x1e01f268L, 0x6906c2feL, 0xf762575dL, 0x806567cbL,
+ 0x196c3671L, 0x6e6b06e7L, 0xfed41b76L, 0x89d32be0L, 0x10da7a5aL, 0x67dd4accL,
+ 0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L, 0xd6d6a3e8L, 0xa1d1937eL,
+ 0x38d8c2c4L, 0x4fdff252L, 0xd1bb67f1L, 0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL,
+ 0xd80d2bdaL, 0xaf0a1b4cL, 0x36034af6L, 0x41047a60L, 0xdf60efc3L, 0xa867df55L,
+ 0x316e8eefL, 0x4669be79L, 0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
+ 0xcc0c7795L, 0xbb0b4...
[truncated message content] |