diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp index a18110a..3eb58ab 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp @@ -56,6 +56,8 @@ enum class WheelspeedStatus : int { WS_CONVERGED = 1, }; +enum class SignalType : uint8_t { Invalid, GPS, Galileo, BeiDou, GLONASS }; + struct ImuData { EIGEN_MAKE_ALIGNED_OPERATOR_NEW times::GpsTime stamp; @@ -116,21 +118,21 @@ struct OdometryData { struct NavSatStatusData { enum class Status : int8_t { - STATUS_NO_FIX = -1, // Unable to fix position - STATUS_FIX = 0, // Unaugmented fix - STATUS_SBAS_FIX = 1, // With satellite-based augmentation - STATUS_GBAS_FIX = 2, // With ground-based augmentation + STATUS_NO_FIX = -1, // Unable to fix position + STATUS_FIX = 0, // Unaugmented fix + STATUS_SBAS_FIX = 1, // With satellite-based augmentation + STATUS_GBAS_FIX = 2, // With ground-based augmentation }; enum class Service : uint16_t { - SERVICE_NONE = 0, - SERVICE_GPS = 1, - SERVICE_GLONASS = 2, - SERVICE_COMPASS = 4, // includes BeiDou. - SERVICE_GALILEO = 8, - SERVICE_ALL = 15, + SERVICE_NONE = 0, + SERVICE_GPS = 1, + SERVICE_GLONASS = 2, + SERVICE_COMPASS = 4, // includes BeiDou. + SERVICE_GALILEO = 8, + SERVICE_ALL = 15, }; - + int8_t status; uint16_t service; NavSatStatusData() : status(static_cast(Status::STATUS_NO_FIX)), service(0) {} @@ -158,7 +160,9 @@ struct GpggaData { Eigen::Matrix cov; int position_covariance_type; bool valid; - GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0), valid(false) { cov.setZero(); } + GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0), valid(false) { + cov.setZero(); + } }; struct GpzdaData { diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp index 1e2eafd..995517b 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp @@ -38,7 +38,7 @@ struct GP_GGA { float diff_age; std::string diff_sta; std::string sentence; - + // Message structure static constexpr char frame_id[] = "LLH"; static constexpr char child_frame_id[] = "FP_POI"; @@ -88,7 +88,7 @@ struct GP_GLL { char lon_ew; char status; char mode; - + // Message structure const std::string frame_id = "LLH"; const std::string child_frame_id = "FP_POI"; @@ -129,7 +129,7 @@ struct GN_GSA { float hdop; float vdop; int8_t gnss_id; - + // Message structure const std::string frame_id = "LLH"; const std::string child_frame_id = "FP_POI"; @@ -173,7 +173,7 @@ struct GP_GST { float std_lat; float std_lon; float std_alt; - + // Message structure const std::string frame_id = "LLH"; const std::string child_frame_id = "FP_POI"; @@ -218,14 +218,22 @@ struct GX_GSV { std::vector elev; std::vector azim; std::vector cno; - std::string constellation; std::string signal_id; - + SignalType type; + // Message structure const std::string frame_id = "LLH"; const std::string child_frame_id = "FP_POI"; const std::string header_ = "GXGST"; - unsigned int kSize_ = 4; // Maximum size: 4 + num_sats * 4 + unsigned int kSize_ = 4; // Maximum size: 4 + num_sats * 4 + + SignalType string2enum(const std::string& name) { + if (name == "GP") return SignalType::GPS; // GPS + if (name == "GA") return SignalType::Galileo; // Galileo + if (name == "GB") return SignalType::BeiDou; // BeiDou + if (name == "GL") return SignalType::GLONASS; // GLONASS + return SignalType::Invalid; + } GX_GSV() { sentences = 0; @@ -235,7 +243,7 @@ struct GX_GSV { elev.clear(); azim.clear(); cno.clear(); - signal_id = "Unknown"; + type = SignalType::Invalid; } void ConvertFromTokens(const std::vector& tokens); @@ -248,7 +256,7 @@ struct GX_GSV { elev.clear(); azim.clear(); cno.clear(); - signal_id = "Unknown"; + type = SignalType::Invalid; } }; @@ -260,7 +268,7 @@ struct GP_HDT { // Message fields float heading; char true_ind; - + // Message structure const std::string frame_id = "LLH"; const std::string child_frame_id = "FP_POI"; @@ -296,7 +304,7 @@ struct GP_RMC { float speed_ms; float course; char mode; - + // Message structure static constexpr char frame_id[] = "LLH"; static constexpr char child_frame_id[] = "FP_POI"; @@ -347,7 +355,7 @@ struct GP_VTG { float sog_kph; char sog_unit_k; char mode; - + // Message structure static constexpr char frame_id[] = "LLH"; static constexpr char child_frame_id[] = "FP_POI"; @@ -387,12 +395,12 @@ struct GP_ZDA { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Message fields - std::string date_str; // Format: dd/mm/yyyy - std::string time_str; // Format: hhmmss.ss(ss) + std::string date_str; // Format: dd/mm/yyyy + std::string time_str; // Format: hhmmss.ss(ss) times::GpsTime stamp; uint8_t local_hr; uint8_t local_min; - + // Message structure static constexpr char frame_id[] = "LLH"; static constexpr char child_frame_id[] = "FP_POI"; @@ -419,32 +427,24 @@ struct GP_ZDA { }; // ------------ NmeaMessage ------------ - -// TODO: Optimize cn0 allocation for L1 and L2, repeated signals struct GnssSignalStats { - uint8_t num_sats; - std::string sat_id_name; - std::vector sat_id; - std::vector elev; - std::vector azim; - std::vector cno; + unsigned int azim; + unsigned int elev; + unsigned int cno_l1; + unsigned int cno_l2; GnssSignalStats() { - num_sats = 0; - sat_id_name = ""; - sat_id.clear(); - elev.clear(); - azim.clear(); - cno.clear(); + azim = 0; + elev = 0; + cno_l1 = 0; + cno_l2 = 0; } void ResetData() { - num_sats = 0; - sat_id_name = ""; - sat_id.clear(); - elev.clear(); - azim.clear(); - cno.clear(); + azim = 0; + elev = 0; + cno_l1 = 0; + cno_l2 = 0; } }; @@ -452,50 +452,35 @@ struct NmeaMessage { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Message fields - std::string gpgga_time_str; // GP_GGA - std::string gpzda_time_str; // GP_ZDA - std::string time_str; // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC) - std::string date_str; // GP_ZDA (alt. GP_RMC) - times::GpsTime stamp; // GP_ZDA - Eigen::Vector3d llh; // GP_GGA (alt. LL only for GP_GLL, GP_RMC) - uint8_t quality; // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA) - uint8_t num_sv; // GP_GGA - std::vector ids; // GN_GSA - float hdop_receiver; // GP_GGA - float pdop; // GN_GSA - float hdop; // GN_GSA (alt. GP_GGA) - float vdop; // GN_GSA - float rms_range; // GP_GST - float std_major; // GP_GST - float std_minor; // GP_GST - float angle_major; // GP_GST - float std_lat; // GP_GST (alt. GP_GGA) - float std_lon; // GP_GST (alt. GP_GGA) - float std_alt; // GP_GST (alt. GP_GGA) - Eigen::Matrix cov; // GP_GST (alt. GP_GGA) - uint8_t cov_type; // GP_GST (alt. GP_GGA) - float heading; // GP_HDT - float speed; // GP_RMC (alt. GP_VTG) - float course; // GP_RMC (alt. GP_VTG) - float diff_age; // GP_GGA - std::string diff_sta; // GP_GGA - std::unordered_map gnss_signals; // GX_GSV - - std::unordered_map signal_id_lut = { - {"GP0", "GPS No signal"}, - {"GP1", "GPS/SBAS L1C/A"}, - {"GP6", "GPS L2C-L"}, - {"GA0", "Galileo No signal"}, - {"GA7", "Galileo L1-BC"}, - {"GA2", "Galileo E5b"}, - {"GB0", "BeiDou No signal"}, - {"GB1", "BeiDou B1I"}, - {"GBB", "BeiDou B2I"}, - {"GL0", "GLONASS No signal"}, - {"GL1", "GLONASS G1 C/A"}, - {"GL3", "GLONASS G2 C/A"} - }; - + std::string gpgga_time_str; // GP_GGA + std::string gpzda_time_str; // GP_ZDA + std::string time_str; // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC) + std::string date_str; // GP_ZDA (alt. GP_RMC) + times::GpsTime stamp; // GP_ZDA + Eigen::Vector3d llh; // GP_GGA (alt. LL only for GP_GLL, GP_RMC) + uint8_t quality; // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA) + uint8_t num_sv; // GP_GGA + std::vector ids; // GN_GSA + float hdop_receiver; // GP_GGA + float pdop; // GN_GSA + float hdop; // GN_GSA (alt. GP_GGA) + float vdop; // GN_GSA + float rms_range; // GP_GST + float std_major; // GP_GST + float std_minor; // GP_GST + float angle_major; // GP_GST + float std_lat; // GP_GST (alt. GP_GGA) + float std_lon; // GP_GST (alt. GP_GGA) + float std_alt; // GP_GST (alt. GP_GGA) + Eigen::Matrix cov; // GP_GST (alt. GP_GGA) + uint8_t cov_type; // GP_GST (alt. GP_GGA) + float heading; // GP_HDT + float speed; // GP_RMC (alt. GP_VTG) + float course; // GP_RMC (alt. GP_VTG) + float diff_age; // GP_GGA + std::string diff_sta; // GP_GGA + std::unordered_map> gnss_signals; // GX_GSV + /** * @brief Check if GNSS epoch is complete */ diff --git a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp b/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp index 238f95d..b80c2c4 100644 --- a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp +++ b/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp @@ -47,8 +47,15 @@ void GX_GSV::ConvertFromTokens(const std::vector& tokens) { offset++; } - constellation = tokens.front(); + // Obtain signal ID and type + std::string constellation = tokens.front(); signal_id = tokens.back(); + + if (constellation.size() >= 2) { + type = string2enum(constellation.substr(0,2)); + } else { + type = SignalType::Invalid; + } } } // namespace fixposition diff --git a/fixposition_driver_lib/src/messages/nmea/nmea.cpp b/fixposition_driver_lib/src/messages/nmea/nmea.cpp index 062a22e..f56d2f9 100644 --- a/fixposition_driver_lib/src/messages/nmea/nmea.cpp +++ b/fixposition_driver_lib/src/messages/nmea/nmea.cpp @@ -89,41 +89,22 @@ void NmeaMessage::AddNmeaEpoch(const GP_GST& msg) { std_alt = msg.std_alt; } -void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) { - // Get signal ID - std::string signal_id; - if (msg.constellation.size() >= 2) { - signal_id = msg.constellation.substr(0,2) + msg.signal_id; - } else { - signal_id = msg.constellation + msg.signal_id; +void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) { + // Populate GNSS signal stats + for (u_int8_t i = 0; i < msg.sat_id.size(); i++) { + GnssSignalStats *stats = &gnss_signals[msg.type][msg.sat_id.at(i)]; + + // Populate necessary fields + if (stats->elev == 0) { stats->elev = msg.elev.at(i); } + if (stats->azim == 0) { stats->azim = msg.azim.at(i); } + + // Populate CNO + if (msg.signal_id == "1" || msg.signal_id == "7") { + stats->cno_l1 = msg.cno.at(i); + } else if (msg.signal_id == "6" || msg.signal_id == "2" || msg.signal_id == "B" || msg.signal_id == "3") { + stats->cno_l2 = msg.cno.at(i); + } } - - // Create new struct if signal ID is not in map - if (gnss_signals.find(signal_id) == gnss_signals.end()) { - gnss_signals[signal_id] = GnssSignalStats(); - } - - // Get signal ID - if (signal_id_lut.find(signal_id) == signal_id_lut.end()) { - gnss_signals.at(signal_id).sat_id_name = "Key not found"; - } else { - gnss_signals.at(signal_id).sat_id_name = signal_id_lut.at(signal_id); - } - - // Get number of satellites in view - gnss_signals.at(signal_id).num_sats = msg.num_sats; - - // Safety measure for vectors - if (gnss_signals.at(signal_id).sat_id.size() >= 100) { gnss_signals.at(signal_id).sat_id.clear(); } - if (gnss_signals.at(signal_id).azim.size() >= 100) { gnss_signals.at(signal_id).azim.clear(); } - if (gnss_signals.at(signal_id).elev.size() >= 100) { gnss_signals.at(signal_id).elev.clear(); } - if (gnss_signals.at(signal_id).cno.size() >= 100) { gnss_signals.at(signal_id).cno.clear(); } - - // Populate GNSS satellites in view (priority) - gnss_signals.at(signal_id).sat_id.insert(gnss_signals.at(signal_id).sat_id.end(), msg.sat_id.begin(), msg.sat_id.end()); - gnss_signals.at(signal_id).azim.insert(gnss_signals.at(signal_id).azim.end(), msg.azim.begin(), msg.azim.end()); - gnss_signals.at(signal_id).elev.insert(gnss_signals.at(signal_id).elev.end(), msg.elev.begin(), msg.elev.end()); - gnss_signals.at(signal_id).cno.insert(gnss_signals.at(signal_id).cno.end(), msg.cno.begin(), msg.cno.end()); } void NmeaMessage::AddNmeaEpoch(const GP_HDT& msg) { diff --git a/fixposition_driver_ros1/msg/GnssSats.msg b/fixposition_driver_ros1/msg/GnssSats.msg index 78aab85..a965233 100644 --- a/fixposition_driver_ros1/msg/GnssSats.msg +++ b/fixposition_driver_ros1/msg/GnssSats.msg @@ -13,25 +13,11 @@ # #################################################################################################### -# Format | Field | Unit | Description -# -------|------------|---------|----------------------------------------------------------------------| -string signal_id # [Hex] | Signal ID (see below). -int16 num_sats # [-] | Total number of satellites in view. -int16[] sat_id # [-] | Satellite ID number. -int16[] elev # [deg] | Satellite elevation. -int16[] azim # [deg] | Satellite azimuth from true North. -int16[] cno # [db-Hz] | Satellite SNR (C/No). - -# Signal ID table -# -# | ID | Signal | -# |----|-------------------------------| -# | 0 | No signal (any talker ID) | -# | 1 | GPS/SBAS L1C/A (talker ID GP) | -# | 6 | GPS L2C-L (talker ID GP) | -# | 7 | Galileo L1-BC (talker ID GA) | -# | 2 | Galileo E5b (talker ID GA) | -# | 1 | BeiDou B1I (talker ID GB) | -# | B | BeiDou B2I (talker ID GB) | -# | 1 | GLONASS G1 C/A (talker ID GL) | -# | 3 | GLONASS G2 C/A (talker ID GL) | \ No newline at end of file +# Format | Field | Unit | Description +# -------|---------------|---------|------------------------------------| +string constellation # [Hex] | Signal ID (see below). +int16[] sat_id # [-] | Satellite ID number. +int16[] azim # [deg] | Satellite azimuth from true North. +int16[] elev # [deg] | Satellite elevation. +int16[] cno_l1 # [db-Hz] | Satellite SNR (C/No) for L1-band. +int16[] cno_l2 # [db-Hz] | Satellite SNR (C/No) for L2-band. diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 91922dc..fde208c 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -272,28 +272,45 @@ void FixpositionDriverNode::PublishNmea() { msg.cov_type = nmea_message_.cov_type; // Populate GNSS satellites in view - for (auto it = nmea_message_.gnss_signals.begin(); it != nmea_message_.gnss_signals.end(); ++it) { - GnssSignalStats gnss_data = it->second; + for (auto gsv_it = nmea_message_.gnss_signals.begin(); gsv_it != nmea_message_.gnss_signals.end(); ++gsv_it) { + SignalType msg_type = gsv_it->first; + std::map *gnss_data = &gsv_it->second; // Populate GnssSats message fixposition_driver_ros1::GnssSats sats_msg; - sats_msg.signal_id = gnss_data.sat_id_name; - sats_msg.num_sats = gnss_data.num_sats; - for (unsigned int i = 0; i < gnss_data.sat_id.size(); i++) { - sats_msg.sat_id.push_back(gnss_data.sat_id.at(i)); - sats_msg.azim.push_back(gnss_data.azim.at(i)); - sats_msg.elev.push_back(gnss_data.elev.at(i)); - sats_msg.cno.push_back(gnss_data.cno.at(i)); + + // Get constellation name + if (msg_type == SignalType::GPS) { + sats_msg.constellation = "GPS"; + } else if (msg_type == SignalType::Galileo) { + sats_msg.constellation = "Galileo"; + } else if (msg_type == SignalType::BeiDou) { + sats_msg.constellation = "BeiDou"; + } else if (msg_type == SignalType::GLONASS) { + sats_msg.constellation = "GLONASS"; + } else { + sats_msg.constellation = "Unknown"; + } + + // Get signal statistics + for (auto it = gnss_data->begin(); it != gnss_data->end(); ++it) { + unsigned int sat_id = it->first; + GnssSignalStats signals = it->second; + + sats_msg.sat_id.push_back(sat_id); + sats_msg.azim.push_back(signals.azim); + sats_msg.elev.push_back(signals.elev); + sats_msg.cno_l1.push_back(signals.cno_l1); + sats_msg.cno_l2.push_back(signals.cno_l2); } + + // Add GnssSats to NMEA message msg.gnss_sats.push_back(sats_msg); - - // Clear vectors - it->second.sat_id.clear(); - it->second.azim.clear(); - it->second.elev.clear(); - it->second.cno.clear(); } + // Clear map + nmea_message_.gnss_signals.clear(); + // True heading msg.heading = nmea_message_.heading; diff --git a/fixposition_driver_ros2/msg/Gnsssats.msg b/fixposition_driver_ros2/msg/Gnsssats.msg index 8477e0c..ecc5f7c 100644 --- a/fixposition_driver_ros2/msg/Gnsssats.msg +++ b/fixposition_driver_ros2/msg/Gnsssats.msg @@ -10,25 +10,11 @@ # #################################################################################################### -# Format | Field | Unit | Description -# -------|------------|---------|----------------------------------------------------------------------| -string signal_id # [Hex] | Signal ID (see below). -int16 num_sats # [-] | Total number of satellites in view. -int16[] sat_id # [-] | Satellite ID number. -int16[] elev # [deg] | Satellite elevation. -int16[] azim # [deg] | Satellite azimuth from true North. -int16[] cno # [db-Hz] | Satellite SNR (C/No). - -# Signal ID table -# -# | ID | Signal | -# |----|-------------------------------| -# | 0 | No signal (any talker ID) | -# | 1 | GPS/SBAS L1C/A (talker ID GP) | -# | 6 | GPS L2C-L (talker ID GP) | -# | 7 | Galileo L1-BC (talker ID GA) | -# | 2 | Galileo E5b (talker ID GA) | -# | 1 | BeiDou B1I (talker ID GB) | -# | B | BeiDou B2I (talker ID GB) | -# | 1 | GLONASS G1 C/A (talker ID GL) | -# | 3 | GLONASS G2 C/A (talker ID GL) | \ No newline at end of file +# Format | Field | Unit | Description +# -------|---------------|---------|------------------------------------| +string constellation # [Hex] | Signal ID (see below). +int16[] sat_id # [-] | Satellite ID number. +int16[] azim # [deg] | Satellite azimuth from true North. +int16[] elev # [deg] | Satellite elevation. +int16[] cno_l1 # [db-Hz] | Satellite SNR (C/No) for L1-band. +int16[] cno_l2 # [db-Hz] | Satellite SNR (C/No) for L2-band. diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 520332c..cb4d427 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -287,28 +287,45 @@ void FixpositionDriverNode::PublishNmea() { msg.cov_type = nmea_message_.cov_type; // Populate GNSS satellites in view - for (auto it = nmea_message_.gnss_signals.begin(); it != nmea_message_.gnss_signals.end(); ++it) { - GnssSignalStats gnss_data = it->second; + for (auto gsv_it = nmea_message_.gnss_signals.begin(); gsv_it != nmea_message_.gnss_signals.end(); ++gsv_it) { + SignalType msg_type = gsv_it->first; + std::map *gnss_data = &gsv_it->second; // Populate GnssSats message fixposition_driver_ros2::msg::Gnsssats sats_msg; - sats_msg.signal_id = gnss_data.sat_id_name; - sats_msg.num_sats = gnss_data.num_sats; - for (unsigned int i = 0; i < gnss_data.sat_id.size(); i++) { - sats_msg.sat_id.push_back(gnss_data.sat_id.at(i)); - sats_msg.azim.push_back(gnss_data.azim.at(i)); - sats_msg.elev.push_back(gnss_data.elev.at(i)); - sats_msg.cno.push_back(gnss_data.cno.at(i)); + + // Get constellation name + if (msg_type == SignalType::GPS) { + sats_msg.constellation = "GPS"; + } else if (msg_type == SignalType::Galileo) { + sats_msg.constellation = "Galileo"; + } else if (msg_type == SignalType::BeiDou) { + sats_msg.constellation = "BeiDou"; + } else if (msg_type == SignalType::GLONASS) { + sats_msg.constellation = "GLONASS"; + } else { + sats_msg.constellation = "Unknown"; + } + + // Get signal statistics + for (auto it = gnss_data->begin(); it != gnss_data->end(); ++it) { + unsigned int sat_id = it->first; + GnssSignalStats signals = it->second; + + sats_msg.sat_id.push_back(sat_id); + sats_msg.azim.push_back(signals.azim); + sats_msg.elev.push_back(signals.elev); + sats_msg.cno_l1.push_back(signals.cno_l1); + sats_msg.cno_l2.push_back(signals.cno_l2); } + + // Add GnssSats to NMEA message msg.gnss_sats.push_back(sats_msg); - - // Clear vectors - it->second.sat_id.clear(); - it->second.azim.clear(); - it->second.elev.clear(); - it->second.cno.clear(); } + // Clear map + nmea_message_.gnss_signals.clear(); + // True heading msg.heading = nmea_message_.heading;