Merge pull request #256 from Muhammad540/ros2

fixed CfgNAVX5 message generation for protocol version >= 18
KumarRobotics · Dec 4, 2024 · 594c999 · 594c999
2 parents 577ef65 + 83f26d5
commit 594c999
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 14 deletions.
diff --git a/ublox_gps/config/neo_m8u_rover.yaml b/ublox_gps/config/neo_m8u_rover.yaml
@@ -11,15 +11,15 @@ 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)
     dynamic_model: airborne2    # Airborne < 2G, 2D fix not supported (3D only),
                                 # 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
 

diff --git 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<diagnostic_updater::Updater> 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<diagnostic_updater::Updater> 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
diff --git 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/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<diagnostic_updater::Updater> 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<diagnostic_updater::Updater> 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<ublox_gps::Gps> 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");
   }

diff --git 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);
 }

diff --git 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<TimProduct>(frame_id_, updater_, this));
   } else if (product_category == "ADR" ||
              product_category == "UDR") {
-    components_.push_back(std::make_shared<AdrUdrProduct>(nav_rate_, meas_rate_, frame_id_, updater_, this));
+    components_.push_back(std::make_shared<AdrUdrProduct>(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this));
   } else if (product_category == "FTS") {
     components_.push_back(std::make_shared<FtsProduct>());
   } else if (product_category == "HPS") {
-    components_.push_back(std::make_shared<AdrUdrProduct>(nav_rate_, meas_rate_, frame_id_, updater_, this));
+    components_.push_back(std::make_shared<AdrUdrProduct>(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this));
     components_.push_back(std::make_shared<HpgRovProduct>(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.");