Added Firmware v9 GNSS configuration

ublox_gps/include/ublox_gps/ublox_firmware8.hpp

@@ -61,7 +61,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::msg::NavPVT> {
    */
   void subscribe(std::shared_ptr<ublox_gps::Gps> gps) override;
 
- private:
+protected:
   // Set from ROS parameters
   //! Whether or not to enable the Galileo GNSS
   bool enable_galileo_{false};
@@ -75,6 +75,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::msg::NavPVT> {
   bool clear_bbr_{false};
   bool save_on_shutdown_{false};
 
+private:
   rclcpp::Publisher<ublox_msgs::msg::NavSAT>::SharedPtr nav_sat_pub_;
   rclcpp::Publisher<ublox_msgs::msg::MonHW>::SharedPtr mon_hw_pub_;
   rclcpp::Publisher<ublox_msgs::msg::RxmRTCM>::SharedPtr rxm_rtcm_pub_;

ublox_gps/include/ublox_gps/ublox_firmware9.hpp

@@ -7,6 +7,9 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <ublox_msgs/msg/cfg_valset.hpp>
+#include <ublox_msgs/msg/cfg_valset_cfgdata.hpp>
+
 #include <ublox_gps/fix_diagnostic.hpp>
 #include <ublox_gps/gnss.hpp>
 #include <ublox_gps/ublox_firmware8.hpp>
@@ -15,12 +18,29 @@ namespace ublox_node {
 
 /**
  *  @brief Implements functions for firmware version 9.
- *  For now it simply re-uses the firmware version 8 class
- *  but allows for future expansion of functionality
  */
 class UbloxFirmware9 final : public UbloxFirmware8 {
 public:
   explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr<diagnostic_updater::Updater> updater, std::shared_ptr<FixDiagnostic> freq_diag, std::shared_ptr<Gnss> gnss, rclcpp::Node* node);
+
+  /**
+    * @brief Configure settings specific to firmware 9 based on ROS parameters.
+    *
+    * @details Configure GNSS.  The hardware has internal logic for
+    * detecting differences between the new and active GNSS
+    * configuration and will internally trigger a reset if necessary.
+    * Configure the NMEA if desired by the user. It also may clear the
+    * flash memory based on the ROS parameters.
+    */
+  bool configureUblox(std::shared_ptr<ublox_gps::Gps> gps) override;
+
+private:
+  /**
+    * @brief Populate the CfgVALSETCfgData data type
+    *
+    * @details A helper function used to generate a configuration for a single signal. 
+    */
+  ublox_msgs::msg::CfgVALSETCfgdata generateSignalConfig(uint32_t signalID, bool enable);
 };
 
 }  // namespace ublox_node

ublox_gps/src/node.cpp

@@ -701,6 +701,23 @@ void UbloxNode::processMonVer() {
             }
             continue;
           }
+          // u-blox F9 modules support additional positioning signals
+          else if (strs[0] == "MOD")
+          {
+            std::vector<std::string> moduleField;
+            moduleField = stringSplit(strs[1], "-");
+            if (moduleField.size() > 1)
+            {
+              if (moduleField[1].substr(0,2) == "F9")
+              {
+                gnss_->add("GPS_L2C");
+                gnss_->add("GAL_E5B");
+                gnss_->add("BDS_B2");
+                gnss_->add("QZSS_L2C");
+                gnss_->add("GLO_L2");
+              }
+            }
+          }
         }
       }
       // Last 1-2 lines contain supported GNSS

ublox_gps/src/ublox_firmware9.cpp

@@ -1,5 +1,6 @@
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -16,4 +17,106 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr<dia
 {
 }
 
+bool UbloxFirmware9::configureUblox(std::shared_ptr<ublox_gps::Gps> gps)
+{
+  if (clear_bbr_)
+  {
+    // clear flash memory
+    if (!gps->clearBbr())
+    {
+      RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory");
+    }
+  }
+
+  gps->setSaveOnShutdown(save_on_shutdown_);
+
+  //
+  // Configure the GNSS, only if the configuration is different
+  //
+  // First, get the current GNSS configuration
+  ublox_msgs::msg::CfgGNSS cfg_gnss;
+  if (gps->poll(cfg_gnss))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config.");
+    RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw);
+    RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use);
+  }
+  else
+  {
+    throw std::runtime_error("Failed to read the GNSS config.");
+  }
+
+  ublox_msgs::msg::CfgVALSET cfg_signal;
+  cfg_signal.layers = ublox_msgs::msg::CfgVALSET::LAYER_RAM;
+
+  using signal = ublox_msgs::msg::CfgVALSETCfgdata;
+
+  // Configure GPS Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_ENABLE, enable_gps_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L1CA_ENABLE, enable_gps_));
+  if (gnss_->isSupported("GPS_L2C"))
+  {
+    cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L2C_ENABLE, enable_gps_));
+  }
+
+  // Configure SBAS Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_ENABLE, enable_gps_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_L1CA_ENABLE, enable_gps_));
+
+  // Configure Galileo Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_ENABLE, enable_galileo_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E1_ENABLE, enable_galileo_));
+  if (gnss_->isSupported("GAL_E5B"))
+  {
+    cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E5B_ENABLE, enable_galileo_));
+  }
+
+  // Configure Beidou Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_ENABLE, enable_beidou_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B1_ENABLE, enable_beidou_));
+  if (gnss_->isSupported("BDS_B2"))
+  {
+    cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B2_ENABLE, enable_beidou_));
+  }
+
+  // Configure QZSS Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_ENABLE, enable_qzss_ && enable_gps_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L1CA_ENABLE, enable_qzss_ && enable_gps_));
+  if (gnss_->isSupported("QZSS_L2C"))
+  {
+    cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L2C_ENABLE, enable_qzss_ && enable_gps_));
+  }
+
+  // Configure GLONASS Signals
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_ENABLE, enable_glonass_));
+  cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L1_ENABLE, enable_glonass_));
+  if (gnss_->isSupported("GLO_L2"))
+  {
+    cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L2_ENABLE, enable_glonass_));
+  }
+
+  RCLCPP_DEBUG(node_->get_logger(), "Ready to configure");
+  gps->configure(cfg_signal, false);
+
+  //
+  // NMEA config
+  //
+  if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_))
+  {
+    throw std::runtime_error("Failed to configure NMEA");
+  }
+
+  return true;
+}
+
+ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t signalID, bool enable)
+{
+  ublox_msgs::msg::CfgVALSETCfgdata signalConfig;
+  signalConfig.key = signalID;
+  signalConfig.data.resize(1);
+  signalConfig.data[0] = enable;
+  return signalConfig;
+}
+
+
 }  // namespace ublox_node

ublox_msgs/msg/CfgVALSETCfgdata.msg

@@ -1,3 +1,28 @@
+# CFG-SIGNAL Keys
+uint32 GPS_ENABLE =         271646751   # 0x1031001f
+uint32 GPS_L1CA_ENABLE =    271646721   # 0x10310001
+uint32 GPS_L2C_ENABLE =     271646723   # 0x10310003
+
+uint32 SBAS_ENABLE =        271646752   # 0x10310020
+uint32 SBAS_L1CA_ENABLE =   271646725   # 0x10310005
+
+uint32 GAL_ENABLE =         271646753   # 0x10310021
+uint32 GAL_E1_ENABLE =      271646727   # 0x10310007
+uint32 GAL_E5B_ENABLE =     271646730   # 0x1031000a
+
+uint32 BDS_ENABLE =         271646754   # 0x10310022
+uint32 BDS_B1_ENABLE =      271646733   # 0x1031000d
+uint32 BDS_B2_ENABLE =      271646734   # 0x1031000e
+
+uint32 QZSS_ENABLE =        271646756   # 0x10310024
+uint32 QZSS_L1CA_ENABLE =   271646738   # 0x10310012
+uint32 QZSS_L1S_ENABLE =    271646740   # 0x10310014
+uint32 QZSS_L2C_ENABLE =    271646741   # 0x10310015
+
+uint32 GLO_ENABLE =         271646757   # 0x10310025
+uint32 GLO_L1_ENABLE =      271646744   # 0x10310018
+uint32 GLO_L2_ENABLE =      271646746   # 0x1031001a
+
 # See Cfg-VALSET
 #