diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp index f69b10f7..c9a46820 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -45,11 +46,12 @@ class UbloxFirmware9 final : public UbloxFirmware8 { /** * @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::SharedPtr nav_timegps_pub_; rclcpp::Publisher::SharedPtr nav_timeutc_pub_; + rclcpp::Publisher::SharedPtr nav_pl_pub_; }; } // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 39c14821..83bf79d0 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -427,6 +427,8 @@ void UbloxNode::getRosParams() { this->declare_parameter("publish.nav.timegps", getRosBoolean(this, "publish.nav.all")); this->declare_parameter("publish.nav.timeutc", 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 index 1a51a021..f57697ae 100644 --- a/ublox_gps/src/ublox_firmware9.cpp +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -23,6 +23,10 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptrcreate_publisher("navtimeutc", 1); } + if (getRosBoolean(node_, "publish.nav.pl")) + { + nav_pl_pub_ = node_->create_publisher("navpl", 1); + } } bool UbloxFirmware9::configureUblox(std::shared_ptr gps) @@ -127,7 +131,7 @@ ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t } void UbloxFirmware9::subscribe(std::shared_ptr gps) -{ +{ UbloxFirmware8::subscribe(gps); // Subscribe to NAV TIMEGPS messages @@ -141,6 +145,12 @@ void UbloxFirmware9::subscribe(std::shared_ptr gps) gps->subscribe([this](const ublox_msgs::msg::NavTIMEUTC &m) { nav_timeutc_pub_->publish(m); }, 1); } + + // Subscribe to Nav PL + if (getRosBoolean(node_, "publish.nav.pl")) { + gps->subscribe([this](const ublox_msgs::msg::NavPL &m) { + nav_pl_pub_->publish(m); }, 1); + } } diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 4e3db305..d762fdd2 100644 --- 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 index 2ce349e4..99dece01 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -2500,6 +2500,68 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer> +{ + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavPL_ &m) + { + UbloxIStream stream(const_cast(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_ &m) + { + (void)m; + return 52; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavPL_ &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 struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, @@ -2567,7 +2629,7 @@ struct UbloxSerializer > { stream.next(m.valid); } }; - + /// /// @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 index b1ac7155..f00280d3 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -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 new file mode 100644 index 00000000..9229b7b4 --- /dev/null +++ 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 \ No newline at end of file diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index ac905307..e70b88eb 100644 --- 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