diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp index 9e0a4d2b..9d0646d4 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -34,13 +35,24 @@ class UbloxFirmware9 final : public UbloxFirmware8 { */ bool configureUblox(std::shared_ptr 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 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::SharedPtr nav_pl_pub_; }; } // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..4117c6b1 100644 --- 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 index 1eb3ddf4..cab4990c 100644 --- 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 updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) { + if (getRosBoolean(node_, "publish.nav.pl")) { + nav_pl_pub_ = node_->create_publisher("navpl", 1); + } } bool UbloxFirmware9::configureUblox(std::shared_ptr gps) @@ -109,6 +112,17 @@ bool UbloxFirmware9::configureUblox(std::shared_ptr gps) return true; } +void UbloxFirmware9::subscribe(std::shared_ptr gps) +{ + UbloxFirmware8::subscribe(gps); + + // 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); + } +} + 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 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 14418e39..f8d06955 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, @@ -2529,7 +2591,7 @@ struct UbloxSerializer > { 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 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