diff --git a/README.md b/README.md index f7e895c4..c03ec509 100644 --- a/README.md +++ b/README.md @@ -91,6 +91,10 @@ The `ublox_gps` node supports the following parameters for all products and firm * `nmea/gnssToFilt/beidou`: Disable reporting of BeiDou satellites. Defaults to false. * `nmea/main_talker_id`: This field enables the main Talker ID to be overridden. Defaults to 0. * `nmea/gsv_talker_id`: This field enables the GSV Talker ID to be overridden. Defaults to [0, 0]. +* `agps` parameters: + * `agps/enable`: If true, the AGPS will be send to the gps at init. + * `agps/token`: Token to access to ublox assisted gps services. + * `agps/path`: Path to store the agps file. ### For devices with firmware >= 8: * `save_on_shutdown`: If true, the node will send a `UBX-UPD-SOS` command to save the BBR to flash memory on shutdown. Defaults to false. diff --git a/ublox_gps/config/c94_m8p_base.yaml b/ublox_gps/config/c94_m8p_base.yaml index 0aad2a4b..42c64618 100644 --- a/ublox_gps/config/c94_m8p_base.yaml +++ b/ublox_gps/config/c94_m8p_base.yaml @@ -69,4 +69,10 @@ publish: aid: hui: false nav: - posecef: false \ No newline at end of file + posecef: false + +# Enable AGPS and settings +agps: + enable: true + token: noToken # Token to request the agps file + path: /data # path to store the agps file diff --git a/ublox_gps/config/c94_m8p_rover.yaml b/ublox_gps/config/c94_m8p_rover.yaml index f154b45d..35b9a596 100644 --- a/ublox_gps/config/c94_m8p_rover.yaml +++ b/ublox_gps/config/c94_m8p_rover.yaml @@ -43,4 +43,12 @@ publish: hui: false nav: - posecef: false \ No newline at end of file + posecef: false + +# Enable AGPS and settings +agps: + enable: true + token: noToken # Token to request the agps file + path: /data # path to store the agps file + + diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.h index 5e49f80d..8d446a7c 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.h @@ -30,14 +30,21 @@ #ifndef UBLOX_GPS_H #define UBLOX_GPS_H // STL -#include -#include +#include +#include #include +#include +#include #include +#include +#include +// ifstream constructor. +#include // std::ifstream +#include // std::cout // Boost +#include #include #include -#include #include // ROS #include @@ -47,6 +54,19 @@ #include #include +#define AGPS_URL \ + "http://online-live1.services.u-blox.com/GetOnlineData.ashx?token=" +#define AGPS_URL2 \ + "http://online-live2.services.u-blox.com/GetOnlineData.ashx?token=" +#define AGPS_OPTIONS_INIT ";gnss=" +#define AGPS_OPTIONS_END ";datatype=eph,alm,aux" +#define AGPS_FILE "/agps.ubx" +#define AGPS_FILE_TEMP "/agpsTemp.ubx" +#define AGPS_PATH_DEFAULT "/data" +#define AGPS_WGET_LOG "/wegetlog" +#define AGPS_SERVER_202 "HTTP request sent, awaiting response... 200 OK" +#define AGPS_TIME_SIZE 32 + /** * @namespace ublox_gps * This namespace is for I/O communication with the u-blox device, including @@ -54,19 +74,13 @@ */ namespace ublox_gps { //! Possible baudrates for u-blox devices -constexpr static unsigned int kBaudrates[] = { 4800, - 9600, - 19200, - 38400, - 57600, - 115200, - 230400, - 460800 }; +constexpr static unsigned int kBaudrates[] = {4800, 9600, 19200, 38400, + 57600, 115200, 230400, 460800}; /** * @brief Handles communication with and configuration of the u-blox device */ class Gps { - public: +public: //! Sleep time [ms] after setting the baudrate constexpr static int kSetBaudrateSleepMs = 500; //! Default timeout for ACK messages in seconds @@ -77,6 +91,17 @@ class Gps { Gps(); virtual ~Gps(); + /** + * @brief + */ + void configureAGPS(void); + + void setAgpsParams(std::string apgs_path_, std::string apgs_options_, std::string token_); + /** + * @brief + */ + bool coldReset(const boost::posix_time::time_duration &wait); + /** * @brief If called, when the node shuts down, it will send a command to * save the flash memory. @@ -121,7 +146,7 @@ class Gps { * @brief Reset I/O communications. * @param wait Time to wait before restarting communications */ - void reset(const boost::posix_time::time_duration& wait); + void reset(const boost::posix_time::time_duration &wait); /** * @brief Send a reset message to the u-blox device. @@ -139,7 +164,7 @@ class Gps { * I/O reset successfully */ bool configGnss(ublox_msgs::CfgGNSS gnss, - const boost::posix_time::time_duration& wait); + const boost::posix_time::time_duration &wait); /** * @brief Send a message to the receiver to delete the BBR data stored in @@ -165,7 +190,7 @@ class Gps { * configuration parameters * @return true on ACK, false on other conditions. */ - bool disableUart1(ublox_msgs::CfgPRT& prev_cfg); + bool disableUart1(ublox_msgs::CfgPRT &prev_cfg); /** * @brief Configure the USB Port. @@ -218,8 +243,7 @@ class Gps { * @param fixed_pos_acc Fixed position 3D accuracy [m] * @return true on ACK, false if settings are incorrect or on other conditions */ - bool configTmode3Fixed(bool lla_flag, - std::vector arp_position, + bool configTmode3Fixed(bool lla_flag, std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc); @@ -325,8 +349,8 @@ class Gps { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, - const boost::posix_time::time_duration& timeout = default_timeout_); + bool read(T &message, + const boost::posix_time::time_duration &timeout = default_timeout_); bool isInitialized() const { return worker_ != 0; } bool isConfigured() const { return isInitialized() && configured_; } @@ -340,9 +364,9 @@ class Gps { * @param timeout the amount of time to wait for the desired message */ template - bool poll(ConfigT& message, - const std::vector& payload = std::vector(), - const boost::posix_time::time_duration& timeout = default_timeout_); + bool poll(ConfigT &message, + const std::vector &payload = std::vector(), + const boost::posix_time::time_duration &timeout = default_timeout_); /** * Poll a u-blox message. * @param class_id the u-blox message class id @@ -352,7 +376,7 @@ class Gps { * @param timeout the amount of time to wait for the desired message */ bool poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload = std::vector()); + const std::vector &payload = std::vector()); /** * @brief Send the given configuration message. @@ -362,7 +386,7 @@ class Gps { * wait was set to false */ template - bool configure(const ConfigT& message, bool wait = true); + bool configure(const ConfigT &message, bool wait = true); /** * @brief Wait for an acknowledge message until the timeout @@ -371,29 +395,54 @@ class Gps { * @param msg_id the expected message ID of the ACK * @return true if expected ACK received, false otherwise */ - bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, + bool waitForAcknowledge(const boost::posix_time::time_duration &timeout, uint8_t class_id, uint8_t msg_id); - private: +private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK enum AckType { NACK, //! Not acknowledged - ACK, //! Acknowledge - WAIT //! Waiting for ACK + ACK, //! Acknowledge + WAIT //! Waiting for ACK }; //! Stores ACK/NACK messages struct Ack { - AckType type; //!< The ACK type + AckType type; //!< The ACK type uint8_t class_id; //!< The class ID of the ACK - uint8_t msg_id; //!< The message ID of the ACK + uint8_t msg_id; //!< The message ID of the ACK }; + // executeShellCommands + std::string executeShellCommand(std::string command); + // Download and check the APGS file + bool downloadAGPS(void); + + bool checkAGPSFile(void); + + bool fileSuccess(void); + + void processMgaAck(const ublox_msgs::MgaACK &m); + + void processGetNAVX5(const ublox_msgs::CfgNAVX5 &m); + + bool getNAVX5(); + + std::string agps_path; + std::string agps_path_temp; + std::string log_; + std::string options; + std::string token; + + ublox_msgs::MgaINITIMEUTC getMgaUtc(); + + std::vector split(std::string *s, char delimiter); + /** * @brief Set the I/O worker * @param an I/O handler */ - void setWorker(const boost::shared_ptr& worker); + void setWorker(const boost::shared_ptr &worker); /** * @brief Subscribe to ACK/NACK messages and UPD-SOS-ACK messages. @@ -415,7 +464,10 @@ class Gps { /** * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process - */ + */ // ROS +#include +// Other u-blox packages +#include void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m); /** @@ -449,9 +501,10 @@ class Gps { }; template -void Gps::subscribe( - typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return; +void Gps::subscribe(typename CallbackHandler_::Callback callback, + unsigned int rate) { + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) + return; subscribe(callback); } @@ -467,22 +520,24 @@ void Gps::subscribeId(typename CallbackHandler_::Callback callback, } template -bool Gps::poll(ConfigT& message, - const std::vector& payload, - const boost::posix_time::time_duration& timeout) { - if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; +bool Gps::poll(ConfigT &message, const std::vector &payload, + const boost::posix_time::time_duration &timeout) { + if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) + return false; return read(message, timeout); } template -bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) { - if (!worker_) return false; +bool Gps::read(T &message, const boost::posix_time::time_duration &timeout) { + if (!worker_) + return false; return callbacks_.read(message, timeout); } template -bool Gps::configure(const ConfigT& message, bool wait) { - if (!worker_) return false; +bool Gps::configure(const ConfigT &message, bool wait) { + if (!worker_) + return false; // Reset ack Ack ack; @@ -500,14 +555,14 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Send the message to the device worker_->send(out.data(), writer.end() - out.data()); - if (!wait) return true; + if (!wait) + return true; // Wait for an acknowledgment and return whether or not it was received - return waitForAcknowledge(default_timeout_, - message.CLASS_ID, + return waitForAcknowledge(default_timeout_, message.CLASS_ID, message.MESSAGE_ID); } -} // namespace ublox_gps +} // namespace ublox_gps -#endif // UBLOX_GPS_H +#endif // UBLOX_GPS_H diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.h index 8231894e..5b32a55d 100644 --- a/ublox_gps/include/ublox_gps/node.h +++ b/ublox_gps/include/ublox_gps/node.h @@ -31,18 +31,20 @@ #define UBLOX_GPS_NODE_H // STL -#include #include +#include // Boost #include #include #include // ROS includes -#include -#include -#include #include #include +#include +#include +#include +#include +#include // ROS messages #include #include @@ -131,15 +133,14 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) { + UbloxTopicDiagnostic(std::string topic, double freq_tol, int freq_window) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic( + topic, *updater, freq_param); } /** @@ -152,15 +153,14 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { + UbloxTopicDiagnostic(std::string topic, double freq_min, double freq_max, + double freq_tol, int freq_window) { min_freq = freq_min; max_freq = freq_max; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic( + topic, *updater, freq_param); } //! Topic frequency diagnostic updater @@ -184,8 +184,8 @@ struct FixDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics * @param stamp_min the minimum allowed time delay */ - FixDiagnostic (std::string name, double freq_tol, int freq_window, - double stamp_min) { + FixDiagnostic(std::string name, double freq_tol, int freq_window, + double stamp_min) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -193,10 +193,8 @@ struct FixDiagnostic { freq_tol, freq_window); double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = new diagnostic_updater::TopicDiagnostic(name, - *updater, - freq_param, - time_param); + diagnostic = new diagnostic_updater::TopicDiagnostic( + name, *updater, freq_param, time_param); } //! Topic frequency diagnostic updater @@ -225,7 +223,7 @@ FixDiagnostic freq_diag; * @return DynamicModel * @throws std::runtime_error on invalid argument. */ -uint8_t modelFromString(const std::string& model); +uint8_t modelFromString(const std::string &model); /** * @brief Determine fix mode from human-readable string. @@ -236,7 +234,7 @@ uint8_t modelFromString(const std::string& model); * @return FixMode * @throws std::runtime_error on invalid argument. */ -uint8_t fixModeFromString(const std::string& mode); +uint8_t fixModeFromString(const std::string &mode); /** * @brief Check that the parameter is above the minimum. @@ -247,7 +245,7 @@ uint8_t fixModeFromString(const std::string& mode); */ template void checkMin(V val, T min, std::string name) { - if(val < min) { + if (val < min) { std::stringstream oss; oss << "Invalid settings: " << name << " must be > " << min; throw std::runtime_error(oss.str()); @@ -264,10 +262,10 @@ void checkMin(V val, T min, std::string name) { */ template void checkRange(V val, T min, T max, std::string name) { - if(val < min || val > max) { + if (val < min || val > max) { std::stringstream oss; - oss << "Invalid settings: " << name << " must be in range [" << min << - ", " << max << "]."; + oss << "Invalid settings: " << name << " must be in range [" << min << ", " + << max << "]."; throw std::runtime_error(oss.str()); } } @@ -282,7 +280,7 @@ void checkRange(V val, T min, T max, std::string name) { */ template void checkRange(std::vector val, T min, T max, std::string name) { - for(size_t i = 0; i < val.size(); i++) { + for (size_t i = 0; i < val.size(); i++) { std::stringstream oss; oss << name << "[" << i << "]"; checkRange(val[i], min, max, oss.str()); @@ -296,16 +294,16 @@ void checkRange(std::vector val, T min, T max, std::string name) { * @throws std::runtime_error if the parameter is out of bounds * @return true if found, false if not found. */ -template -bool getRosUint(const std::string& key, U &u) { +template bool getRosUint(const std::string &key, U &u) { int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (U) param; + u = (U)param; return true; } @@ -318,8 +316,8 @@ bool getRosUint(const std::string& key, U &u) { * @return true if found, false if not found. */ template -void getRosUint(const std::string& key, U &u, V default_val) { - if(!getRosUint(key, u)) +void getRosUint(const std::string &key, U &u, V default_val) { + if (!getRosUint(key, u)) u = default_val; } @@ -329,9 +327,10 @@ void getRosUint(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, std::vector &u) { +bool getRosUint(const std::string &key, std::vector &u) { std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); @@ -350,16 +349,16 @@ bool getRosUint(const std::string& key, std::vector &u) { * @throws std::runtime_error if the parameter is out of bounds * @return true if found, false if not found. */ -template -bool getRosInt(const std::string& key, I &u) { +template bool getRosInt(const std::string &key, I &u) { int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); I max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (I) param; + u = (I)param; return true; } @@ -372,8 +371,8 @@ bool getRosInt(const std::string& key, I &u) { * @return true if found, false if not found. */ template -void getRosInt(const std::string& key, U &u, V default_val) { - if(!getRosInt(key, u)) +void getRosInt(const std::string &key, U &u, V default_val) { + if (!getRosInt(key, u)) u = default_val; } @@ -383,9 +382,10 @@ void getRosInt(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, std::vector &i) { +bool getRosInt(const std::string &key, std::vector &i) { std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); @@ -406,9 +406,9 @@ bool getRosInt(const std::string& key, std::vector &i) { * @param topic the topic to publish the message on */ template -void publish(const MessageT& m, const std::string& topic) { - static ros::Publisher publisher = nh->advertise(topic, - kROSQueueSize); +void publish(const MessageT &m, const std::string &topic) { + static ros::Publisher publisher = + nh->advertise(topic, kROSQueueSize); publisher.publish(m); } @@ -417,9 +417,7 @@ void publish(const MessageT& m, const std::string& topic) { * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES * @return true if the device supports the given GNSS */ -bool supportsGnss(std::string gnss) { - return supported.count(gnss) > 0; -} +bool supportsGnss(std::string gnss) { return supported.count(gnss) > 0; } /** * @brief This interface is used to add functionality to the main node. @@ -428,7 +426,7 @@ bool supportsGnss(std::string gnss) { * besides the main node, hardware versions, and firmware versions. */ class ComponentInterface { - public: +public: /** * @brief Get the ROS parameters. * @throws std::runtime_error if a parameter is invalid or required @@ -473,7 +471,7 @@ typedef boost::shared_ptr ComponentPtr; * element in the components vector. */ class UbloxNode : public virtual ComponentInterface { - public: +public: //! How long to wait during I/O reset [s] constexpr static double kResetWait = 10.0; //! how often (in seconds) to call poll messages @@ -519,8 +517,7 @@ class UbloxNode : public virtual ComponentInterface { */ void printInf(const ublox_msgs::Inf &m, uint8_t id); - private: - +private: /** * @brief Initialize the I/O handling. */ @@ -570,19 +567,23 @@ class UbloxNode : public virtual ComponentInterface { * @brief Poll messages from the U-Blox device. * @param event a timer indicating how often to poll the messages */ - void pollMessages(const ros::TimerEvent& event); + void pollMessages(const ros::TimerEvent &event); /** * @brief Configure INF messages, call after subscribe. */ void configureInf(); + void coldResetCb(const std_msgs::Int8::ConstPtr &msg); + void configureAgpsCb(const std_msgs::Bool::ConstPtr &msg); + ros::Subscriber coldResetSub; + ros::Subscriber configureAgpsSub; //! The u-blox node components /*! * The node will call the functions in these interfaces for each object * in the vector. */ - std::vector > components_; + std::vector> components_; //! Determined From Mon VER float protocol_version_ = 0; @@ -611,7 +612,7 @@ class UbloxNode : public virtual ComponentInterface { //! USB in protocol (see CfgPRT message for constants) uint16_t usb_in_; //! USB out protocol (see CfgPRT message for constants) - uint16_t usb_out_ ; + uint16_t usb_out_; //! The measurement rate in Hz double rate_; //! If true, set configure the User-Defined Datum @@ -631,7 +632,9 @@ class UbloxNode : public virtual ComponentInterface { //! Parameters to load from non-volatile memory during configuration ublox_msgs::CfgCFG load_; //! Parameters to save to non-volatile memory after configuration - ublox_msgs::CfgCFG save_; + ublox_msgs::CfgCFG save_; + //! Enabling the AssitNow gps online + bool enable_agps_; }; /** @@ -640,25 +643,25 @@ class UbloxNode : public virtual ComponentInterface { * @details The Firmware components update the fix diagnostics. */ class UbloxFirmware : public virtual ComponentInterface { - public: +public: /** * @brief Add the fix diagnostics to the updater. */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Handle to send fix status to ROS diagnostics. */ - virtual void fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + virtual void + fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0; }; /** * @brief Implements functions for firmware version 6. */ class UbloxFirmware6 : public UbloxFirmware { - public: +public: UbloxFirmware6(); /** @@ -677,14 +680,14 @@ class UbloxFirmware6 : public UbloxFirmware { */ void subscribe(); - protected: +protected: /** * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL * messages. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); - private: +private: /** * @brief Publish the fix and call the fix diagnostic updater. * @@ -692,7 +695,7 @@ class UbloxFirmware6 : public UbloxFirmware { * message if publishing is enabled. * @param m the message to process */ - void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m); + void callbackNavPosLlh(const ublox_msgs::NavPOSLLH &m); /** * @brief Update the last known velocity. @@ -700,7 +703,7 @@ class UbloxFirmware6 : public UbloxFirmware { * @details Publish the message if publishing is enabled. * @param m the message to process */ - void callbackNavVelNed(const ublox_msgs::NavVELNED& m); + void callbackNavVelNed(const ublox_msgs::NavVELNED &m); /** * @brief Update the number of SVs used for the fix. @@ -708,7 +711,7 @@ class UbloxFirmware6 : public UbloxFirmware { * @details Publish the message if publishing is enabled. * @param m the message to process */ - void callbackNavSol(const ublox_msgs::NavSOL& m); + void callbackNavSol(const ublox_msgs::NavSOL &m); //! The last received navigation position ublox_msgs::NavPOSLLH last_nav_pos_; @@ -736,9 +739,8 @@ class UbloxFirmware6 : public UbloxFirmware { * * @typedef NavPVT the NavPVT message type for the given firmware version */ -template -class UbloxFirmware7Plus : public UbloxFirmware { - public: +template class UbloxFirmware7Plus : public UbloxFirmware { +public: /** * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. * @@ -747,11 +749,11 @@ class UbloxFirmware7Plus : public UbloxFirmware { * is published. This function also calls the ROS diagnostics updater. * @param m the message to publish */ - void callbackNavPvt(const NavPVT& m) { - if(enabled["nav_pvt"]) { + void callbackNavPvt(const NavPVT &m) { + if (enabled["nav_pvt"]) { // NavPVT publisher - static ros::Publisher publisher = nh->advertise("navpvt", - kROSQueueSize); + static ros::Publisher publisher = + nh->advertise("navpvt", kROSQueueSize); publisher.publish(m); } @@ -775,17 +777,16 @@ class UbloxFirmware7Plus : public UbloxFirmware { fix.header.stamp = ros::Time::now(); } // Set the LLA - fix.latitude = m.lat * 1e-7; // to deg - fix.longitude = m.lon * 1e-7; // to deg + fix.latitude = m.lat * 1e-7; // to deg + fix.longitude = m.lon * 1e-7; // to deg fix.altitude = m.height * 1e-3; // to [m] // Set the Fix status bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; if (fixOk && m.fixType >= m.FIX_TYPE_2D) { fix.status.status = fix.status.STATUS_FIX; - if(m.flags & m.CARRIER_PHASE_FIXED) + if (m.flags & m.CARRIER_PHASE_FIXED) fix.status.status = fix.status.STATUS_GBAS_FIX; - } - else { + } else { fix.status.status = fix.status.STATUS_NO_FIX; } // Set the service based on GNSS configuration @@ -822,7 +823,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { velocity.twist.covariance[cols * 0 + 0] = covSpeed; velocity.twist.covariance[cols * 1 + 1] = covSpeed; velocity.twist.covariance[cols * 2 + 2] = covSpeed; - velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported velocityPublisher.publish(velocity); @@ -834,12 +835,11 @@ class UbloxFirmware7Plus : public UbloxFirmware { updater->update(); } - protected: - +protected: /** * @brief Update the fix diagnostics from Nav PVT message. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) { // check the last message, convert to diagnostic if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { @@ -896,13 +896,19 @@ class UbloxFirmware7Plus : public UbloxFirmware { bool enable_sbas_; //! The QZSS Signal configuration, see CfgGNSS message uint32_t qzss_sig_cfg_; + //AGPS path to store the ubx config file + std::string agps_path_; + //AGPS token to request the ubx config file + std::string agps_token_; + //! Enabling the AssitNow gps online + bool enable_agps_; }; /** * @brief Implements functions for firmware version 7. */ class UbloxFirmware7 : public UbloxFirmware7Plus { - public: +public: UbloxFirmware7(); /** @@ -924,21 +930,21 @@ class UbloxFirmware7 : public UbloxFirmware7Plus { */ void subscribe(); - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; +private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::CfgNMEA7 cfg_nmea_; + //! Whether or not to Configure the NMEA settings + bool set_nmea_; }; /** * @brief Implements functions for firmware version 8. */ class UbloxFirmware8 : public UbloxFirmware7Plus { - public: +public: UbloxFirmware8(); /** @@ -966,7 +972,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ void subscribe(); - private: +private: // Set from ROS parameters //! Whether or not to enable the Galileo GNSS bool enable_galileo_; @@ -985,8 +991,8 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { /** * @brief Implements functions for Raw Data products. */ -class RawDataProduct: public virtual ComponentInterface { - public: +class RawDataProduct : public virtual ComponentInterface { +public: static constexpr double kRtcmFreqTol = 0.15; static constexpr int kRtcmFreqWindow = 25; @@ -1013,7 +1019,7 @@ class RawDataProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - private: +private: //! Topic diagnostic updaters std::vector freq_diagnostics_; }; @@ -1022,8 +1028,8 @@ class RawDataProduct: public virtual ComponentInterface { * @brief Implements functions for Automotive Dead Reckoning (ADR) and * Untethered Dead Reckoning (UDR) Devices. */ -class AdrUdrProduct: public virtual ComponentInterface { - public: +class AdrUdrProduct : public virtual ComponentInterface { +public: /** * @brief Get the ADR/UDR parameters. * @@ -1055,7 +1061,7 @@ class AdrUdrProduct: public virtual ComponentInterface { "unimplemented. See AdrUdrProduct class in node.h & node.cpp."); } - protected: +protected: //! Whether or not to enable dead reckoning bool use_adr_; }; @@ -1064,7 +1070,7 @@ class AdrUdrProduct: public virtual ComponentInterface { * @brief Implements functions for FTS products. Currently unimplemented. * @todo Unimplemented. */ -class FtsProduct: public virtual ComponentInterface { +class FtsProduct : public virtual ComponentInterface { /** * @brief Get the FTS parameters. * @todo Currently unimplemented. @@ -1097,8 +1103,8 @@ class FtsProduct: public virtual ComponentInterface { * @brief Implements functions for High Precision GNSS Reference station * devices. */ -class HpgRefProduct: public virtual ComponentInterface { - public: +class HpgRefProduct : public virtual ComponentInterface { +public: /** * @brief Get the ROS parameters specific to the Reference Station * configuration. @@ -1142,14 +1148,14 @@ class HpgRefProduct: public virtual ComponentInterface { */ void callbackNavSvIn(ublox_msgs::NavSVIN m); - protected: +protected: /** * @brief Update the TMODE3 diagnostics. * * @details Updates the status of the survey-in if in survey-in mode or the * RTCM messages if in time mode. */ - void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); /** * @brief Set the device mode to time mode (internal state variable). @@ -1195,19 +1201,19 @@ class HpgRefProduct: public virtual ComponentInterface { //! Status of device time mode enum { - INIT, //!< Initialization mode (before configuration) - FIXED, //!< Fixed mode (should switch to time mode almost immediately) - DISABLED, //!< Time mode disabled + INIT, //!< Initialization mode (before configuration) + FIXED, //!< Fixed mode (should switch to time mode almost immediately) + DISABLED, //!< Time mode disabled SURVEY_IN, //!< Survey-In mode - TIME //!< Time mode, after survey-in or after configuring fixed mode + TIME //!< Time mode, after survey-in or after configuring fixed mode } mode_; }; /** * @brief Implements functions for High Precision GNSS Rover devices. */ -class HpgRovProduct: public virtual ComponentInterface { - public: +class HpgRovProduct : public virtual ComponentInterface { +public: // Constants for diagnostic updater //! Diagnostic updater: RTCM topic frequency min [Hz] constexpr static double kRtcmFreqMin = 1; @@ -1243,13 +1249,13 @@ class HpgRovProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Update the rover diagnostics, including the carrier phase solution * status (float or fixed). */ - void carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat); + void + carrierPhaseDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); /** * @brief Set the last received message and call rover diagnostic updater @@ -1258,7 +1264,6 @@ class HpgRovProduct: public virtual ComponentInterface { */ void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); - //! Last relative position (used for diagnostic updater) ublox_msgs::NavRELPOSNED last_rel_pos_; @@ -1274,13 +1279,14 @@ class HpgRovProduct: public virtual ComponentInterface { * @brief Implements functions for Time Sync products. * @todo partially implemented */ -class TimProduct: public virtual ComponentInterface { +class TimProduct : public virtual ComponentInterface { /** * @brief Get the Time Sync parameters. * @todo Currently unimplemented. */ void getRosParams() { - ROS_WARN("Functionality specific to u-blox TIM devices is only %s", + ROS_WARN( + "Functionality specific to u-blox TIM devices is only %s", "partially implemented. See TimProduct class in node.h & node.cpp."); } @@ -1303,7 +1309,6 @@ class TimProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics() {} }; - } #endif diff --git a/ublox_gps/launch/ublox_device.launch b/ublox_gps/launch/ublox_device.launch index 9bd8ce10..c3795eea 100644 --- a/ublox_gps/launch/ublox_device.launch +++ b/ublox_gps/launch/ublox_device.launch @@ -1,18 +1,16 @@ - - + + - + respawn="$(arg respawn)" > diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 344e0903..16a92e3d 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -27,8 +27,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include #include +#include namespace ublox_gps { @@ -37,15 +37,37 @@ using namespace ublox_msgs; const boost::posix_time::time_duration Gps::default_timeout_ = boost::posix_time::seconds(Gps::kDefaultAckTimeout); -Gps::Gps() : configured_(false) { subscribeAcks(); } +Gps::Gps() : configured_(false) { + subscribeAcks(); + +} Gps::~Gps() { close(); } -void Gps::setWorker(const boost::shared_ptr& worker) { - if (worker_) return; +void Gps::setAgpsParams(std::string apgs_path_, std::string apgs_options_, std::string token_){ + token=token_; + std::string agps_filename(AGPS_FILE); + std::string agps_filename_temp(AGPS_FILE_TEMP); + std::string apgs_wget_filename(AGPS_WGET_LOG); + std::string apgs_options_init(AGPS_OPTIONS_INIT); + std::string apgs_options_end(AGPS_OPTIONS_END); + + agps_path=getenv("GPS_PATH")!=NULL?getenv("GPS_PATH"):apgs_path_; //agps_path could be configured also using an environment variable + log_=agps_path+apgs_wget_filename; + agps_path_temp=agps_path+agps_filename_temp; + agps_path=agps_path+agps_filename; + + options= apgs_options_init + apgs_options_ + apgs_options_end; + + ROS_INFO("AGPS path %s & path temp %s log_ %s",agps_path.c_str(),agps_path_temp.c_str(),log_.c_str()); +} + +void Gps::setWorker(const boost::shared_ptr &worker) { + if (worker_) + return; worker_ = worker; - worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, - &callbacks_, _1, _2)); + worker_->setCallback( + boost::bind(&CallbackHandlers::readCallback, &callbacks_, _1, _2)); configured_ = static_cast(worker); } @@ -59,6 +81,14 @@ void Gps::subscribeAcks() { // Set UPD-SOS-ACK handler subscribe( boost::bind(&Gps::processUpdSosAck, this, _1)); + // Set UBX-MGA-ACK-DATA0 handler + subscribeId(boost::bind(&Gps::processMgaAck, this, _1), + ublox_msgs::Message::MGA::ACK); + + // Set UBX-MGA-ACK-DATA0 handler + subscribeId( + boost::bind(&Gps::processGetNAVX5, this, _1), + ublox_msgs::Message::CFG::NAVX5); } void Gps::processAck(const ublox_msgs::Ack &m) { @@ -69,8 +99,8 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.msg_id = m.msgID; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.clsID, m.msgID); + ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", m.clsID, + m.msgID); } void Gps::processNack(const ublox_msgs::Ack &m) { @@ -84,6 +114,35 @@ void Gps::processNack(const ublox_msgs::Ack &m) { ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID); } +void Gps::processMgaAck(const ublox_msgs::MgaACK &m) { + if (m.type == MgaACK::ACK_OK) { + ROS_DEBUG_COND(debug>=2 || m.msgId==64 ,"MgaACK %d", m.msgId); + } else { + std::string infoCode; + switch (m.infoCode) { + case 1: + infoCode = MgaACK::INFO_CODE1; + break; + case 2: + infoCode = MgaACK::INFO_CODE2; + break; + case 3: + infoCode = MgaACK::INFO_CODE3; + break; + case 4: + infoCode = MgaACK::INFO_CODE4; + break; + case 5: + infoCode = MgaACK::INFO_CODE5; + break; + case 6: + infoCode = MgaACK::INFO_CODE6; + break; + } + ROS_WARN("MgaACK fail infoCode: %s", infoCode.c_str()); + } +} + void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) { Ack ack; @@ -94,7 +153,7 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { ack_.store(ack, boost::memory_order_seq_cst); ROS_DEBUG_COND(ack.type == ACK && debug >= 2, "U-blox: received UPD SOS Backup ACK"); - if(ack.type == NACK) + if (ack.type == NACK) ROS_ERROR("U-blox: received UPD SOS Backup NACK"); } } @@ -110,15 +169,14 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } catch (std::runtime_error &e) { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + + " " + e.what()); } ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - if(BOOST_VERSION < 106600) - { + if (BOOST_VERSION < 106600) { // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but // until v1.66.0 there was a bug which didn't enable the relevant code, // fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 @@ -130,7 +188,8 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } // Set the I/O worker - if (worker_) return; + if (worker_) + return; setWorker(boost::shared_ptr( new AsyncWorker(serial, io_service))); @@ -140,21 +199,20 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, boost::asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { + for (int i = 0; i < sizeof(kBaudrates) / sizeof(kBaudrates[0]); i++) { if (current_baudrate.value() == baudrate) break; // Don't step down, unless the desired baudrate is lower - if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) + if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) continue; - serial->set_option( - boost::asio::serial_port_base::baud_rate(kBaudrates[i])); + serial->set_option(boost::asio::serial_port_base::baud_rate(kBaudrates[i])); boost::this_thread::sleep( boost::posix_time::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } configured_ = configUart1(baudrate, uart_in, uart_out); - if(!configured_ || current_baudrate.value() != baudrate) { + if (!configured_ || current_baudrate.value() != baudrate) { throw std::runtime_error("Could not configure serial baud rate"); } } @@ -168,15 +226,16 @@ void Gps::resetSerial(std::string port) { // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } catch (std::runtime_error &e) { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + + " " + e.what()); } ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker - if (worker_) return; + if (worker_) + return; setWorker(boost::shared_ptr( new AsyncWorker(serial, io_service))); configured_ = false; @@ -189,9 +248,9 @@ void Gps::resetSerial(std::string port) { return; } CfgPRT prt; - if(!read(prt, default_timeout_)) { + if (!read(prt, default_timeout_)) { ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - "message"); + "message"); return; } @@ -211,17 +270,17 @@ void Gps::initializeTcp(std::string host, std::string port) { boost::asio::ip::tcp::resolver resolver(*io_service); endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not resolve" + host + " " + - port + " " + e.what()); + } catch (std::runtime_error &e) { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + + " " + e.what()); } boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + new boost::asio::ip::tcp::socket(*io_service)); try { socket->connect(*endpoint); - } catch (std::runtime_error& e) { + } catch (std::runtime_error &e) { throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + ": " + e.what()); @@ -230,15 +289,15 @@ void Gps::initializeTcp(std::string host, std::string port) { ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; + if (worker_) + return; setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + new AsyncWorker(socket, io_service))); } void Gps::close() { - if(save_on_shutdown_) { - if(saveOnShutdown()) + if (save_on_shutdown_) { + if (saveOnShutdown()) ROS_INFO("U-Blox Flash BBR saved"); else ROS_INFO("U-Blox Flash BBR failed to save"); @@ -247,7 +306,7 @@ void Gps::close() { configured_ = false; } -void Gps::reset(const boost::posix_time::time_duration& wait) { +void Gps::reset(const boost::posix_time::time_duration &wait) { worker_.reset(); configured_ = false; // sleep because of undefined behavior after I/O reset @@ -272,12 +331,7 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { return true; } -bool Gps::configGnss(CfgGNSS gnss, - const boost::posix_time::time_duration& wait) { - // Configure the GNSS settings - ROS_DEBUG("Re-configuring GNSS."); - if (!configure(gnss)) - return false; +bool Gps::coldReset(const boost::posix_time::time_duration &wait) { // Cold reset the GNSS ROS_WARN("GNSS re-configured, cold resetting device."); if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS)) @@ -285,6 +339,18 @@ bool Gps::configGnss(CfgGNSS gnss, ros::Duration(1.0).sleep(); // Reset the I/O reset(wait); + return true; +} + +bool Gps::configGnss(CfgGNSS gnss, + const boost::posix_time::time_duration &wait) { + // Configure the GNSS settings + ROS_DEBUG("Re-configuring GNSS."); + if (!configure(gnss)) + return false; + // Cold reset the GNSS + if (!coldReset(wait)) + return false; return isConfigured(); } @@ -293,7 +359,7 @@ bool Gps::saveOnShutdown() { CfgRST rst; rst.navBbrMask = rst.NAV_BBR_HOT_START; rst.resetMode = rst.RESET_MODE_GNSS_STOP; - if(!configure(rst)) + if (!configure(rst)) return false; // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK @@ -311,7 +377,8 @@ bool Gps::clearBbr() { bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; + if (!worker_) + return true; ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", baudrate, in_proto_mask, out_proto_mask); @@ -326,7 +393,7 @@ bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, return configure(port); } -bool Gps::disableUart1(CfgPRT& prev_config) { +bool Gps::disableUart1(CfgPRT &prev_config) { ROS_DEBUG("Disabling UART1"); // Poll UART PRT Config @@ -336,7 +403,7 @@ bool Gps::disableUart1(CfgPRT& prev_config) { ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } - if(!read(prev_config, default_timeout_)) { + if (!read(prev_config, default_timeout_)) { ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); return false; } @@ -352,13 +419,13 @@ bool Gps::disableUart1(CfgPRT& prev_config) { return configure(port); } -bool Gps::configUsb(uint16_t tx_ready, - uint16_t in_proto_mask, +bool Gps::configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; + if (!worker_) + return true; - ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - tx_ready, in_proto_mask, out_proto_mask); + ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", tx_ready, + in_proto_mask, out_proto_mask); CfgPRT port; port.portID = CfgPRT::PORT_ID_USB; @@ -370,19 +437,19 @@ bool Gps::configUsb(uint16_t tx_ready, bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { ROS_DEBUG("Configuring measurement rate to %u and nav rate to %u", meas_rate, - nav_rate); + nav_rate); CfgRATE rate; rate.measRate = meas_rate; - rate.navRate = nav_rate; // must be fixed at 1 for ublox 5 and 6 + rate.navRate = nav_rate; // must be fixed at 1 for ublox 5 and 6 rate.timeRef = CfgRATE::TIME_REF_GPS; return configure(rate); } bool Gps::configRtcm(std::vector ids, std::vector rates) { - for(size_t i = 0; i < ids.size(); ++i) { + for (size_t i = 0; i < ids.size(); ++i) { ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { + if (!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); return false; } @@ -400,11 +467,10 @@ bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { return configure(msg); } -bool Gps::configTmode3Fixed(bool lla_flag, - std::vector arp_position, +bool Gps::configTmode3Fixed(bool lla_flag, std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc) { - if(arp_position.size() != 3 || arp_position_hp.size() != 3) { + if (arp_position.size() != 3 || arp_position_hp.size() != 3) { ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", "& arp_position_hp args must be 3"); return false; @@ -417,7 +483,7 @@ bool Gps::configTmode3Fixed(bool lla_flag, tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; // Set position - if(lla_flag) { + if (lla_flag) { // Convert from [deg] to [deg * 1e-7] tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7); tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7); @@ -494,11 +560,14 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { bool Gps::setPpp(bool enable) { ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); - ublox_msgs::CfgNAVX5 msg; + msg.version = 2; msg.usePPP = enable; - msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; - return configure(msg); + msg.mask1 = + ublox_msgs::CfgNAVX5::MASK1_PPP | ublox_msgs::CfgNAVX5::MASK1_ACK_AID; + msg.ackAiding = 1; + // configure(msg); + return configure(msg, true); } bool Gps::setDgnss(uint8_t mode) { @@ -518,8 +587,9 @@ bool Gps::setUseAdr(bool enable) { } bool Gps::poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload) { - if (!worker_) return false; + const std::vector &payload) { + if (!worker_) + return false; std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); @@ -530,24 +600,184 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, return true; } -bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, +bool Gps::waitForAcknowledge(const boost::posix_time::time_duration &timeout, uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", - class_id, msg_id); + ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", class_id, + msg_id); boost::posix_time::ptime wait_until( boost::posix_time::second_clock::local_time() + timeout); Ack ack = ack_.load(boost::memory_order_seq_cst); - while (boost::posix_time::second_clock::local_time() < wait_until - && (ack.class_id != class_id - || ack.msg_id != msg_id - || ack.type == WAIT)) { + while ( + boost::posix_time::second_clock::local_time() < wait_until && + (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) { worker_->wait(timeout); ack = ack_.load(boost::memory_order_seq_cst); } - bool result = ack.type == ACK - && ack.class_id == class_id - && ack.msg_id == msg_id; + bool result = + ack.type == ACK && ack.class_id == class_id && ack.msg_id == msg_id; return result; } -} // namespace ublox_gps + +bool Gps::downloadAGPS() { + std::string url(AGPS_URL); + std::string url_ = url + token + options; + std::string command = + "wget -O " + agps_path_temp + " -o " + log_ + " \"" + url_ + "\""; + ROS_INFO("downloading AGPS... %s", command.c_str()); + executeShellCommand(command); + + if (fileSuccess()) { + command = "mv " + agps_path_temp + " " + agps_path; // Update ublox file + executeShellCommand(command); + return true; + } + // Cleaning temp file + command = "rm " + agps_path_temp; + executeShellCommand(command); + return false; +} + +bool Gps::fileSuccess() { + ROS_INFO("checking file success"); + std::string line; + std::ifstream logFile(log_, std::ifstream::in); + int successLine = 0; + if (logFile.is_open()) { + while (logFile.good()) { + getline(logFile, line); + successLine++; + // ROS_INFO("line %d %s", successLine, line.c_str()); + if (successLine == 4) + return line.compare(AGPS_SERVER_202) == 0; + } + logFile.close(); + } else + ROS_WARN("Unable to open wget logfile"); + + ROS_ERROR("agps file download error"); + return false; +} + +bool Gps::checkAGPSFile() { + struct stat result; + if ((stat(agps_path.c_str(), &result) == 0)) { + int modified = (long)result.st_mtime; + double time_modification = + (double)(ros::Time::now().toSec() - modified) / 60; // Minutes + ROS_INFO("checkAGPSFile last %0.4f", time_modification); + if (time_modification > 60) + return downloadAGPS(); + else { + ROS_DEBUG_STREAM("File is still valid"); + return true; + } + } else + return downloadAGPS(); +} + +void Gps::configureAGPS() { + if (checkAGPSFile()) { + // Create a MGA_INI_TIME_UTC + coldReset(boost::posix_time::seconds(1)); + ublox_msgs::MgaINITIMEUTC mga_time = getMgaUtc(); + ROS_DEBUG("configuringAGPS"); + configure(mga_time); + // Get the rest of the file + std::ifstream ubxFile(agps_path, std::ios::binary); + std::vector buffer((std::istreambuf_iterator(ubxFile)), + (std::istreambuf_iterator())); + + int size = buffer.size() - AGPS_TIME_SIZE; + std::vector apgsData(size); + + for (int i = 0; i < buffer.size(); i++) { + if (i >= AGPS_TIME_SIZE) + apgsData[i - AGPS_TIME_SIZE] = buffer[i]; + } + worker_->send(apgsData.data(), size); + } +} + +std::vector Gps::split(std::string *s, char delimiter) { + size_t pos = 0; + + std::vector resp; + while ((pos = s->find(delimiter)) != std::string::npos) { + resp.push_back(s->substr(0, pos)); + s->erase(0, pos + 1); + } + return resp; +} + +void Gps::processGetNAVX5(const ublox_msgs::CfgNAVX5 &m) { + ROS_WARN("processGetNAVX5 ackAiding %d", m.ackAiding); +} + +bool Gps::getNAVX5() { + ROS_DEBUG("getNAVX5"); + ublox_msgs::CfgNAVX5_GET msg; + return configure(msg, true); +} + +ublox_msgs::MgaINITIMEUTC Gps::getMgaUtc() { + ublox_msgs::MgaINITIMEUTC mga_time; + mga_time.type = 0x10; // Message type (0x10 for this type) + mga_time.version = 0; // Message version (0x00 for this version) + mga_time.leapSecs = + 0x80; // Number of leap seconds since 1980 (or 0x80 =-128 if unknown) + boost::posix_time::ptime now_posix_time = ros::Time::now().toBoost(); + std::string iso_time_str = + boost::posix_time::to_iso_extended_string(now_posix_time); + + std::string iso_time_str_line = iso_time_str + "-"; + std::string iso_time_str_dot = iso_time_str + "."; + std::vector date = split(&iso_time_str_line, '-'); + std::vector date2 = split(&date[2], 'T'); + std::vector date3 = split(&date[2], ':'); + std::vector date4 = split(&iso_time_str_dot, '.'); + std::string date4_dot = date4[0] + ":"; + std::vector date5 = split(&date4_dot, ':'); + + mga_time.year = std::stoi(date[0], 0, 10); // Year + mga_time.month = std::stoi(date[1], 0, 10); // Month, starting at 1 + mga_time.day = std::stoi(date2[0], 0, 10); // Day, starting at 1 + mga_time.hour = std::stoi(date3[0], 0, 10); // Hour, from 0 to 23 + mga_time.minute = std::stoi(date5[1], 0, 10); // Minute, from 0 to 59 + mga_time.second = std::stoi(date5[2], 0, 10); // Seconds, from 0 to 59 + mga_time.reserved1 = 0; // reserved1 + mga_time.ns = + std::stoi(date4[1], 0, 10) * 1000; // Nanoseconds, from 0 to 999,999,999 + mga_time.tAccS = 0; // Seconds part of time accuracy + mga_time.reserved2 = {0, 0}; // reserved2 + mga_time.tAccNs = + 0; // Nanoseconds part of time accuracy, from 0 to 999,999,999 + + return mga_time; +} + +std::string Gps::executeShellCommand(std::string command) { + + std::array buffer; + std::shared_ptr pipe(popen(command.c_str(), "r"), pclose); + std::string out = ""; + + if (!pipe) { + throw std::runtime_error("popen() failed!"); + } + + while (!feof(pipe.get())) { + + if (fgets(buffer.data(), 1024, pipe.get()) != NULL) { + out += buffer.data(); + } + } + + if (out.compare("") != 0) { + out = out.substr(0, out.size() - 1); + } + + return out; +} + +} // namespace ublox_gps diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 16b1fef2..a8c50d89 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -34,26 +34,26 @@ using namespace ublox_node; // // ublox_node namespace // -uint8_t ublox_node::modelFromString(const std::string& model) { +uint8_t ublox_node::modelFromString(const std::string &model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if(lower == "portable") { + if (lower == "portable") { return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; - } else if(lower == "stationary") { + } else if (lower == "stationary") { return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; - } else if(lower == "pedestrian") { + } else if (lower == "pedestrian") { return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if(lower == "automotive") { + } else if (lower == "automotive") { return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if(lower == "sea") { + } else if (lower == "sea") { return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; - } else if(lower == "airborne1") { + } else if (lower == "airborne1") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if(lower == "airborne2") { + } else if (lower == "airborne2") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if(lower == "airborne4") { + } else if (lower == "airborne4") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if(lower == "wristwatch") { + } else if (lower == "wristwatch") { return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; } @@ -61,7 +61,7 @@ uint8_t ublox_node::modelFromString(const std::string& model) { " is not a valid dynamic model."); } -uint8_t ublox_node::fixModeFromString(const std::string& mode) { +uint8_t ublox_node::fixModeFromString(const std::string &mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { @@ -79,9 +79,7 @@ uint8_t ublox_node::fixModeFromString(const std::string& mode) { // // u-blox ROS Node // -UbloxNode::UbloxNode() { - initialize(); -} +UbloxNode::UbloxNode() { initialize(); } void UbloxNode::addFirmwareInterface() { int ublox_version; @@ -98,7 +96,6 @@ void UbloxNode::addFirmwareInterface() { ROS_INFO("U-Blox Firmware Version: %d", ublox_version); } - void UbloxNode::addProductInterface(std::string product_category, std::string ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) @@ -112,7 +109,7 @@ void UbloxNode::addProductInterface(std::string product_category, components_.push_back(ComponentPtr(new AdrUdrProduct)); else if (product_category.compare("FTS") == 0) components_.push_back(ComponentPtr(new FtsProduct)); - else if(product_category.compare("SPG") != 0) + else if (product_category.compare("SPG") != 0) ROS_WARN("Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), "options are HPG REF, HPG ROV, TIM, ADR, UDR, FTS, SPG"); @@ -130,29 +127,29 @@ void UbloxNode::getRosParams() { // UART 1 params getRosUint("uart1/baudrate", baudrate_, 9600); - getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX - | ublox_msgs::CfgPRT::PROTO_NMEA - | ublox_msgs::CfgPRT::PROTO_RTCM); + getRosUint("uart1/in", uart_in_, + ublox_msgs::CfgPRT::PROTO_UBX | ublox_msgs::CfgPRT::PROTO_NMEA | + ublox_msgs::CfgPRT::PROTO_RTCM); getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); // USB params if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { set_usb_ = true; - if(!getRosUint("usb/in", usb_in_)) { + if (!getRosUint("usb/in", usb_in_)) { throw std::runtime_error(std::string("usb/out is set, therefore ") + - "usb/in must be set"); + "usb/in must be set"); } - if(!getRosUint("usb/out", usb_out_)) { + if (!getRosUint("usb/out", usb_out_)) { throw std::runtime_error(std::string("usb/in is set, therefore ") + - "usb/out must be set"); + "usb/out must be set"); } getRosUint("usb/tx_ready", usb_tx_, 0); } // Measurement rate params - nh->param("rate", rate_, 4.0); // in Hz - getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles + nh->param("rate", rate_, 4.0); // in Hz + getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles // RTCM params - getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs - getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates + getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs + getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting nh->param("enable_ppp", enable_ppp_, false); // SBAS params, only for some devices @@ -168,7 +165,7 @@ void UbloxNode::getRosParams() { checkMin(rate_, 0, "rate"); - if(rtcm_ids.size() != rtcm_rates.size()) + if (rtcm_ids.size() != rtcm_rates.size()) throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); @@ -176,16 +173,16 @@ void UbloxNode::getRosParams() { fmode_ = fixModeFromString(fix_mode_); nh->param("dat/set", set_dat_, false); - if(set_dat_) { + if (set_dat_) { std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.majA) - || nh->getParam("dat/flat", cfg_dat_.flat) - || nh->getParam("dat/shift", shift) - || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) - throw std::runtime_error(std::string("dat/set is true, therefore ") + - "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); - if(shift.size() != 3 || rot.size() != 3) + if (!nh->getParam("dat/majA", cfg_dat_.majA) || + nh->getParam("dat/flat", cfg_dat_.flat) || + nh->getParam("dat/shift", shift) || nh->getParam("dat/rot", rot) || + nh->getParam("dat/scale", cfg_dat_.scale)) + throw std::runtime_error( + std::string("dat/set is true, therefore ") + + "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); + if (shift.size() != 3 || rot.size() != 3) throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + "must be 3"); checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA"); @@ -205,10 +202,10 @@ void UbloxNode::getRosParams() { } // measurement period [ms] - meas_rate = 1000 / rate_; + meas_rate = 1000 / rate_; } -void UbloxNode::pollMessages(const ros::TimerEvent& event) { +void UbloxNode::pollMessages(const ros::TimerEvent &event) { static std::vector payload(1, 1); if (enabled["aid_alm"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); @@ -247,18 +244,21 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) - gps.subscribe(boost::bind( - publish, _1, "navstatus"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "navstatus"), + kSubscribeRate); nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) - gps.subscribe(boost::bind( - publish, _1, "navposecef"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "navposecef"), + kSubscribeRate); nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) - gps.subscribe(boost::bind( - publish, _1, "navclock"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "navclock"), + kSubscribeRate); // INF messages nh->param("inf/debug", enabled["inf_debug"], false); @@ -299,20 +299,20 @@ void UbloxNode::subscribe() { // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) - gps.subscribe(boost::bind( - publish, _1, "aidalm"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "aidalm"), kSubscribeRate); nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) - gps.subscribe(boost::bind( - publish, _1, "aideph"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "aideph"), kSubscribeRate); nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) - gps.subscribe(boost::bind( - publish, _1, "aidhui"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "aidhui"), kSubscribeRate); - for(int i = 0; i < components_.size(); i++) + for (int i = 0; i < components_.size(); i++) components_[i]->subscribe(); } @@ -324,37 +324,36 @@ void UbloxNode::initializeRosDiagnostics() { updater->setHardwareID("ublox"); // configure diagnostic updater for frequency - freq_diag = FixDiagnostic(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin); - for(int i = 0; i < components_.size(); i++) + freq_diag = FixDiagnostic(std::string("fix"), kFixFreqTol, kFixFreqWindow, + kTimeStampStatusMin); + for (int i = 0; i < components_.size(); i++) components_[i]->initializeRosDiagnostics(); } - void UbloxNode::processMonVer() { ublox_msgs::MonVER monVer; if (!gps.poll(monVer)) throw std::runtime_error("Failed to poll MonVER & set relevant settings"); ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), - monVer.hwVersion.c_array()); + monVer.hwVersion.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); - for(std::size_t i = 0; i < monVer.extension.size(); ++i) { + for (std::size_t i = 0; i < monVer.extension.size(); ++i) { ROS_DEBUG("%s", monVer.extension[i].field.c_array()); // Find the end of the string (null character) - unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); + unsigned char *end = std::find(monVer.extension[i].field.begin(), + monVer.extension[i].field.end(), '\0'); extension.push_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) { std::size_t found = extension[i].find("PROTVER"); if (found != std::string::npos) { - protocol_version_ = ::atof( - extension[i].substr(8, extension[i].size()-8).c_str()); + protocol_version_ = + ::atof(extension[i].substr(8, extension[i].size() - 8).c_str()); break; } } @@ -363,22 +362,23 @@ void UbloxNode::processMonVer() { "Defaulting to firmware version 6."); addFirmwareInterface(); - if(protocol_version_ < 18) { + if (protocol_version_ < 18) { // Final line contains supported GNSS delimited by ; std::vector strs; - if(extension.size() > 0) - boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + if (extension.size() > 0) + boost::split(strs, extension[extension.size() - 1], + boost::is_any_of(";")); + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); } else { - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) { std::vector strs; // Up to 2nd to last line - if(i <= extension.size() - 2) { + if (i <= extension.size() - 2) { boost::split(strs, extension[i], boost::is_any_of("=")); - if(strs.size() > 1) { + if (strs.size() > 1) { if (strs[0].compare(std::string("FWVER")) == 0) { - if(strs[1].length() > 8) + if (strs[1].length() > 8) addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); else addProductInterface(strs[1].substr(0, 3)); @@ -387,9 +387,9 @@ void UbloxNode::processMonVer() { } } // Last 1-2 lines contain supported GNSS - if(i >= extension.size() - 2) { + if (i >= extension.size() - 2) { boost::split(strs, extension[i], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); } } @@ -407,12 +407,13 @@ bool UbloxNode::configureUblox() { "from memory"); if (load_.loadMask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", - "communications."); + "communications."); boost::posix_time::seconds wait(kResetWait); gps.reset(wait); if (!gps.isConfigured()) - throw std::runtime_error(std::string("Failed to reset serial I/O") + - "after loading I/O configurations from device memory."); + throw std::runtime_error( + std::string("Failed to reset serial I/O") + + "after loading I/O configurations from device memory."); } } @@ -426,7 +427,7 @@ bool UbloxNode::configureUblox() { throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) { if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + @@ -435,8 +436,12 @@ bool UbloxNode::configureUblox() { } if (!gps.setPpp(enable_ppp_)) throw std::runtime_error(std::string("Failed to ") + - ((enable_ppp_) ? "enable" : "disable") - + " PPP."); + ((enable_ppp_) ? "enable" : "disable") + + " PPP."); + if (!gps.setPpp(enable_ppp_)) + throw std::runtime_error(std::string("Failed to ") + + ((enable_ppp_) ? "enable" : "disable") + + " PPP."); if (!gps.setDynamicModel(dmodel_)) throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); if (!gps.setFixMode(fmode_)) @@ -450,16 +455,20 @@ bool UbloxNode::configureUblox() { throw std::runtime_error("Failed to set user-defined datum."); // Configure each component for (int i = 0; i < components_.size(); i++) { - if(!components_[i]->configureUblox()) + if (!components_[i]->configureUblox()) return false; } if (save_.saveMask != 0) { ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.saveMask, save_.deviceMask); - if(!gps.configure(save_)) + if (!gps.configure(save_)) ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } - } catch (std::exception& e) { + nh->param("agps/enable", enable_agps_, false); + if(enable_agps_) + gps.configureAGPS(); + + } catch (std::exception &e) { ROS_FATAL("Error configuring u-blox: %s", e.what()); return false; } @@ -516,14 +525,27 @@ void UbloxNode::initializeIo() { } } +void UbloxNode::coldResetCb(const std_msgs::Int8::ConstPtr &msg) { + ROS_WARN("coldResetCb"); + gps.coldReset(boost::posix_time::seconds(msg->data)); +} + +void UbloxNode::configureAgpsCb(const std_msgs::Bool::ConstPtr &msg) { + ROS_WARN("configureAgpsCb"); + if(enable_agps_) + gps.configureAGPS(); + else + ROS_WARN("AGPS is not enabled"); +} + void UbloxNode::initialize() { // Params must be set before initializing IO getRosParams(); initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if(protocol_version_ <= 14) { - if(nh->param("raw_data", false)) + if (protocol_version_ <= 14) { + if (nh->param("raw_data", false)) components_.push_back(ComponentPtr(new RawDataProduct)); } // Must set firmware & hardware params before initializing diagnostics @@ -531,7 +553,10 @@ void UbloxNode::initialize() { components_[i]->getRosParams(); // Do this last initializeRosDiagnostics(); - + coldResetSub = nh->subscribe("coldReset", 10, + &UbloxNode::coldResetCb, this); + configureAgpsSub = nh->subscribe( + "configureAgps", 10, &UbloxNode::configureAgpsCb, this); if (configureUblox()) { ROS_INFO("U-Blox configured successfully."); // Subscribe to all U-Blox messages @@ -541,8 +566,7 @@ void UbloxNode::initialize() { ros::Timer poller; poller = nh->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); + &UbloxNode::pollMessages, this); poller.start(); ros::spin(); } @@ -579,16 +603,16 @@ void UbloxFirmware6::getRosParams() { if (!getRosUint("nmea/version", cfg_nmea_.version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -628,31 +652,35 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); - gps.subscribe(boost::bind( - // Subscribe to Nav SOL - &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); + gps.subscribe( + boost::bind(&UbloxFirmware6::callbackNavPosLlh, this, _1), + kSubscribeRate); + gps.subscribe( + boost::bind( + // Subscribe to Nav SOL + &UbloxFirmware6::callbackNavSol, this, _1), + kSubscribeRate); // Subscribe to Nav VELNED - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); + gps.subscribe( + boost::bind(&UbloxFirmware6::callbackNavVelNed, this, _1), + kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), + gps.subscribe( + boost::bind(publish, _1, "navsvinfo"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "monhw"), kSubscribeRate); } void UbloxFirmware6::fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) { + diagnostic_updater::DiagnosticStatusWrapper &stat) { // Set the diagnostic level based on the fix status if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; @@ -693,8 +721,8 @@ void UbloxFirmware6::fixDiagnostic( stat.add("# SVs used", (int)last_nav_sol_.numSV); } -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if(enabled["nav_posllh"]) { +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH &m) { + if (enabled["nav_posllh"]) { static ros::Publisher publisher = nh->advertise("navposllh", kROSQueueSize); publisher.publish(m); @@ -736,8 +764,8 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { updater->update(); } -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if(enabled["nav_velned"]) { +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED &m) { + if (enabled["nav_velned"]) { static ros::Publisher publisher = nh->advertise("navvelned", kROSQueueSize); publisher.publish(m); @@ -746,7 +774,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { // Example geometry message static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", - kROSQueueSize); + kROSQueueSize); if (m.iTOW == last_nav_pos_.iTOW) velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh else @@ -764,14 +792,14 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { velocity_.twist.covariance[cols * 0 + 0] = varSpeed; velocity_.twist.covariance[cols * 1 + 1] = varSpeed; velocity_.twist.covariance[cols * 2 + 2] = varSpeed; - velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported velocityPublisher.publish(velocity_); last_nav_vel_ = m; } -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if(enabled["nav_sol"]) { +void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL &m) { + if (enabled["nav_sol"]) { static ros::Publisher publisher = nh->advertise("navsol", kROSQueueSize); publisher.publish(m); @@ -793,29 +821,30 @@ void UbloxFirmware7::getRosParams() { nh->param("gnss/glonass", enable_glonass_, false); nh->param("gnss/qzss", enable_qzss_, false); getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); - if(enable_gps_ && !supportsGnss("GPS")) + if (enable_gps_ && !supportsGnss("GPS")) ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - if(enable_glonass_ && !supportsGnss("GLO")) + if (enable_glonass_ && !supportsGnss("GLO")) ROS_WARN("gnss/glonass is true, but GLONASS is not %s", "supported by this device"); - if(enable_qzss_ && !supportsGnss("QZSS")) + if (enable_qzss_ && !supportsGnss("QZSS")) ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if(enable_sbas_ && !supportsGnss("SBAS")) + if (enable_sbas_ && !supportsGnss("SBAS")) ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - if(nh->hasParam("gnss/galileo")) + if (nh->hasParam("gnss/galileo")) ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - if(nh->hasParam("gnss/beidou")) + if (nh->hasParam("gnss/beidou")) ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - if(nh->hasParam("gnss/imes")) + if (nh->hasParam("gnss/imes")) ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; + fix_status_service = + sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; // // NMEA Configuration @@ -826,19 +855,19 @@ void UbloxFirmware7::getRosParams() { if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -884,13 +913,13 @@ bool UbloxFirmware7::configureUblox() { } ublox_msgs::CfgGNSS cfgGNSSWrite; - cfgGNSSWrite.numConfigBlocks = 1; // do services one by one + cfgGNSSWrite.numConfigBlocks = 1; // do services one by one cfgGNSSWrite.numTrkChHw = cfgGNSSRead.numTrkChHw; cfgGNSSWrite.numTrkChUse = cfgGNSSRead.numTrkChUse; cfgGNSSWrite.msgVer = 0; // configure GLONASS - if(supportsGnss("GLO")) { + if (supportsGnss("GLO")) { ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_GLONASS; block.resTrkCh = block.RES_TRK_CH_GLONASS; @@ -904,7 +933,7 @@ bool UbloxFirmware7::configureUblox() { } } - if(supportsGnss("QZSS")) { + if (supportsGnss("QZSS")) { // configure QZSS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_QZSS; @@ -919,7 +948,7 @@ bool UbloxFirmware7::configureUblox() { } } - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) { // configure SBAS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_SBAS; @@ -934,9 +963,9 @@ bool UbloxFirmware7::configureUblox() { } } - if(set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) throw std::runtime_error("Failed to configure NMEA"); - + return true; } @@ -945,22 +974,22 @@ void UbloxFirmware7::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps.subscribe(boost::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, _1), - kSubscribeRate); + gps.subscribe( + boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), + kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), + gps.subscribe( + boost::bind(publish, _1, "navsvinfo"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "monhw"), kSubscribeRate); } // @@ -983,7 +1012,7 @@ void UbloxFirmware8::getRosParams() { nh->param("gnss/sbas", enable_sbas_, false); // QZSS Signal Configuration getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !supportsGnss("GPS")) ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", @@ -1006,10 +1035,11 @@ void UbloxFirmware8::getRosParams() { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; + fix_status_service = + sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; // // NMEA Configuration @@ -1022,19 +1052,19 @@ void UbloxFirmware8::getRosParams() { // Verify that parameters are set if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -1077,13 +1107,36 @@ void UbloxFirmware8::getRosParams() { cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0]; cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1]; } -} + // AGPS parameters + nh->param("agps/enable", enable_agps_, false); + nh->param("agps/token", agps_token_, std::string("noToken")); + nh->param("agps/path", agps_path_, std::string("/data")); + + std::string agps_options=""; + agps_options=enable_gps_ ? "gps," : ""; + agps_options=agps_options+(enable_galileo_ ? "gal," : ""); + agps_options=agps_options+(enable_beidou_ ? "bds," : ""); + agps_options=agps_options+(enable_glonass_ ? "glo," : ""); + agps_options=agps_options+(enable_qzss_ ? "qzss," : ""); + enable_agps_= enable_agps_ & agps_token_.compare("noToken")!= 0 & agps_options.compare("")!=0; + + if(enable_agps_){ + agps_options=agps_options.substr(0,agps_options.size()-1); + gps.setAgpsParams(agps_path_, agps_options, agps_token_); + nh->setParam("agps/enable", true); + ROS_WARN("Warning: AGPS is enabled - with file path %s and token %s - this is an expert setting.", agps_path_.c_str(), agps_token_.c_str()); + } + else if(agps_token_.compare("noToken") == 0){ + ROS_WARN("Warning: AGPS not enabled, configuration error"); + nh->setParam("agps/enable", false); + } +} bool UbloxFirmware8::configureUblox() { - if(clear_bbr_) { + if (clear_bbr_) { // clear flash memory - if(!gps.clearBbr()) + if (!gps.clearBbr()) ROS_ERROR("u-blox failed to clear flash memory"); } // @@ -1103,42 +1156,41 @@ bool UbloxFirmware8::configureUblox() { bool correct = true; for (int i = 0; i < cfg_gnss.blocks.size(); i++) { ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i]; - if (block.gnssId == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { + if (block.gnssId == block.GNSS_ID_GPS && + enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_SBAS - && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnssId == block.GNSS_ID_SBAS && + enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnssId == block.GNSS_ID_GALILEO && + enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnssId == block.GNSS_ID_BEIDOU && + enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnssId == block.GNSS_ID_IMES && + enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnssId == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) - || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { + } else if (block.gnssId == block.GNSS_ID_QZSS && + (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) || + (enable_qzss_ && + qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { ROS_DEBUG("QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, - enable_qzss_); + block.flags & block.FLAGS_ENABLE, enable_qzss_); correct = false; ROS_DEBUG("QZSS Configuration: %u", block.flags); cfg_gnss.blocks[i].flags = @@ -1147,8 +1199,8 @@ bool UbloxFirmware8::configureUblox() { if (enable_qzss_) // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } else if (block.gnssId == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnssId == block.GNSS_ID_GLONASS && + enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; @@ -1160,7 +1212,8 @@ bool UbloxFirmware8::configureUblox() { // since this requires a cold reset if (correct) ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15))) + else if (!gps.configGnss( + cfg_gnss, boost::posix_time::seconds(2))) // changed from 15 to 2 throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); @@ -1178,25 +1231,28 @@ void UbloxFirmware8::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT gps.subscribe( - boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); + boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), + kSubscribeRate); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) - gps.subscribe(boost::bind( - publish, _1, "navsat"), kNavSvInfoSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "navsat"), + kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "monhw"), kSubscribeRate); // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmrtcm"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmrtcm"), + kSubscribeRate); } // @@ -1209,41 +1265,42 @@ void RawDataProduct::subscribe() { // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmraw"), kSubscribeRate); // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmsfrb"), + kSubscribeRate); // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) - gps.subscribe(boost::bind( - publish, _1, "rxmeph"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmeph"), kSubscribeRate); // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmalm"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmalm"), kSubscribeRate); } void RawDataProduct::initializeRosDiagnostics() { if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, - kRtcmFreqWindow)); + freq_diagnostics_.push_back( + UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow)); if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, - kRtcmFreqWindow)); + freq_diagnostics_.push_back( + UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow)); if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, - kRtcmFreqWindow)); + freq_diagnostics_.push_back( + UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow)); if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, - kRtcmFreqWindow)); + freq_diagnostics_.push_back( + UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow)); } // @@ -1253,14 +1310,14 @@ void AdrUdrProduct::getRosParams() { nh->param("use_adr", use_adr_, true); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate * nav_rate); - if(nav_rate_hz != 1) + if (nav_rate_hz != 1) ROS_WARN("Nav Rate recommended to be 1 Hz"); } bool AdrUdrProduct::configureUblox() { - if(!gps.setUseAdr(use_adr_)) - throw std::runtime_error(std::string("Failed to ") - + (use_adr_ ? "enable" : "disable") + "use_adr"); + if (!gps.setUseAdr(use_adr_)) + throw std::runtime_error(std::string("Failed to ") + + (use_adr_ ? "enable" : "disable") + "use_adr"); return true; } @@ -1270,115 +1327,119 @@ void AdrUdrProduct::subscribe() { // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) - gps.subscribe(boost::bind( - publish, _1, "navatt"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "navatt"), kSubscribeRate); // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) - gps.subscribe(boost::bind( - publish, _1, "esfins"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "esfins"), kSubscribeRate); // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) - gps.subscribe(boost::bind( - publish, _1, "esfmeas"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "esfmeas"), + kSubscribeRate); // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) - gps.subscribe(boost::bind( - publish, _1, "esfraw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "esfraw"), kSubscribeRate); // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) - gps.subscribe(boost::bind( - publish, _1, "esfstatus"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "esfstatus"), + kSubscribeRate); // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) - gps.subscribe(boost::bind( - publish, _1, "hnrpvt"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "hnrpvt"), kSubscribeRate); } // // u-blox High Precision GNSS Reference Station // void HpgRefProduct::getRosParams() { - if(nav_rate * meas_rate != 1000) + if (nav_rate * meas_rate != 1000) ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); - if(!getRosUint("tmode3", tmode3_)) + if (!getRosUint("tmode3", tmode3_)) throw std::runtime_error("Invalid settings: TMODE3 must be set"); - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!nh->getParam("arp/position", arp_position_)) - throw std::runtime_error(std::string("Invalid settings: arp/position ") - + "must be set if TMODE3 is fixed"); - if(!getRosInt("arp/position_hp", arp_position_hp_)) - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/acc", fixed_pos_acc_)) - throw std::runtime_error(std::string("Invalid settings: arp/acc ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/lla_flag", lla_flag_)) { + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!nh->getParam("arp/position", arp_position_)) + throw std::runtime_error(std::string("Invalid settings: arp/position ") + + "must be set if TMODE3 is fixed"); + if (!getRosInt("arp/position_hp", arp_position_hp_)) + throw std::runtime_error( + std::string("Invalid settings: arp/position_hp ") + + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/acc", fixed_pos_acc_)) + throw std::runtime_error(std::string("Invalid settings: arp/acc ") + + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/lla_flag", lla_flag_)) { ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { nh->param("sv_in/reset", svin_reset_, true); - if(!getRosUint("sv_in/min_dur", sv_in_min_dur_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") - + "must be set if TMODE3 is survey-in"); - if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") - + "must be set if TMODE3 is survey-in"); - } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") - + " flag constants for possible values."); + if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + + "must be set if TMODE3 is survey-in"); + if (!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + + "must be set if TMODE3 is survey-in"); + } else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + throw std::runtime_error( + std::string("tmode3 param invalid. See CfgTMODE3") + + " flag constants for possible values."); } } bool HpgRefProduct::configureUblox() { // Configure TMODE3 - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if(!gps.disableTmode3()) + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + if (!gps.disableTmode3()) throw std::runtime_error("Failed to disable TMODE3."); mode_ = DISABLED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, fixed_pos_acc_)) throw std::runtime_error("Failed to set TMODE3 to fixed."); - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) throw std::runtime_error("Failed to set RTCM rates"); mode_ = FIXED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if(!svin_reset_) { + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + if (!svin_reset_) { ublox_msgs::NavSVIN nav_svin; - if(!gps.poll(nav_svin)) + if (!gps.poll(nav_svin)) throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); // Don't reset survey-in if it's already active - if(nav_svin.active) { + if (nav_svin.active) { mode_ = SURVEY_IN; return true; } // Don't reset survey-in if it already has a valid value - if(nav_svin.valid) { + if (nav_svin.valid) { setTimeMode(); return true; } ublox_msgs::NavPVT nav_pvt; - if(!gps.poll(nav_pvt)) + if (!gps.poll(nav_pvt)) throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { + if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY && + nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { setTimeMode(); return true; } @@ -1387,19 +1448,19 @@ bool HpgRefProduct::configureUblox() { // For Survey in, meas rate must be at least 1 Hz uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] // If measurement period isn't a factor of 1000, set to default - if(1000 % meas_rate_temp != 0) + if (1000 % meas_rate_temp != 0) meas_rate_temp = kDefaultMeasPeriod; // Set nav rate to 1 Hz during survey in - if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) + if (!gps.configRate(meas_rate_temp, (int)1000 / meas_rate_temp)) throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); // As recommended in the documentation, first disable, then set to survey in - if(!gps.disableTmode3()) + if (!gps.disableTmode3()) ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); else mode_ = DISABLED; // Set to Survey in mode - if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) + if (!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) throw std::runtime_error("Failed to set TMODE3 to survey-in."); mode_ = SURVEY_IN; } @@ -1410,12 +1471,12 @@ void HpgRefProduct::subscribe() { // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In - gps.subscribe(boost::bind( - &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); + gps.subscribe( + boost::bind(&HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); } void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { - if(enabled["nav_svin"]) { + if (enabled["nav_svin"]) { static ros::Publisher publisher = nh->advertise("navsvin", kROSQueueSize); publisher.publish(m); @@ -1423,7 +1484,7 @@ void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { last_nav_svin_ = m; - if(!m.active && m.valid && mode_ == SURVEY_IN) { + if (!m.active && m.valid && mode_ == SURVEY_IN) { setTimeMode(); } @@ -1436,11 +1497,11 @@ bool HpgRefProduct::setTimeMode() { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if(!gps.configRate(meas_rate, nav_rate)) + if (!gps.configRate(meas_rate, nav_rate)) ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, "navigation rate to ", nav_rate); // Enable the RTCM out messages - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) { ROS_ERROR("Failed to configure RTCM IDs"); return false; } @@ -1453,11 +1514,11 @@ void HpgRefProduct::initializeRosDiagnostics() { } void HpgRefProduct::tmode3Diagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { + diagnostic_updater::DiagnosticStatusWrapper &stat) { if (mode_ == INIT) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Not configured"; - } else if (mode_ == DISABLED){ + } else if (mode_ == DISABLED) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Disabled"; } else if (mode_ == SURVEY_IN) { @@ -1485,10 +1546,10 @@ void HpgRefProduct::tmode3Diagnostics( stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4); stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4); stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4); - } else if(mode_ == FIXED) { + } else if (mode_ == FIXED) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed Position"; - } else if(mode_ == TIME) { + } else if (mode_ == TIME) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time"; } @@ -1500,12 +1561,12 @@ void HpgRefProduct::tmode3Diagnostics( void HpgRovProduct::getRosParams() { // default to float, see CfgDGNSS message for details getRosUint("dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); + ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } bool HpgRovProduct::configureUblox() { // Configure the DGNSS - if(!gps.setDgnss(dgnss_mode_)) + if (!gps.setDgnss(dgnss_mode_)) throw std::runtime_error(std::string("Failed to Configure DGNSS")); return true; } @@ -1514,21 +1575,22 @@ void HpgRovProduct::subscribe() { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe( + boost::bind(&HpgRovProduct::callbackNavRelPosNed, this, _1), + kSubscribeRate); } void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); + freq_rtcm_ = + UbloxTopicDiagnostic(std::string("rxmrtcm"), kRtcmFreqMin, kRtcmFreqMax, + kRtcmFreqTol, kRtcmFreqWindow); updater->add("Carrier Phase Solution", this, - &HpgRovProduct::carrierPhaseDiagnostics); + &HpgRovProduct::carrierPhaseDiagnostics); updater->force_update(); } void HpgRovProduct::carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { + diagnostic_updater::DiagnosticStatusWrapper &stat) { uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; stat.add("iTow", last_rel_pos_.iTow); if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || @@ -1546,12 +1608,12 @@ void HpgRovProduct::carrierPhaseDiagnostics( } stat.add("Ref Station ID", last_rel_pos_.refStationId); - double rel_pos_n = (last_rel_pos_.relPosN - + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.relPosE - + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.relPosD - + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; + double rel_pos_n = + (last_rel_pos_.relPosN + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; + double rel_pos_e = + (last_rel_pos_.relPosE + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; + double rel_pos_d = + (last_rel_pos_.relPosD + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; stat.add("Relative Position N [m]", rel_pos_n); stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4); stat.add("Relative Position E [m]", rel_pos_e); @@ -1579,25 +1641,26 @@ void TimProduct::subscribe() { // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmraw"), + kSubscribeRate); // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + gps.subscribe( + boost::bind(publish, _1, "rxmsfrb"), + kSubscribeRate); } -int main(int argc, char** argv) { +int main(int argc, char **argv) { ros::init(argc, argv, "ublox_gps"); nh.reset(new ros::NodeHandle("~")); - nh->param("debug", ublox_gps::debug, 1); - if(ublox_gps::debug) { + nh->param("debug", ublox_gps::debug, 3); + if (ublox_gps::debug) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - + ros::console::notifyLoggerLevelsChanged(); } UbloxNode node; return 0; diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 3dbf3dab..a7aa399f 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -4,6 +4,8 @@ project(ublox_msgs) find_package(catkin REQUIRED COMPONENTS message_generation ublox_serialization std_msgs sensor_msgs) add_message_files(DIRECTORY msg) +add_message_files(FILES MgaACK.msg) +add_message_files(FILES CfgNAVX5_GET.msg) generate_messages(DEPENDENCIES std_msgs sensor_msgs) catkin_package( diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.h b/ublox_msgs/include/ublox_msgs/ublox_msgs.h index 800da0af..29d8e9af 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.h +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.h @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -103,6 +104,9 @@ #include #include +#include +#include +#include #include @@ -210,13 +214,14 @@ namespace Message { static const uint8_t MSG = CfgMSG::MESSAGE_ID; static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; + static const uint8_t NAVX5_GET = CfgNAVX5_GET::MESSAGE_ID; static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; static const uint8_t PRT = CfgPRT::MESSAGE_ID; static const uint8_t RATE = CfgRATE::MESSAGE_ID; static const uint8_t RST = CfgRST::MESSAGE_ID; static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; - static const uint8_t USB = CfgUSB::MESSAGE_ID; + static const uint8_t USB = CfgUSB::MESSAGE_ID; } namespace UPD { @@ -239,6 +244,9 @@ namespace Message { namespace MGA { static const uint8_t GAL = MgaGAL::MESSAGE_ID; + static const uint8_t INITIMEUTC = MgaINITIMEUTC::MESSAGE_ID; + static const uint8_t INITIMEGNSS = MgaINITIMEGNSS::MESSAGE_ID; + static const uint8_t ACK = MgaACK::MESSAGE_ID; } namespace HNR { diff --git a/ublox_msgs/msg/CfgNAVX5.msg b/ublox_msgs/msg/CfgNAVX5.msg index 9540dc11..c9d06b19 100644 --- a/ublox_msgs/msg/CfgNAVX5.msg +++ b/ublox_msgs/msg/CfgNAVX5.msg @@ -6,7 +6,7 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 35 -uint16 version # Message version (set to 0) +uint16 version # Message version (set to 2) uint16 mask1 # First parameters bitmask (possible values below) uint16 MASK1_MIN_MAX = 4 # apply min/max SVs settings @@ -48,19 +48,22 @@ uint8 sigAttenCompMode # Permanently attenuated signal compensation [dBHz] # 1..63 = maximum expected C/N0 value # Firmware 8 only -uint8[5] reserved4 # Always set to zero +uint8 reserved4 # Always set to zero +uint8[2] reserved5 # Always set to zero +uint8[2] reserved6 # Always set to zero uint8 usePPP # Enable/disable PPP (on supported units) uint8 aopCfg # AssistNow Autonomous configuration, 1: enabled -uint8[2] reserved5 # Always set to zero +uint8[2] reserved7 # Always set to zero uint16 aopOrbMaxErr # Maximum acceptable (modeled) autonomous orbit # error [m] # valid range = 5..1000 # 0 = reset to firmware default -uint8[7] reserved6 # Always set to zero +uint8[4] reserved8 # Always set to zero +uint8[3] reserved9 # Always set to zero uint8 useAdr # Enable/disable ADR sensor fusion # 1: enabled, 0: disabled diff --git a/ublox_msgs/msg/CfgNAVX5_GET.msg b/ublox_msgs/msg/CfgNAVX5_GET.msg new file mode 100644 index 00000000..4b233eea --- /dev/null +++ b/ublox_msgs/msg/CfgNAVX5_GET.msg @@ -0,0 +1,5 @@ +# CFG-NAVX5 (0x06 0x23) +# Navigation Engine Expert Settings GET REQUEST COMMAND + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 35 diff --git a/ublox_msgs/msg/MgaACK.msg b/ublox_msgs/msg/MgaACK.msg new file mode 100644 index 00000000..b24d2e89 --- /dev/null +++ b/ublox_msgs/msg/MgaACK.msg @@ -0,0 +1,31 @@ +# UBX-MGA-ACK-DATA0(0x13 0x60) +# Multiple GNSS Acknowledge message +# +# This message is sent by a u-blox receiver to acknowledge the receipt of an assistance +# message. Acknowledgments are enabled by setting the ackAiding parameter in the +# UBX-CFG-NAVX5 message. See the description of flow control for details. + +uint8 CLASS_ID = 19 +uint8 MESSAGE_ID = 96 +uint8 ACK_OK = 1 + +string INFO_CODE1= 1: The receiver does not know the time so can not use the data (To resolve this a UBX-MGA-INI-TIME_UTC message should be supplied first) +string INFO_CODE2= 2: The message version is not supported by the receiver +string INFO_CODE3= 3: The message size does not match the message version +string INFO_CODE4= 4: The message data could not be stored to the database +string INFO_CODE5= 5: The receiver is not ready to use the message data +string INFO_CODE6= 6: The message type is unknown + + +uint8 type # Type of acknowledgment: 0: The message was not used by the receiver (see infoCode field for an indication of why) 1: The message was accepted for use by the receiver (the infoCode field will be 0) +uint8 version # Message version (0x00 for this version) +uint8 infoCode # Provides greater information on what the receiver chose to do with the message contents: + #0: The receiver accepted the data + #1: The receiver doesn't know the time so can't use the data (To resolve this a UBX-MGA-INI-TIME_UTC message should be supplied first) + #2: The message version is not supported by the receiver + #3: The message size does not match the message version + #4: The message data could not be stored to the database + #5: The receiver is not ready to use the message data + #6: The message type is unknownNumber of leap seconds since 1980 (or 0x80 =-128 if unknown) +uint8 msgId # UBX message ID of the ack'ed message +uint8[4] msgPayloadStart # The first 4 bytes of the ack'ed message's payload diff --git a/ublox_msgs/msg/MgaINITIMEGNSS.msg b/ublox_msgs/msg/MgaINITIMEGNSS.msg new file mode 100644 index 00000000..1a05267f --- /dev/null +++ b/ublox_msgs/msg/MgaINITIMEGNSS.msg @@ -0,0 +1,21 @@ +# MGA-INI-TIME_GNSS (0x13 0x40) +# Initial Time Assistance +# +# This message allows the delivery of time assistance to a receiver in a chosen GNSS +# timebase. This message is equivalent to the UBX-MGA-INI-TIME_UTC message, except +# for the time base. See the description of AssistNow Online for details. + +uint8 CLASS_ID = 19 +uint8 MESSAGE_ID = 64 + +uint8 type # Message type (0x11 for this type) +uint8 version # Message version (0x00 for this version) +uint8 ref # Reference to be used to set time +uint8 gnssId # Source of time information. Currently supported: 0: GPS time 2: Galileo time 3: BeiDou time 6: GLONASS time: week = 834 + ((N4-1)*1461 + Nt)/7, tow = (((N4-1)*1461 + Nt) % 7) * 86400 + tod +uint8[2] reserved1 # reserved1 +uint16 week # GNSS week number +uint32 tow # GNSS time of week +uint32 ns # GNSS time of week, nanosecond part from 0 to 999,999,999 +uint16 tAccS # Seconds part of time accuracy +uint8[2] reserved2 # reserved2 +uint32 tAccNs # Nanoseconds part of time accuracy, from 0 to 999,999,999 \ No newline at end of file diff --git a/ublox_msgs/msg/MgaINITIMEUTC.msg b/ublox_msgs/msg/MgaINITIMEUTC.msg new file mode 100644 index 00000000..2b71b08e --- /dev/null +++ b/ublox_msgs/msg/MgaINITIMEUTC.msg @@ -0,0 +1,25 @@ +# MGA-INI-TIME_UTC(0x13 0x40) +# Initial Time Assistance +# +# This message allows the delivery of UTC time assistance to a receiver. This message is +# equivalent to the UBX-MGA-INI-TIME_GNSS message, except for the time base. See the +# description of AssistNow Online for details. + +uint8 CLASS_ID = 19 +uint8 MESSAGE_ID = 64 + +uint8 type # Message type (0x11 for this type) +uint8 version # Message version (0x10 for this version) +uint8 ref #Reference to be used to set time (see graphic below) +int8 leapSecs # Number of leap seconds since 1980 (or 0x80 =-128 if unknown) +uint16 year # Year +uint8 month # Month, starting at 1 +uint8 day # Day, starting at 1 +uint8 hour # Hour, from 0 to 23 +uint8 minute # Minute, from 0 to 59 +uint8 second # Seconds, from 0 to 59 +uint8 reserved1 # reserved1 +uint32 ns # Nanoseconds, from 0 to 999,999,999 +uint16 tAccS # Seconds part of time accuracy +uint8[2] reserved2 # reserved2 +uint32 tAccNs # Nanoseconds part of time accuracy, from 0 to 999,999,999 diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index b012c2fa..a14b4933 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -14,9 +14,9 @@ // endorse or promote products derived from this software without // specific prior written permission. -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -29,163 +29,167 @@ #include template -std::vector > ublox::Message::keys_; +std::vector > ublox::Message::keys_; -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, ublox_msgs, NavATT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, ublox_msgs, NavCLOCK); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, ublox_msgs, NavDGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, ublox_msgs, NavDOP); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, ublox_msgs, NavPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, ublox_msgs, NavPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED, - ublox_msgs, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, + ublox_msgs::Message::NAV::RELPOSNED, ublox_msgs, NavRELPOSNED); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, ublox_msgs, NavSBAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, ublox_msgs, NavSOL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, ublox_msgs, NavSAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, ublox_msgs, NavSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, ublox_msgs, NavSVIN); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, ublox_msgs, NavSVINFO); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, ublox_msgs, NavTIMEGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, ublox_msgs, NavTIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, ublox_msgs, NavVELECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, ublox_msgs, NavVELNED); -// ACK messages are declared differently because they both have the same +// ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, ublox_msgs, Ack); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, - ublox_msgs, Ack, ACK); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, + ublox_msgs, Ack, ACK); -// INF messages are declared differently because they all have the same +// INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, ublox_msgs, Inf); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::WARNING, - ublox_msgs, Inf, WARNING); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::NOTICE, - ublox_msgs, Inf, NOTICE); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::TEST, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::WARNING, ublox_msgs, Inf, + WARNING); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::NOTICE, ublox_msgs, Inf, + NOTICE); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::TEST, ublox_msgs, Inf, TEST); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::DEBUG, - ublox_msgs, Inf, DEBUG); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::DEBUG, ublox_msgs, Inf, + DEBUG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, ublox_msgs, RxmALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, ublox_msgs, RxmEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, ublox_msgs, RxmRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, ublox_msgs, RxmRAWX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, ublox_msgs, RxmRTCM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, ublox_msgs, RxmSFRB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, ublox_msgs, RxmSFRBX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, ublox_msgs, RxmSVSI); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, ublox_msgs, CfgANT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, ublox_msgs, CfgCFG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, ublox_msgs, CfgDAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, ublox_msgs, CfgDGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, ublox_msgs, CfgGNSS); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, ublox_msgs, CfgHNR); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, ublox_msgs, CfgINF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, ublox_msgs, CfgMSG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, ublox_msgs, CfgNAV5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, ublox_msgs, CfgNAVX5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5_GET, + ublox_msgs, CfgNAVX5_GET); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, ublox_msgs, CfgPRT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, ublox_msgs, CfgRATE); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, ublox_msgs, CfgRST); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, ublox_msgs, CfgTMODE3); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, ublox_msgs, CfgUSB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS); // SOS and SOS_Ack have the same message ID, but different lengths -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS_Ack); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, ublox_msgs, MonGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, ublox_msgs, MonVER); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, ublox_msgs, AidALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, ublox_msgs, AidEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, ublox_msgs, AidHUI); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, ublox_msgs, EsfINS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, ublox_msgs, EsfMEAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, ublox_msgs, EsfRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, ublox_msgs, EsfSTATUS); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, ublox_msgs, MgaGAL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::ACK, + ublox_msgs, MgaACK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, + ublox_msgs::Message::MGA::INITIMEUTC, ublox_msgs, + MgaINITIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, ublox_msgs, HnrPVT);