From f9396806c2e6febe7d55c1ba8adc9ab376eb4c44 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:09:23 +0300 Subject: [PATCH 1/4] fix ADR/UDR product detection --- ublox_gps/include/ublox_gps/adr_udr_product.hpp | 5 +++-- ublox_gps/src/adr_udr_product.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 4952a7c8..b00022df 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -28,7 +28,7 @@ namespace ublox_node { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); + explicit AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -64,6 +64,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning bool use_adr_; + float protocol_version_; sensor_msgs::msg::Imu imu_; sensor_msgs::msg::TimeReference t_ref_; @@ -89,4 +90,4 @@ class AdrUdrProduct final : public virtual ComponentInterface { } // namespace ublox_node -#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP +#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP \ No newline at end of file diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 0035b20c..7713ba7c 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -25,8 +25,8 @@ namespace ublox_node { // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) - : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) +AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) + : protocol_version_(protocol_version) ,use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { if (getRosBoolean(node_, "publish.esf.meas")) { imu_pub_ = @@ -63,7 +63,7 @@ void AdrUdrProduct::getRosParams() { } bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { - if (!gps->setUseAdr(use_adr_)) { + if (!gps->setUseAdr(use_adr_, protocol_version_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); } From 4542ddb1083b623af292c75053d91d9c71858671 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:09:59 +0300 Subject: [PATCH 2/4] fix: protocol version --- ublox_gps/include/ublox_gps/gps.hpp | 4 ++-- ublox_gps/src/gps.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 3f3f7a93..431d03df 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -300,7 +300,7 @@ class Gps final { * @note This is part of the expert settings. It is recommended you check * the ublox manual first. */ - bool setPpp(bool enable); + bool setPpp(bool enable, float protocol_version); /** * @brief Set the DGNSS mode (see CfgDGNSS message for details). @@ -314,7 +314,7 @@ class Gps final { * @param enable If true, enable ADR. * @return true on ACK, false on other conditions. */ - bool setUseAdr(bool enable); + bool setUseAdr(bool enable, float protocol_version); /** * @brief Configure the U-Blox to UTC time diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index ad404d01..87b1e078 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -555,11 +555,14 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { return configure(msg); } -bool Gps::setPpp(bool enable) { +bool Gps::setPpp(bool enable, float protocol_version) { RCLCPP_DEBUG(logger_,"%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_ppp = enable; + if(protocol_version >= 18){ + msg.version = 2; + } msg.mask1 = ublox_msgs::msg::CfgNAVX5::MASK1_PPP; return configure(msg); } @@ -571,11 +574,14 @@ bool Gps::setDgnss(uint8_t mode) { return configure(cfg); } -bool Gps::setUseAdr(bool enable) { +bool Gps::setUseAdr(bool enable, float protocol_version) { RCLCPP_DEBUG(logger_, "%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_adr = enable; + if(protocol_version >= 18){ + msg.version = 2; + } msg.mask2 = ublox_msgs::msg::CfgNAVX5::MASK2_ADR; return configure(msg); } From 95aa8185c6b3c6cc5869fdd257087dc65c47a679 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:10:20 +0300 Subject: [PATCH 3/4] fix: for protocol version >= 18 --- ublox_gps/src/node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..22e29721 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -229,11 +229,11 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(frame_id_, updater_, this)); } else if (product_category == "ADR" || product_category == "UDR") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); } else if (product_category == "FTS") { components_.push_back(std::make_shared()); } else if (product_category == "HPS") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); components_.push_back(std::make_shared(nav_rate_, updater_, this)); } else { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", @@ -772,7 +772,7 @@ bool UbloxNode::configureUblox() { " SBAS."); } } - if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"))) { + if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"), protocol_version_)) { throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") + " PPP."); From 83f26d550a9ebbefc22e01c2056c5178454a3904 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:10:59 +0300 Subject: [PATCH 4/4] fix: neo_m8u_rover.yaml type issues --- ublox_gps/config/neo_m8u_rover.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ublox_gps/config/neo_m8u_rover.yaml b/ublox_gps/config/neo_m8u_rover.yaml index dc364feb..351b5d68 100644 --- a/ublox_gps/config/neo_m8u_rover.yaml +++ b/ublox_gps/config/neo_m8u_rover.yaml @@ -11,7 +11,7 @@ ublox_gps_node: device: /dev/ttyACM0 frame_id: m8u - rate: 4 # in Hz + rate: 4.0 # in Hz nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may # be either 5 Hz (Dual constellation) or # 8 Hz (GPS only) @@ -19,7 +19,7 @@ ublox_gps_node: # Max Alt: 50km # Max Horizontal Velocity: 250 m/s, # Max Vertical Velocity: 100 m/s - fix_mode: 3 + fix_mode: 3d enable_ppp: true dr_limit: 1