gps.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef UBLOX_GPS_H
00030 #define UBLOX_GPS_H
00031 
00032 #include <boost/asio/io_service.hpp>
00033 #include <vector>
00034 #include <map>
00035 
00036 #include <ublox/serialization/ublox_msgs.h>
00037 #include <ublox_gps/async_worker.h>
00038 #include <ublox_gps/callback.h>
00039 
00040 namespace ublox_gps {
00041 
00042 class Gps
00043 {
00044 public:
00045   Gps();
00046   virtual ~Gps();
00047 
00048   template <typename StreamT> void initialize(StreamT& stream, boost::asio::io_service& io_service);
00049   void initialize(const boost::shared_ptr<Worker> &worker);
00050   void close();
00051 
00052   bool configure();
00053   bool setBaudrate(unsigned int baudrate);
00054   bool setRate(uint8_t class_id, uint8_t message_id, unsigned int rate);
00055 
00056   bool enableSBAS(bool onoff);
00057 
00058   template <typename T> Callbacks::iterator subscribe(typename CallbackHandler_<T>::Callback callback, unsigned int rate);
00059   template <typename T> Callbacks::iterator subscribe(typename CallbackHandler_<T>::Callback callback);
00060   template <typename T> bool read(T& message, const boost::posix_time::time_duration& timeout = default_timeout_);
00061 
00062   bool isInitialized() const { return worker_ != 0; }
00063   bool isConfigured() const { return isInitialized() && configured_; }
00064 
00065   template <typename ConfigT> bool poll(ConfigT& message, const boost::posix_time::time_duration& timeout = default_timeout_);
00066   bool poll(uint8_t class_id, uint8_t message_id, const std::vector<uint8_t>& payload = std::vector<uint8_t>());
00067   template <typename ConfigT> bool configure(const ConfigT& message, bool wait = true);
00068   void waitForAcknowledge(const boost::posix_time::time_duration& timeout);
00069 
00070 private:
00071   void readCallback(unsigned char *data, std::size_t& size);
00072 
00073 private:
00074   boost::shared_ptr<Worker> worker_;
00075   bool configured_;
00076   enum { WAIT, ACK, NACK } acknowledge_;
00077   unsigned int baudrate_;
00078   static boost::posix_time::time_duration default_timeout_;
00079 
00080   Callbacks callbacks_;
00081   boost::mutex callback_mutex_;
00082 };
00083 
00084 template <typename StreamT>
00085 void Gps::initialize(StreamT& stream, boost::asio::io_service& io_service)
00086 {
00087   if (worker_) return;
00088   initialize(boost::shared_ptr<Worker>(new AsyncWorker<StreamT>(stream, io_service)));
00089 }
00090 
00091 template <> void Gps::initialize(boost::asio::serial_port& serial_port, boost::asio::io_service& io_service);
00092 extern template void Gps::initialize<boost::asio::ip::tcp::socket>(boost::asio::ip::tcp::socket& stream, boost::asio::io_service& io_service);
00093 // extern template void Gps::initialize<boost::asio::ip::udp::socket>(boost::asio::ip::udp::socket& stream, boost::asio::io_service& io_service);
00094 
00095 template <typename T>
00096 Callbacks::iterator Gps::subscribe(typename CallbackHandler_<T>::Callback callback, unsigned int rate)
00097 {
00098   if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return Callbacks::iterator();
00099   return subscribe<T>(callback);
00100 }
00101 
00102 template <typename T>
00103 Callbacks::iterator Gps::subscribe(typename CallbackHandler_<T>::Callback callback)
00104 {
00105   boost::mutex::scoped_lock lock(callback_mutex_);
00106   CallbackHandler_<T> *handler = new CallbackHandler_<T>(callback);
00107   return callbacks_.insert(std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr<CallbackHandler>(handler)));
00108 }
00109 
00110 template <typename T>
00111 void CallbackHandler_<T>::handle(ublox::Reader &reader) {
00112   boost::mutex::scoped_lock(mutex_);
00113   try {
00114     if (!reader.read<T>(message_)) {
00115       std::cout << "Decoder error for " << static_cast<unsigned int>(reader.classId()) << "/" << static_cast<unsigned int>(reader.messageId()) << " (" << reader.length() << " bytes)" << std::endl;
00116       return;
00117     }
00118   } catch(std::runtime_error& e) {
00119     std::cout << "Decoder error for " << static_cast<unsigned int>(reader.classId()) << "/" << static_cast<unsigned int>(reader.messageId()) << " (" << reader.length() << " bytes): " << std::string(e.what()) << std::endl;
00120     return;
00121   }
00122 
00123   if (func_) func_(message_);
00124   condition_.notify_all();
00125 }
00126 
00127 template <typename ConfigT> bool Gps::poll(ConfigT& message, const boost::posix_time::time_duration& timeout) {
00128   if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID)) return false;
00129   return read(message, timeout);
00130 }
00131 
00132 template <typename T> bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) {
00133   bool result = false;
00134   if (!worker_) return false;
00135 
00136   callback_mutex_.lock();
00137   CallbackHandler_<T> *handler = new CallbackHandler_<T>();
00138   Callbacks::iterator callback = callbacks_.insert((std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr<CallbackHandler>(handler))));
00139   callback_mutex_.unlock();
00140 
00141   if (handler->wait(timeout)) {
00142     message = handler->get();
00143     result = true;
00144   }
00145 
00146   callback_mutex_.lock();
00147   callbacks_.erase(callback);
00148   callback_mutex_.unlock();
00149   return result;
00150 }
00151 
00152 template <typename ConfigT> bool Gps::configure(const ConfigT& message, bool wait) {
00153   if (!worker_) return false;
00154 
00155   acknowledge_ = WAIT;
00156 
00157   std::vector<unsigned char> out(1024);
00158   ublox::Writer writer(out.data(), out.size());
00159   if (!writer.write(message)) return false;
00160   worker_->send(out.data(), writer.end() - out.data());
00161 
00162   if (!wait) return true;
00163 
00164   waitForAcknowledge(default_timeout_);
00165   return (acknowledge_ == ACK);
00166 }
00167 
00168 } // namespace ublox_gps
00169 
00170 #endif // UBLOX_GPS_H


ublox_gps
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 08:16:41