fix ADR/UDR product detection

Author: Muhammad540

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

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");
   }