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/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 int kSetBaudrateSleepMs = 500;
  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 initializeSerial(std::string port, unsigned int baudrate,
                        uint16_t uart_in, uint16_t uart_out);

  void resetSerial(std::string port);

  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);

  bool setDgnss(uint8_t mode);

  bool setUseAdr(bool enable);

  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);

  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