gps.h

Go to the documentation of this file.

//==============================================================================
// 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/ip/udp.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 {
constexpr static unsigned int kBaudrates[] = { 4800,
                                               9600,
                                               19200,
                                               38400,
                                               57600,
                                               115200,
                                               230400,
                                               460800 };
class Gps {
 public:
  constexpr static double kDefaultAckTimeout = 1.0;
  constexpr static int kWriterSize = 2056;
 
  Gps();
  virtual ~Gps();
 
  void setSaveOnShutdown(bool save_on_shutdown) {
    save_on_shutdown_ = save_on_shutdown;
  }
 
  void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; }
 
  void initializeTcp(std::string host, std::string port);
 
  void initializeUdp(std::string host, std::string port);
 
  void initializeSerial(std::string port, unsigned int baudrate,
                        uint16_t uart_in, uint16_t uart_out);
 
  void resetSerial(std::string port);
 
  bool sendRtcm(const std::vector<uint8_t> &message);
 
  void close();
 
  void reset(const boost::posix_time::time_duration& wait);
 
  bool configReset(uint16_t nav_bbr_mask, uint16_t reset_mode);
 
  bool configGnss(ublox_msgs::CfgGNSS gnss,
                  const boost::posix_time::time_duration& wait);
 
  bool clearBbr();
 
  bool configUart1(unsigned int baudrate, uint16_t in_proto_mask,
                   uint16_t out_proto_mask);
 
  bool disableUart1(ublox_msgs::CfgPRT& prev_cfg);
 
  bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask,
                 uint16_t out_proto_mask);
 
  bool configRate(uint16_t meas_rate, uint16_t nav_rate);
 
  bool configRtcm(std::vector<uint8_t> ids, std::vector<uint8_t> rates);
 
  bool configSbas(bool enable, uint8_t usage, uint8_t max_sbas);
 
  bool configTmode3Fixed(bool lla_flag,
                         std::vector<float> arp_position,
                         std::vector<int8_t> arp_position_hp,
                         float fixed_pos_acc);
 
  bool configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit);
 
  bool disableTmode3();
 
  bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate);
 
  bool setDynamicModel(uint8_t model);
 
  bool setFixMode(uint8_t mode);
 
  bool setDeadReckonLimit(uint8_t limit);
 
  bool setPpp(bool enable, float protocol_version);
 
  bool setDgnss(uint8_t mode);
 
  bool setUseAdr(bool enable, float protocol_version);
 
  bool setUTCtime();
 
  bool setTimtm2(uint8_t rate);
 
  template <typename T>
  void subscribe(typename CallbackHandler_<T>::Callback callback,
                 unsigned int rate);
  template <typename T>
  void subscribe(typename CallbackHandler_<T>::Callback callback);
 
  void subscribe_nmea(boost::function<void(const std::string&)> callback) { callbacks_.set_nmea_callback(callback); }
 
  template <typename T>
  void subscribeId(typename CallbackHandler_<T>::Callback callback,
                   unsigned int message_id);
 
  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(); }
 
  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_);
  bool poll(uint8_t class_id, uint8_t message_id,
            const std::vector<uint8_t>& payload = std::vector<uint8_t>());
 
  template <typename ConfigT>
  bool configure(const ConfigT& message, bool wait = true);
 
  bool waitForAcknowledge(const boost::posix_time::time_duration& timeout,
                          uint8_t class_id, uint8_t msg_id);
 
  void setRawDataCallback(const Worker::Callback& callback);
 
 private:
  enum AckType {
    NACK, 
    ACK, 
    WAIT 
  };
 
  struct Ack {
    AckType type; 
    uint8_t class_id; 
    uint8_t msg_id; 
  };
 
  void setWorker(const boost::shared_ptr<Worker>& worker);
 
  void subscribeAcks();
 
  void processAck(const ublox_msgs::Ack &m);
 
  void processNack(const ublox_msgs::Ack &m);
 
  void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m);
 
  bool saveOnShutdown();
 
  boost::shared_ptr<Worker> worker_;
  bool configured_;
  bool save_on_shutdown_;
  bool config_on_startup_flag_;
 
 
  static const boost::posix_time::time_duration default_timeout_;
  mutable boost::atomic<Ack> ack_;
 
  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