gps.cpp
Go to the documentation of this file.
00001 
00002 //=================================================================================================
00003 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00004 // All rights reserved.
00005 
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of the Flight Systems and Automatic Control group,
00014 //       TU Darmstadt, nor the names of its contributors may be used to
00015 //       endorse or promote products derived from this software without
00016 //       specific prior written permission.
00017 
00018 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00022 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 //=================================================================================================
00029 
00030 #include <ublox_gps/gps.h>
00031 
00032 namespace ublox_gps {
00033 
00034 using namespace ublox_msgs;
00035 
00036 boost::posix_time::time_duration Gps::default_timeout_(boost::posix_time::seconds(1.0));
00037 Gps::Gps()
00038   : configured_(false)
00039   , baudrate_(57600)
00040 {
00041 }
00042 
00043 Gps::~Gps()
00044 {
00045   close();
00046 }
00047 
00048 bool Gps::setBaudrate(unsigned int baudrate)
00049 {
00050   baudrate_ = baudrate;  
00051   if (!worker_) return true;
00052 
00053   CfgPRT port;
00054   port.baudRate = baudrate_;
00055   port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1;
00056   port.inProtoMask = CfgPRT::PROTO_UBX | CfgPRT::PROTO_NMEA | CfgPRT::PROTO_RTCM;
00057   port.outProtoMask = CfgPRT::PROTO_UBX;
00058   port.portID = CfgPRT::PORT_ID_UART1;
00059 
00060   return configure(port);
00061 }
00062 
00063 void Gps::initialize(const boost::shared_ptr<Worker> &worker)
00064 {
00065   if (worker_) return;
00066   worker_ = worker;
00067   worker_->setCallback(boost::bind(&Gps::readCallback, this, _1, _2));
00068   configured_ = worker;
00069 }
00070 
00071 template void Gps::initialize(boost::asio::ip::tcp::socket& stream, boost::asio::io_service& io_service);
00072 // template void Gps::initialize(boost::asio::ip::udp::socket& stream, boost::asio::io_service& io_service);
00073 
00074 template <>
00075 void Gps::initialize(boost::asio::serial_port& serial_port, boost::asio::io_service& io_service)
00076 {
00077   if (worker_) return;
00078   initialize(boost::shared_ptr<Worker>(new AsyncWorker<boost::asio::serial_port>(serial_port, io_service)));
00079 
00080   configured_ = false;
00081 
00082   boost::asio::serial_port_base::baud_rate current_baudrate;
00083 
00084   serial_port.set_option(boost::asio::serial_port_base::baud_rate(4800));
00085   boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00086   if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
00087   configured_ = setBaudrate(baudrate_);
00088   if (configured_) return;
00089 
00090   serial_port.set_option(boost::asio::serial_port_base::baud_rate(9600));
00091   boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00092   if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
00093   configured_ = setBaudrate(baudrate_);
00094   if (configured_) return;
00095 
00096   serial_port.set_option(boost::asio::serial_port_base::baud_rate(19200));
00097   boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00098   if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
00099   configured_ = setBaudrate(baudrate_);
00100   if (configured_) return;
00101 
00102   serial_port.set_option(boost::asio::serial_port_base::baud_rate(38400));
00103   boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00104   if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
00105   configured_ = setBaudrate(baudrate_);
00106   if (configured_) return;
00107 
00108   serial_port.set_option(boost::asio::serial_port_base::baud_rate(baudrate_));
00109   boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00110   if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
00111   configured_ = setBaudrate(baudrate_);
00112   if (configured_) return;
00113 }
00114 
00115 void Gps::close()
00116 {
00117   worker_.reset();
00118   configured_ = false;
00119 }
00120 
00121 bool Gps::setRate(uint8_t class_id, uint8_t message_id, unsigned int rate)
00122 {
00123   CfgMSG msg;
00124   msg.msgClass = class_id;
00125   msg.msgID = message_id;
00126   msg.rate = rate;
00127   return configure(msg);
00128 }
00129 
00130 bool Gps::enableSBAS(bool onoff) {
00131   CfgSBAS msg;
00132   msg.mode = (onoff ? CfgSBAS::MODE_ENABLED : 0);
00133   msg.usage = 255;
00134   msg.maxSBAS = 3;
00135   return configure(msg);
00136 }
00137 
00138 bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector<uint8_t>& payload) {
00139   if (!worker_) return false;
00140 
00141   std::vector<unsigned char> out(1024);
00142   ublox::Writer writer(out.data(), out.size());
00143   if (!writer.write(payload.data(), payload.size(), class_id, message_id)) return false;
00144   worker_->send(out.data(), writer.end() - out.data());
00145 
00146   return true;
00147 }
00148 
00149 bool Gps::configure()
00150 {
00151   configured_ = false;
00152 
00153   // unconfigure all messages
00154 //  for(unsigned int id = 0; id < 256; ++id) {
00155 //    CfgMSG msg;
00156 //    msg.msgClass = Class::NAV;
00157 //    msg.msgID = id;
00158 //    msg.rate = 0;
00159 //    configure(msg, false);
00160 
00161 //    msg.msgClass = Class::RXM;
00162 //    msg.msgID = id;
00163 //    msg.rate = 0;
00164 //    configure(msg, false);
00165 //  }
00166 
00167   CfgRATE rate;
00168   rate.measRate = 250;
00169   rate.navRate = 1;
00170   rate.timeRef = CfgRATE::TIME_REF_GPS;
00171   if (!configure(rate)) return false;
00172 
00173   configured_ = true;
00174   return true;
00175 }
00176 
00177 void Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout) {
00178   boost::posix_time::ptime wait_until(boost::posix_time::second_clock::local_time() + timeout);
00179 
00180   while(acknowledge_ == WAIT && boost::posix_time::second_clock::local_time() < wait_until) {
00181     worker_->wait(timeout);
00182   }
00183 }
00184 
00185 void Gps::readCallback(unsigned char *data, std::size_t& size) {
00186   ublox::Reader reader(data, size);
00187 
00188   while(reader.search() != reader.end() && reader.found()) {
00189     if (debug >= 3) {
00190       std::cout << "received ublox " << reader.length() + 8 << " bytes" << std::endl;
00191       for(ublox::Reader::iterator it = reader.pos(); it != reader.pos() + reader.length() + 8; ++it) std::cout << std::hex << static_cast<unsigned int>(*it) << " ";
00192       std::cout << std::dec << std::endl;
00193     }
00194 
00195     callback_mutex_.lock();
00196     Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId());
00197     for(Callbacks::iterator callback = callbacks_.lower_bound(key); callback != callbacks_.upper_bound(key); ++callback) callback->second->handle(reader);
00198     callback_mutex_.unlock();
00199 
00200     if (reader.classId() == 0x05) {
00201       acknowledge_ = (reader.messageId() == 0x00) ? NACK : ACK;
00202       if (debug) std::cout << "received " << (acknowledge_ == ACK ? "ACK" : "NACK") << std::endl;
00203     }
00204   }
00205 
00206   // delete read bytes from input buffer
00207   std::copy(reader.pos(), reader.end(), data);
00208   size -= reader.pos() - data;
00209 }
00210 
00211 bool CallbackHandler::wait(const boost::posix_time::time_duration &timeout) {
00212   boost::mutex::scoped_lock lock(mutex_);
00213   return condition_.timed_wait(lock, timeout);
00214 }
00215 
00216 } // namespace ublox_gps


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