Skip to content

Commit 577ef65

Browse files
brandonbeggsclalancette
brandonbeggs
and
clalancette
authored
Added Firmware v9 GNSS configuration (#229)
* Added Firmware v9 GNSS configuration * Update CfgVALSETCfgdata.msg Co-authored-by: Chris Lalancette <clalancette@gmail.com>
1 parent e668f03 commit 577ef65

File tree

5 files changed

+169
-3
lines changed

5 files changed

+169
-3
lines changed

‎ublox_gps/include/ublox_gps/ublox_firmware8.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::msg::NavPVT> {
6161
*/
6262
void subscribe(std::shared_ptr<ublox_gps::Gps> gps) override;
6363

64-
private:
64+
protected:
6565
// Set from ROS parameters
6666
//! Whether or not to enable the Galileo GNSS
6767
bool enable_galileo_{false};
@@ -75,6 +75,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::msg::NavPVT> {
7575
bool clear_bbr_{false};
7676
bool save_on_shutdown_{false};
7777

78+
private:
7879
rclcpp::Publisher<ublox_msgs::msg::NavSAT>::SharedPtr nav_sat_pub_;
7980
rclcpp::Publisher<ublox_msgs::msg::MonHW>::SharedPtr mon_hw_pub_;
8081
rclcpp::Publisher<ublox_msgs::msg::RxmRTCM>::SharedPtr rxm_rtcm_pub_;

‎ublox_gps/include/ublox_gps/ublox_firmware9.hpp‎

Lines changed: 22 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,9 @@
77
#include <diagnostic_updater/diagnostic_updater.hpp>
88
#include <rclcpp/rclcpp.hpp>
99

10+
#include <ublox_msgs/msg/cfg_valset.hpp>
11+
#include <ublox_msgs/msg/cfg_valset_cfgdata.hpp>
12+
1013
#include <ublox_gps/fix_diagnostic.hpp>
1114
#include <ublox_gps/gnss.hpp>
1215
#include <ublox_gps/ublox_firmware8.hpp>
@@ -15,12 +18,29 @@ namespace ublox_node {
1518

1619
/**
1720
* @brief Implements functions for firmware version 9.
18-
* For now it simply re-uses the firmware version 8 class
19-
* but allows for future expansion of functionality
2021
*/
2122
class UbloxFirmware9 final : public UbloxFirmware8 {
2223
public:
2324
explicit 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);
25+
26+
/**
27+
* @brief Configure settings specific to firmware 9 based on ROS parameters.
28+
*
29+
* @details Configure GNSS. The hardware has internal logic for
30+
* detecting differences between the new and active GNSS
31+
* configuration and will internally trigger a reset if necessary.
32+
* Configure the NMEA if desired by the user. It also may clear the
33+
* flash memory based on the ROS parameters.
34+
*/
35+
bool configureUblox(std::shared_ptr<ublox_gps::Gps> gps) override;
36+
37+
private:
38+
/**
39+
* @brief Populate the CfgVALSETCfgData data type
40+
*
41+
* @details A helper function used to generate a configuration for a single signal.
42+
*/
43+
ublox_msgs::msg::CfgVALSETCfgdata generateSignalConfig(uint32_t signalID, bool enable);
2444
};
2545

2646
} // namespace ublox_node

‎ublox_gps/src/node.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -701,6 +701,23 @@ void UbloxNode::processMonVer() {
701701
}
702702
continue;
703703
}
704+
// u-blox F9 modules support additional positioning signals
705+
else if (strs[0] == "MOD")
706+
{
707+
std::vector<std::string> moduleField;
708+
moduleField = stringSplit(strs[1], "-");
709+
if (moduleField.size() > 1)
710+
{
711+
if (moduleField[1].substr(0,2) == "F9")
712+
{
713+
gnss_->add("GPS_L2C");
714+
gnss_->add("GAL_E5B");
715+
gnss_->add("BDS_B2");
716+
gnss_->add("QZSS_L2C");
717+
gnss_->add("GLO_L2");
718+
}
719+
}
720+
}
704721
}
705722
}
706723
// Last 1-2 lines contain supported GNSS

‎ublox_gps/src/ublox_firmware9.cpp‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include <memory>
22
#include <string>
3+
#include <vector>
34

45
#include <diagnostic_updater/diagnostic_updater.hpp>
56
#include <rclcpp/rclcpp.hpp>
@@ -16,4 +17,106 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr<dia
1617
{
1718
}
1819

20+
bool UbloxFirmware9::configureUblox(std::shared_ptr<ublox_gps::Gps> gps)
21+
{
22+
if (clear_bbr_)
23+
{
24+
// clear flash memory
25+
if (!gps->clearBbr())
26+
{
27+
RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory");
28+
}
29+
}
30+
31+
gps->setSaveOnShutdown(save_on_shutdown_);
32+
33+
//
34+
// Configure the GNSS, only if the configuration is different
35+
//
36+
// First, get the current GNSS configuration
37+
ublox_msgs::msg::CfgGNSS cfg_gnss;
38+
if (gps->poll(cfg_gnss))
39+
{
40+
RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config.");
41+
RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw);
42+
RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use);
43+
}
44+
else
45+
{
46+
throw std::runtime_error("Failed to read the GNSS config.");
47+
}
48+
49+
ublox_msgs::msg::CfgVALSET cfg_signal;
50+
cfg_signal.layers = ublox_msgs::msg::CfgVALSET::LAYER_RAM;
51+
52+
using signal = ublox_msgs::msg::CfgVALSETCfgdata;
53+
54+
// Configure GPS Signals
55+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_ENABLE, enable_gps_));
56+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L1CA_ENABLE, enable_gps_));
57+
if (gnss_->isSupported("GPS_L2C"))
58+
{
59+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L2C_ENABLE, enable_gps_));
60+
}
61+
62+
// Configure SBAS Signals
63+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_ENABLE, enable_gps_));
64+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_L1CA_ENABLE, enable_gps_));
65+
66+
// Configure Galileo Signals
67+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_ENABLE, enable_galileo_));
68+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E1_ENABLE, enable_galileo_));
69+
if (gnss_->isSupported("GAL_E5B"))
70+
{
71+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E5B_ENABLE, enable_galileo_));
72+
}
73+
74+
// Configure Beidou Signals
75+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_ENABLE, enable_beidou_));
76+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B1_ENABLE, enable_beidou_));
77+
if (gnss_->isSupported("BDS_B2"))
78+
{
79+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B2_ENABLE, enable_beidou_));
80+
}
81+
82+
// Configure QZSS Signals
83+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_ENABLE, enable_qzss_ && enable_gps_));
84+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L1CA_ENABLE, enable_qzss_ && enable_gps_));
85+
if (gnss_->isSupported("QZSS_L2C"))
86+
{
87+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L2C_ENABLE, enable_qzss_ && enable_gps_));
88+
}
89+
90+
// Configure GLONASS Signals
91+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_ENABLE, enable_glonass_));
92+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L1_ENABLE, enable_glonass_));
93+
if (gnss_->isSupported("GLO_L2"))
94+
{
95+
cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L2_ENABLE, enable_glonass_));
96+
}
97+
98+
RCLCPP_DEBUG(node_->get_logger(), "Ready to configure");
99+
gps->configure(cfg_signal, false);
100+
101+
//
102+
// NMEA config
103+
//
104+
if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_))
105+
{
106+
throw std::runtime_error("Failed to configure NMEA");
107+
}
108+
109+
return true;
110+
}
111+
112+
ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t signalID, bool enable)
113+
{
114+
ublox_msgs::msg::CfgVALSETCfgdata signalConfig;
115+
signalConfig.key = signalID;
116+
signalConfig.data.resize(1);
117+
signalConfig.data[0] = enable;
118+
return signalConfig;
119+
}
120+
121+
19122
} // namespace ublox_node

‎ublox_msgs/msg/CfgVALSETCfgdata.msg‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,28 @@
1+
# CFG-SIGNAL Keys
2+
uint32 GPS_ENABLE = 271646751 # 0x1031001f
3+
uint32 GPS_L1CA_ENABLE = 271646721 # 0x10310001
4+
uint32 GPS_L2C_ENABLE = 271646723 # 0x10310003
5+
6+
uint32 SBAS_ENABLE = 271646752 # 0x10310020
7+
uint32 SBAS_L1CA_ENABLE = 271646725 # 0x10310005
8+
9+
uint32 GAL_ENABLE = 271646753 # 0x10310021
10+
uint32 GAL_E1_ENABLE = 271646727 # 0x10310007
11+
uint32 GAL_E5B_ENABLE = 271646730 # 0x1031000a
12+
13+
uint32 BDS_ENABLE = 271646754 # 0x10310022
14+
uint32 BDS_B1_ENABLE = 271646733 # 0x1031000d
15+
uint32 BDS_B2_ENABLE = 271646734 # 0x1031000e
16+
17+
uint32 QZSS_ENABLE = 271646756 # 0x10310024
18+
uint32 QZSS_L1CA_ENABLE = 271646738 # 0x10310012
19+
uint32 QZSS_L1S_ENABLE = 271646740 # 0x10310014
20+
uint32 QZSS_L2C_ENABLE = 271646741 # 0x10310015
21+
22+
uint32 GLO_ENABLE = 271646757 # 0x10310025
23+
uint32 GLO_L1_ENABLE = 271646744 # 0x10310018
24+
uint32 GLO_L2_ENABLE = 271646746 # 0x1031001a
25+
126
# See Cfg-VALSET
227
#
328

0 commit comments

Comments
 (0)