Add support for protection level messages (KumarRobotics#247)

gennartan · Aug 30, 2024 · 7668505 · 7668505
1 parent 577ef65
commit 7668505
Show file tree

Hide file tree

Showing 8 changed files with 130 additions and 2 deletions.
diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp
@@ -9,6 +9,7 @@
 
 #include <ublox_msgs/msg/cfg_valset.hpp>
 #include <ublox_msgs/msg/cfg_valset_cfgdata.hpp>
+#include <ublox_msgs/msg/nav_pl.hpp>
 
 #include <ublox_gps/fix_diagnostic.hpp>
 #include <ublox_gps/gnss.hpp>
@@ -34,13 +35,24 @@ class UbloxFirmware9 final : public UbloxFirmware8 {
     */
   bool configureUblox(std::shared_ptr<ublox_gps::Gps> gps) override;
 
+  /**
+   * @brief Subscribe to u-blox messages which are not generic to all firmware
+   * versions.
+   *
+   * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based
+   * on user settings.
+   */
+  void subscribe(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. 
+    * @details A helper function used to generate a configuration for a single signal.
     */
   ublox_msgs::msg::CfgVALSETCfgdata generateSignalConfig(uint32_t signalID, bool enable);
+
+  rclcpp::Publisher<ublox_msgs::msg::NavPL>::SharedPtr nav_pl_pub_;
 };
 
 }  // namespace ublox_node

diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp
@@ -425,6 +425,8 @@ void UbloxNode::getRosParams() {
   this->declare_parameter("publish.nav.svinfo", getRosBoolean(this, "publish.nav.all"));
   this->declare_parameter("publish.nav.status", getRosBoolean(this, "publish.nav.all"));
   this->declare_parameter("publish.nav.velned", getRosBoolean(this, "publish.nav.all"));
+  this->declare_parameter("publish.nav.pl", getRosBoolean(this, "publish.nav.all"));
+
 
   this->declare_parameter("publish.rxm.all", getRosBoolean(this, "publish.all"));
   this->declare_parameter("publish.rxm.almRaw", getRosBoolean(this, "publish.rxm.all"));

diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp
@@ -15,6 +15,9 @@ namespace ublox_node {
 UbloxFirmware9::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)
   : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node)
 {
+  if (getRosBoolean(node_, "publish.nav.pl")) {
+    nav_pl_pub_ = node_->create_publisher<ublox_msgs::msg::NavPL>("navpl", 1);
+  }
 }
 
 bool UbloxFirmware9::configureUblox(std::shared_ptr<ublox_gps::Gps> gps)
@@ -109,6 +112,17 @@ bool UbloxFirmware9::configureUblox(std::shared_ptr<ublox_gps::Gps> gps)
   return true;
 }
 
+void UbloxFirmware9::subscribe(std::shared_ptr<ublox_gps::Gps> gps)
+{
+  UbloxFirmware8::subscribe(gps);
+
+  // Subscribe to Nav PL
+  if (getRosBoolean(node_, "publish.nav.pl")) {
+    gps->subscribe<ublox_msgs::msg::NavPL>([this](const ublox_msgs::msg::NavPL &m) { nav_pl_pub_->publish(m); },
+                                        1);
+  }
+}
+
 ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t signalID, bool enable)
 {
   ublox_msgs::msg::CfgVALSETCfgdata signalConfig;

diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt
@@ -85,6 +85,7 @@ set(msg_files
   "msg/NavTIMEUTC.msg"
   "msg/NavVELECEF.msg"
   "msg/NavVELNED.msg"
+  "msg/NavPL.msg"
   "msg/RxmALM.msg"
   "msg/RxmEPH.msg"
   "msg/RxmRAW.msg"

diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp
@@ -2500,6 +2500,68 @@ struct UbloxSerializer<ublox_msgs::msg::NavVELNED_<ContainerAllocator> > {
   }
 };
 
+template <typename ContainerAllocator>
+struct UbloxSerializer<ublox_msgs::msg::NavPL_<ContainerAllocator>>
+{
+  inline static void read(const uint8_t *data, uint32_t count,
+                          ublox_msgs::msg::NavPL_<ContainerAllocator> &m)
+  {
+    UbloxIStream stream(const_cast<uint8_t *>(data), count);
+    stream.next(m.msg_version);
+    stream.next(m.tmir);
+    stream.next(m.tmir_exp);
+    stream.next(m.pl_pos_valid);
+    stream.next(m.pl_pos_frame);
+    stream.next(m.pl_vel_valid);
+    stream.next(m.pl_vel_frame);
+    stream.next(m.pl_time_valid);
+    stream.next(m.reserved0);
+    stream.next(m.i_tow);
+    stream.next(m.pl_pos1);
+    stream.next(m.pl_pos2);
+    stream.next(m.pl_pos3);
+    stream.next(m.pl_vel1);
+    stream.next(m.pl_vel2);
+    stream.next(m.pl_vel3);
+    stream.next(m.pl_pos_horiz_orientation);
+    stream.next(m.pl_vel_horiz_orientation);
+    stream.next(m.pl_time);
+    stream.next(m.reserved1);
+  }
+
+  inline static uint32_t serializedLength(const ublox_msgs::msg::NavPL_<ContainerAllocator> &m)
+  {
+    (void)m;
+    return 52;
+  }
+
+  inline static void write(uint8_t *data, uint32_t size,
+                           const ublox_msgs::msg::NavPL_<ContainerAllocator> &m)
+  {
+    UbloxOStream stream(data, size);
+    stream.next(m.msg_version);
+    stream.next(m.tmir);
+    stream.next(m.tmir_exp);
+    stream.next(m.pl_pos_valid);
+    stream.next(m.pl_pos_frame);
+    stream.next(m.pl_vel_valid);
+    stream.next(m.pl_vel_frame);
+    stream.next(m.pl_time_valid);
+    stream.next(m.reserved0);
+    stream.next(m.i_tow);
+    stream.next(m.pl_pos1);
+    stream.next(m.pl_pos2);
+    stream.next(m.pl_pos3);
+    stream.next(m.pl_vel1);
+    stream.next(m.pl_vel2);
+    stream.next(m.pl_vel3);
+    stream.next(m.pl_pos_horiz_orientation);
+    stream.next(m.pl_vel_horiz_orientation);
+    stream.next(m.pl_time);
+    stream.next(m.reserved1);
+  }
+};
+
 template <typename ContainerAllocator>
 struct UbloxSerializer<ublox_msgs::msg::NavTIMEGPS_<ContainerAllocator> > {
   inline static void read(const uint8_t *data, uint32_t count,
@@ -2529,7 +2591,7 @@ struct UbloxSerializer<ublox_msgs::msg::NavTIMEGPS_<ContainerAllocator> > {
     stream.next(m.t_acc);
   }
 };
-  
+
 ///
 /// @brief Serializes the RxmALM message which has a repeated block.
 ///

diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp
@@ -50,6 +50,7 @@
 #include <ublox_msgs/msg/nav_timeutc.hpp>
 #include <ublox_msgs/msg/nav_velecef.hpp>
 #include <ublox_msgs/msg/nav_velned.hpp>
+#include <ublox_msgs/msg/nav_pl.hpp>
 
 #include <ublox_msgs/msg/rxm_alm.hpp>
 #include <ublox_msgs/msg/rxm_eph.hpp>
@@ -177,6 +178,7 @@ namespace Message {
     static const uint8_t TIMEUTC = ublox_msgs::msg::NavTIMEUTC::MESSAGE_ID;
     static const uint8_t VELECEF = ublox_msgs::msg::NavVELECEF::MESSAGE_ID;
     static const uint8_t VELNED = ublox_msgs::msg::NavVELNED::MESSAGE_ID;
+    static const uint8_t PL = ublox_msgs::msg::NavPL::MESSAGE_ID;
   }  // namespace NAV
 
   namespace RXM {

diff --git a/ublox_msgs/msg/NavPL.msg b/ublox_msgs/msg/NavPL.msg
@@ -0,0 +1,33 @@
+# NAV_PL (0x01 0x62)
+# Navigation protection level
+
+uint8 CLASS_ID = 1
+uint8 MESSAGE_ID = 0x62
+
+uint8 msg_version
+uint8 tmir # Target misleading information risk [%MI/epoch]
+int8 tmir_exp
+
+uint8 pl_pos_valid
+uint8 pl_pos_frame
+uint8 pl_vel_valid
+uint8 pl_vel_frame
+uint8 pl_time_valid
+
+uint32 reserved0
+
+uint32 i_tow # GPS Millisecond time of week [ms]
+
+uint32 pl_pos1 # [mm]
+uint32 pl_pos2 # [mm]
+uint32 pl_pos3 # [mm]
+
+uint32 pl_vel1 # [mm/s]
+uint32 pl_vel2 # [mm/s]
+uint32 pl_vel3 # [mm/s]
+
+uint16 pl_pos_horiz_orientation # [deg - clockwise from true North]
+uint16 pl_vel_horiz_orientation # [deg - clockwise from true North]
+
+uint32 pl_time # [ns]
+uint32 reserved1
diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp
@@ -80,6 +80,8 @@ 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,
                       ublox_msgs, NavVELNED)
+DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PL,
+                      ublox_msgs, NavPL)
 
 // ACK messages are declared differently because they both have the same
 // protocol, so only 1 ROS message is used