Changed layout for organizational coherence.

software/rover/ublox-master/ublox_gps/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ublox_gps)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roscpp_serialization
+  ublox_msgs
+  ublox_serialization
+  diagnostic_updater
+)
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS roscpp ublox_msgs ublox_serialization)
+
+# include boost
+find_package(Boost REQUIRED COMPONENTS system regex thread)
+link_directories(${Boost_LIBRARY_DIR})
+include_directories(${Boost_INCLUDE_DIR})
+
+# include other ublox packages
+include_directories(${PROJECT_SOURCE_DIR}/include)
+include_directories(${catkin_INCLUDE_DIRS})
+
+# link pthread
+SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -std=c++11 -pthread")
+
+# build library
+add_library(ublox_gps src/gps.cpp)
+
+# fix msg compile order bug
+add_dependencies(ublox_gps ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(ublox_gps
+  boost_system
+  boost_regex
+  boost_thread
+)
+
+target_link_libraries(ublox_gps
+  ${catkin_LIBRARIES}
+)
+
+# build node
+add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c)
+set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps)
+
+target_link_libraries(ublox_gps_node boost_system boost_regex boost_thread)
+target_link_libraries(ublox_gps_node ${catkin_LIBRARIES})
+target_link_libraries(ublox_gps_node ublox_gps)
+
+install(TARGETS ublox_gps ublox_gps_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)

software/rover/ublox-master/ublox_gps/config/c94_m8p_base.yaml

@@ -0,0 +1,72 @@
+# Configuration Settings for C94-M8P device
+
+debug: 1                    # Range 0-4 (0 means no debug statements will print)
+
+save:
+  mask: 3103                # Save I/O, Message, INF Message, Nav, Receiver 
+                            # Manager, Antenna, and Logging Configuration
+  device: 4                 # Save to EEPROM
+
+device: /dev/ttyACM0
+frame_id: gps_base
+dynamic_model: stationary   # Velocity restricted to 0 m/s. Zero dynamics 
+                            # assumed.
+fix_mode: auto
+dr_limit: 0
+enable_ppp: false           # Not supported by C94-M8P
+
+rate: 4                     # Measurement rate in Hz
+nav_rate: 4                 # in number of measurement cycles
+
+uart1:
+  baudrate: 19200           # C94-M8P specific
+  in: 0                     # No UART in for base
+  out: 32                   # RTCM 3
+
+# TMODE3 Config
+tmode3: 1                   # Survey-In Mode
+sv_in:
+  reset: false              # True: disables and re-enables survey-in (resets)
+                            # False: Disables survey-in only if TMODE3 is  
+                            # disabled
+  min_dur: 300              # Survey-In Minimum Duration [s]
+  acc_lim: 3.0              # Survey-In Accuracy Limit [m]
+
+# RTCM out config
+rtcm:
+  ids: [5, 87, 77, 230]     # RTCM Messages to configure for Base station
+                                   # Enabled: GPS MSM7, GLONASS MSM7, 
+                                   # GLONASS CP bias, Stationary RTK ref
+                                   # 0xF5 0x05 Stationary RTK reference station
+                                   # ARP
+                                   # 0xF5 0x4A GPS MSM4
+                                   # 0xF5 0x4D GPS MSM7
+                                   # 0xF5 0x54 GLONASS MSM4
+                                   # 0xF5 0x57 GLONASS MSM7
+                                   # 0xF5 0x7C BeiDou MSM4
+                                   # 0xF5 0x7F BeiDou MSM7
+                                   # 0xF5 0xE6 GLONASS code-phase biases
+                                   # 0xF5 0xFE Reference station PVT 
+                                   #(u-blox proprietary RTCM Message)
+  rates: [1, 1, 1, 10]      # in number of navigation solutions, must match 
+                            # nav_rate, except for 0xE6
+
+dat:
+  set: false
+
+# GNSS Config
+gnss:
+  glonass: true             # Supported by C94-M8P
+  beidou: false             # Supported by C94-M8P
+  qzss: false               # Supported by C94-M8P
+
+inf: 
+  all: true                   # Whether to display all INF messages in console
+
+# Enable u-blox message publishers
+publish:
+  all: true
+  aid:
+    hui: false
+  nav:
+    posecef: false

software/rover/ublox-master/ublox_gps/config/c94_m8p_rover.yaml

@@ -0,0 +1,46 @@
+# Configuration Settings for C94-M8P device
+
+debug: 1                    # Range 0-4 (0 means no debug statements will print)
+
+save:
+  mask: 3103                # Save I/O, Message, INF Message, Nav, Receiver 
+                            # Manager, Antenna, and Logging Configuration
+  device: 4                 # Save to EEPROM
+
+device: /dev/ttyACM1
+frame_id: gps
+rate: 4                     # in Hz
+nav_rate: 4                 # [# of measurement cycles], recommended 1 Hz, may 
+                            # be either 5 Hz (Dual constellation) or 
+                            # 8 Hz (GPS only)
+dynamic_model: airborne2    # Airborne < 2G, 2D fix not supported (3D only), 
+                            # Max Alt: 50km
+                            # Max Horizontal Velocity: 250 m/s, 
+                            # Max Vertical Velocity: 100 m/s
+fix_mode: auto
+enable_ppp: false           # Not supported by C94-M8P
+dr_limit: 0
+
+uart1:
+  baudrate: 19200           # C94-M8P specific
+  in: 32                    # RTCM 3
+  out: 0                    # No UART out for rover
+
+gnss:
+  glonass: true             # Supported by C94-M8P
+  beidou: false             # Supported by C94-M8P
+  qzss: false               # Supported by C94-M8P
+
+dgnss_mode: 3               # Fixed mode
+
+inf: 
+  all: true                   # Whether to display all INF messages in console
+
+# Enable u-blox message publishers
+publish:
+  all: true
+  aid:
+    hui: false
+
+  nav:
+    posecef: false

software/rover/ublox-master/ublox_gps/config/m8n_rover.yaml

@@ -0,0 +1,46 @@
+# Configuration Settings for C94-M8P device
+
+debug: 1                    # Range 0-4 (0 means no debug statements will print)
+
+save:
+  mask: 3103                # Save I/O, Message, INF Message, Nav, Receiver 
+                            # Manager, Antenna, and Logging Configuration
+  device: 4                 # Save to EEPROM
+
+device: /dev/ttyUSB3
+frame_id: gps
+rate: 4                     # in Hz
+nav_rate: 4                 # [# of measurement cycles], recommended 1 Hz, may 
+                            # be either 5 Hz (Dual constellation) or 
+                            # 8 Hz (GPS only)
+dynamic_model: airborne2    # Airborne < 2G, 2D fix not supported (3D only), 
+                            # Max Alt: 50km
+                            # Max Horizontal Velocity: 250 m/s, 
+                            # Max Vertical Velocity: 100 m/s
+fix_mode: auto
+enable_ppp: false           # Not supported by C94-M8P
+dr_limit: 0
+
+uart1:
+  baudrate: 9600           # C94-M8P specific
+  in: 32                    # RTCM 3
+  out: 0                    # No UART out for rover
+
+gnss:
+  glonass: true             # Supported by C94-M8P
+  beidou: false             # Supported by C94-M8P
+  qzss: false               # Supported by C94-M8P
+
+dgnss_mode: 3               # Fixed mode
+
+inf: 
+  all: true                   # Whether to display all INF messages in console
+
+# Enable u-blox message publishers
+publish:
+  all: true
+  aid:
+    hui: false
+
+  nav:
+    posecef: false

software/rover/ublox-master/ublox_gps/config/nmea.yaml

@@ -0,0 +1,31 @@
+# Sample NMEA parameter configuration
+device: /dev/rover/ttyGPS
+
+uart1:
+  baudrate: 9600           # C94-M8P specific
+  
+nmea:
+  set: true
+  version: 65
+  num_sv: 8
+  sv_numbering: 1
+  compat: true
+  consider: true
+  limit82: true
+  high_prec: false
+  filter:
+    pos: true
+    msk_pos: true
+    time: true
+    date: true
+    gps_only: true
+    track: true
+  gnssToFilter:
+    gps: false
+    sbas: true
+    qzss: true
+    glonass: false
+    beidou: true
+  main_talker_id: 1
+  gsv_talker_id: 1
+  bds_talker_id: [0,0]

software/rover/ublox-master/ublox_gps/include/ublox_gps/async_worker.h

@@ -0,0 +1,258 @@
+//==============================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//==============================================================================
+
+#ifndef UBLOX_GPS_ASYNC_WORKER_H
+#define UBLOX_GPS_ASYNC_WORKER_H
+
+#include <ublox_gps/gps.h>
+
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/condition.hpp>
+
+
+#include "worker.h"
+
+namespace ublox_gps {
+
+int debug; //!< Used to determine which debug messages to display
+
+/**
+ * @brief Handles Asynchronous I/O reading and writing.
+ */
+template <typename StreamT>
+class AsyncWorker : public Worker {
+ public:
+  typedef boost::mutex Mutex;
+  typedef boost::mutex::scoped_lock ScopedLock;
+
+  /**
+   * @brief Construct an Asynchronous I/O worker.
+   * @param stream the stream for th I/O service
+   * @param io_service the I/O service
+   * @param buffer_size the size of the input and output buffers
+   */
+  AsyncWorker(boost::shared_ptr<StreamT> stream,
+              boost::shared_ptr<boost::asio::io_service> io_service,
+              std::size_t buffer_size = 8192);
+  virtual ~AsyncWorker();
+
+  /**
+   * @brief Set the callback function which handles input messages.
+   * @param callback the read callback which handles received messages
+   */
+  void setCallback(const Callback& callback) { read_callback_ = callback; }
+
+  /**
+   * @brief Send the data bytes via the I/O stream.
+   * @param data the buffer of data bytes to send
+   * @param size the size of the buffer
+   */
+  bool send(const unsigned char* data, const unsigned int size);
+  /**
+   * @brief Wait for incoming messages.
+   * @param timeout the maximum time to wait
+   */
+  void wait(const boost::posix_time::time_duration& timeout);
+
+  bool isOpen() const { return stream_->is_open(); }
+
+ protected:
+  /**
+   * @brief Read the input stream.
+   */
+  void doRead();
+
+  /**
+   * @brief Process messages read from the input stream.
+   * @param error_code an error code for read failures
+   * @param the number of bytes received
+   */
+  void readEnd(const boost::system::error_code&, std::size_t);
+
+  /**
+   * @brief Send all the data in the output buffer.
+   */
+  void doWrite();
+
+  /**
+   * @brief Close the I/O stream.
+   */
+  void doClose();
+
+  boost::shared_ptr<StreamT> stream_; //!< The I/O stream
+  boost::shared_ptr<boost::asio::io_service> io_service_; //!< The I/O service
+
+  Mutex read_mutex_; //!< Lock for the input buffer
+  boost::condition read_condition_;
+  std::vector<unsigned char> in_; //!< The input buffer
+  std::size_t in_buffer_size_; //!< number of bytes currently in the input
+                               //!< buffer
+
+  Mutex write_mutex_; //!< Lock for the output buffer
+  boost::condition write_condition_;
+  std::vector<unsigned char> out_; //!< The output buffer
+
+  boost::shared_ptr<boost::thread> background_thread_; //!< thread for the I/O
+                                                       //!< service
+  Callback read_callback_; //!< Callback function to handle received messages
+
+  bool stopping_; //!< Whether or not the I/O service is closed
+};
+
+template <typename StreamT>
+AsyncWorker<StreamT>::AsyncWorker(boost::shared_ptr<StreamT> stream,
+        boost::shared_ptr<boost::asio::io_service> io_service,
+        std::size_t buffer_size)
+    : stopping_(false) {
+  stream_ = stream;
+  io_service_ = io_service;
+  in_.resize(buffer_size);
+  in_buffer_size_ = 0;
+
+  out_.reserve(buffer_size);
+
+  io_service_->post(boost::bind(&AsyncWorker<StreamT>::doRead, this));
+  background_thread_.reset(new boost::thread(
+      boost::bind(&boost::asio::io_service::run, io_service_)));
+}
+
+template <typename StreamT>
+AsyncWorker<StreamT>::~AsyncWorker() {
+  io_service_->post(boost::bind(&AsyncWorker<StreamT>::doClose, this));
+  background_thread_->join();
+  //io_service_->reset();
+}
+
+template <typename StreamT>
+bool AsyncWorker<StreamT>::send(const unsigned char* data,
+                                const unsigned int size) {
+  ScopedLock lock(write_mutex_);
+  if(size == 0) {
+    ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0");
+    return true;
+  }
+
+  if (out_.capacity() - out_.size() < size) {
+    ROS_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message");
+    return false;
+  }
+  out_.insert(out_.end(), data, data + size);
+
+  io_service_->post(boost::bind(&AsyncWorker<StreamT>::doWrite, this));
+  return true;
+}
+
+template <typename StreamT>
+void AsyncWorker<StreamT>::doWrite() {
+  ScopedLock lock(write_mutex_);
+  // Do nothing if out buffer is empty
+  if (out_.size() == 0) {
+    return;
+  }
+  // Write all the data in the out buffer
+  boost::asio::write(*stream_, boost::asio::buffer(out_.data(), out_.size()));
+
+  if (debug >= 2) {
+    // Print the data that was sent
+    std::ostringstream oss;
+    for (std::vector<unsigned char>::iterator it = out_.begin();
+         it != out_.end(); ++it)
+      oss << boost::format("%02x") % static_cast<unsigned int>(*it) << " ";
+    ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str());
+  }
+  // Clear the buffer & unlock
+  out_.clear();
+  write_condition_.notify_all();
+}
+
+template <typename StreamT>
+void AsyncWorker<StreamT>::doRead() {
+  ScopedLock lock(read_mutex_);
+  stream_->async_read_some(
+      boost::asio::buffer(in_.data() + in_buffer_size_,
+                          in_.size() - in_buffer_size_),
+                          boost::bind(&AsyncWorker<StreamT>::readEnd, this,
+                              boost::asio::placeholders::error,
+                              boost::asio::placeholders::bytes_transferred));
+}
+
+template <typename StreamT>
+void AsyncWorker<StreamT>::readEnd(const boost::system::error_code& error,
+                                   std::size_t bytes_transfered) {
+  ScopedLock lock(read_mutex_);
+  if (error) {
+    ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li",
+              error.message().c_str(),
+              bytes_transfered);
+  } else if (bytes_transfered > 0) {
+    in_buffer_size_ += bytes_transfered;
+
+    if (debug >= 4) {
+      std::ostringstream oss;
+      for (std::vector<unsigned char>::iterator it =
+               in_.begin() + in_buffer_size_ - bytes_transfered;
+           it != in_.begin() + in_buffer_size_; ++it)
+        oss << boost::format("%02x") % static_cast<unsigned int>(*it) << " ";
+      ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered,
+               oss.str().c_str());
+    }
+
+    if (read_callback_)
+      read_callback_(in_.data(), in_buffer_size_);
+
+    read_condition_.notify_all();
+  }
+
+  if (!stopping_)
+    io_service_->post(boost::bind(&AsyncWorker<StreamT>::doRead, this));
+}
+
+template <typename StreamT>
+void AsyncWorker<StreamT>::doClose() {
+  ScopedLock lock(read_mutex_);
+  stopping_ = true;
+  boost::system::error_code error;
+  stream_->close(error);
+  if(error)
+    ROS_ERROR_STREAM(
+        "Error while closing the AsyncWorker stream: " << error.message());
+}
+
+template <typename StreamT>
+void AsyncWorker<StreamT>::wait(
+    const boost::posix_time::time_duration& timeout) {
+  ScopedLock lock(read_mutex_);
+  read_condition_.timed_wait(lock, timeout);
+}
+
+}  // namespace ublox_gps
+
+#endif  // UBLOX_GPS_ASYNC_WORKER_H

software/rover/ublox-master/ublox_gps/include/ublox_gps/callback.h

@@ -0,0 +1,238 @@
+//==============================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//==============================================================================
+
+#ifndef UBLOX_GPS_CALLBACK_H
+#define UBLOX_GPS_CALLBACK_H
+
+#include <ros/console.h>
+#include <ublox/serialization/ublox_msgs.h>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/thread.hpp>
+
+namespace ublox_gps {
+
+/**
+ * @brief A callback handler for a u-blox message.
+ */
+class CallbackHandler {
+ public:
+  /**
+   * @brief Decode the u-blox message.
+   */
+  virtual void handle(ublox::Reader& reader) = 0;
+
+  /**
+   * @brief Wait for on the condition.
+   */
+  bool wait(const boost::posix_time::time_duration& timeout) {
+    boost::mutex::scoped_lock lock(mutex_);
+    return condition_.timed_wait(lock, timeout);
+  }
+
+ protected:
+  boost::mutex mutex_; //!< Lock for the handler
+  boost::condition_variable condition_; //!< Condition for the handler lock
+};
+
+/**
+ * @brief A callback handler for a u-blox message.
+ * @typedef T the message type
+ */
+template <typename T>
+class CallbackHandler_ : public CallbackHandler {
+ public:
+  typedef boost::function<void(const T&)> Callback; //!< A callback function
+
+  /** 
+   * @brief Initialize the Callback Handler with a callback function
+   * @param func a callback function for the message, defaults to none
+   */
+  CallbackHandler_(const Callback& func = Callback()) : func_(func) {}
+  
+  /**
+   * @brief Get the last received message.
+   */
+  virtual const T& get() { return message_; }
+
+  /**
+   * @brief Decode the U-Blox message & call the callback function if it exists.
+   * @param reader a reader to decode the message buffer
+   */
+  void handle(ublox::Reader& reader) {
+    boost::mutex::scoped_lock(mutex_);
+    try {
+      if (!reader.read<T>(message_)) {
+        ROS_DEBUG_COND(debug >= 2, 
+                       "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", 
+                       static_cast<unsigned int>(reader.classId()),
+                       static_cast<unsigned int>(reader.messageId()),
+                       reader.length());
+        condition_.notify_all();
+        return;
+      }
+    } catch (std::runtime_error& e) {
+      ROS_DEBUG_COND(debug >= 2, 
+                     "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", 
+                     static_cast<unsigned int>(reader.classId()),
+                     static_cast<unsigned int>(reader.messageId()),
+                     reader.length());
+      condition_.notify_all();
+      return;
+    }
+
+    if (func_) func_(message_);
+    condition_.notify_all();
+  }
+  
+ private:
+  Callback func_; //!< the callback function to handle the message
+  T message_; //!< The last received message
+};
+
+/**
+ * @brief Callback handlers for incoming u-blox messages.
+ */
+class CallbackHandlers {
+ public:
+  /**
+   * @brief Add a callback handler for the given message type.
+   * @param callback the callback handler for the message
+   * @typedef.a ublox_msgs message with CLASS_ID and MESSAGE_ID constants
+   */
+  template <typename T>
+  void insert(typename CallbackHandler_<T>::Callback callback) {
+    boost::mutex::scoped_lock lock(callback_mutex_);
+    CallbackHandler_<T>* handler = new CallbackHandler_<T>(callback);
+    callbacks_.insert(
+      std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
+                     boost::shared_ptr<CallbackHandler>(handler)));
+  }
+
+  /**
+   * @brief Add a callback handler for the given message type and ID. This is 
+   * used for messages in which have the same structure (and therefore msg file)
+   * and same class ID but different message IDs. (e.g. INF, ACK)
+   * @param callback the callback handler for the message
+   * @param message_id the ID of the message
+   * @typedef.a ublox_msgs message with a CLASS_ID constant
+   */
+  template <typename T>
+  void insert(
+      typename CallbackHandler_<T>::Callback callback, 
+      unsigned int message_id) {
+    boost::mutex::scoped_lock lock(callback_mutex_);
+    CallbackHandler_<T>* handler = new CallbackHandler_<T>(callback);
+    callbacks_.insert(
+      std::make_pair(std::make_pair(T::CLASS_ID, message_id),
+                     boost::shared_ptr<CallbackHandler>(handler)));
+  }
+
+  /**
+   * @brief Calls the callback handler for the message in the reader.
+   * @param reader a reader containing a u-blox message
+   */
+  void handle(ublox::Reader& reader) {
+    // Find the callback handlers for the message & decode it
+    boost::mutex::scoped_lock lock(callback_mutex_);
+    Callbacks::key_type key =
+        std::make_pair(reader.classId(), reader.messageId());
+    for (Callbacks::iterator callback = callbacks_.lower_bound(key);
+         callback != callbacks_.upper_bound(key); ++callback)
+      callback->second->handle(reader);
+  }
+
+  /**
+   * @brief Read a u-blox message of the given type.
+   * @param message the received u-blox message
+   * @param timeout the amount of time to wait for the desired message
+   */
+  template <typename T>
+  bool read(T& message, const boost::posix_time::time_duration& timeout) {
+    bool result = false;
+    // Create a callback handler for this message
+    callback_mutex_.lock();
+    CallbackHandler_<T>* handler = new CallbackHandler_<T>();
+    Callbacks::iterator callback = callbacks_.insert(
+      (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
+                      boost::shared_ptr<CallbackHandler>(handler))));
+    callback_mutex_.unlock();
+
+    // Wait for the message
+    if (handler->wait(timeout)) {
+      message = handler->get();
+      result = true;
+    }
+    
+    // Remove the callback handler
+    callback_mutex_.lock();
+    callbacks_.erase(callback);
+    callback_mutex_.unlock();
+    return result;
+  }
+
+  /**
+   * @brief Processes u-blox messages in the given buffer & clears the read
+   * messages from the buffer.
+   * @param data the buffer of u-blox messages to process
+   * @param size the size of the buffer
+   */
+  void readCallback(unsigned char* data, std::size_t& size) {
+    ublox::Reader reader(data, size);
+    // Read all U-Blox messages in buffer
+    while (reader.search() != reader.end() && reader.found()) {
+      if (debug >= 3) {
+        // Print the received bytes
+        std::ostringstream oss;
+        for (ublox::Reader::iterator it = reader.pos();
+             it != reader.pos() + reader.length() + 8; ++it)
+          oss << boost::format("%02x") % static_cast<unsigned int>(*it) << " ";
+        ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, 
+                 oss.str().c_str());
+      }
+
+      handle(reader);
+    }
+
+    // delete read bytes from ASIO input buffer
+    std::copy(reader.pos(), reader.end(), data);
+    size -= reader.pos() - data;
+  }
+
+ private:
+  typedef std::multimap<std::pair<uint8_t, uint8_t>,
+                        boost::shared_ptr<CallbackHandler> > Callbacks;
+
+  // Call back handlers for u-blox messages
+  Callbacks callbacks_;
+  boost::mutex callback_mutex_;
+};
+
+}  // namespace ublox_gps
+
+#endif  // UBLOX_GPS_CALLBACK_H

software/rover/ublox-master/ublox_gps/include/ublox_gps/gps.h

@@ -0,0 +1,513 @@
+//==============================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//==============================================================================
+
+#ifndef UBLOX_GPS_H
+#define UBLOX_GPS_H
+// STL
+#include <map>
+#include <vector>
+#include <locale>
+#include <stdexcept>
+// Boost
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/serial_port.hpp>
+#include <boost/asio/io_service.hpp>
+#include <boost/atomic.hpp>
+// ROS
+#include <ros/console.h>
+// Other u-blox packages
+#include <ublox/serialization/ublox_msgs.h>
+// u-blox gps
+#include <ublox_gps/async_worker.h>
+#include <ublox_gps/callback.h>
+
+/**
+ * @namespace ublox_gps
+ * This namespace is for I/O communication with the u-blox device, including
+ * read callbacks.
+ */
+namespace ublox_gps {
+//! Possible baudrates for u-blox devices
+constexpr static unsigned int kBaudrates[] = { 4800,
+                                               9600,
+                                               19200,
+                                               38400,
+                                               57600,
+                                               115200,
+                                               230400,
+                                               460800 };
+/**
+ * @brief Handles communication with and configuration of the u-blox device
+ */
+class Gps {
+ public:
+  //! Sleep time [ms] after setting the baudrate
+  constexpr static int kSetBaudrateSleepMs = 500;
+  //! Default timeout for ACK messages in seconds
+  constexpr static double kDefaultAckTimeout = 1.0;
+  //! Size of write buffer for output messages
+  constexpr static int kWriterSize = 1024;
+
+  Gps();
+  virtual ~Gps();
+
+  /**
+   * @brief If called, when the node shuts down, it will send a command to
+   * save the flash memory.
+   */
+  void setSaveOnShutdown(bool save_on_shutdown) {
+    save_on_shutdown_ = save_on_shutdown;
+  }
+
+  /**
+   * @brief Initialize TCP I/O.
+   * @param host the TCP host
+   * @param port the TCP port
+   */
+  void initializeTcp(std::string host, std::string port);
+
+  /**
+   * @brief Initialize the Serial I/O port.
+   * @param port the device port address
+   * @param baudrate the desired baud rate of the port
+   * @param uart_in the UART In protocol, see CfgPRT for options
+   * @param uart_out the UART Out protocol, see CfgPRT for options
+   */
+  void initializeSerial(std::string port, unsigned int baudrate,
+                        uint16_t uart_in, uint16_t uart_out);
+
+  /**
+   * @brief Reset the Serial I/O port after u-blox reset.
+   * @param port the device port address
+   * @param baudrate the desired baud rate of the port
+   * @param uart_in the UART In protocol, see CfgPRT for options
+   * @param uart_out the UART Out protocol, see CfgPRT for options
+   */
+  void resetSerial(std::string port);
+
+  /**
+   * @brief Closes the I/O port, and initiates save on shutdown procedure
+   * if enabled.
+   */
+  void close();
+
+  /**
+   * @brief Reset I/O communications.
+   * @param wait Time to wait before restarting communications
+   */
+  void reset(const boost::posix_time::time_duration& wait);
+
+  /**
+   * @brief Send a reset message to the u-blox device.
+   * @param nav_bbr_mask The BBR sections to clear, see CfgRST message
+   * @param reset_mode The reset type, see CfgRST message
+   * @return true if the message was successfully sent, false otherwise
+   */
+  bool configReset(uint16_t nav_bbr_mask, uint16_t reset_mode);
+
+  /**
+   * @brief Configure the GNSS, cold reset the device, and reset the I/O.
+   * @param gnss the desired GNSS configuration
+   * @param wait the time to wait after resetting I/O before restarting
+   * @return true if the GNSS was configured, the device was reset, and the
+   * I/O reset successfully
+   */
+  bool configGnss(ublox_msgs::CfgGNSS gnss,
+                  const boost::posix_time::time_duration& wait);
+
+  /**
+   * @brief Send a message to the receiver to delete the BBR data stored in
+   * flash.
+   * @return true if sent message and received ACK, false otherwise
+   */
+  bool clearBbr();
+
+  /**
+   * @brief Configure the UART1 Port.
+   * @param baudrate the baudrate of the port
+   * @param in_proto_mask the in protocol mask, see CfgPRT message
+   * @param out_proto_mask the out protocol mask, see CfgPRT message
+   * @return true on ACK, false on other conditions.
+   */
+  bool configUart1(unsigned int baudrate, uint16_t in_proto_mask,
+                   uint16_t out_proto_mask);
+
+  /**
+   * @brief Disable the UART Port. Sets in/out protocol masks to 0. Does not
+   * modify other values.
+   * @param prev_cfg an empty message which will be filled with the previous
+   * configuration parameters
+   * @return true on ACK, false on other conditions.
+   */
+  bool disableUart1(ublox_msgs::CfgPRT& prev_cfg);
+
+  /**
+   * @brief Configure the USB Port.
+   * @param tx_ready the TX ready pin configuration, see CfgPRT message
+   * @param in_proto_mask the in protocol mask, see CfgPRT message
+   * @param out_proto_mask the out protocol mask, see CfgPRT message
+   * @return true on ACK, false on other conditions.
+   */
+  bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask,
+                 uint16_t out_proto_mask);
+
+  /**
+   * @brief Configure the device navigation and measurement rate settings.
+   * @param meas_rate Period in milliseconds between subsequent measurements.
+   * @param nav_rate the rate at which navigation solutions are generated by the
+   * receiver in number measurement cycles
+   * @return true on ACK, false on other conditions.
+   */
+  bool configRate(uint16_t meas_rate, uint16_t nav_rate);
+
+  /**
+   * @brief Configure the RTCM messages with the given IDs to the set rate.
+   * @param ids the RTCM message ids, valid range: [0, 255]
+   * @param rates the send rates for each RTCM message ID, valid range: [0, 255]
+   * @return true on ACK, false on other conditions.
+   */
+  bool configRtcm(std::vector<uint8_t> ids, std::vector<uint8_t> rates);
+
+  /**
+   * @brief Configure the SBAS settings.
+   * @param enable If true, enable SBAS. Deprecated in firmware 8, use CfgGNSS
+   * instead.
+   * @param usage SBAS usage, see CfgSBAS for options
+   * @param max_sbas Maximum Number of SBAS prioritized tracking channels
+   * @return true on ACK, false on other conditions.
+   */
+  bool configSbas(bool enable, uint8_t usage, uint8_t max_sbas);
+
+  /**
+   * @brief Set the TMODE3 settings to fixed.
+   *
+   * @details Sets the at the given antenna reference point (ARP) position in
+   * either Latitude Longitude Altitude (LLA) or ECEF coordinates.
+   * @param arp_position a vector of size 3 representing the ARP position in
+   * ECEF coordinates [m] or LLA coordinates [deg]
+   * @param arp_position_hp a vector of size 3 a vector of size 3 representing
+   * the ARP position in ECEF coordinates [0.1 mm] or LLA coordinates
+   * [deg * 1e-9]
+   * @param lla_flag true if position is given in LAT/LON/ALT, false if ECEF
+   * @param fixed_pos_acc Fixed position 3D accuracy [m]
+   * @return true on ACK, false if settings are incorrect or on other conditions
+   */
+  bool configTmode3Fixed(bool lla_flag,
+                         std::vector<float> arp_position,
+                         std::vector<int8_t> arp_position_hp,
+                         float fixed_pos_acc);
+
+  /**
+   * @brief Set the TMODE3 settings to survey-in.
+   * @param svin_min_dur Survey-in minimum duration [s]
+   * @param svin_acc_limit Survey-in position accuracy limit [m]
+   * @return true on ACK, false on other conditions.
+   */
+  bool configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit);
+
+  /**
+   * @brief Set the TMODE3 settings to disabled. Should only be called for
+   * High Precision GNSS devices, otherwise the device will return a NACK.
+   * @return true on ACK, false on other conditions.
+   */
+  bool disableTmode3();
+
+  /**
+   * @brief Set the rate at which the U-Blox device sends the given message
+   * @param class_id the class identifier of the message
+   * @param message_id the message identifier
+   * @param rate the updated rate in Hz
+   * @return true on ACK, false on other conditions.
+   */
+  bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate);
+
+  /**
+   * @brief Set the device dynamic model.
+   * @param model Dynamic model to use. Consult ublox protocol spec for details.
+   * @return true on ACK, false on other conditions.
+   */
+  bool setDynamicModel(uint8_t model);
+
+  /**
+   * @brief Set the device fix mode.
+   * @param mode 2D only, 3D only or auto.
+   * @return true on ACK, false on other conditions.
+   */
+  bool setFixMode(uint8_t mode);
+
+  /**
+   * @brief Set the dead reckoning time limit
+   * @param limit Time limit in seconds.
+   * @return true on ACK, false on other conditions.
+   */
+  bool setDeadReckonLimit(uint8_t limit);
+
+  /**
+   * @brief Enable or disable PPP (precise-point-positioning).
+   * @param enable If true, enable PPP.
+   * @return true on ACK, false on other conditions.
+   *
+   * @note This is part of the expert settings. It is recommended you check
+   * the ublox manual first.
+   */
+  bool setPpp(bool enable);
+
+  /**
+   * @brief Set the DGNSS mode (see CfgDGNSS message for details).
+   * @param mode the DGNSS mode (see CfgDGNSS message for options)
+   * @return true on ACK, false on other conditions
+   */
+  bool setDgnss(uint8_t mode);
+
+  /**
+   * @brief Enable or disable ADR (automotive dead reckoning).
+   * @param enable If true, enable ADR.
+   * @return true on ACK, false on other conditions.
+   */
+  bool setUseAdr(bool enable);
+
+  /**
+   * @brief Configure the U-Blox send rate of the message & subscribe to the
+   * given message
+   * @param the callback handler for the message
+   * @param rate the rate in Hz of the message
+   */
+  template <typename T>
+  void subscribe(typename CallbackHandler_<T>::Callback callback,
+                 unsigned int rate);
+  /**
+   * @brief Subscribe to the given Ublox message.
+   * @param the callback handler for the message
+   */
+  template <typename T>
+  void subscribe(typename CallbackHandler_<T>::Callback callback);
+
+  /**
+   * @brief Subscribe to the message with the given ID. This is used for
+   * messages which have the same format but different message IDs,
+   * e.g. INF messages.
+   * @param the callback handler for the message
+   * @param message_id the U-Blox message ID
+   */
+  template <typename T>
+  void subscribeId(typename CallbackHandler_<T>::Callback callback,
+                   unsigned int message_id);
+
+  /**
+   * Read a u-blox message of the given type.
+   * @param message the received u-blox message
+   * @param timeout the amount of time to wait for the desired message
+   */
+  template <typename T>
+  bool read(T& message,
+            const boost::posix_time::time_duration& timeout = default_timeout_);
+
+  bool isInitialized() const { return worker_ != 0; }
+  bool isConfigured() const { return isInitialized() && configured_; }
+  bool isOpen() const { return worker_->isOpen(); }
+
+  /**
+   * Poll a u-blox message of the given type.
+   * @param message the received u-blox message output
+   * @param payload the poll message payload sent to the device
+   * defaults to empty
+   * @param timeout the amount of time to wait for the desired message
+   */
+  template <typename ConfigT>
+  bool poll(ConfigT& message,
+            const std::vector<uint8_t>& payload = std::vector<uint8_t>(),
+            const boost::posix_time::time_duration& timeout = default_timeout_);
+  /**
+   * Poll a u-blox message.
+   * @param class_id the u-blox message class id
+   * @param message_id the u-blox message id
+   * @param payload the poll message payload sent to the device,
+   * defaults to empty
+   * @param timeout the amount of time to wait for the desired message
+   */
+  bool poll(uint8_t class_id, uint8_t message_id,
+            const std::vector<uint8_t>& payload = std::vector<uint8_t>());
+
+  /**
+   * @brief Send the given configuration message.
+   * @param message the configuration message
+   * @param wait if true, wait for an ACK
+   * @return true if message sent successfully and either ACK was received or
+   * wait was set to false
+   */
+  template <typename ConfigT>
+  bool configure(const ConfigT& message, bool wait = true);
+
+  /**
+   * @brief Wait for an acknowledge message until the timeout
+   * @param timeout maximum time to wait in seconds
+   * @param class_id the expected class ID of the ACK
+   * @param msg_id the expected message ID of the ACK
+   * @return true if expected ACK received, false otherwise
+   */
+  bool waitForAcknowledge(const boost::posix_time::time_duration& timeout,
+                          uint8_t class_id, uint8_t msg_id);
+
+ private:
+  //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK
+  enum AckType {
+    NACK, //! Not acknowledged
+    ACK, //! Acknowledge
+    WAIT //! Waiting for ACK
+  };
+
+  //! Stores ACK/NACK messages
+  struct Ack {
+    AckType type; //!< The ACK type
+    uint8_t class_id; //!< The class ID of the ACK
+    uint8_t msg_id; //!< The message ID of the ACK
+  };
+
+  /**
+   * @brief Set the I/O worker
+   * @param an I/O handler
+   */
+  void setWorker(const boost::shared_ptr<Worker>& worker);
+
+  /**
+   * @brief Subscribe to ACK/NACK messages and UPD-SOS-ACK messages.
+   */
+  void subscribeAcks();
+
+  /**
+   * @brief Callback handler for UBX-ACK message.
+   * @param m the message to process
+   */
+  void processAck(const ublox_msgs::Ack &m);
+
+  /**
+   * @brief Callback handler for UBX-NACK message.
+   * @param m the message to process
+   */
+  void processNack(const ublox_msgs::Ack &m);
+
+  /**
+   * @brief Callback handler for UBX-UPD-SOS-ACK message.
+   * @param m the message to process
+   */
+  void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m);
+
+  /**
+   * @brief Execute save on shutdown procedure.
+   *
+   * @details Execute the procedure recommended in the u-blox 8 documentation.
+   * Send a stop message to the receiver and instruct it to dump its
+   * current state to the attached flash memory (where fitted) as part of the
+   * shutdown procedure. The flash data is automatically retrieved when the
+   * receiver is restarted.
+   * @return true if the receiver reset & saved the BBR contents to flash
+   */
+  bool saveOnShutdown();
+
+  //! Processes I/O stream data
+  boost::shared_ptr<Worker> worker_;
+  //! Whether or not the I/O port has been configured
+  bool configured_;
+  //! Whether or not to save Flash BBR on shutdown
+  bool save_on_shutdown_;
+
+  //! The default timeout for ACK messages
+  static const boost::posix_time::time_duration default_timeout_;
+  //! Stores last received ACK accessed by multiple threads
+  mutable boost::atomic<Ack> ack_;
+
+  //! Callback handlers for u-blox messages
+  CallbackHandlers callbacks_;
+
+  std::string host_, port_;
+};
+
+template <typename T>
+void Gps::subscribe(
+    typename CallbackHandler_<T>::Callback callback, unsigned int rate) {
+  if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return;
+  subscribe<T>(callback);
+}
+
+template <typename T>
+void Gps::subscribe(typename CallbackHandler_<T>::Callback callback) {
+  callbacks_.insert<T>(callback);
+}
+
+template <typename T>
+void Gps::subscribeId(typename CallbackHandler_<T>::Callback callback,
+                      unsigned int message_id) {
+  callbacks_.insert<T>(callback, message_id);
+}
+
+template <typename ConfigT>
+bool Gps::poll(ConfigT& message,
+               const std::vector<uint8_t>& payload,
+               const boost::posix_time::time_duration& timeout) {
+  if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false;
+  return read(message, timeout);
+}
+
+template <typename T>
+bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) {
+  if (!worker_) return false;
+  return callbacks_.read(message, timeout);
+}
+
+template <typename ConfigT>
+bool Gps::configure(const ConfigT& message, bool wait) {
+  if (!worker_) return false;
+
+  // Reset ack
+  Ack ack;
+  ack.type = WAIT;
+  ack_.store(ack, boost::memory_order_seq_cst);
+
+  // Encode the message
+  std::vector<unsigned char> out(kWriterSize);
+  ublox::Writer writer(out.data(), out.size());
+  if (!writer.write(message)) {
+    ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x",
+              message.CLASS_ID, message.MESSAGE_ID);
+    return false;
+  }
+  // Send the message to the device
+  worker_->send(out.data(), writer.end() - out.data());
+
+  if (!wait) return true;
+
+  // Wait for an acknowledgment and return whether or not it was received
+  return waitForAcknowledge(default_timeout_,
+                            message.CLASS_ID,
+                            message.MESSAGE_ID);
+}
+
+}  // namespace ublox_gps
+
+#endif  // UBLOX_GPS_H

software/rover/ublox-master/ublox_gps/include/ublox_gps/mkgmtime.h

@@ -0,0 +1,55 @@
+/* mkgmtime.h -- make a time_t from a gmtime struct tm
+ $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $
+ 
+ * Copyright (c) 1998-2003 Carnegie Mellon University.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer. 
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * 3. The name "Carnegie Mellon University" must not be used to
+ *    endorse or promote products derived from this software without
+ *    prior written permission. For permission or any other legal
+ *    details, please contact  
+ *      Office of Technology Transfer
+ *      Carnegie Mellon University
+ *      5000 Forbes Avenue
+ *      Pittsburgh, PA  15213-3890
+ *      (412) 268-4387, fax: (412) 268-7395
+ *      tech-transfer@andrew.cmu.edu
+ *
+ * 4. Redistributions of any form whatsoever must retain the following
+ *    acknowledgment:
+ *    "This product includes software developed by Computing Services
+ *     at Carnegie Mellon University (http://www.cmu.edu/computing/)."
+ *
+ * CARNEGIE MELLON UNIVERSITY DISCLAIMS ALL WARRANTIES WITH REGARD TO
+ * THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS, IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
+ * FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
+ * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING
+ * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#ifndef INCLUDED_MKGMTIME_H
+#define INCLUDED_MKGMTIME_H
+
+#include <time.h>
+
+/**
+ * @brief Get the UTC time in seconds and nano-seconds from a time struct in
+ * GM time.
+ */
+extern time_t mkgmtime(struct tm * const tmp);
+
+#endif /* INCLUDED_MKGMTIME_H */

software/rover/ublox-master/ublox_gps/include/ublox_gps/utils.h

@@ -0,0 +1,31 @@
+#ifndef UBLOX_GPS_UTILS_H
+#define UBLOX_GPS_UTILS_H
+
+#include <math.h>
+#include <time.h>
+#include "ublox_msgs/NavPVT.h"
+
+extern "C" {
+  #include "ublox_gps/mkgmtime.h"
+}
+
+/**
+ * @brief Convert date/time to UTC time in seconds.
+ */
+template<typename NavPVT>
+long toUtcSeconds(const NavPVT& msg) {
+  // Create TM struct for mkgmtime
+  struct tm time = {0};
+  time.tm_year = msg.year - 1900; 
+  time.tm_mon = msg.month - 1; 
+  time.tm_mday = msg.day;
+  time.tm_hour = msg.hour;   
+  time.tm_min = msg.min; 
+  time.tm_sec = msg.sec;
+  // C++ STL doesn't have a mkgmtime (though other libraries do)
+  // STL mktime converts date/time to seconds in local time 
+  // A modified version of code external library is used for mkgmtime
+  return mkgmtime(&time);
+}
+
+#endif

software/rover/ublox-master/ublox_gps/include/ublox_gps/worker.h

@@ -0,0 +1,72 @@
+//==============================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//==============================================================================
+
+#ifndef UBLOX_GPS_WORKER_H
+#define UBLOX_GPS_WORKER_H
+
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/function.hpp>
+
+namespace ublox_gps {
+
+/**
+ * @brief Handles I/O reading and writing.
+ */
+class Worker {
+ public:
+  typedef boost::function<void(unsigned char*, std::size_t&)> Callback;
+  virtual ~Worker() {}
+
+  /**
+   * @brief Set the callback function for received messages.
+   * @param callback the callback function which process messages in the buffer
+   */
+  virtual void setCallback(const Callback& callback) = 0;
+
+  /**
+   * @brief Send the data in the buffer.
+   * @param data the bytes to send
+   * @param size the size of the buffer
+   */
+  virtual bool send(const unsigned char* data, const unsigned int size) = 0;
+  
+  /**
+   * @brief Wait for an incoming message.
+   * @param timeout the maximum time to wait.
+   */
+  virtual void wait(const boost::posix_time::time_duration& timeout) = 0;
+
+  /**
+   * @brief Whether or not the I/O stream is open.
+   */
+  virtual bool isOpen() const = 0;
+};
+
+}  // namespace ublox_gps
+
+#endif  // UBLOX_GPS_WORKER_H

software/rover/ublox-master/ublox_gps/launch/ublox_device.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <node name="rover_gps" pkg="ublox_gps" type="ublox_gps" output="screen" clear_params="false" respawn="true" respawn_delay="10">
+    <rosparam command="load" file="$(find ublox_gps)/config/nmea.yaml" />
+  </node>
+</launch>

software/rover/ublox-master/ublox_gps/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format='2'>
+  <name>ublox_gps</name>
+  <version>1.1.2</version>
+  <description>
+     Driver for u-blox GPS devices.
+  </description>
+  <author>Johannes Meyer</author>
+  <maintainer email="gcross.code@icloud.com">Gareth Cross</maintainer>
+  <maintainer email="quchao@seas.upenn.edu">Chao Qu</maintainer>
+  <maintainer email="vmlane@alum.mit.edu">Veronica Lane</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/ublox</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>ublox_serialization</depend>
+  <depend>ublox_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>roscpp_serialization</depend>
+  <depend>diagnostic_updater</depend>
+
+</package>

software/rover/ublox-master/ublox_gps/src/gps.cpp

@@ -0,0 +1,540 @@
+//==============================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//==============================================================================
+
+#include <ublox_gps/gps.h>
+
+namespace ublox_gps {
+
+using namespace ublox_msgs;
+
+const boost::posix_time::time_duration Gps::default_timeout_ =
+    boost::posix_time::seconds(Gps::kDefaultAckTimeout);
+
+Gps::Gps() : configured_(false) { subscribeAcks(); }
+
+Gps::~Gps() { close(); }
+
+void Gps::setWorker(const boost::shared_ptr<Worker>& worker) {
+  if (worker_) return;
+  worker_ = worker;
+  worker_->setCallback(boost::bind(&CallbackHandlers::readCallback,
+                                   &callbacks_, _1, _2));
+  configured_ = static_cast<bool>(worker);
+}
+
+void Gps::subscribeAcks() {
+  // Set NACK handler
+  subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processNack, this, _1),
+                               ublox_msgs::Message::ACK::NACK);
+  // Set ACK handler
+  subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processAck, this, _1),
+                               ublox_msgs::Message::ACK::ACK);
+  // Set UPD-SOS-ACK handler
+  subscribe<ublox_msgs::UpdSOS_Ack>(
+      boost::bind(&Gps::processUpdSosAck, this, _1));
+}
+
+void Gps::processAck(const ublox_msgs::Ack &m) {
+  // Process ACK/NACK messages
+  Ack ack;
+  ack.type = ACK;
+  ack.class_id = m.clsID;
+  ack.msg_id = m.msgID;
+  // store the ack atomically
+  ack_.store(ack, boost::memory_order_seq_cst);
+  ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x",
+                 m.clsID, m.msgID);
+}
+
+void Gps::processNack(const ublox_msgs::Ack &m) {
+  // Process ACK/NACK messages
+  Ack ack;
+  ack.type = NACK;
+  ack.class_id = m.clsID;
+  ack.msg_id = m.msgID;
+  // store the ack atomically
+  ack_.store(ack, boost::memory_order_seq_cst);
+  ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID);
+}
+
+void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) {
+  if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) {
+    Ack ack;
+    ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK;
+    ack.class_id = m.CLASS_ID;
+    ack.msg_id = m.MESSAGE_ID;
+    // store the ack atomically
+    ack_.store(ack, boost::memory_order_seq_cst);
+    ROS_DEBUG_COND(ack.type == ACK && debug >= 2,
+                   "U-blox: received UPD SOS Backup ACK");
+    if(ack.type == NACK)
+      ROS_ERROR("U-blox: received UPD SOS Backup NACK");
+  }
+}
+
+void Gps::initializeSerial(std::string port, unsigned int baudrate,
+                           uint16_t uart_in, uint16_t uart_out) {
+  port_ = port;
+  boost::shared_ptr<boost::asio::io_service> io_service(
+      new boost::asio::io_service);
+  boost::shared_ptr<boost::asio::serial_port> serial(
+      new boost::asio::serial_port(*io_service));
+
+  // open serial port
+  try {
+    serial->open(port);
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not open serial port :"
+                             + port + " " + e.what());
+  }
+
+  ROS_INFO("U-Blox: Opened serial port %s", port.c_str());
+
+  // Set the I/O worker
+  if (worker_) return;
+  setWorker(boost::shared_ptr<Worker>(
+      new AsyncWorker<boost::asio::serial_port>(serial, io_service)));
+
+  configured_ = false;
+
+  // Set the baudrate
+  boost::asio::serial_port_base::baud_rate current_baudrate;
+  serial->get_option(current_baudrate);
+  // Incrementally increase the baudrate to the desired value
+  for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) {
+    if (current_baudrate.value() == baudrate)
+      break;
+    // Don't step down, unless the desired baudrate is lower
+    if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i])
+      continue;
+    serial->set_option(
+        boost::asio::serial_port_base::baud_rate(kBaudrates[i]));
+    boost::this_thread::sleep(
+        boost::posix_time::milliseconds(kSetBaudrateSleepMs));
+    serial->get_option(current_baudrate);
+    ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value());
+  }
+  configured_ = configUart1(baudrate, uart_in, uart_out);
+  if(!configured_ || current_baudrate.value() != baudrate) {
+    throw std::runtime_error("Could not configure serial baud rate");
+  }
+}
+
+void Gps::resetSerial(std::string port) {
+  boost::shared_ptr<boost::asio::io_service> io_service(
+      new boost::asio::io_service);
+  boost::shared_ptr<boost::asio::serial_port> serial(
+      new boost::asio::serial_port(*io_service));
+
+  // open serial port
+  try {
+    serial->open(port);
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not open serial port :"
+                             + port + " " + e.what());
+  }
+
+  ROS_INFO("U-Blox: Reset serial port %s", port.c_str());
+
+  // Set the I/O worker
+  if (worker_) return;
+  setWorker(boost::shared_ptr<Worker>(
+      new AsyncWorker<boost::asio::serial_port>(serial, io_service)));
+  configured_ = false;
+
+  // Poll UART PRT Config
+  std::vector<uint8_t> payload;
+  payload.push_back(CfgPRT::PORT_ID_UART1);
+  if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) {
+    ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT");
+    return;
+  }
+  CfgPRT prt;
+  if(!read(prt, default_timeout_)) {
+    ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s",
+                "message");
+    return;
+  }
+
+  // Set the baudrate
+  serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baudRate));
+  configured_ = true;
+}
+
+void Gps::initializeTcp(std::string host, std::string port) {
+  host_ = host;
+  port_ = port;
+  boost::shared_ptr<boost::asio::io_service> io_service(
+      new boost::asio::io_service);
+  boost::asio::ip::tcp::resolver::iterator endpoint;
+
+  try {
+    boost::asio::ip::tcp::resolver resolver(*io_service);
+    endpoint =
+        resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not resolve" + host + " " +
+                             port + " " + e.what());
+  }
+
+  boost::shared_ptr<boost::asio::ip::tcp::socket> socket(
+    new boost::asio::ip::tcp::socket(*io_service));
+
+  try {
+    socket->connect(*endpoint);
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not connect to " +
+                             endpoint->host_name() + ":" +
+                             endpoint->service_name() + ": " + e.what());
+  }
+
+  ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(),
+           endpoint->service_name().c_str());
+
+  if (worker_) return;
+  setWorker(boost::shared_ptr<Worker>(
+      new AsyncWorker<boost::asio::ip::tcp::socket>(socket,
+                                                    io_service)));
+}
+
+void Gps::close() {
+  if(save_on_shutdown_) {
+    if(saveOnShutdown())
+      ROS_INFO("U-Blox Flash BBR saved");
+    else
+      ROS_INFO("U-Blox Flash BBR failed to save");
+  }
+  worker_.reset();
+  configured_ = false;
+}
+
+void Gps::reset(const boost::posix_time::time_duration& wait) {
+  worker_.reset();
+  configured_ = false;
+  // sleep because of undefined behavior after I/O reset
+  boost::this_thread::sleep(wait);
+  if (host_ == "")
+    resetSerial(port_);
+  else
+    initializeTcp(host_, port_);
+}
+
+bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) {
+  ROS_WARN("Resetting u-blox. If device address changes, %s",
+           "node must be relaunched.");
+
+  CfgRST rst;
+  rst.navBbrMask = nav_bbr_mask;
+  rst.resetMode = reset_mode;
+
+  // Don't wait for ACK, return if it fails
+  if (!configure(rst, false))
+    return false;
+  return true;
+}
+
+bool Gps::configGnss(CfgGNSS gnss,
+                     const boost::posix_time::time_duration& wait) {
+  // Configure the GNSS settings
+  ROS_DEBUG("Re-configuring GNSS.");
+  if (!configure(gnss))
+    return false;
+  // Cold reset the GNSS
+  ROS_WARN("GNSS re-configured, cold resetting device.");
+  if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS))
+    return false;
+  ros::Duration(1.0).sleep();
+  // Reset the I/O
+  reset(wait);
+  return isConfigured();
+}
+
+bool Gps::saveOnShutdown() {
+  // Command the receiver to stop
+  CfgRST rst;
+  rst.navBbrMask = rst.NAV_BBR_HOT_START;
+  rst.resetMode = rst.RESET_MODE_GNSS_STOP;
+  if(!configure(rst))
+    return false;
+  // Command saving the contents of BBR to flash memory
+  // And wait for UBX-UPD-SOS-ACK
+  UpdSOS backup;
+  return configure(backup);
+}
+
+bool Gps::clearBbr() {
+  // Command saving the contents of BBR to flash memory
+  // And wait for UBX-UPD-SOS-ACK
+  UpdSOS sos;
+  sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR;
+  return configure(sos);
+}
+
+bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask,
+                      uint16_t out_proto_mask) {
+  if (!worker_) return true;
+
+  ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u",
+            baudrate, in_proto_mask, out_proto_mask);
+
+  CfgPRT port;
+  port.portID = CfgPRT::PORT_ID_UART1;
+  port.baudRate = baudrate;
+  port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT |
+              CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1;
+  port.inProtoMask = in_proto_mask;
+  port.outProtoMask = out_proto_mask;
+  return configure(port);
+}
+
+bool Gps::disableUart1(CfgPRT& prev_config) {
+  ROS_DEBUG("Disabling UART1");
+
+  // Poll UART PRT Config
+  std::vector<uint8_t> payload;
+  payload.push_back(CfgPRT::PORT_ID_UART1);
+  if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) {
+    ROS_ERROR("disableUart: Could not poll UART1 CfgPRT");
+    return false;
+  }
+  if(!read(prev_config, default_timeout_)) {
+    ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message");
+    return false;
+  }
+  // Keep original settings, but disable in/out
+  CfgPRT port;
+  port.portID = CfgPRT::PORT_ID_UART1;
+  port.mode = prev_config.mode;
+  port.baudRate = prev_config.baudRate;
+  port.inProtoMask = 0;
+  port.outProtoMask = 0;
+  port.txReady = prev_config.txReady;
+  port.flags = prev_config.flags;
+  return configure(port);
+}
+
+bool Gps::configUsb(uint16_t tx_ready,
+                    uint16_t in_proto_mask,
+                    uint16_t out_proto_mask) {
+  if (!worker_) return true;
+
+  ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u",
+            tx_ready, in_proto_mask, out_proto_mask);
+
+  CfgPRT port;
+  port.portID = CfgPRT::PORT_ID_USB;
+  port.txReady = tx_ready;
+  port.inProtoMask = in_proto_mask;
+  port.outProtoMask = out_proto_mask;
+  return configure(port);
+}
+
+bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) {
+  ROS_DEBUG("Configuring measurement rate to %u and nav rate to %u", meas_rate,
+           nav_rate);
+
+  CfgRATE rate;
+  rate.measRate = meas_rate;
+  rate.navRate = nav_rate;  //  must be fixed at 1 for ublox 5 and 6
+  rate.timeRef = CfgRATE::TIME_REF_GPS;
+  return configure(rate);
+}
+
+bool Gps::configRtcm(std::vector<uint8_t> ids, std::vector<uint8_t> rates) {
+  for(size_t i = 0; i < ids.size(); ++i) {
+    ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]);
+    if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) {
+      ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]);
+      return false;
+    }
+  }
+  return true;
+}
+
+bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) {
+  ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas);
+
+  ublox_msgs::CfgSBAS msg;
+  msg.mode = (enable ? CfgSBAS::MODE_ENABLED : 0);
+  msg.usage = usage;
+  msg.maxSBAS = max_sbas;
+  return configure(msg);
+}
+
+bool Gps::configTmode3Fixed(bool lla_flag,
+                            std::vector<float> arp_position,
+                            std::vector<int8_t> arp_position_hp,
+                            float fixed_pos_acc) {
+  if(arp_position.size() != 3 || arp_position_hp.size() != 3) {
+    ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s",
+              "& arp_position_hp args must be 3");
+    return false;
+  }
+
+  ROS_DEBUG("Configuring TMODE3 to Fixed");
+
+  CfgTMODE3 tmode3;
+  tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK;
+  tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0;
+
+  // Set position
+  if(lla_flag) {
+    // Convert from [deg] to [deg * 1e-7]
+    tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7);
+    tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7);
+    tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7);
+  } else {
+    // Convert from m to cm
+    tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2);
+    tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2);
+    tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e2);
+  }
+  tmode3.ecefXOrLatHP = arp_position_hp[0];
+  tmode3.ecefYOrLonHP = arp_position_hp[1];
+  tmode3.ecefZOrAltHP = arp_position_hp[2];
+  // Convert from m to [0.1 mm]
+  tmode3.fixedPosAcc = (uint32_t)round(fixed_pos_acc * 1e4);
+  return configure(tmode3);
+}
+
+bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur,
+                               float svin_acc_limit) {
+  CfgTMODE3 tmode3;
+  ROS_DEBUG("Setting TMODE3 to Survey In");
+  tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK;
+  tmode3.svinMinDur = svin_min_dur;
+  // Convert from m to [0.1 mm]
+  tmode3.svinAccLimit = (int)round(svin_acc_limit * 1e4);
+  return configure(tmode3);
+}
+
+bool Gps::disableTmode3() {
+  ROS_DEBUG("Disabling TMODE3");
+
+  CfgTMODE3 tmode3;
+  tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK;
+  return configure(tmode3);
+}
+
+bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) {
+  ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id,
+                 message_id, rate);
+  ublox_msgs::CfgMSG msg;
+  msg.msgClass = class_id;
+  msg.msgID = message_id;
+  msg.rate = rate;
+  return configure(msg);
+}
+
+bool Gps::setDynamicModel(uint8_t model) {
+  ROS_DEBUG("Setting dynamic model to %u", model);
+
+  ublox_msgs::CfgNAV5 msg;
+  msg.dynModel = model;
+  msg.mask = ublox_msgs::CfgNAV5::MASK_DYN;
+  return configure(msg);
+}
+
+bool Gps::setFixMode(uint8_t mode) {
+  ROS_DEBUG("Setting fix mode to %u", mode);
+
+  ublox_msgs::CfgNAV5 msg;
+  msg.fixMode = mode;
+  msg.mask = ublox_msgs::CfgNAV5::MASK_FIX_MODE;
+  return configure(msg);
+}
+
+bool Gps::setDeadReckonLimit(uint8_t limit) {
+  ROS_DEBUG("Setting DR Limit to %u", limit);
+
+  ublox_msgs::CfgNAV5 msg;
+  msg.drLimit = limit;
+  msg.mask = ublox_msgs::CfgNAV5::MASK_DR_LIM;
+  return configure(msg);
+}
+
+bool Gps::setPpp(bool enable) {
+  ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling"));
+
+  ublox_msgs::CfgNAVX5 msg;
+  msg.usePPP = enable;
+  msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP;
+  return configure(msg);
+}
+
+bool Gps::setDgnss(uint8_t mode) {
+  CfgDGNSS cfg;
+  ROS_DEBUG("Setting DGNSS mode to %u", mode);
+  cfg.dgnssMode = mode;
+  return configure(cfg);
+}
+
+bool Gps::setUseAdr(bool enable) {
+  ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling"));
+
+  ublox_msgs::CfgNAVX5 msg;
+  msg.useAdr = enable;
+  msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR;
+  return configure(msg);
+}
+
+bool Gps::poll(uint8_t class_id, uint8_t message_id,
+               const std::vector<uint8_t>& payload) {
+  if (!worker_) return false;
+
+  std::vector<unsigned char> out(kWriterSize);
+  ublox::Writer writer(out.data(), out.size());
+  if (!writer.write(payload.data(), payload.size(), class_id, message_id))
+    return false;
+  worker_->send(out.data(), writer.end() - out.data());
+
+  return true;
+}
+
+bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout,
+                             uint8_t class_id, uint8_t msg_id) {
+  ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x",
+                 class_id, msg_id);
+  boost::posix_time::ptime wait_until(
+      boost::posix_time::second_clock::local_time() + timeout);
+
+  Ack ack = ack_.load(boost::memory_order_seq_cst);
+  while (boost::posix_time::second_clock::local_time() < wait_until
+         && (ack.class_id != class_id
+             || ack.msg_id != msg_id
+             || ack.type == WAIT)) {
+    worker_->wait(timeout);
+    ack = ack_.load(boost::memory_order_seq_cst);
+  }
+  bool result = ack.type == ACK
+                && ack.class_id == class_id
+                && ack.msg_id == msg_id;
+  return result;
+}
+}  // namespace ublox_gps

software/rover/ublox-master/ublox_gps/src/mkgmtime.c

@@ -0,0 +1,156 @@
+/* mkgmtime.c - make time corresponding to a GMT timeval struct
+ $Id: mkgmtime.c,v 1.10 2003/10/22 18:50:12 rjs3 Exp $
+ 
+ * Copyright (c) 1998-2003 Carnegie Mellon University.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer. 
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * 3. The name "Carnegie Mellon University" must not be used to
+ *    endorse or promote products derived from this software without
+ *    prior written permission. For permission or any other legal
+ *    details, please contact  
+ *      Office of Technology Transfer
+ *      Carnegie Mellon University
+ *      5000 Forbes Avenue
+ *      Pittsburgh, PA  15213-3890
+ *      (412) 268-4387, fax: (412) 268-7395
+ *      tech-transfer@andrew.cmu.edu
+ *
+ * 4. Redistributions of any form whatsoever must retain the following
+ *    acknowledgment:
+ *    "This product includes software developed by Computing Services
+ *     at Carnegie Mellon University (http://www.cmu.edu/computing/)."
+ *
+ * CARNEGIE MELLON UNIVERSITY DISCLAIMS ALL WARRANTIES WITH REGARD TO
+ * THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS, IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
+ * FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
+ * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING
+ * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ *
+ */
+/*
+ * Copyright (c) 1987, 1989, 1993
+ *  The Regents of the University of California.  All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Arthur David Olson of the National Cancer Institute.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *  This product includes software developed by the University of
+ *  California, Berkeley and its contributors.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+** Adapted from code provided by Robert Elz, who writes:
+**  The "best" way to do mktime I think is based on an idea of Bob
+**  Kridle's (so its said...) from a long time ago. (mtxinu!kridle now).
+**  It does a binary search of the time_t space.  Since time_t's are
+**  just 32 bits, its a max of 32 iterations (even at 64 bits it
+**  would still be very reasonable).
+*/
+
+#include "ublox_gps/mkgmtime.h"
+#ifndef WRONG
+#define WRONG   (-1)
+#endif /* !defined WRONG */
+
+static int tmcomp(register const struct tm * const  atmp, 
+                  register const struct tm * const btmp)
+{
+    register int    result;
+
+    if ((result = (atmp->tm_year - btmp->tm_year)) == 0 &&
+        (result = (atmp->tm_mon - btmp->tm_mon)) == 0 &&
+        (result = (atmp->tm_mday - btmp->tm_mday)) == 0 &&
+        (result = (atmp->tm_hour - btmp->tm_hour)) == 0 &&
+        (result = (atmp->tm_min - btmp->tm_min)) == 0)
+            result = atmp->tm_sec - btmp->tm_sec;
+    return result;
+}
+
+time_t mkgmtime(struct tm * const tmp) {
+    register int            dir;
+    register int            bits;
+    register int            saved_seconds;
+    time_t              t;
+    struct tm           yourtm, *mytm;
+
+    yourtm = *tmp;
+    saved_seconds = yourtm.tm_sec;
+    yourtm.tm_sec = 0;
+    /*
+    ** Calculate the number of magnitude bits in a time_t
+    ** (this works regardless of whether time_t is
+    ** signed or unsigned, though lint complains if unsigned).
+    */
+    for (bits = 0, t = 1; t > 0; ++bits, t <<= 1)
+        ;
+    /*
+    ** If time_t is signed, then 0 is the median value,
+    ** if time_t is unsigned, then 1 << bits is median.
+    */
+    t = (t < 0) ? 0 : ((time_t) 1 << bits);
+
+    /* Some gmtime() implementations are broken and will return
+     * NULL for time_ts larger than 40 bits even on 64-bit platforms
+     * so we'll just cap it at 40 bits */
+    if(bits > 40) bits = 40;
+
+    for ( ; ; ) {
+        mytm = gmtime(&t);
+
+        if(!mytm) return WRONG;
+
+        dir = tmcomp(mytm, &yourtm);
+        if (dir != 0) {
+            if (bits-- < 0)
+                return WRONG;
+            if (bits < 0)
+                --t;
+            else if (dir > 0)
+                t -= (time_t) 1 << bits;
+            else    t += (time_t) 1 << bits;
+            continue;
+        }
+        break;
+    }
+    t += saved_seconds;
+    return t;
+}