diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ae4adab126d01b43f9cb6b1e78aa7854918fd806
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,105 @@
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+
+if(COMMAND cmake_policy)
+ cmake_policy(SET CMP0005 NEW)
+ cmake_policy(SET CMP0003 NEW)
+
+endif(COMMAND cmake_policy)
+
+# The project name and the type of project
+PROJECT(asterx1_gps)
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+SET(CMAKE_INSTALL_PREFIX /usr/local)
+
+IF (NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE "DEBUG")
+ENDIF (NOT CMAKE_BUILD_TYPE)
+
+SET(CMAKE_C_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_C_FLAGS_RELEASE "-O3 -D_REENTRANT")
+
+ADD_SUBDIRECTORY(src)
+
+FIND_PACKAGE(Doxygen)
+
+FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
+IF (IRI_DOC_DIR)
+ ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
+ELSE (IRI_DOC_DIR)
+ ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
+ENDIF (IRI_DOC_DIR)
+
+ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
+
+IF (UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "distribution clean"
+ COMMAND make ARGS clean
+ COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/CMakeFiles*
+ COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/src*
+ COMMAND rm ARGS CMakeCache.txt
+ COMMAND rm ARGS cmake_install.cmake
+ COMMAND rm ARGS install_manifest.txt
+ COMMAND rm ARGS Makefile
+
+ TARGET distclean
+ )
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "distclean only implemented in unix"
+ TARGET distclean
+ )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
+
+IF (UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "uninstall package"
+ COMMAND xargs ARGS rm < install_manifest.txt
+
+ TARGET uninstall
+ )
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "uninstall only implemented in unix"
+ TARGET uninstall
+ )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (dep @echo install package dependencies)
+
+IF (UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "make package dependencies"
+ COMMAND ./make_dep.sh ARGS -d ../dependencies
+ TARGET dep
+ )
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "make package dependencies only implemented in unix"
+ TARGET dep
+ )
+ENDIF(UNIX)
+
+
+IF (UNIX)
+ SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${DISTRIB}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+ SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
+ SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Part of IRI-laboratory libraries. More information at http://wikiri.upc.es/index.php/Robotics_Lab")
+ SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
+ SET(CPACK_GENERATOR "DEB")
+ SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "labrobotica@iri.upc.edu")
+ SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
+
+ INCLUDE(CPack)
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "packaging only implemented in unix"
+ TARGET uninstall
+ )
+ENDIF(UNIX)
+
diff --git a/Findasterx1.cmake b/Findasterx1.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..1ef5ebc8f6836cc7a7626149ff90a7c2f7dd8dfc
--- /dev/null
+++ b/Findasterx1.cmake
@@ -0,0 +1,22 @@
+#edit the following line to add the librarie's header files
+#FIND_PATH(asterx1_INCLUDE_DIR src/asterx1.h src/asterx1exceptions.h /usr/include/iridrivers /usr/local/include/iridrivers)
+FIND_PATH(asterx1_INCLUDE_DIR asterx1.h asterx1exceptions.h /usr/include/iridrivers /usr/local/include/iridrivers)
+
+FIND_LIBRARY(asterx1_LIBRARY
+ NAMES asterx1
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
+
+IF (asterx1_INCLUDE_DIR AND asterx1_LIBRARY)
+ SET(asterx1_FOUND TRUE)
+ENDIF (asterx1_INCLUDE_DIR AND asterx1_LIBRARY)
+
+IF (asterx1_FOUND)
+ IF (NOT asterx1_FIND_QUIETLY)
+ MESSAGE(STATUS "Found asterx1: ${asterx1_LIBRARY}")
+ ENDIF (NOT asterx1_FIND_QUIETLY)
+ELSE (asterx1_FOUND)
+ IF (asterx1_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find asterx1")
+ ENDIF (asterx1_FIND_REQUIRED)
+ENDIF (asterx1_FOUND)
+
diff --git a/Findasterx1_gps.cmake b/Findasterx1_gps.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..74f474d6849aed412ba0b903de0b98e1c678942d
--- /dev/null
+++ b/Findasterx1_gps.cmake
@@ -0,0 +1,21 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(asterx1_gps_INCLUDE_DIR asterx1_gps.h asterx1exceptions.h /usr/include/iridrivers /usr/local/include/iridrivers)
+
+FIND_LIBRARY(asterx1_gps_LIBRARY
+ NAMES asterx1_gps
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
+
+IF (asterx1_gps_INCLUDE_DIR AND asterx1_gps_LIBRARY)
+ SET(asterx1_gps_FOUND TRUE)
+ENDIF (asterx1_gps_INCLUDE_DIR AND asterx1_gps_LIBRARY)
+
+IF (asterx1_gps_FOUND)
+ IF (NOT asterx1_gps_FIND_QUIETLY)
+ MESSAGE(STATUS "Found asterx1_gps: ${asterx1_gps_LIBRARY}")
+ ENDIF (NOT asterx1_gps_FIND_QUIETLY)
+ELSE (asterx1_gps_FOUND)
+ IF (asterx1_gps_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find asterx1_gps")
+ ENDIF (asterx1_gps_FIND_REQUIRED)
+ENDIF (asterx1_gps_FOUND)
+
diff --git a/ReadMe.txt b/ReadMe.txt
new file mode 100644
index 0000000000000000000000000000000000000000..466329af1e09d0934bd4d88fffb48c7461f491b3
--- /dev/null
+++ b/ReadMe.txt
@@ -0,0 +1,19 @@
+Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Author mmorta (mmorta@iri.upc.edu)
+All rights reserved.
+
+This file is part of asterx1_gps library
+asterx1_gps library is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>
+
+
diff --git a/build/make_dep.sh b/build/make_dep.sh
new file mode 100755
index 0000000000000000000000000000000000000000..827a487c5416a2c114194ac16e026485a6d2a57b
--- /dev/null
+++ b/build/make_dep.sh
@@ -0,0 +1,92 @@
+#! /bin/bash
+
+# check wether the script is called as root or not
+if [ "$(id -u)" != "0" ]; then
+ echo "This script must be run as root" 1>&2
+ exit
+fi
+
+DEP_FILE=
+
+# parse the arguments
+while getopts “d:†OPTION
+do
+ case $OPTION in
+ d)
+ DEP_FILE=$OPTARG
+ ;;
+ ?)
+ echo invalid argument $OPTION
+ exit
+ ;;
+ esac
+done
+
+# check wether a dependencies file has been provided
+if [ "$DEP_FILE" ]
+then
+ echo "using $DEP_FILE file for dependencies ..."
+else
+ echo "No dependencies file provided, aborting ..."
+ exit
+fi
+
+LOCAL_REPO_PATH=/usr/local/src/iri
+
+#check wether the repository directory exists or not
+if [ ! -d $LOCAL_REPO_PATH ]
+then
+ mkdir $LOCAL_REPO_PATH
+fi
+
+while read curline; do
+ COMPILE=0
+ MAKE_DEP=0
+ cd $LOCAL_REPO_PATH
+ echo "checking dependency $curline ..."
+ DEP_NAME=$(basename $curline)
+ if [ -d "$DEP_NAME" ]
+ then
+ echo "The $DEP_NAME library is present ... checking updates."
+ cd $DEP_NAME
+ LOCAL_REV_NUM=$(svn info | sed -n 's/^Revision: \([0-9]\+\)$/\1/p')
+ echo "Local $DEP_NAME library revision: $LOCAL_REV_NUM"
+ REPO_REV_NUM=$(svn info | sed -n 's/^Last Changed Rev: \([0-9]\+\)$/\1/p')
+ echo "Repository $DEP_NAME library revision: $REPO_REV_NUM"
+ if [ $LOCAL_REV_NUM -gt $REPO_REV_NUM ]
+ then
+ echo "There exist local changes that are not commited to the repository"
+ else
+ if [ $LOCAL_REV_NUM -lt $REPO_REV_NUM ]
+ then
+ echo "Updating to the latest revision ..."
+ svn update
+ COMPILE=1
+ else
+ echo "Already at the latest revision."
+ MAKE_DEP=1
+ fi
+ fi
+ else
+ echo "The $DEP_NAME library is not present ... downloading."
+ svn checkout $curline
+ cd $DEP_NAME
+ COMPILE=1
+ fi
+ cd build
+ if [ $MAKE_DEP -eq 1 ]
+ then
+ make dep
+ fi
+ # build if necessary
+ if [ $COMPILE -eq 1 ]
+ then
+ cmake ..
+ make dep
+ make
+ make install
+ fi
+ #back to the root repository source path
+ cd ../../
+done < $DEP_FILE
+
diff --git a/dependencies b/dependencies
new file mode 100644
index 0000000000000000000000000000000000000000..0bcfb859069feca8aee821b9e304a427d8b64665
--- /dev/null
+++ b/dependencies
@@ -0,0 +1,2 @@
+https://haydn.upc.es/labrobotica/pub/drivers/iriutils
+https://haydn.upc.es/labrobotica/pub/drivers/comm
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
new file mode 100644
index 0000000000000000000000000000000000000000..347af2ef86bf4786d49b13d48c12146b0d001b01
--- /dev/null
+++ b/doc/doxygen.conf
@@ -0,0 +1,251 @@
+# Doxyfile 1.5.5
+
+@INCLUDE_PATH = ../doc/
+@INCLUDE = doxygen_project_name.conf
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING = UTF-8
+PROJECT_NUMBER =
+OUTPUT_DIRECTORY = ../doc
+CREATE_SUBDIRS = NO
+OUTPUT_LANGUAGE = English
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = NO
+ABBREVIATE_BRIEF =
+ALWAYS_DETAILED_SEC = NO
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = YES
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = NO
+QT_AUTOBRIEF = NO
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP = NO
+INHERIT_DOCS = YES
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 8
+ALIASES =
+OPTIMIZE_OUTPUT_FOR_C = YES
+OPTIMIZE_OUTPUT_JAVA = NO
+OPTIMIZE_FOR_FORTRAN = NO
+OPTIMIZE_OUTPUT_VHDL = NO
+BUILTIN_STL_SUPPORT = NO
+CPP_CLI_SUPPORT = NO
+SIP_SUPPORT = NO
+DISTRIBUTE_GROUP_DOC = NO
+SUBGROUPING = YES
+TYPEDEF_HIDES_STRUCT = NO
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = NO
+EXTRACT_PRIVATE = NO
+EXTRACT_STATIC = NO
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+EXTRACT_ANON_NSPACES = NO
+HIDE_UNDOC_MEMBERS = NO
+HIDE_UNDOC_CLASSES = NO
+HIDE_FRIEND_COMPOUNDS = NO
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+CASE_SENSE_NAMES = YES
+HIDE_SCOPE_NAMES = NO
+SHOW_INCLUDE_FILES = YES
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = NO
+SORT_BRIEF_DOCS = NO
+SORT_GROUP_NAMES = NO
+SORT_BY_SCOPE_NAME = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = YES
+GENERATE_BUGLIST = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+MAX_INITIALIZER_LINES = 30
+SHOW_USED_FILES = YES
+SHOW_DIRECTORIES = YES
+FILE_VERSION_FILTER =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = YES
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_FORMAT = "$file:$line: $text"
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT = ../src \
+ ../doc/main.dox
+INPUT_ENCODING = UTF-8
+FILE_PATTERNS = *.c \
+ *.h \
+ *.cpp
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXCLUDE_PATTERNS = *.tab.c \
+ *.tab.h \
+ lex* \
+ *glr.h \
+ *llr.h \
+ *glr.c \
+ *llr.c \
+ *general.h
+EXCLUDE_SYMBOLS =
+EXAMPLE_PATH = ../src/examples
+EXAMPLE_PATTERNS =
+EXAMPLE_RECURSIVE = NO
+IMAGE_PATH = ../doc/images
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = YES
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = YES
+REFERENCES_RELATION = YES
+REFERENCES_LINK_SOURCE = YES
+USE_HTAGS = NO
+VERBATIM_HEADERS = YES
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = YES
+COLS_IN_ALPHA_INDEX = 5
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = YES
+HTML_OUTPUT = html
+HTML_FILE_EXTENSION = .html
+HTML_HEADER =
+HTML_FOOTER =
+HTML_STYLESHEET =
+HTML_ALIGN_MEMBERS = YES
+GENERATE_HTMLHELP = NO
+GENERATE_DOCSET = NO
+DOCSET_FEEDNAME = "Doxygen generated docs"
+DOCSET_BUNDLE_ID = org.doxygen.Project
+HTML_DYNAMIC_SECTIONS = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+BINARY_TOC = NO
+TOC_EXPAND = NO
+DISABLE_INDEX = NO
+ENUM_VALUES_PER_LINE = 4
+GENERATE_TREEVIEW = NO
+TREEVIEW_WIDTH = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME = latex
+MAKEINDEX_CMD_NAME = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4
+EXTRA_PACKAGES =
+LATEX_HEADER =
+PDF_HYPERLINKS = YES
+USE_PDFLATEX = NO
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = NO
+XML_OUTPUT = xml
+XML_SCHEMA =
+XML_DTD =
+XML_PROGRAMLISTING = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = NO
+EXPAND_ONLY_PREDEF = NO
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+PREDEFINED = _USE_MPI=1
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES =
+GENERATE_TAGFILE =
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+PERL_PATH = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+MSCGEN_PATH =
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = YES
+CLASS_GRAPH = YES
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = NO
+INCLUDED_BY_GRAPH = NO
+CALL_GRAPH = YES
+CALLER_GRAPH = YES
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = NO
+DOT_IMAGE_FORMAT = png
+DOT_PATH =
+DOTFILE_DIRS =
+DOT_GRAPH_MAX_NODES = 50
+MAX_DOT_GRAPH_DEPTH = 2
+DOT_TRANSPARENT = YES
+DOT_MULTI_TARGETS = NO
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine
+#---------------------------------------------------------------------------
+SEARCHENGINE = NO
diff --git a/doc/doxygen_project_name.conf b/doc/doxygen_project_name.conf
new file mode 100644
index 0000000000000000000000000000000000000000..e8d97291fd31e3fdfcc4d47aa84628a0374233d5
--- /dev/null
+++ b/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "asterx1_gps"
diff --git a/doc/main.dox b/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..f88fff805df992f9f50f9cda996cffb7d9196c5d
--- /dev/null
+++ b/doc/main.dox
@@ -0,0 +1,107 @@
+/*! \mainpage asterx1_gps
+
+ \section Introduction
+
+ \subsection Pre-Requisites
+
+ This package requires of the following libraries and packages
+ - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
+ - <A href="http://www.doxygen.org">doxygen</a> and
+ <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
+ - stdc++,
+ .
+
+ Under linux all of these utilities are available in ready-to-use packages.
+
+ Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
+
+ \subsection Compilation
+
+ Just download this package, uncompress it, and execute
+ - cd build
+ - cmake ..
+ .
+ to generate the makefile and then
+ - make
+ .
+ to obtain the shared library (in this case called <em>iriutils.so</em>) and
+ also all the example programs.
+
+ The <em>cmake</em> only need to be executed once (make will automatically call
+ <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+ To generate this documentation type
+ - make doc
+ .
+
+ The files in the <em>build</em> directory are genetated by <em>cmake</em>
+ and <em>make</em> and can be safely removed.
+ After doing so you will need to call cmake manually again.
+
+ \subsection Configuration
+
+ The default build mode is DEBUG. That is, objects and executables
+ include debug information.
+
+ The RELEASE build mode optimizes for speed. To build in this mode
+ execute
+ - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+ .
+ The release mode will be kept until next time cmake is executed.
+
+ \subsection Installation
+
+ In order to be able to use the library, it it necessary to copy it into the system.
+ To do that, execute
+
+ - make install
+ .
+
+ as root and the shared libraries will be copied to <em>/usr/local/lib/iriutils</em> directory
+ and the header files will be copied to <em>/usr/local/include/iriutils</em> dierctory. At
+ this point, the library may be used by any user.
+
+ To remove the library from the system, exceute
+ - make uninstall
+ .
+ as root, and all the associated files will be removed from the system.
+
+ \section Customization
+
+ To build a new application using these library, first it is necessary to locate if the library
+ has been installed or not using the following command
+
+ - FIND_PACKAGE(asterx1 REQUIRED)
+
+ In the case that the package is present, it is necessary to add the header files directory to
+ the include directory path by using
+
+ - INCLUDE_DIRECTORIES(${asterx1_INCLUDE_DIR})
+
+ Finally, it is also nevessary to link with the desired libraries by using the following command
+
+ - TARGET_LINK_LIBRARIES(<executable name> ${asterx1_LIBRARY})
+ .
+
+ \section License
+
+ This package is licensed under a
+ <a href="http://www.gnu.org/licenses/lgpl.html">
+ LGPL 3.0 License</a>.
+
+ \section Disclaimer
+
+ This is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+ */
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f76fd514f1a4ebe76a353db1600d50c1c4f301a2
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,31 @@
+# driver source files
+SET(sources asterx1_gps.cpp)
+
+# application header files
+SET(headers asterx1_gps.h)
+
+# locate the the necessary dependencies
+FIND_PACKAGE(iriutils)
+FIND_PACKAGE(comm)
+
+# add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+ INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+ INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+
+# create the shared library
+ADD_LIBRARY(asterx1_gps SHARED ${sources})
+
+# link necessary libraries
+TARGET_LINK_LIBRARIES(asterx1_gps ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(asterx1_gps ${comm_LIBRARY})
+INSTALL(TARGETS asterx1_gps
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib/iridrivers
+ ARCHIVE DESTINATION lib/iridrivers)
+
+INSTALL(FILES ${headers} DESTINATION include/iridrivers)
+
+INSTALL(FILES ../Findasterx1_gps.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+ADD_SUBDIRECTORY(examples)
diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed57e5879bd1b52ff53370922a12ccec6730887a
--- /dev/null
+++ b/src/asterx1_gps.cpp
@@ -0,0 +1,854 @@
+#include "asterx1_gps.h"
+
+CasteRx1::CasteRx1(const string & hwPortName, const int acqRateIndex)
+{
+ status = 0;
+ config.portName = hwPortName;
+ config.acqPeriod = acqPeriodValues[acqRateIndex];
+}
+
+CasteRx1::~CasteRx1()
+{
+ blockList.clear();
+ if( (status & ACQUISITION_RUNNING) ) stopAcquisition();
+ if( (status & DEVICE_OPEN) ) closeDevice();
+ cout << "CasteRx1::~CasteRx1(). End of CasteRx1 destructor" << endl; //bye bye message
+}
+
+int CasteRx1::openDevice(int xx)
+{
+ cout << xx << endl;
+ int retValue = BASIC_SUCCESS;
+ TRS232_config serialConfig;
+
+ try{
+ serialPort = new CRS232(config.portName); //creates a new object to manage serial connection
+ }catch(CException &e)
+ {
+ cout << e.what() << endl;
+ cout << "CasteRx1::openDevice(). ERROR. Allocation error when creating new object CRS232" << endl;
+ retValue = PORT_OPEN_ERROR;
+ }
+
+ if ( retValue == BASIC_SUCCESS ) //if successfully created , we can open it
+ {
+ try{
+ serialPort->open( (void*)&config.portName );
+ }catch(CCommException &e)
+ {
+ cout << e.what() << endl;
+ cout << "CasteRx1::openDevice(). ERROR. Fail to open serial port" << endl;
+ retValue = PORT_OPEN_ERROR;
+ }
+ }
+
+ if ( retValue == BASIC_SUCCESS ) //if open is successful, we can configure
+ {
+ serialConfig.baud = ASTERX_BAUDRATE;
+ serialConfig.num_bits = 8;
+ serialConfig.parity = none;
+ serialConfig.stop_bits = 1;
+ try{
+ serialPort->config( &serialConfig );
+ }catch(CCommException &e)
+ {
+ cout << e.what() << endl;
+ cout << "CasteRx1::openDevice(). ERROR. Fail to configure serial port" << endl;
+ retValue = PORT_CONFIG_ERROR;
+ }
+ }
+
+ if (retValue == BASIC_SUCCESS)
+ {
+ status |= DEVICE_OPEN;
+ cout << "CasteRx1::openDevice(). SUCCESS. Serial port " << config.portName << " opened successfully." << endl;
+
+ }
+
+ return retValue;
+}
+
+int CasteRx1::closeDevice()
+{
+ int retValue = BASIC_SUCCESS;
+
+ if ( (status & DEVICE_OPEN) )
+ {
+ try{
+ serialPort->close();
+ //status &= ~DEVICE_OPEN;
+ }catch(CException &e)// catch(CCommException &e)
+ {
+ cout << e.what() << endl;
+ cout << "CasteRx1::openDevice(). ERROR. Fail to close serial port " << config.portName << endl;
+ retValue = PORT_CLOSE_ERROR;
+ //status |= DEVICE_OPEN; //close() has failed , so it is still opened
+ }
+
+ }
+ if ( retValue == BASIC_SUCCESS ) //if close() is successful, we delete the object
+ {
+ status &= ~DEVICE_OPEN;
+ cout << "CasteRx1::closeDevice(). SUCCESS. Serial port " << config.portName << " closed successfully." << endl;
+ delete serialPort;
+ }
+
+ return retValue;
+}
+
+int CasteRx1::startAcquisition()
+{
+ int retValue=0;
+ sbfBlockManager *blockMng;
+ string cmnd;
+
+ //enables commands as input at USB1
+ retValue += sendCommand("setDataInOut,USB1,CMD,none\n");
+
+ //stops data transmission if it was running
+ retValue += sendCommand("setSBFOutput,all,USB1,none\n");
+
+ //enables septentrio binary format (SBF) as output at USB1
+ retValue += sendCommand("setDataInOut,USB1,CMD,+SBF\n");
+
+ //GPS signal processing options for urban mobile robots
+ retValue += sendCommand("setMultipathMitigation,on\n");//enables processing for multipath removing (pag.26)
+ retValue += sendCommand("setReceiverDynamics,Moderate\n");//Mobile robots moves at moderate speeds
+ retValue += sendCommand("setPVTMode,Rover,StandAlone\n");//rover mode and enables standalone
+
+ //data configuration. Add blocks of interest to "group 1"
+ retValue += sendCommand("setSBFGroups,Group1,PVTCartesian\n");
+ blockMng = new sbfBlockManager; blockMng->blockId = PVTCartesian_ID; blockMng->received = false;
+ blockList.push_back(*blockMng);
+
+ retValue += sendCommand("setSBFGroups,Group1,+PosCovCartesian\n");
+ blockMng = new sbfBlockManager; blockMng->blockId = PosCovCartesian_ID; blockMng->received = false;
+ blockList.push_back(*blockMng);
+
+ retValue += sendCommand("setSBFGroups,Group1,+DOP\n");
+ blockMng = new sbfBlockManager; blockMng->blockId = DOP_ID; blockMng->received = false;
+ blockList.push_back(*blockMng);
+
+ retValue += sendCommand("setSBFGroups,Group1,+PVTGeodetic\n");
+ blockMng = new sbfBlockManager; blockMng->blockId = PVTGeodetic_ID; blockMng->received = false;
+ blockList.push_back(*blockMng);
+
+ //requests "Group1" data to be output to USB1 at specified rate
+ cmnd = "setSBFOutput,Stream1,USB1,Group1,";
+ cmnd.append(config.acqPeriod);
+ cmnd.append("\n");
+ retValue += sendCommand(cmnd);
+ //retValue += sendCommand("setSBFOutput,Stream1,USB1,Group1,msec500\n");
+
+ if ( retValue == BASIC_SUCCESS*11 ) //success on all commands
+ {
+ status |= ACQUISITION_RUNNING;
+ retValue = BASIC_SUCCESS;
+ cout << "CasteRx1::startAcquisition(). SUCCESS on device configuration. The device is now transmitting data ..." << endl;
+ }
+ else //otherwise
+ {
+ status &= ~ACQUISITION_RUNNING;
+ retValue = ASTERX_CONFIG_ERROR;
+ cout << "CasteRx1::startAcquisition(). ERROR on device configuration for data transmission" << endl;
+ }
+
+ return retValue;
+}
+
+int CasteRx1::stopAcquisition()
+{
+ int retValue=0;
+
+ if( (status & ACQUISITION_RUNNING) )
+ {
+ blockList.clear();
+
+ retValue += sendCommand("setSBFOutput,Stream1,USB1,none\n");//stops data transmission from device
+ retValue += sendCommand("setDataInOut,USB1,CMD,none\n");//enables commands as input at USB1
+ if ( retValue == BASIC_SUCCESS*2 )
+ {
+ status &= ~ACQUISITION_RUNNING;
+ retValue = BASIC_SUCCESS;
+ cout << "CasteRx1::stopAcquisition(). SUCCESS on stopping the device data transmission." << endl;
+ }
+ else
+ {
+ retValue = ASTERX1_STOPPINTG_ERROR;
+ cout << "CasteRx1::stopAcquisition(). ERROR on stopping the device data transmission." << endl;
+ }
+ }
+ else
+ {
+ cout << "CasteRx1::stopAcquisition(). Nothing to do, the acquisition was already stopped." << endl;
+ retValue = BASIC_SUCCESS;
+ }
+
+ return retValue;
+}
+
+int CasteRx1::readDataFromDevice()
+{
+ int retValue;
+ unsigned char buffer[MAX_BLOCK_SIZE];
+ unsigned short int crcValue, crcComputed; //crc value from device and crc computed
+ unsigned short int blockId; //data block id from receiver
+ unsigned short int blockLength; //size of the SBF block in bytes
+ timeval tmStmp;
+ list<sbfBlockManager>::iterator iiBlock;
+ bool allReceived = false;
+
+ //reset received field of block managers
+ for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++){ iiBlock->received = false; }
+
+ //acquisition up to all requested blocks are received
+ while (!allReceived)
+ {
+ //Sets Time Stamp (this is the system time stamp, not the GPS time!)
+ gettimeofday(&tmStmp, NULL);
+ timeStamp = (double)(tmStmp.tv_sec + tmStmp.tv_usec/1e6);
+
+ retValue = synchronizeHeader((const unsigned char *)"$@", DATA_TIMEOUT);
+ if ( retValue != BASIC_SUCCESS ) //if !BASIC_SUCCESS, status will be either COMMTIMEOUT or SERIALCOMMERROR
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR. when synchronizing with header. Expected header = $@" << endl;
+ return ACQUISITION_ERROR;
+ }
+
+ //starts reading header
+ retValue = 0;
+ retValue += readNbytes(&buffer[0],2,DATA_TIMEOUT);//reads transmitted crc value
+ crcValue = getUI2(&buffer[0]); //decodes crc value
+ retValue += readNbytes(&buffer[0],2,DATA_TIMEOUT);//reads block id field
+ blockId = (buffer[0] & 0xff)|((buffer[1] & 0x1f)<<8);//decodes blockId (usies only the 13 LS bits)
+ retValue += readNbytes(&buffer[2],2,DATA_TIMEOUT);//reads block length
+ blockLength = getUI2(&buffer[2]);//decodes block length (in bytes)
+
+ //check for some errors
+ if ( retValue != 3*BASIC_SUCCESS )
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR. Error reading block header." << endl;
+ return ACQUISITION_ERROR;
+ }
+ if ( (blockLength%4!=0) || (blockLength < MIN_BLOCK_SIZE) || (blockLength > MAX_BLOCK_SIZE) )
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR. SBF block Length not correct: " << blockLength << " bytes" << endl;
+ return ACQUISITION_ERROR;
+ }
+
+ //cout << "readDataFromDevice(): "<< __LINE__ << "; blockLength = " << blockLength << endl;
+
+ //reads the rest of the block and loads it to the buffer
+ retValue = readNbytes(&buffer[4],(blockLength-8),DATA_TIMEOUT);
+ if ( retValue != BASIC_SUCCESS )
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR. Error reading block data." << endl;
+ return ACQUISITION_ERROR;
+ }
+
+ //Computes & checks CRC
+ crcComputed = CRC_compute16CCITT(buffer, blockLength-4);
+ //cout << "Expected value: " << crcValue << "; Computed value: " << crcComputed << endl;
+ if ( crcComputed != crcValue )
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR. CRC error. Expected value: " << crcValue << "; Computed value: " << crcComputed << endl;
+ return ACQUISITION_ERROR;
+ }
+
+ //decodes SBF block
+ switch(blockId)
+ {
+ case PVTCartesian_ID:
+ retValue = decodePVTCartesian(&buffer[4]);
+ markBlockAsReceived(PVTCartesian_ID);
+ break;
+
+ case PVTGeodetic_ID:
+ retValue = decodePVTGeodetic(&buffer[4]);
+ markBlockAsReceived(PVTGeodetic_ID);
+ break;
+
+ case PosCovCartesian_ID:
+ retValue = decodePosCovCartesian(&buffer[4]);
+ markBlockAsReceived(PosCovCartesian_ID);
+ break;
+
+ case DOP_ID:
+ retValue = decodeDOP(&buffer[4]);
+ markBlockAsReceived(DOP_ID);
+ break;
+
+ default:
+ cout << "CasteRx1::readDataFromDevice(). ERROR. Unknown SBF blockId: " << blockId << endl;
+ retValue = ACQUISITION_ERROR;
+ break;
+ }
+
+ //check resulting status of the decoding function
+ if ( retValue != BASIC_SUCCESS )
+ {
+ status &= ~DATA_BLOCK_OK;
+ cout << "CasteRx1::readDataFromDevice(). ERROR on decoding SBF block. BlockId = " << blockId << ". Status = " << status << endl;
+ return ACQUISITION_ERROR;
+ }
+ else//sets status and checks recursively if all blocks have been received
+ {
+ status |= DATA_BLOCK_OK;
+ allReceived = true;
+ for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++){ allReceived = iiBlock->received & allReceived; }
+ }
+ }
+
+ //check gps availability
+ if ( pvtError == 0 )
+ {
+ status |= GPS_AVAILABLE;
+ fromWgsToMap(); //conversion to map coordinates
+ computeTMenu(); //compute rotation matrix form WGS to local tangential ENU frame
+ computeENUcovs(); //compute covariances from WGS to local tangential ENU frame
+ }
+ else
+ {
+ status &= ~GPS_AVAILABLE;
+ }
+
+ return BASIC_SUCCESS;
+}
+
+void CasteRx1::printData(ostream & ost)
+{
+ streamsize nn;
+
+ ost << status << " ";
+ nn=ost.precision(12); ost << timeStamp << " "; ost.precision(nn);
+ ost << wnc << " " << tow << " " << numSatellites << " " << PDOP << " ";
+ nn=ost.precision(12); ost << lat << " " << lon << " " << alt << " "; ost.precision(nn);
+ ost << xWgs << " " << yWgs << " " << zWgs << " ";
+ ost << xMap << " " << yMap << " " << zMap << " ";
+ ost << vxWgs << " " << vyWgs << " " << vzWgs << " ";
+ ost << vxMap << " " << vyMap << " " << vzMap << " ";
+ ost << cxxWgs << " " << cyyWgs << " " << czzWgs << " " << cxyWgs << " ";
+ ost << cxxMap << " " << cyyMap << " " << czzMap << " " << cxyMap << " ";
+ ost << endl;
+}
+
+void CasteRx1::setMapOrigin(double mapLat0, double mapLon0, double mapAlt0, double mapAlpha0)
+{
+ lat0 = mapLat0*M_PI/180.0;
+ lon0 = mapLon0*M_PI/180.0;
+ alt0 = mapAlt0;
+ alpha0 = mapAlpha0*M_PI/180.0;
+
+ computeTM();
+}
+
+void CasteRx1::setPortName(string pName)
+{
+ if( (status & DEVICE_OPEN) ) //if device is open, port name can't be changed
+ {
+ cout << "CasteRx1::setPortName(): WARNING! Serial port resource can't be changed when port is open. Keeping old settings: " << config.portName << endl;
+ }
+ else
+ {
+ config.portName = pName;
+ cout << "CasteRx1::setPortName(): Serial port name setting has changed. New serial port is " << config.portName << endl;
+ }
+}
+
+void CasteRx1::setAcquisitionRate(int rateIndex)
+{
+ if (status & ACQUISITION_RUNNING)
+ {
+ cout << "CasteRx1::setAcquisitionRate(): WARNING! Rate can't be changed when acquisition is running. Keeping old settings: " << config.acqPeriod << endl;
+ }
+ else
+ {
+ if ( ( rateIndex<ONCHANGE )|| ( rateIndex>SEC10 ) )
+ {
+ cout << "CasteRx1::setAcquisitionRate(): WARNING! Invalid rate index. Keeping old settings: " << config.acqPeriod << endl;
+ }
+ else
+ {
+ config.acqPeriod = acqPeriodValues[rateIndex];
+ cout << "CasteRx1::setAcquisitionRate(): Acquisition rate has changed. New rate is " << config.acqPeriod << endl;
+ }
+ }
+}
+
+void CasteRx1::printTM()
+{
+ streamsize nn;
+
+ cout << endl << "MAP ORIGIN (GEODETIC) ----------------------------------" << endl;
+ nn=cout.precision(15);
+ cout << "lat0 = " << lat0 << endl << "lon0 = " << lon0 << endl << "alt0 = " << alt0 << endl << "alpha0 = " << alpha0 << endl;
+ cout.precision(nn);
+
+ cout << endl << "MATRIX TRANSFORM FROM ECEF to MAP-----------------------" << endl;
+ cout << TM[0][0] << '\t' << TM[0][1] << '\t' << TM[0][2] << '\t' << TM[0][3] << endl
+ << TM[1][0] << '\t' << TM[1][1] << '\t' << TM[1][2] << '\t' << TM[1][3] << endl
+ << TM[2][0] << '\t' << TM[2][1] << '\t' << TM[2][2] << '\t' << TM[2][3] << endl
+ << TM[3][0] << '\t' << '\t' << TM[3][1] << '\t' << '\t' << TM[3][2] << '\t' << '\t' << TM[3][3] << endl;
+ cout << "--------------------------------------------------------" << endl << endl;
+}
+
+unsigned int CasteRx1::getStatus(){ return status; }
+double CasteRx1::getTimeStamp(){ return timeStamp; }
+unsigned short int CasteRx1::getWnc(){ return wnc; }
+unsigned int CasteRx1::getTow(){ return tow; }
+unsigned int CasteRx1::getNumSatellites(){ return numSatellites; }
+float CasteRx1::getPDOP(){ return PDOP; }
+float CasteRx1::getTDOP(){ return TDOP; }
+float CasteRx1::getHDOP(){ return HDOP; }
+float CasteRx1::getVDOP(){ return VDOP; }
+float CasteRx1::getUndulation(){ return undulation; }
+unsigned short int CasteRx1::getPVTerror(){ return pvtError;}
+
+double CasteRx1::getLat(bool units)
+{
+ if ( units == inRADS ) return lat;
+ else return lat*180.0/M_PI;
+}
+
+double CasteRx1::getLon(bool units)
+{
+ if ( units == inRADS ) return lon;
+ else return lon*180.0/M_PI;
+}
+
+double CasteRx1::getAlt(){ return alt; }
+double CasteRx1::getXWgs(){ return xWgs; }
+double CasteRx1::getYWgs(){ return yWgs; }
+double CasteRx1::getZWgs(){ return zWgs; }
+double CasteRx1::getXMap(){ return xMap; }
+double CasteRx1::getYMap(){ return yMap; }
+double CasteRx1::getZMap(){ return zMap; }
+double CasteRx1::getVxWgs(){ return vxWgs; }
+double CasteRx1::getVyWgs(){ return vyWgs; }
+double CasteRx1::getVzWgs(){ return vzWgs; }
+double CasteRx1::getVxMap(){ return vxMap; }
+double CasteRx1::getVyMap(){ return vyMap; }
+double CasteRx1::getVzMap(){ return vzMap; }
+double CasteRx1::getCxxWgs(){ return cxxWgs; }
+double CasteRx1::getCyyWgs(){ return cyyWgs; }
+double CasteRx1::getCzzWgs(){ return czzWgs; }
+double CasteRx1::getCxyWgs(){ return cxyWgs; }
+double CasteRx1::getCxxMap(){ return cxxMap; }
+double CasteRx1::getCyyMap(){ return cyyMap; }
+double CasteRx1::getCzzMap(){ return czzMap; }
+double CasteRx1::getCxyMap(){ return cxyMap; }
+double CasteRx1::getCxxEnu(){ return cxxEnu; }
+double CasteRx1::getCyyEnu(){ return cyyEnu; }
+double CasteRx1::getCzzEnu(){ return czzEnu; }
+double CasteRx1::getCxyEnu(){ return cxyEnu; }
+double CasteRx1::getCxzEnu(){ return cxzEnu; }
+double CasteRx1::getCyzEnu(){ return cyzEnu; }
+
+
+/******************************* PROTECTED MEMBERS ************************************************/
+
+int CasteRx1::readNbytes(unsigned char *buffer, unsigned int nn, unsigned int msecTimeOut)
+{
+ CEventServer *eventServer=CEventServer::instance();
+ string rxEvent;
+ list<string> eventList;
+ unsigned int bytesRead = 0;
+ unsigned int queueSize = 0;
+ unsigned int desiredBytes = 0;
+ int retValue = BASIC_SUCCESS;
+
+ //sets the event list with the reception event from serialPort object
+ rxEvent = serialPort->get_rx_event_id();
+ eventList.push_back(rxEvent);
+
+ //first check if some byte is already in the queue
+ queueSize = serialPort->get_num_data();
+ if ( queueSize > 0)
+ {
+ desiredBytes = min( queueSize , (nn-bytesRead) );
+ bytesRead += serialPort->read((unsigned char *)(buffer+bytesRead),desiredBytes);
+ }
+
+ //starts the waiting loop if not all bytes are already read
+ while ( (bytesRead<nn) && (retValue == BASIC_SUCCESS) )
+ {
+ try{
+ eventServer->wait_all(eventList, msecTimeOut);//waits for reception event
+ }catch(CEventServerException &e)
+ {
+ cout << e.what() << ". Line: " << __LINE__ << endl;
+ retValue = PORT_READ_TIMEOUT;
+ }
+ if (retValue == BASIC_SUCCESS)
+ {
+ queueSize = serialPort->get_num_data();
+ desiredBytes = min( queueSize , (nn-bytesRead) );
+ bytesRead += serialPort->read((unsigned char *)(buffer+bytesRead),desiredBytes);
+ }
+ }
+
+ return retValue;
+}
+
+int CasteRx1::synchronizeHeader(const unsigned char *mark, int msecTimeOut, unsigned int markSize)
+{
+ unsigned char cc;
+ string readMark, targetMark;
+ bool endLoop = false;
+ int retValue = BASIC_SUCCESS;
+ int ii=0;
+
+ //sets targetMark
+ targetMark.append((const char*)mark,markSize);
+
+ //reads mark from comm device
+ for (ii=0; ii<markSize; ii++)
+ {
+ retValue = readNbytes(&cc,1,msecTimeOut);
+ readMark.push_back((char)cc);
+ }
+
+ //continue reading while mark is not reached or while timeout
+ while ( (readMark.compare(targetMark) != 0) && (retValue == BASIC_SUCCESS) )
+ {
+ for (ii=1; ii<markSize; ii++) readMark.at(ii-1) = readMark.at(ii); //left displacement of all bytes
+ retValue = readNbytes(&cc,1,msecTimeOut); //gets a new byte
+ readMark.at(markSize-1) = (char)cc; //replace the last byte by the new one
+ //cout << endl << "readMark: " << readMark << endl << "targetMark: " << targetMark << endl;
+ }
+
+ return retValue;
+}
+
+int CasteRx1::sendCommand(const string & cmnd)
+{
+ int retValue;
+ int nn;
+ unsigned char replyBuffer[200];
+ string receivedReply, expectedReply;
+
+ //relax if we concatenate commands. (200 ms were suggested by Septentrio support team in a personal communication). 50ms tested and it seems it is not enough!!!
+ usleep(100000);
+
+ //sends the command string
+ serialPort->write((unsigned char*)cmnd.c_str(),cmnd.size());
+
+ //synchronize with the reply header
+ retValue = synchronizeHeader((const unsigned char *)"$R",500);
+
+ //cout << "CasteRx1. Line: " << __LINE__ << ". Reply Sync achieved" << endl;
+ if ( retValue != BASIC_SUCCESS )
+ {
+ cout << "CasteRx1::sendCommand(). ERROR. Synchronization with repky header $R not achieved after timeout" << endl;
+ return retValue; //it will be either COMMTIMEOUT or SERIALCOMMERROR
+ }
+
+ //reads response up to receive expected length bytes (2 + cmnd.size()). Expected response : "$R: <cmnd>", but $R has been already read
+ retValue = readNbytes(&replyBuffer[0],cmnd.size()+2,500);
+ if ( retValue != BASIC_SUCCESS )
+ {
+ cout << "CasteRx1::sendCommand(). ERROR. Expected reply string not received after timeout" << endl;
+ return retValue; //it will be either COMMTIMEOUT or SERIALCOMMERROR
+ }
+
+ //check response
+ if ( replyBuffer[0] == '?' )
+ {
+ cout << "CasteRx1::sendCommand(). ERROR. Unknown command: " << cmnd << endl;
+ return ASTERX1_INVALID_COMMAND;
+ }
+ receivedReply.append((const char*)replyBuffer,cmnd.size()+2);
+ receivedReply.append("\n");
+ expectedReply = ": ";
+ expectedReply.append(cmnd);
+ expectedReply.append("\n");
+ if ( receivedReply.compare(0, cmnd.size()+1, expectedReply, 0, cmnd.size()+1) == 0) //expected = received
+ {
+ //cout << "CasteRx1::sendCommand(). Command Successful: " << cmnd << endl;
+ return BASIC_SUCCESS;
+ }
+ else //otherwise
+ {
+ cout << "CasteRx1::sendCommand(). Error. " << endl;
+ cout << " Sent command: " << cmnd << endl;
+ cout << " Device Reply: " << receivedReply << endl;
+ cout << " Expected Reply: " << expectedReply << endl;
+ return ASTERX_CONFIG_ERROR;
+ }
+}
+
+unsigned short int CasteRx1::getUI1(unsigned char *buf)
+{
+ //return ( (buf[0] & 0xff) | (0x0 << 8) );
+ return (buf[0] & 0x00ff);
+}
+
+unsigned short int CasteRx1::getUI2(unsigned char *buf)
+{
+ return ( (buf[0] & 0xff) | ((buf[1]&0xff)<<8) );
+}
+
+unsigned int CasteRx1::getUI4(unsigned char *buf)
+{
+ return ( (buf[0]&0xff) | ((buf[1]&0xff)<<8) | ((buf[2]&0xff)<<16) | ((buf[3]&0xff)<<24) );
+}
+
+float CasteRx1::getFloat(unsigned char *buf)
+{
+ unsigned int uintL;
+ uniFloat uniF;
+
+ uintL=(buf[0]&0xff)|((buf[1]&0xff)<<8)|((buf[2]&0xff)<<16)|((buf[3]&0xff)<<24);
+ uniF.i = uintL;
+
+ return uniF.flt;
+}
+
+double CasteRx1::getDouble(unsigned char *buf)
+{
+ unsigned int uintL;
+ unsigned long long int uintH;
+ uniDouble uniD;
+
+ uintL=(buf[0]&0xff)|((buf[1]&0xff)<<8)|((buf[2]&0xff)<<16)|((buf[3]&0xff)<<24);
+ uintH=(buf[4]&0xff)|((buf[5]&0xff)<<8)|((buf[6]&0xff)<<16)|((buf[7]&0xff)<<24);
+ //uniD.i = uintL|((uintH&0xffff)<<32);
+ uniD.i = uintL|uintH<<32;
+
+ return uniD.dbl;
+}
+
+int CasteRx1::decodePVTCartesian(unsigned char *sbfMsg)
+{
+ unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
+
+ tow = getUI4(&sbfMsg[0]);
+ wnc = getUI2(&sbfMsg[4]);
+ mode = getUI1(&sbfMsg[6]);
+ pvtError = getUI1(&sbfMsg[7]);
+ if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
+ {
+ //cout << "CasteRx1::decodePVTCartesian(): PVT_ERROR: " << pvtError << endl;
+ }
+ else
+ {
+ xWgs = getDouble(&sbfMsg[8]);
+ yWgs = getDouble(&sbfMsg[16]);
+ zWgs = getDouble(&sbfMsg[24]);
+ undulation = (double) getFloat(&sbfMsg[32]);
+ vxWgs = (double) getFloat(&sbfMsg[36]);
+ vyWgs = (double) getFloat(&sbfMsg[40]);
+ vzWgs = (double) getFloat(&sbfMsg[44]);
+ numSatellites = getUI1(&sbfMsg[66]);
+ }
+
+ //cout << "CasteRx1::decodePVTCartesian()" << endl;
+ return BASIC_SUCCESS;
+}
+
+int CasteRx1::decodePVTGeodetic(unsigned char *sbfMsg)
+{
+ unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
+
+ tow = getUI4(&sbfMsg[0]);
+ wnc = getUI2(&sbfMsg[4]);
+ mode = getUI1(&sbfMsg[6]);
+ pvtError = getUI1(&sbfMsg[7]);
+ if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
+ {
+ //cout << "CasteRx1::decodePVTGeodetic(): PVT_ERROR: " << pvtError << endl;
+ }
+ else
+ {
+ lat = getDouble(&sbfMsg[8]);
+ lon = getDouble(&sbfMsg[16]);
+ alt = getDouble(&sbfMsg[24]);
+ }
+
+ //cout << "CasteRx1::decodePVTGeodetic()" << endl;
+ return BASIC_SUCCESS;
+}
+
+int CasteRx1::decodePosCovCartesian(unsigned char *sbfMsg)
+{
+ unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
+
+ tow = getUI4(&sbfMsg[0]);
+ wnc = getUI2(&sbfMsg[4]);
+ mode = getUI1(&sbfMsg[6]);
+ pvtError = getUI1(&sbfMsg[7]);
+ if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
+ {
+ //cout << "CasteRx1::decodePosCovCartesian(): PVT_ERROR: " << pvtError << endl;
+ }
+ else
+ {
+ cxxWgs = (double) getFloat(&sbfMsg[8]);
+ cyyWgs = (double) getFloat(&sbfMsg[12]);
+ czzWgs = (double) getFloat(&sbfMsg[16]);
+ cxyWgs = (double) getFloat(&sbfMsg[24]);
+ cxzWgs = (double) getFloat(&sbfMsg[28]);
+ cyzWgs = (double) getFloat(&sbfMsg[36]);
+ }
+
+ //cout << "CasteRx1::decodePosCovCartesian()" << endl;
+ return BASIC_SUCCESS;
+}
+
+int CasteRx1::decodeDOP(unsigned char *sbfMsg)
+{
+ unsigned char nrsv;//total number of satellites for DOP computation
+
+ tow = getUI4(&sbfMsg[0]);
+ wnc = getUI2(&sbfMsg[4]);
+ nrsv = getUI1(&sbfMsg[6]);
+
+ PDOP = (double)getUI2(&sbfMsg[8])/100.0;
+ TDOP = (double)getUI2(&sbfMsg[10])/100.0;
+ HDOP = (double)getUI2(&sbfMsg[12])/100.0;
+ VDOP = (double)getUI2(&sbfMsg[14])/100.0;
+
+ //cout << "CasteRx1::decodeDOP()" << endl;
+ return BASIC_SUCCESS;
+}
+
+//unsigned short CasteRx1::CRC_compute16CCITT(const void *buf, size_t buf_length)
+unsigned short CasteRx1::CRC_compute16CCITT(unsigned char *buf, unsigned short int nBytes)
+{
+ unsigned int ii;
+ unsigned short crc = 0;
+ //const unsigned char *buf8 = buf; /* Convert the type to access by byte. */
+
+ /* see for example the BINEX web site */
+ for (ii=0; ii<nBytes; ii++) {
+ crc = (crc << 8) ^ CRC_16CCIT_LookUp[ (crc >> 8) ^ buf[ii] ];
+ }
+
+ return crc;
+}
+
+void CasteRx1::computeTM()
+{
+ double kk; //constant
+ double xx0,yy0,zz0; //wgs-ecef coordinates of the origin
+ double auxTM[4][4];
+
+ //sets the rotation part of TM, from ECEF to ENU coordinates
+ auxTM[0][0] = -sin(lon0);
+ auxTM[0][1] = cos(lon0);
+ auxTM[0][2] = 0.0;
+ auxTM[1][0] = -sin(lat0)*cos(lon0);
+ auxTM[1][1] = -sin(lat0)*sin(lon0);
+ auxTM[1][2] = cos(lat0);
+ auxTM[2][0] = cos(lat0)*cos(lon0);
+ auxTM[2][1] = cos(lat0)*sin(lon0);
+ auxTM[2][2] = sin(lat0);
+
+ //computes final alignement of x with respect to the campus baseline as a rotation of alpha0 on z axis
+ //Ra=[cos(alpha0) sin(alpha0) 0;
+ // -sin(alpha0) cos(alpha0) 0;
+ // 0 0 1];
+ TM[0][0]=cos(alpha0)*auxTM[0][0] + sin(alpha0)*auxTM[1][0];
+ TM[0][1]=cos(alpha0)*auxTM[0][1] + sin(alpha0)*auxTM[1][1];
+ TM[0][2]=cos(alpha0)*auxTM[0][2] + sin(alpha0)*auxTM[1][2];
+ TM[1][0]=-sin(alpha0)*auxTM[0][0] + cos(alpha0)*auxTM[1][0];
+ TM[1][1]=-sin(alpha0)*auxTM[0][1] + cos(alpha0)*auxTM[1][1];
+ TM[1][2]=-sin(alpha0)*auxTM[0][2] + cos(alpha0)*auxTM[1][2];
+ TM[2][0]=auxTM[2][0];
+ TM[2][1]=auxTM[2][1];
+ TM[2][2]=auxTM[2][2];
+
+ //computes the translation part
+ kk = EARTH_RADIUS/sqrt(1-(pow(EARTH_EXCENTRICITY*sin(lat0),2.0)));
+ xx0 = (kk + alt0) * cos(lat0) * cos(lon0);
+ yy0 = (kk + alt0) * cos(lat0) * sin(lon0);
+ zz0 = ((1-EARTH_EXCENTRICITY*EARTH_EXCENTRICITY)* kk + alt0) * sin(lat0);
+ TM[0][3] = - ( TM[0][0]*xx0 + TM[0][1]*yy0 + TM[0][2]*zz0 );
+ TM[1][3] = - ( TM[1][0]*xx0 + TM[1][1]*yy0 + TM[1][2]*zz0 );
+ TM[2][3] = - ( TM[2][0]*xx0 + TM[2][1]*yy0 + TM[2][2]*zz0 );
+ TM[3][0] = 0.0;
+ TM[3][1] = 0.0;
+ TM[3][2] = 0.0;
+ TM[3][3] = 1.0;
+}
+
+void CasteRx1::computeTMenu()
+{
+ //uses current position to compute ENU rotation
+ TMenu[0][0] = -sin(lon);
+ TMenu[0][1] = cos(lon);
+ TMenu[0][2] = 0.0;
+ TMenu[1][0] = -sin(lat)*cos(lon);
+ TMenu[1][1] = -sin(lat)*sin(lon);
+ TMenu[1][2] = cos(lat);
+ TMenu[2][0] = cos(lat)*cos(lon);
+ TMenu[2][1] = cos(lat)*sin(lon);
+ TMenu[2][2] = sin(lat);
+}
+
+void CasteRx1::fromWgsToMap()
+{
+ double auxM[3][3];
+ int ii;
+
+ //compute map coords
+ xMap=TM[0][0]*xWgs+TM[0][1]*yWgs+TM[0][2]*zWgs+TM[0][3];
+ yMap=TM[1][0]*xWgs+TM[1][1]*yWgs+TM[1][2]*zWgs+TM[1][3];
+ zMap=TM[2][0]*xWgs+TM[2][1]*yWgs+TM[2][2]*zWgs+TM[2][3];
+
+ //compute velocities
+ vxMap=TM[0][0]*vxWgs+TM[0][1]*vyWgs+TM[0][2]*vzWgs;
+ vyMap=TM[1][0]*vxWgs+TM[1][1]*vyWgs+TM[1][2]*vzWgs;
+ vzMap=TM[2][0]*vxWgs+TM[2][1]*vyWgs+TM[2][2]*vzWgs;
+
+ //compute covariance matrix
+ //firts step: auxM=TM*covEcef
+ for(ii=0;ii<3;ii++)
+ {
+ auxM[ii][0]=TM[ii][0]*cxxWgs+TM[ii][1]*cxyWgs+TM[ii][2]*cxzWgs;
+ auxM[ii][1]=TM[ii][0]*cxyWgs+TM[ii][1]*cyyWgs+TM[ii][2]*cyzWgs;
+ auxM[ii][2]=TM[ii][0]*cxzWgs+TM[ii][1]*cyzWgs+TM[ii][2]*czzWgs;
+ }
+ //second step: covMap = auxM*TM^T
+ cxxMap=auxM[0][0]*TM[0][0]+auxM[0][1]*TM[0][1]+auxM[0][2]*TM[0][2];
+ cyyMap=auxM[1][0]*TM[1][0]+auxM[1][1]*TM[1][1]+auxM[1][2]*TM[1][2];
+ czzMap=auxM[2][0]*TM[2][0]+auxM[2][1]*TM[2][1]+auxM[2][2]*TM[2][2];
+ cxyMap=auxM[0][0]*TM[1][0]+auxM[0][1]*TM[1][1]+auxM[0][2]*TM[1][2];
+}
+
+void CasteRx1::computeENUcovs()
+{
+ double auxM[3][3];
+ int ii;
+
+ //compute covariance matrix
+ //firts step: auxM=TMenu*covEcef
+ for(ii=0;ii<3;ii++)
+ {
+ auxM[ii][0]=TMenu[ii][0]*cxxWgs+TMenu[ii][1]*cxyWgs+TMenu[ii][2]*cxzWgs;
+ auxM[ii][1]=TMenu[ii][0]*cxyWgs+TMenu[ii][1]*cyyWgs+TMenu[ii][2]*cyzWgs;
+ auxM[ii][2]=TMenu[ii][0]*cxzWgs+TMenu[ii][1]*cyzWgs+TMenu[ii][2]*czzWgs;
+ }
+ //second step: covMap = auxM*TMenu^T
+ cxxEnu=auxM[0][0]*TMenu[0][0]+auxM[0][1]*TMenu[0][1]+auxM[0][2]*TMenu[0][2];
+ cyyEnu=auxM[1][0]*TMenu[1][0]+auxM[1][1]*TMenu[1][1]+auxM[1][2]*TMenu[1][2];
+ czzEnu=auxM[2][0]*TMenu[2][0]+auxM[2][1]*TMenu[2][1]+auxM[2][2]*TMenu[2][2];
+ cxyEnu=auxM[0][0]*TMenu[1][0]+auxM[0][1]*TMenu[1][1]+auxM[0][2]*TMenu[1][2];
+ cxzEnu=auxM[0][0]*TMenu[2][0]+auxM[0][1]*TMenu[2][1]+auxM[0][2]*TMenu[2][2];
+ cyzEnu=auxM[1][0]*TMenu[2][0]+auxM[1][1]*TMenu[2][1]+auxM[1][2]*TMenu[2][2];
+}
+
+void CasteRx1::markBlockAsReceived(unsigned short int id)
+{
+ list<sbfBlockManager>::iterator iiBlock;
+
+ for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++)
+ {
+ if ( iiBlock->blockId == id ) iiBlock->received = true;
+ }
+}
diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h
new file mode 100644
index 0000000000000000000000000000000000000000..77701d61ce6872a7246ee158b901ab1ca7feb073
--- /dev/null
+++ b/src/asterx1_gps.h
@@ -0,0 +1,400 @@
+
+#ifndef asterx1_gps_h
+#define asterx1_gps_h
+
+//include
+#include <iostream>
+#include <list>
+#include <math.h>
+#include "rs232.h"
+#include "eventserver.h"
+#include "commexceptions.h"
+#include "eventexceptions.h"
+#include <time.h>
+#include <sys/time.h>
+
+using namespace std;
+
+/* Look up table for fast computation of the CCITT 16-bit CRC. */
+static const unsigned short CRC_16CCIT_LookUp[256] = {
+ 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+ 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+ 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+ 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+ 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+ 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+ 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+ 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+ 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+ 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+ 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+ 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+ 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+ 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+ 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+ 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+ 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+ 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+ 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+ 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+ 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+ 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+ 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+ 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+ 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+ 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+ 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+ 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+ 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+ 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+ 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+ 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+union uniFloat {
+ float flt;
+ unsigned int i;
+};
+
+union uniDouble {
+ double dbl;
+ unsigned long long int i;
+};
+
+struct sbfBlockManager {
+ unsigned short int blockId;
+ bool received;
+};
+
+struct asterx1DeviceConfig {
+ string portName; //serial port system name
+ string acqPeriod; //string indicating acquisition period. Takes one of the values of the vector "acqPeriodValues"
+};
+
+//physical constants
+const double EARTH_RADIUS = 6378137.0;
+const double EARTH_EXCENTRICITY = 0.081819190842622;
+
+//Device protocol constants (SBF protocol)
+const unsigned short int PVTCartesian_ID = 4006;
+const unsigned short int PVTGeodetic_ID = 4007;
+const unsigned short int PosCovCartesian_ID = 5905;
+const unsigned short int DOP_ID = 4001;
+
+//acquisition constants
+const unsigned int ASTERX_BAUDRATE = 115200;
+const int DATA_TIMEOUT = 11000; //msec
+const int CONFIG_TIMEOUT = 500; //msec
+const string acqPeriodValues[] = {"onChange","msec20","msec50","msec100","msec200","msec500","sec1","sec2","sec5","sec10"};
+enum acqPeriodValuesIndex {ONCHANGE = 0, MSEC20, MSEC50, MSEC100, MSEC200, MSEC500, SEC1, SEC2, SEC5, SEC10};
+
+//data sizes
+const unsigned int BLOCK_HEADER_SIZE = 8;
+const unsigned int MIN_BLOCK_SIZE = BLOCK_HEADER_SIZE +1;
+const unsigned int MAX_BLOCK_SIZE = 200;
+
+/**
+ Member functions that returns an integer associated to success or not, should return one of these values
+*/
+const int BASIC_SUCCESS = 1;
+const int PORT_OPEN_ERROR = -1;
+const int PORT_CLOSE_ERROR = -2;
+const int PORT_CONFIG_ERROR = -3;
+const int ASTERX_CONFIG_ERROR = -4;
+const int ASTERX1_INVALID_COMMAND = -5;
+const int ASTERX1_STOPPINTG_ERROR = -6;
+const int PORT_READ_TIMEOUT = -7;
+const int ACQUISITION_ERROR = -8;
+
+/**
+ Status of a class object is a bitwise OR of different values. The given bit is set when positive cases, and reset if negative significance
+*/
+const unsigned int DEVICE_OPEN = 0x1;
+const unsigned int ACQUISITION_RUNNING = 0x2;
+const unsigned int DATA_BLOCK_OK = 0x4;
+const unsigned int GPS_AVAILABLE = 0x8;
+const unsigned int ALL_OK = DEVICE_OPEN | ACQUISITION_RUNNING | DATA_BLOCK_OK | GPS_AVAILABLE;
+
+//unit identifiers
+const bool inRADS = 0;
+const bool inDEGREES = 1;
+
+/**
+ CasteRx1 implements continuous data acquisition from the asteRx1 GPS receiver.
+ Serial communications are implemented by the class CRS232, inherited from CComm (both belonging to IRI communication library)
+ Detailed documentation about SBF data transfer protocol can be found in the device manual.
+ PLEASE NOTE: This driver only implements a part of the AsteRx1 device functionalities (the most useful part for mobile robot navigation).
+ PLEASE NOTE: For extended functionalities , please refer to the manual.
+ Programmer: Andreu Corominas Murtra, acorominas@iri.upc.edu
+ Device website: www.septentrio.com
+*/
+class CasteRx1
+{
+ public:
+ /**
+ Constructor: just catch the name of the serial port
+ */
+ CasteRx1(const string & hwPortName = "/dev/ttyACM0", const int acqRateIndex = MSEC500);
+
+ /**
+ Destructor: Stops acquisition in case it was running and closes serial Port in case it was opened.
+ */
+ virtual ~CasteRx1();
+
+ /**
+ Opens serial communications and starts serial comm thread.
+ If success , sets status bit DEVICE_OPEN.
+ Return values are:
+ BASIC_SUCCESS
+ PORT_OPEN_ERROR if thread can't be created or port can't be opened
+ PORT_CONFIG_ERROR if port can't be configured
+ */
+ int openDevice(int xx=3);
+
+ /**
+ Closes serial comm's , stops comm thread and deletes comm object.
+ If success , resets status bit DEVICE_OPEN.
+ Return values are:
+ BASIC_SUCCESS
+ PORT_CLOSE_ERROR
+ */
+ int closeDevice();
+
+ /**
+ Configures the device to perform some signal processing basics for mobile robotics.
+ Starts a mode of operation of continuos data flow from the device at a rate of 2Hz.
+ Return values are:
+ BASIC_SUCCESS
+ ASTERX_CONFIG_ERROR
+ */
+ int startAcquisition();
+
+ /**
+ Stops mode of operation of continuous data flow
+ Return values are:
+ BASIC_SUCCESS when device stop is ok
+ ASTERX1_STOPPINTG_ERROR otherwise.
+ */
+ int stopAcquisition();
+
+ /**
+ Reads current data produced by the device. Ends when all data blocks requested have been received.
+ Sets status and all data fields.
+ Return values are:
+ BASIC_SUCCESS if all blocks are successfully decodes.
+ ACQUISITION_ERROR otherwise.
+ Afterwards, call getStatus() to check for gps availability.
+ */
+ int readDataFromDevice();
+
+ /**
+ Prints all current data in a single row of the given ostream (cout or file)
+ */
+ void printData(ostream & ost);
+
+ /**
+ Sets map origin and computes transformation matrix.
+ Angles in degrees and altitude in meters.
+ A call to this function is mandatory if local coordinate related position is required.
+ */
+ void setMapOrigin(double mapLat0, double mapLon0, double mapAlt0, double mapAlpha0);
+
+ /**
+ Sets serial port name
+ */
+ void setPortName(string pName);
+
+ /**
+ Sets acquisition rate as on of the rateId indicated by the acqPeriodValuesIndex enum variable
+ */
+ void setAcquisitionRate(int rateIndex);
+
+ /**
+ Prints the homogeneous transformation matrix used to convert data from ECEF-WGS to local map.
+ */
+ void printTM();
+
+ /**
+ Get data ...
+ TimeStamp is set by the computer running the driver.
+ Tow is the GPS "time of week".
+ PVTerror codes are explained in the device manual, pag XX.
+ Lat and Lon are expressed in radians by default. Pass "inDEGREES" if degree values are required.
+ *Wgs is data with respect to the ECEF-WGS coordinate frame.
+ *Map is data with respect to the local (map) coordinate frame.
+ */
+ unsigned int getStatus();
+ double getTimeStamp();
+ unsigned short int getWnc();
+ unsigned int getTow();
+ unsigned int getNumSatellites();
+ float getPDOP();
+ float getTDOP();
+ float getHDOP();
+ float getVDOP();
+ float getUndulation();
+ unsigned short int getPVTerror();
+ double getLat(bool units=inRADS);
+ double getLon(bool units=inRADS);
+ double getAlt();
+ double getXWgs();
+ double getYWgs();
+ double getZWgs();
+ double getXMap();
+ double getYMap();
+ double getZMap();
+ double getVxWgs();
+ double getVyWgs();
+ double getVzWgs();
+ double getVxMap();
+ double getVyMap();
+ double getVzMap();
+ double getCxxWgs();
+ double getCyyWgs();
+ double getCzzWgs();
+ double getCxyWgs();
+ double getCxxMap();
+ double getCyyMap();
+ double getCzzMap();
+ double getCxyMap();
+ double getCxxEnu();
+ double getCyyEnu();
+ double getCzzEnu();
+ double getCxyEnu();
+ double getCxzEnu();
+ double getCyzEnu();
+
+ protected:
+ CRS232 *serialPort; /**<Object for serial comms. This object launch an independent thread managing byte I/O through serial port (USB or RS232)*/
+ asterx1DeviceConfig config; /**<Devices configuration. Currently a single string with serial port name*/
+
+ //variables for device operation and configuration
+ unsigned int status;/**<Status related to the execution of this process*/
+ list<sbfBlockManager> blockList; /**<this list manages which data blocks are requested and has been (or not) received in the current iteration*/
+ double lat0, lon0, alt0, alpha0; /**<coordinates of the map origin. Alpha is the orientation of the north vector wrt the map X axis*/
+ double TM[4][4]; /**homogeneous transformation matrix to convert from ecef-wgs84 to map coordinates*/
+ double TMenu[3][3]; /**Rotation transformation matrix to convert from ecef-wgs84 to local tangential ENU (east north up) coordinates. Useful to output covariances following current ENU axis*/
+
+ //GPS data to be published
+ double timeStamp; /**<time stamp set by the computer executing the process (not GPS time)*/
+ unsigned short int wnc; /**<gps week number*/
+ unsigned int tow; /**<gps time of week in milliseconds*/
+ unsigned int numSatellites; /**<number of visible satellites*/
+ double PDOP, TDOP, HDOP, VDOP; /**<Dillution of precsion DOP's*/
+ double undulation; /**<local geoid undulation between Datum ellipsoide and reference geoide*/
+ unsigned short int pvtError; /**<error codes related to GPS position computation. (see page 50 of Firmware User Manual)*/
+ double lat, lon, alt; /**<position in geodetic frame*/
+ double xWgs, yWgs, zWgs; /**<position in WGS frame*/
+ double xMap, yMap, zMap; /**<position in Map frame, a given reference frame*/
+ double vxWgs,vyWgs,vzWgs; /**<velocities referred to WGS*/
+ double vxMap, vyMap, vzMap; /**<velocities in Map frame, a given reference frame*/
+ double cxxWgs,cyyWgs,czzWgs,cxyWgs,cxzWgs,cyzWgs; /**<covariance parameters of WGS position*/
+ double cxxMap, cyyMap, czzMap, cxyMap; /**<covariance parameters in Map frame, a given reference frame*/
+ double cxxEnu,cyyEnu,czzEnu,cxyEnu,cxzEnu,cyzEnu; /**<covariance parameters in the local tangential plane of the current position following the East North Up axis*/
+
+ /**
+ Reads nn bytes from the serial communication. If there are no bytes available, this function blocks up to timeout is reached.
+ Success when ALL bytes have been received, it returns BASIC_SUCCESS
+ If timeout, it returns PORT_READ_TIMEOUT.
+ */
+ int readNbytes(unsigned char *buffer, unsigned int nn, unsigned int msecTimeOut);
+
+ /**
+ Synchronizes data reading with a given header mark. (markSixe in bytes).
+ Return values are:
+ BASIC_SUCCESS if success: the mark is found at the ffdd file descriptor input
+ PORT_READ_TIMEOUT if timeout is reached and the mark is not found
+ */
+ int synchronizeHeader(const unsigned char *mark, int msecTimeOut=1000, unsigned int markSize=2);
+
+ /**
+ Sends a command to the AsteRx1 device.
+ Return values are :
+ BASIC_SUCCESS if device has acknowledged the command.
+ PORT_READ_TIMEOUT if timeout is reached and the receiver has not replied with an acknowledment.
+ ASTERX1_INVALID_COMMAND when device doesn't know the command.
+ ASTERX_CONFIG_ERROR otherwise.
+ */
+ int sendCommand(const string & cmnd);
+
+ /**
+ Returns 1 byte integer in buffer (little-endian reading)
+ */
+ unsigned short int getUI1(unsigned char *buf);
+
+ /**
+ Returns 2 bytes integer in buffer (little-endian reading)
+ */
+ unsigned short int getUI2(unsigned char *buf);
+
+ /**
+ Returns 4 bytes integer in buffer (little-endian reading)
+ */
+ unsigned int getUI4(unsigned char *buf);
+
+ /**
+ Returns 4 byte float in buffer (little-endian reading)
+ */
+ float getFloat(unsigned char *buf);
+
+ /**
+ Returns 8 byte float in buffer (little-endian reading)
+ */
+ double getDouble(unsigned char *buf);
+
+ /**
+ Decodes PVTCartesian data block. Implementation of the SBF data transfer protocol.
+ */
+ int decodePVTCartesian(unsigned char *sbfMsg);
+
+ /**
+ Decodes PVTGeodetic data block. Implementation of the SBF data transfer protocol.
+ */
+ int decodePVTGeodetic(unsigned char *sbfMsg);
+
+ /**
+ Decodes PosCovCartesian data block. Implementation of the SBF data transfer protocol.
+ */
+ int decodePosCovCartesian(unsigned char *sbfMsg);
+
+ /**
+ Decodes DOP data block. Implementation of the SBF data transfer protocol.
+ */
+ int decodeDOP(unsigned char *sbfMsg);
+
+ /**
+ computes the crc-ccitt of buf with polynomic 0x1021
+ */
+ //unsigned short int computeCrc(unsigned char *buf, unsigned short int nBytes);
+ //unsigned short int CRC_compute16CCITT(const void * buf, size_t buf_length);
+ unsigned short int CRC_compute16CCITT(unsigned char *buf, unsigned short int nBytes);
+
+ /**
+ Computes the matrix to transform positions from ECEF-WGS to a local coordinate frame.
+ Local coordinate frame origin should be set by public function setMapOrigin().
+ */
+ void computeTM();
+
+ /**
+ Computes the matrix to transform positions from ECEF-WGS to the local tangential plane of the current position folloing the East North Up (ENU) axis
+ This matris is useful to output a covariance matrix of the WGS position following the ENU local tangential coordinates
+ */
+ void computeTMenu();
+
+ /**
+ Transforms ECEF-WGS positions, velocities and covariances to local coordinate frame.
+ Local coordinate frame origin shouold be set by public function setMapOrigin().
+ */
+ void fromWgsToMap();
+
+ /**
+ Computes covariance elements of the position referenced to the ENU axis of the current local tangential plane
+ */
+ void computeENUcovs();
+
+ /**
+ Sets to true the received field of the member in blockList that have blockId = id
+ */
+ void markBlockAsReceived(unsigned short int id);
+};
+#endif
diff --git a/src/asterx1exceptions.cpp b/src/asterx1exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48601198744d9cc9711f1f338fbadb7080bde1c7
--- /dev/null
+++ b/src/asterx1exceptions.cpp
@@ -0,0 +1,28 @@
+// Copyright (C) 2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Author Martà Morta (mmorta@iri.upc.edu)
+// All rights reserved.
+//
+// This is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hokuyoexceptions.h"
+#include <string.h>
+#include <stdio.h>
+
+const std::string hokuyo_exception_msg="[CHokuyo class] - ";
+
+CHokuyoException::CHokuyoException(const std::string& where,const std::string& error_msg):CException(where,hokuyo_exception_msg)
+{
+ this->error_msg+=error_msg;
+}
+
diff --git a/src/asterx1exceptions.h b/src/asterx1exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..0971abbae344f0932daa19ba294ed6f5646eeda5
--- /dev/null
+++ b/src/asterx1exceptions.h
@@ -0,0 +1,65 @@
+// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Author Martà Morta (mmorta@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of iriutils
+// iriutils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef _HOKUYO_EXCEPTIONS
+#define _HOKUYO_EXCEPTIONS
+
+#include "exceptions.h"
+
+/**
+ * \brief Mutual exclusion exception class
+ *
+ * This class implements the exceptions for the CMutex class. It does not add
+ * any feature to the base clase CException, but it is implemented to easyly
+ * distinguish between different types of exceptions.
+ *
+ * In addition to the general exception message added by the base class, this
+ * class adds the "[CHokuyo class] - " string to the user message to identify
+ * the class that generated the exception.
+ *
+ */
+class CHokuyoException : public CException
+{
+ public:
+ /**
+ * \brief Constructor
+ *
+ * The constructor calls the base class constructor to add the general
+ * exception identifier and then adds the class identifier string
+ * "[CHokuyo class]" and the supplied error message. The total exception
+ * message will look like this:
+ *
+ * \verbatim
+ * [Exception caught] - <where>
+ * Error: [CHokuyo class] - <error message>
+ * \endverbatim
+ *
+ * \param where a null terminated string with the information about the name
+ * of the function, the source code filename and the line where
+ * the exception was generated. This string must be generated
+ * by the _HERE_ macro.
+ *
+ * \param error_msg a null terminated string that contains the error message.
+ * This string may have any valid character and there is no
+ * limit on its length.
+ */
+ CHokuyoException(const std::string& where,const std::string& error_msg);
+};
+
+#endif
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4d441f04b0b93d008f512c254c2524531b018fdd
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,9 @@
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(testasterx1 testasterx1.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(testasterx1 asterx1_gps)
+
+#fast building
+# ADD_EXECUTABLE(testasterx1 testasterx1.cpp ../asterx1.cpp)
+
diff --git a/src/examples/testasterx1.cpp b/src/examples/testasterx1.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ef7afd7dab8a5f1fa0a8c619a7f9e605579acc8
--- /dev/null
+++ b/src/examples/testasterx1.cpp
@@ -0,0 +1,134 @@
+/*! \example testasterx1.cpp
+
+In this example almost all driver functionalities are tested.
+
+\b Usage:
+
+USB Default case (just one device which uses ttyACM connected)
+\code
+./testasterx1
+\endcode
+
+USB any port
+\code
+./testasterx1 [-u DEVICEPORT (/dev/ttyACM0)] [-n NUMITERATIONS]
+\endcode
+ */
+
+#include <stdlib.h>
+#include <asterx1_gps.h>
+
+using namespace std;
+
+int main(int argc, char **argv)
+{
+ int opt;
+ int ii;
+ int retValue;
+ int nIterations = 10;
+ int rate = MSEC500; //ONCHANGE, MSEC20, ... , MSEC100, ... SEC1, SEC5
+ string serial_device="/dev/ttyACM0";
+ streamsize tsDigits;
+ CasteRx1 *myGps;
+
+ cout << "\n\n TEST AsteRx1 GPS RECEIVER \n\n" << endl;
+
+ //cout << "acqPeriodValues[ONCHANGE]: " << acqPeriodValues[ONCHANGE] << endl;
+
+ // Argument management
+ while ((opt = getopt(argc, argv, "u:n:r:h?"))!= -1)
+ {
+ switch (opt)
+ {
+ case 'u': // change USB device
+ serial_device = optarg;
+ break;
+ case 'n':
+ nIterations = atoi(optarg);
+ break;
+ case 'r':
+ rate = atoi(optarg);
+ break;
+ case '?': // help
+ case 'h':
+ default:
+ cout << " USAGE" << endl
+ << " " << argv[0] << " [options]" << endl
+ << " OPTIONS" << endl
+ << " -u SERIAL_DEVICE (default: " << serial_device << ")" << endl
+ << " -n NUMBER OF ITERATIONS (default: " << nIterations << ")" << endl
+ << " -r ACQUISITION RATE: (default: " << rate << ")" << endl;
+ for (ii=0; ii<10; ii++)
+ {
+ cout << " " << ii << ": " << acqPeriodValues[ii] << endl;
+ }
+ cout << endl;
+ return 1;
+ }
+ }
+
+ myGps = new CasteRx1(serial_device,rate);
+
+ myGps->setMapOrigin(41.388595, 2.113133, 120, 44.2); //sets map origin for coordinate transformation
+ //myGps->printTM(); //prints to stdout the transformation matrix
+
+ retValue = myGps->openDevice();//open device comm's
+
+ retValue = myGps->closeDevice();//close device comm's (stressing tests ...)
+ retValue = myGps->openDevice();//open device comm's, again
+
+ myGps->setPortName("/dev/ttyS1"); //checking that when device is open, port configuration can't be achieved
+ myGps->setAcquisitionRate(56); //checking that when invalid rate indexes prdouces an error and don't change the rate
+ myGps->setAcquisitionRate(SEC2); //checking that when device is open rate configuration can be achieved
+ myGps->setAcquisitionRate(rate); //checking that when device is open rate configuration can be achieved
+
+ if ( retValue == BASIC_SUCCESS )
+ {
+ retValue = myGps->startAcquisition();//start data acquisition
+ retValue = myGps->stopAcquisition();//stop data acquisition (stressing tests ...)
+ retValue = myGps->startAcquisition();//start data acquisition, again
+ if ( retValue == BASIC_SUCCESS )
+ {
+ for (ii=0; ii<nIterations; ii++)
+ {
+ cout << endl;
+ myGps->readDataFromDevice();
+ //myGps->printData(cout); //prints to cout all current data in a single row. (cout can be repolaced by another ostream object reference)
+ cout << "Status = " << myGps->getStatus() << endl;
+ tsDigits=cout.precision(12);
+ cout << "TimeStamp = " << myGps->getTimeStamp() << endl;
+ cout.precision(tsDigits);
+ cout << "PVT error = " << myGps->getPVTerror() << endl;
+ cout << "TOW = " << myGps->getTow() << endl;
+
+ if ( myGps->getStatus() == ALL_OK )
+ {
+ cout << "Num Of Satellites = " << myGps->getNumSatellites() << endl;
+ tsDigits=cout.precision(12);
+ cout << "lat = " << myGps->getLat(inDEGREES) << endl;
+ cout << "lon = " << myGps->getLon(inDEGREES) << endl;
+ cout.precision(tsDigits);
+ cout << "alt = " << myGps->getAlt() << endl;
+ cout << "xWgs = " << myGps->getXWgs() << endl;
+ cout << "yWgs = " << myGps->getYWgs() << endl;
+ cout << "zWgs = " << myGps->getZWgs() << endl;
+ cout << "xMap = " << myGps->getXMap() << endl;
+ cout << "yMap = " << myGps->getYMap() << endl;
+ cout << "zMap = " << myGps->getZMap() << endl;
+ cout << "vxMap = " << myGps->getVxMap() << endl;
+ cout << "vyMap = " << myGps->getVyMap() << endl;
+ cout << "vzMap = " << myGps->getVzMap() << endl;
+ cout << "PDOP = " << myGps->getPDOP() << endl;
+ }
+ }
+
+ retValue = myGps->stopAcquisition();
+ }
+
+ retValue = myGps->closeDevice();
+ }
+
+ delete myGps;
+
+ return 0;
+}