From 08666141b362842bd4d7777ca769b004987c16e1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Apr 2020 20:13:39 +0000 Subject: [PATCH 1/4] Don't wait for acknowledge in setTimeMode. The basic problem is that setTimeMode can be called from within a GPS object callback (particularly when running as a "base"). Trying to reconfigure the Rate and RTCM while in the callback leads to a deadlock, as those reconfigure calls try to wait for data to come in, but new data will never come in since we are still handling the previous read. The solution here is to not wait for acknowledgement and just go on, hoping for the best. It's not an ideal solution, but it solves the problem for now. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 14 ++++++++------ ublox_gps/src/gps.cpp | 14 +++++++------- ublox_gps/src/hpg_ref_product.cpp | 15 +++++++++++---- ublox_gps/src/node.cpp | 2 +- 4 files changed, 27 insertions(+), 18 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 003aa31b..5b4cf77e 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -191,18 +191,19 @@ class Gps final { * @brief Configure the device navigation and measurement rate settings. * @param meas_rate Period in milliseconds between subsequent measurements. * @param nav_rate the rate at which navigation solutions are generated by the + * @param wait Whether to wait for acknowledgement * receiver in number measurement cycles * @return true on ACK, false on other conditions. */ - bool configRate(uint16_t meas_rate, uint16_t nav_rate); + bool configRate(uint16_t meas_rate, uint16_t nav_rate, bool wait); /** * @brief Configure the RTCM messages with the given IDs to the set rate. - * @param ids the RTCM message ids, valid range: [0, 255] - * @param rates the send rates for each RTCM message ID, valid range: [0, 255] + * @param rtcms the RTCM message structures containing ids and rates + * @param wait Whether to wait for acknowledgement * @return true on ACK, false on other conditions. */ - bool configRtcm(const std::vector & rtcms); + bool configRtcm(const std::vector & rtcms, bool wait); /** * @brief Configure the SBAS settings. @@ -253,9 +254,10 @@ class Gps final { * @param class_id the class identifier of the message * @param message_id the message identifier * @param rate the updated rate in Hz + * @param wait Whether to wait for acknowledgement * @return true on ACK, false on other conditions. */ - bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate); + bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate, bool wait); /** * @brief Set the device dynamic model. @@ -493,7 +495,7 @@ class Gps final { template void Gps::subscribe( typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) { + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate, true)) { return; } subscribe(callback); diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 3d63f43d..a8e97ac9 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -270,7 +270,7 @@ void Gps::reset(const std::chrono::milliseconds& wait) { configured_ = false; // sleep because of undefined behavior after I/O reset std::this_thread::sleep_for(wait); - if (host_ == "") { + if (host_.empty()) { resetSerial(port_); } else { initializeTcp(host_, port_); @@ -392,7 +392,7 @@ bool Gps::configUsb(uint16_t tx_ready, return configure(port); } -bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { +bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate, bool wait) { RCLCPP_DEBUG(logger_, "Configuring measurement rate to %u ms and nav rate to %u cycles", meas_rate, nav_rate); @@ -400,13 +400,13 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { rate.meas_rate = meas_rate; rate.nav_rate = nav_rate; // must be fixed at 1 for ublox 5 and 6 rate.time_ref = ublox_msgs::msg::CfgRATE::TIME_REF_GPS; - return configure(rate); + return configure(rate, wait); } -bool Gps::configRtcm(const std::vector & rtcms) { +bool Gps::configRtcm(const std::vector & rtcms, bool wait) { for (const Rtcm & rtcm : rtcms) { RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcm.id, rtcm.rate); - if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate)) { + if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate, wait)) { RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcm.id, rtcm.rate); return false; } @@ -479,14 +479,14 @@ bool Gps::disableTmode3() { return configure(tmode3); } -bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { +bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate, bool wait) { RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, message_id, rate); ublox_msgs::msg::CfgMSG msg; msg.msg_class = class_id; msg.msg_id = message_id; msg.rate = rate; - return configure(msg); + return configure(msg, wait); } bool Gps::setDynamicModel(uint8_t model) { diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index b12caaff..72ea9eaf 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -109,7 +109,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); } - if (!gps->configRtcm(rtcms_)) { + if (!gps->configRtcm(rtcms_, true)) { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; @@ -150,7 +150,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { meas_rate_temp = kDefaultMeasPeriod; } // Set nav rate to 1 Hz during survey in - if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp)) { + if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp, true)) { throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); } @@ -197,12 +197,19 @@ bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if (!gps->configRate(meas_rate_, nav_rate_)) { + + // setTimeMode is called during a GPS read callback. Because of that, + // we *cannot* wait for acknowledgement down in the GPS layer; it would + // result in deadlock. Instead we set the rate without waiting and hope + // for the best. + if (!gps->configRate(meas_rate_, nav_rate_, false)) { RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms navigation rate to %d cycles", meas_rate_, nav_rate_); } // Enable the RTCM out messages - if (!gps->configRtcm(rtcms_)) { + // We cannot wait for acknowledgement that this completed for the same reason + // as above. + if (!gps->configRtcm(rtcms_, false)) { RCLCPP_ERROR(node_->get_logger(), "Failed to configure RTCM IDs"); return false; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 907de3c9..85525ecf 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -701,7 +701,7 @@ bool UbloxNode::configureUblox() { if (set_usb_) { gps_->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps_->configRate(meas_rate_, nav_rate_)) { + if (!gps_->configRate(meas_rate_, nav_rate_, true)) { std::stringstream ss; ss << "Failed to set measurement rate to " << meas_rate_ << "ms and navigation rate to " << nav_rate_; From 89b88b25a470075f4d6d1e6dbfac6bdcc77664dc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Apr 2020 19:40:31 +0000 Subject: [PATCH 2/4] Minor update to the comments in c94_m8p_rover.yaml Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_rover.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/config/c94_m8p_rover.yaml b/ublox_gps/config/c94_m8p_rover.yaml index cd6f16cd..3a6bb9cf 100644 --- a/ublox_gps/config/c94_m8p_rover.yaml +++ b/ublox_gps/config/c94_m8p_rover.yaml @@ -23,7 +23,7 @@ ublox_gps_node: dr_limit: 0 # TMODE3 Config - tmode3: 0 # Survey-In Mode + tmode3: 0 # Disabled sv_in: reset: false # True: disables and re-enables survey-in (resets) # False: Disables survey-in only if TMODE3 is From daad38c36fe1b7cb68eeaf449bec92283efd6986 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Apr 2020 19:41:07 +0000 Subject: [PATCH 3/4] Add CfgMSGS message. We need to use this version so we can set the correct message type for all ports. Signed-off-by: Chris Lalancette --- ublox_gps/src/gps.cpp | 7 +++- ublox_msgs/CMakeLists.txt | 1 + .../include/ublox_msgs/serialization.hpp | 34 +++++++++++++++++++ ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 1 + ublox_msgs/msg/CfgMSGS.msg | 16 +++++++++ 5 files changed, 58 insertions(+), 1 deletion(-) create mode 100644 ublox_msgs/msg/CfgMSGS.msg diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index a8e97ac9..9856b214 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -406,7 +406,12 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate, bool wait) { bool Gps::configRtcm(const std::vector & rtcms, bool wait) { for (const Rtcm & rtcm : rtcms) { RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcm.id, rtcm.rate); - if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate, wait)) { + + ublox_msgs::msg::CfgMSGS msgs; + msgs.msg_class = ublox_msgs::Class::RTCM; + msgs.msg_id = rtcm.id; + msgs.rates[ublox_msgs::msg::CfgMSGS::PORT_ID_UART1] = rtcm.rate; + if (!configure(msgs, wait)) { RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcm.id, rtcm.rate); return false; } diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 46a08336..ff4c670a 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -31,6 +31,7 @@ set(msg_files "msg/CfgINFBlock.msg" "msg/CfgINF.msg" "msg/CfgMSG.msg" + "msg/CfgMSGS.msg" "msg/CfgNAV5.msg" "msg/CfgNAVX5.msg" "msg/CfgNMEA6.msg" diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 9ef607fc..5989a6f2 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -458,6 +458,40 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgMSGS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rates[0]); + stream.next(m.rates[1]); + stream.next(m.rates[2]); + stream.next(m.rates[3]); + stream.next(m.rates[4]); + stream.next(m.rates[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgMSGS_ &m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::CfgMSGS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rates[0]); + stream.next(m.rates[1]); + stream.next(m.rates[2]); + stream.next(m.rates[3]); + stream.next(m.rates[4]); + stream.next(m.rates[5]); + } +}; + template struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index d362c8d0..285b4153 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include diff --git a/ublox_msgs/msg/CfgMSGS.msg b/ublox_msgs/msg/CfgMSGS.msg new file mode 100644 index 00000000..c61ba692 --- /dev/null +++ b/ublox_msgs/msg/CfgMSGS.msg @@ -0,0 +1,16 @@ +# CFG-MSG (0x06 0x01) +# Message Rates +# +# Set message rate for all ports - confusingly has the same name, class, and message ID as CfgMSG, but allows to setup all ports + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 1 + +uint8 msg_class # Message Class +uint8 msg_id # Message Identifier +uint8[6] rates # The rate for each of the ports +uint8 PORT_ID_DDC = 0 +uint8 PORT_ID_UART1 = 1 +uint8 PORT_ID_UART2 = 2 +uint8 PORT_ID_USB = 3 +uint8 PORT_ID_SPI = 4 From 91acd6a1bfc8023e53dc37351aff1f7e45eac9c7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Apr 2020 19:42:07 +0000 Subject: [PATCH 4/4] Small comment fix in CfgMSG. Signed-off-by: Chris Lalancette --- ublox_msgs/msg/CfgMSG.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_msgs/msg/CfgMSG.msg b/ublox_msgs/msg/CfgMSG.msg index d6bc5637..58e3e55e 100644 --- a/ublox_msgs/msg/CfgMSG.msg +++ b/ublox_msgs/msg/CfgMSG.msg @@ -1,5 +1,5 @@ # CFG-MSG (0x06 0x01) -# Message Rate(s) +# Message Rate # # Set message rate for the current port