From f829b6527efe3c60710503f57b600a8e76ebdde3 Mon Sep 17 00:00:00 2001 From: Balachandra Date: Mon, 2 Sep 2024 17:47:30 +0200 Subject: [PATCH] Add NAV-TIMEGPS and NAV-TIMEUTC implementation to Firmvare Ver. 9 --- README.md | 2 + .../include/ublox_gps/ublox_firmware9.hpp | 9 +++++ ublox_gps/src/node.cpp | 2 + ublox_gps/src/ublox_firmware9.cpp | 25 ++++++++++++ .../include/ublox_msgs/serialization.hpp | 38 +++++++++++++++++++ 5 files changed, 76 insertions(+) diff --git a/README.md b/README.md index 15abbe93..65bce136 100644 --- a/README.md +++ b/README.md @@ -180,6 +180,8 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below * `publish/nav/status`: Topic `~navstatus` * `publish/nav/svin`: Topic `~navsvin`. **HPG Reference Station Devices only** * `publish/nav/svinfo`: Topic `~navsvinfo` +* `publish/nav/timegps`: Topic `~navtimegps` +* `publish/nav/timeutc`: Topic `~navtimeutc` * `publish/nav/velned`: Topic `~navvelned`. **Firmware <= 6 only.** For firmware 7 and above, see NavPVT ### ESF messages diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp index 9e0a4d2b..f69b10f7 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include @@ -34,6 +36,11 @@ class UbloxFirmware9 final : public UbloxFirmware8 { */ bool configureUblox(std::shared_ptr gps) override; + /** + * @brief Subscribe to NAV-TIMEUTC, NAV-TIMEGPS messages. + */ + void subscribe(std::shared_ptr gps) override; + private: /** * @brief Populate the CfgVALSETCfgData data type @@ -41,6 +48,8 @@ class UbloxFirmware9 final : public UbloxFirmware8 { * @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_; }; } // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..db565323 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -424,6 +424,8 @@ void UbloxNode::getRosParams() { this->declare_parameter("publish.nav.svin", getRosBoolean(this, "publish.nav.all")); 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.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.rxm.all", getRosBoolean(this, "publish.all")); diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp index 1eb3ddf4..1a51a021 100644 --- a/ublox_gps/src/ublox_firmware9.cpp +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -15,6 +15,14 @@ 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.timegps")) + { + nav_timegps_pub_ = node->create_publisher("navtimegps", 1); + } + if (getRosBoolean(node_, "publish.nav.timeutc")) + { + nav_timeutc_pub_ = node->create_publisher("navtimeutc", 1); + } } bool UbloxFirmware9::configureUblox(std::shared_ptr gps) @@ -118,5 +126,22 @@ ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t return signalConfig; } +void UbloxFirmware9::subscribe(std::shared_ptr gps) +{ + UbloxFirmware8::subscribe(gps); + + // Subscribe to NAV TIMEGPS messages + if (getRosBoolean(node_, "publish.nav.timegps")) { + gps->subscribe([this](const ublox_msgs::msg::NavTIMEGPS &m) { + nav_timegps_pub_->publish(m); }, 1); + } + + // Subscribe to NAV TIMEUTC messages + if (getRosBoolean(node_, "publish.nav.timeutc")) { + gps->subscribe([this](const ublox_msgs::msg::NavTIMEUTC &m) { + nav_timeutc_pub_->publish(m); }, 1); + } +} + } // namespace ublox_node diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 14418e39..2ce349e4 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -2529,6 +2529,44 @@ struct UbloxSerializer > { stream.next(m.t_acc); } }; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavTIMEUTC_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavTIMEUTC_ & m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavTIMEUTC_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + } +}; /// /// @brief Serializes the RxmALM message which has a repeated block.