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 #ifndef UBLOX_GPS_H
00031 #define UBLOX_GPS_H
00032
00033 #include <map>
00034 #include <vector>
00035 #include <locale>
00036 #include <stdexcept>
00037
00038 #include <boost/asio/ip/tcp.hpp>
00039 #include <boost/asio/serial_port.hpp>
00040 #include <boost/asio/io_service.hpp>
00041 #include <boost/atomic.hpp>
00042
00043 #include <ros/console.h>
00044
00045 #include <ublox/serialization/ublox_msgs.h>
00046
00047 #include <ublox_gps/async_worker.h>
00048 #include <ublox_gps/callback.h>
00049
00055 namespace ublox_gps {
00057 constexpr static unsigned int kBaudrates[] = { 4800,
00058 9600,
00059 19200,
00060 38400,
00061 57600,
00062 115200,
00063 230400,
00064 460800 };
00068 class Gps {
00069 public:
00071 constexpr static int kSetBaudrateSleepMs = 500;
00073 constexpr static double kDefaultAckTimeout = 1.0;
00075 constexpr static int kWriterSize = 2056;
00076
00077 Gps();
00078 virtual ~Gps();
00079
00084 void setSaveOnShutdown(bool save_on_shutdown) {
00085 save_on_shutdown_ = save_on_shutdown;
00086 }
00087
00092 void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; }
00093
00099 void initializeTcp(std::string host, std::string port);
00100
00108 void initializeSerial(std::string port, unsigned int baudrate,
00109 uint16_t uart_in, uint16_t uart_out);
00110
00118 void resetSerial(std::string port);
00119
00124 void close();
00125
00130 void reset(const boost::posix_time::time_duration& wait);
00131
00138 bool configReset(uint16_t nav_bbr_mask, uint16_t reset_mode);
00139
00147 bool configGnss(ublox_msgs::CfgGNSS gnss,
00148 const boost::posix_time::time_duration& wait);
00149
00155 bool clearBbr();
00156
00164 bool configUart1(unsigned int baudrate, uint16_t in_proto_mask,
00165 uint16_t out_proto_mask);
00166
00174 bool disableUart1(ublox_msgs::CfgPRT& prev_cfg);
00175
00183 bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask,
00184 uint16_t out_proto_mask);
00185
00193 bool configRate(uint16_t meas_rate, uint16_t nav_rate);
00194
00201 bool configRtcm(std::vector<uint8_t> ids, std::vector<uint8_t> rates);
00202
00211 bool configSbas(bool enable, uint8_t usage, uint8_t max_sbas);
00212
00227 bool configTmode3Fixed(bool lla_flag,
00228 std::vector<float> arp_position,
00229 std::vector<int8_t> arp_position_hp,
00230 float fixed_pos_acc);
00231
00238 bool configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit);
00239
00245 bool disableTmode3();
00246
00254 bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate);
00255
00261 bool setDynamicModel(uint8_t model);
00262
00268 bool setFixMode(uint8_t mode);
00269
00275 bool setDeadReckonLimit(uint8_t limit);
00276
00285 bool setPpp(bool enable);
00286
00292 bool setDgnss(uint8_t mode);
00293
00299 bool setUseAdr(bool enable);
00300
00308 bool setUTCtime();
00309
00318 bool setTimtm2(uint8_t rate);
00319
00326 template <typename T>
00327 void subscribe(typename CallbackHandler_<T>::Callback callback,
00328 unsigned int rate);
00333 template <typename T>
00334 void subscribe(typename CallbackHandler_<T>::Callback callback);
00335
00343 template <typename T>
00344 void subscribeId(typename CallbackHandler_<T>::Callback callback,
00345 unsigned int message_id);
00346
00352 template <typename T>
00353 bool read(T& message,
00354 const boost::posix_time::time_duration& timeout = default_timeout_);
00355
00356 bool isInitialized() const { return worker_ != 0; }
00357 bool isConfigured() const { return isInitialized() && configured_; }
00358 bool isOpen() const { return worker_->isOpen(); }
00359
00367 template <typename ConfigT>
00368 bool poll(ConfigT& message,
00369 const std::vector<uint8_t>& payload = std::vector<uint8_t>(),
00370 const boost::posix_time::time_duration& timeout = default_timeout_);
00379 bool poll(uint8_t class_id, uint8_t message_id,
00380 const std::vector<uint8_t>& payload = std::vector<uint8_t>());
00381
00389 template <typename ConfigT>
00390 bool configure(const ConfigT& message, bool wait = true);
00391
00399 bool waitForAcknowledge(const boost::posix_time::time_duration& timeout,
00400 uint8_t class_id, uint8_t msg_id);
00401
00406 void setRawDataCallback(const Worker::Callback& callback);
00407
00408 private:
00410 enum AckType {
00411 NACK,
00412 ACK,
00413 WAIT
00414 };
00415
00417 struct Ack {
00418 AckType type;
00419 uint8_t class_id;
00420 uint8_t msg_id;
00421 };
00422
00427 void setWorker(const boost::shared_ptr<Worker>& worker);
00428
00432 void subscribeAcks();
00433
00438 void processAck(const ublox_msgs::Ack &m);
00439
00444 void processNack(const ublox_msgs::Ack &m);
00445
00450 void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m);
00451
00462 bool saveOnShutdown();
00463
00465 boost::shared_ptr<Worker> worker_;
00467 bool configured_;
00469 bool save_on_shutdown_;
00471 bool config_on_startup_flag_;
00472
00473
00475 static const boost::posix_time::time_duration default_timeout_;
00477 mutable boost::atomic<Ack> ack_;
00478
00480 CallbackHandlers callbacks_;
00481
00482 std::string host_, port_;
00483 };
00484
00485 template <typename T>
00486 void Gps::subscribe(
00487 typename CallbackHandler_<T>::Callback callback, unsigned int rate) {
00488 if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return;
00489 subscribe<T>(callback);
00490 }
00491
00492 template <typename T>
00493 void Gps::subscribe(typename CallbackHandler_<T>::Callback callback) {
00494 callbacks_.insert<T>(callback);
00495 }
00496
00497 template <typename T>
00498 void Gps::subscribeId(typename CallbackHandler_<T>::Callback callback,
00499 unsigned int message_id) {
00500 callbacks_.insert<T>(callback, message_id);
00501 }
00502
00503 template <typename ConfigT>
00504 bool Gps::poll(ConfigT& message,
00505 const std::vector<uint8_t>& payload,
00506 const boost::posix_time::time_duration& timeout) {
00507 if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false;
00508 return read(message, timeout);
00509 }
00510
00511 template <typename T>
00512 bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) {
00513 if (!worker_) return false;
00514 return callbacks_.read(message, timeout);
00515 }
00516
00517 template <typename ConfigT>
00518 bool Gps::configure(const ConfigT& message, bool wait) {
00519 if (!worker_) return false;
00520
00521
00522 Ack ack;
00523 ack.type = WAIT;
00524 ack_.store(ack, boost::memory_order_seq_cst);
00525
00526
00527 std::vector<unsigned char> out(kWriterSize);
00528 ublox::Writer writer(out.data(), out.size());
00529 if (!writer.write(message)) {
00530 ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x",
00531 message.CLASS_ID, message.MESSAGE_ID);
00532 return false;
00533 }
00534
00535 worker_->send(out.data(), writer.end() - out.data());
00536
00537 if (!wait) return true;
00538
00539
00540 return waitForAcknowledge(default_timeout_,
00541 message.CLASS_ID,
00542 message.MESSAGE_ID);
00543 }
00544
00545 }
00546
00547 #endif // UBLOX_GPS_H