00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00057 subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processNack, this, _1),
00058 ublox_msgs::Message::ACK::NACK);
00059
00060 subscribeId<ublox_msgs::Ack>(boost::bind(&Gps::processAck, this, _1),
00061 ublox_msgs::Message::ACK::ACK);
00062
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
00069 Ack ack;
00070 ack.type = ACK;
00071 ack.class_id = m.clsID;
00072 ack.msg_id = m.msgID;
00073
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
00081 Ack ack;
00082 ack.type = NACK;
00083 ack.class_id = m.clsID;
00084 ack.msg_id = m.msgID;
00085
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
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
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
00126
00127
00128 int fd = serial->native_handle();
00129 termios tio;
00130 tcgetattr(fd, &tio);
00131 cfmakeraw(&tio);
00132 tcsetattr(fd, TCSANOW, &tio);
00133 }
00134
00135
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
00143 boost::asio::serial_port_base::baud_rate current_baudrate;
00144 serial->get_option(current_baudrate);
00145
00146 for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) {
00147 if (current_baudrate.value() == baudrate)
00148 break;
00149
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
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
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
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
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
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
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
00285 ROS_DEBUG("Re-configuring GNSS.");
00286 if (!configure(gnss))
00287 return false;
00288
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
00294 reset(wait);
00295 return isConfigured();
00296 }
00297
00298 bool Gps::saveOnShutdown() {
00299
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
00306
00307 UpdSOS backup;
00308 return configure(backup);
00309 }
00310
00311 bool Gps::clearBbr() {
00312
00313
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
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
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;
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
00427 if(lla_flag) {
00428
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
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
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
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 }