gps.cpp
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"
00018 // 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 #include <boost/version.hpp>
00032 
00033 namespace ublox_gps {
00034 
00035 using namespace ublox_msgs;
00036 
00037 const boost::posix_time::time_duration Gps::default_timeout_ =
00038     boost::posix_time::milliseconds(
00039         static_cast<int>(Gps::kDefaultAckTimeout * 1000));
00040 
00041 Gps::Gps() : configured_(false), config_on_startup_flag_(true) {
00042  subscribeAcks();
00043 }
00044 
00045 Gps::~Gps() { close(); }
00046 
00047 void Gps::setWorker(const boost::shared_ptr<Worker>& worker) {
00048   if (worker_) return;
00049   worker_ = worker;
00050   worker_->setCallback(boost::bind(&CallbackHandlers::readCallback,
00051                                    &callbacks_, _1, _2));
00052   configured_ = static_cast<bool>(worker);
00053 }
00054 
00055 void Gps::subscribeAcks() {
00056   // Set NACK handler
00057   subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processNack, this, _1),
00058                                ublox_msgs::Message::ACK::NACK);
00059   // Set ACK handler
00060   subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processAck, this, _1),
00061                                ublox_msgs::Message::ACK::ACK);
00062   // Set UPD-SOS-ACK handler
00063   subscribe<ublox_msgs::UpdSOS_Ack>(
00064       boost::bind(&Gps::processUpdSosAck, this, _1));
00065 }
00066 
00067 void Gps::processAck(const ublox_msgs::Ack &m) {
00068   // Process ACK/NACK messages
00069   Ack ack;
00070   ack.type = ACK;
00071   ack.class_id = m.clsID;
00072   ack.msg_id = m.msgID;
00073   // store the ack atomically
00074   ack_.store(ack, boost::memory_order_seq_cst);
00075   ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x",
00076                  m.clsID, m.msgID);
00077 }
00078 
00079 void Gps::processNack(const ublox_msgs::Ack &m) {
00080   // Process ACK/NACK messages
00081   Ack ack;
00082   ack.type = NACK;
00083   ack.class_id = m.clsID;
00084   ack.msg_id = m.msgID;
00085   // store the ack atomically
00086   ack_.store(ack, boost::memory_order_seq_cst);
00087   ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID);
00088 }
00089 
00090 void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) {
00091   if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) {
00092     Ack ack;
00093     ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK;
00094     ack.class_id = m.CLASS_ID;
00095     ack.msg_id = m.MESSAGE_ID;
00096     // store the ack atomically
00097     ack_.store(ack, boost::memory_order_seq_cst);
00098     ROS_DEBUG_COND(ack.type == ACK && debug >= 2,
00099                    "U-blox: received UPD SOS Backup ACK");
00100     if(ack.type == NACK)
00101       ROS_ERROR("U-blox: received UPD SOS Backup NACK");
00102   }
00103 }
00104 
00105 void Gps::initializeSerial(std::string port, unsigned int baudrate,
00106                            uint16_t uart_in, uint16_t uart_out) {
00107   port_ = port;
00108   boost::shared_ptr<boost::asio::io_service> io_service(
00109       new boost::asio::io_service);
00110   boost::shared_ptr<boost::asio::serial_port> serial(
00111       new boost::asio::serial_port(*io_service));
00112 
00113   // open serial port
00114   try {
00115     serial->open(port);
00116   } catch (std::runtime_error& e) {
00117     throw std::runtime_error("U-Blox: Could not open serial port :"
00118                              + port + " " + e.what());
00119   }
00120 
00121   ROS_INFO("U-Blox: Opened serial port %s", port.c_str());
00122     
00123   if(BOOST_VERSION < 106600)
00124   {
00125     // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but
00126     // until v1.66.0 there was a bug which didn't enable the relevant code,
00127     // fixed by commit: https://github.com/boostorg/asio/commit/619cea4356
00128     int fd = serial->native_handle();
00129     termios tio;
00130     tcgetattr(fd, &tio);
00131     cfmakeraw(&tio);
00132     tcsetattr(fd, TCSANOW, &tio);
00133   }
00134 
00135   // Set the I/O worker
00136   if (worker_) return;
00137   setWorker(boost::shared_ptr<Worker>(
00138       new AsyncWorker<boost::asio::serial_port>(serial, io_service)));
00139 
00140   configured_ = false;
00141 
00142   // Set the baudrate
00143   boost::asio::serial_port_base::baud_rate current_baudrate;
00144   serial->get_option(current_baudrate);
00145   // Incrementally increase the baudrate to the desired value
00146   for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) {
00147     if (current_baudrate.value() == baudrate)
00148       break;
00149     // Don't step down, unless the desired baudrate is lower
00150     if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i])
00151       continue;
00152     serial->set_option(
00153         boost::asio::serial_port_base::baud_rate(kBaudrates[i]));
00154     boost::this_thread::sleep(
00155         boost::posix_time::milliseconds(kSetBaudrateSleepMs));
00156     serial->get_option(current_baudrate);
00157     ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value());
00158   }
00159   if (config_on_startup_flag_) {
00160     configured_ = configUart1(baudrate, uart_in, uart_out);
00161     if(!configured_ || current_baudrate.value() != baudrate) {
00162       throw std::runtime_error("Could not configure serial baud rate");
00163     }
00164   } else {
00165     configured_ = true;
00166   }
00167 }
00168 
00169 void Gps::resetSerial(std::string port) {
00170   boost::shared_ptr<boost::asio::io_service> io_service(
00171       new boost::asio::io_service);
00172   boost::shared_ptr<boost::asio::serial_port> serial(
00173       new boost::asio::serial_port(*io_service));
00174 
00175   // open serial port
00176   try {
00177     serial->open(port);
00178   } catch (std::runtime_error& e) {
00179     throw std::runtime_error("U-Blox: Could not open serial port :"
00180                              + port + " " + e.what());
00181   }
00182 
00183   ROS_INFO("U-Blox: Reset serial port %s", port.c_str());
00184 
00185   // Set the I/O worker
00186   if (worker_) return;
00187   setWorker(boost::shared_ptr<Worker>(
00188       new AsyncWorker<boost::asio::serial_port>(serial, io_service)));
00189   configured_ = false;
00190 
00191   // Poll UART PRT Config
00192   std::vector<uint8_t> payload;
00193   payload.push_back(CfgPRT::PORT_ID_UART1);
00194   if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) {
00195     ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT");
00196     return;
00197   }
00198   CfgPRT prt;
00199   if(!read(prt, default_timeout_)) {
00200     ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s",
00201                 "message");
00202     return;
00203   }
00204 
00205   // Set the baudrate
00206   serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baudRate));
00207   configured_ = true;
00208 }
00209 
00210 void Gps::initializeTcp(std::string host, std::string port) {
00211   host_ = host;
00212   port_ = port;
00213   boost::shared_ptr<boost::asio::io_service> io_service(
00214       new boost::asio::io_service);
00215   boost::asio::ip::tcp::resolver::iterator endpoint;
00216 
00217   try {
00218     boost::asio::ip::tcp::resolver resolver(*io_service);
00219     endpoint =
00220         resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
00221   } catch (std::runtime_error& e) {
00222     throw std::runtime_error("U-Blox: Could not resolve" + host + " " +
00223                              port + " " + e.what());
00224   }
00225 
00226   boost::shared_ptr<boost::asio::ip::tcp::socket> socket(
00227     new boost::asio::ip::tcp::socket(*io_service));
00228 
00229   try {
00230     socket->connect(*endpoint);
00231   } catch (std::runtime_error& e) {
00232     throw std::runtime_error("U-Blox: Could not connect to " +
00233                              endpoint->host_name() + ":" +
00234                              endpoint->service_name() + ": " + e.what());
00235   }
00236 
00237   ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(),
00238            endpoint->service_name().c_str());
00239 
00240   if (worker_) return;
00241   setWorker(boost::shared_ptr<Worker>(
00242       new AsyncWorker<boost::asio::ip::tcp::socket>(socket,
00243                                                     io_service)));
00244 }
00245 
00246 void Gps::close() {
00247   if(save_on_shutdown_) {
00248     if(saveOnShutdown())
00249       ROS_INFO("U-Blox Flash BBR saved");
00250     else
00251       ROS_INFO("U-Blox Flash BBR failed to save");
00252   }
00253   worker_.reset();
00254   configured_ = false;
00255 }
00256 
00257 void Gps::reset(const boost::posix_time::time_duration& wait) {
00258   worker_.reset();
00259   configured_ = false;
00260   // sleep because of undefined behavior after I/O reset
00261   boost::this_thread::sleep(wait);
00262   if (host_ == "")
00263     resetSerial(port_);
00264   else
00265     initializeTcp(host_, port_);
00266 }
00267 
00268 bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) {
00269   ROS_WARN("Resetting u-blox. If device address changes, %s",
00270            "node must be relaunched.");
00271 
00272   CfgRST rst;
00273   rst.navBbrMask = nav_bbr_mask;
00274   rst.resetMode = reset_mode;
00275 
00276   // Don't wait for ACK, return if it fails
00277   if (!configure(rst, false))
00278     return false;
00279   return true;
00280 }
00281 
00282 bool Gps::configGnss(CfgGNSS gnss,
00283                      const boost::posix_time::time_duration& wait) {
00284   // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox
00285   ROS_DEBUG("Re-configuring GNSS.");
00286   if (!configure(gnss))
00287     return false;
00288   // Cold reset the GNSS
00289   ROS_WARN("GNSS re-configured, cold resetting device.");
00290   if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS))
00291     return false;
00292   ros::Duration(1.0).sleep();
00293   // Reset the I/O
00294   reset(wait);
00295   return isConfigured();
00296 }
00297 
00298 bool Gps::saveOnShutdown() {
00299   // Command the receiver to stop
00300   CfgRST rst;
00301   rst.navBbrMask = rst.NAV_BBR_HOT_START;
00302   rst.resetMode = rst.RESET_MODE_GNSS_STOP;
00303   if(!configure(rst))
00304     return false;
00305   // Command saving the contents of BBR to flash memory
00306   // And wait for UBX-UPD-SOS-ACK
00307   UpdSOS backup;
00308   return configure(backup);
00309 }
00310 
00311 bool Gps::clearBbr() {
00312   // Command saving the contents of BBR to flash memory
00313   // And wait for UBX-UPD-SOS-ACK
00314   UpdSOS sos;
00315   sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR;
00316   return configure(sos);
00317 }
00318 
00319 bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask,
00320                       uint16_t out_proto_mask) {
00321   if (!worker_) return true;
00322 
00323   ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u",
00324             baudrate, in_proto_mask, out_proto_mask);
00325 
00326   CfgPRT port;
00327   port.portID = CfgPRT::PORT_ID_UART1;
00328   port.baudRate = baudrate;
00329   port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT |
00330               CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1;
00331   port.inProtoMask = in_proto_mask;
00332   port.outProtoMask = out_proto_mask;
00333   return configure(port);
00334 }
00335 
00336 bool Gps::disableUart1(CfgPRT& prev_config) {
00337   ROS_DEBUG("Disabling UART1");
00338 
00339   // Poll UART PRT Config
00340   std::vector<uint8_t> payload;
00341   payload.push_back(CfgPRT::PORT_ID_UART1);
00342   if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) {
00343     ROS_ERROR("disableUart: Could not poll UART1 CfgPRT");
00344     return false;
00345   }
00346   if(!read(prev_config, default_timeout_)) {
00347     ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message");
00348     return false;
00349   }
00350   // Keep original settings, but disable in/out
00351   CfgPRT port;
00352   port.portID = CfgPRT::PORT_ID_UART1;
00353   port.mode = prev_config.mode;
00354   port.baudRate = prev_config.baudRate;
00355   port.inProtoMask = 0;
00356   port.outProtoMask = 0;
00357   port.txReady = prev_config.txReady;
00358   port.flags = prev_config.flags;
00359   return configure(port);
00360 }
00361 
00362 bool Gps::configUsb(uint16_t tx_ready,
00363                     uint16_t in_proto_mask,
00364                     uint16_t out_proto_mask) {
00365   if (!worker_) return true;
00366 
00367   ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u",
00368             tx_ready, in_proto_mask, out_proto_mask);
00369 
00370   CfgPRT port;
00371   port.portID = CfgPRT::PORT_ID_USB;
00372   port.txReady = tx_ready;
00373   port.inProtoMask = in_proto_mask;
00374   port.outProtoMask = out_proto_mask;
00375   return configure(port);
00376 }
00377 
00378 bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) {
00379   ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles",
00380     meas_rate, nav_rate);
00381 
00382   CfgRATE rate;
00383   rate.measRate = meas_rate;
00384   rate.navRate = nav_rate;  //  must be fixed at 1 for ublox 5 and 6
00385   rate.timeRef = CfgRATE::TIME_REF_GPS;
00386   return configure(rate);
00387 }
00388 
00389 bool Gps::configRtcm(std::vector<uint8_t> ids, std::vector<uint8_t> rates) {
00390   for(size_t i = 0; i < ids.size(); ++i) {
00391     ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]);
00392     if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) {
00393       ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]);
00394       return false;
00395     }
00396   }
00397   return true;
00398 }
00399 
00400 bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) {
00401   ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas);
00402 
00403   ublox_msgs::CfgSBAS msg;
00404   msg.mode = (enable ? CfgSBAS::MODE_ENABLED : 0);
00405   msg.usage = usage;
00406   msg.maxSBAS = max_sbas;
00407   return configure(msg);
00408 }
00409 
00410 bool Gps::configTmode3Fixed(bool lla_flag,
00411                             std::vector<float> arp_position,
00412                             std::vector<int8_t> arp_position_hp,
00413                             float fixed_pos_acc) {
00414   if(arp_position.size() != 3 || arp_position_hp.size() != 3) {
00415     ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s",
00416               "& arp_position_hp args must be 3");
00417     return false;
00418   }
00419 
00420   ROS_DEBUG("Configuring TMODE3 to Fixed");
00421 
00422   CfgTMODE3 tmode3;
00423   tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK;
00424   tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0;
00425 
00426   // Set position
00427   if(lla_flag) {
00428     // Convert from [deg] to [deg * 1e-7]
00429     tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7);
00430     tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7);
00431     tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7);
00432   } else {
00433     // Convert from m to cm
00434     tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2);
00435     tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2);
00436     tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e2);
00437   }
00438   tmode3.ecefXOrLatHP = arp_position_hp[0];
00439   tmode3.ecefYOrLonHP = arp_position_hp[1];
00440   tmode3.ecefZOrAltHP = arp_position_hp[2];
00441   // Convert from m to [0.1 mm]
00442   tmode3.fixedPosAcc = (uint32_t)round(fixed_pos_acc * 1e4);
00443   return configure(tmode3);
00444 }
00445 
00446 bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur,
00447                                float svin_acc_limit) {
00448   CfgTMODE3 tmode3;
00449   ROS_DEBUG("Setting TMODE3 to Survey In");
00450   tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK;
00451   tmode3.svinMinDur = svin_min_dur;
00452   // Convert from m to [0.1 mm]
00453   tmode3.svinAccLimit = (int)round(svin_acc_limit * 1e4);
00454   return configure(tmode3);
00455 }
00456 
00457 bool Gps::disableTmode3() {
00458   ROS_DEBUG("Disabling TMODE3");
00459 
00460   CfgTMODE3 tmode3;
00461   tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK;
00462   return configure(tmode3);
00463 }
00464 
00465 bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) {
00466   ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id,
00467                  message_id, rate);
00468   ublox_msgs::CfgMSG msg;
00469   msg.msgClass = class_id;
00470   msg.msgID = message_id;
00471   msg.rate = rate;
00472   return configure(msg);
00473 }
00474 
00475 bool Gps::setDynamicModel(uint8_t model) {
00476   ROS_DEBUG("Setting dynamic model to %u", model);
00477 
00478   ublox_msgs::CfgNAV5 msg;
00479   msg.dynModel = model;
00480   msg.mask = ublox_msgs::CfgNAV5::MASK_DYN;
00481   return configure(msg);
00482 }
00483 
00484 bool Gps::setFixMode(uint8_t mode) {
00485   ROS_DEBUG("Setting fix mode to %u", mode);
00486 
00487   ublox_msgs::CfgNAV5 msg;
00488   msg.fixMode = mode;
00489   msg.mask = ublox_msgs::CfgNAV5::MASK_FIX_MODE;
00490   return configure(msg);
00491 }
00492 
00493 bool Gps::setDeadReckonLimit(uint8_t limit) {
00494   ROS_DEBUG("Setting DR Limit to %u", limit);
00495 
00496   ublox_msgs::CfgNAV5 msg;
00497   msg.drLimit = limit;
00498   msg.mask = ublox_msgs::CfgNAV5::MASK_DR_LIM;
00499   return configure(msg);
00500 }
00501 
00502 bool Gps::setPpp(bool enable) {
00503   ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling"));
00504 
00505   ublox_msgs::CfgNAVX5 msg;
00506   msg.usePPP = enable;
00507   msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP;
00508   return configure(msg);
00509 }
00510 
00511 bool Gps::setDgnss(uint8_t mode) {
00512   CfgDGNSS cfg;
00513   ROS_DEBUG("Setting DGNSS mode to %u", mode);
00514   cfg.dgnssMode = mode;
00515   return configure(cfg);
00516 }
00517 
00518 bool Gps::setUseAdr(bool enable) {
00519   ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling"));
00520 
00521   ublox_msgs::CfgNAVX5 msg;
00522   msg.useAdr = enable;
00523   msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR;
00524   return configure(msg);
00525 }
00526 
00527 bool Gps::poll(uint8_t class_id, uint8_t message_id,
00528                const std::vector<uint8_t>& payload) {
00529   if (!worker_) return false;
00530 
00531   std::vector<unsigned char> out(kWriterSize);
00532   ublox::Writer writer(out.data(), out.size());
00533   if (!writer.write(payload.data(), payload.size(), class_id, message_id))
00534     return false;
00535   worker_->send(out.data(), writer.end() - out.data());
00536 
00537   return true;
00538 }
00539 
00540 bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout,
00541                              uint8_t class_id, uint8_t msg_id) {
00542   ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x",
00543                  class_id, msg_id);
00544   boost::posix_time::ptime wait_until(
00545       boost::posix_time::second_clock::local_time() + timeout);
00546 
00547   Ack ack = ack_.load(boost::memory_order_seq_cst);
00548   while (boost::posix_time::second_clock::local_time() < wait_until
00549          && (ack.class_id != class_id
00550              || ack.msg_id != msg_id
00551              || ack.type == WAIT)) {
00552     worker_->wait(timeout);
00553     ack = ack_.load(boost::memory_order_seq_cst);
00554   }
00555   bool result = ack.type == ACK
00556                 && ack.class_id == class_id
00557                 && ack.msg_id == msg_id;
00558   return result;
00559 }
00560 
00561 void Gps::setRawDataCallback(const Worker::Callback& callback) {
00562   if (! worker_) return;
00563   worker_->setRawDataCallback(callback);
00564 }
00565 
00566 bool Gps::setUTCtime() {
00567   ROS_DEBUG("Setting time to UTC time");
00568 
00569   ublox_msgs::CfgNAV5 msg;
00570   msg.utcStandard = 3;
00571   return configure(msg);
00572 }
00573 
00574 bool Gps::setTimtm2(uint8_t rate) {
00575   ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate );
00576   ublox_msgs::CfgMSG msg;
00577   msg.msgClass = ublox_msgs::TimTM2::CLASS_ID;
00578   msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID;
00579   msg.rate  = rate; 
00580   return configure(msg);
00581 }
00582 }  // namespace ublox_gps


ublox_gps
Author(s): Johannes Meyer
autogenerated on Fri Jun 14 2019 19:26:13