Skip to content

Commit

Permalink
Improved NMEA collector
Browse files Browse the repository at this point in the history
  • Loading branch information
Facundo Garcia committed Jul 3, 2024
1 parent 09d9b97 commit 6445386
Show file tree
Hide file tree
Showing 8 changed files with 183 additions and 200 deletions.
28 changes: 16 additions & 12 deletions fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<int8_t>(Status::STATUS_NO_FIX)), service(0) {}
Expand Down Expand Up @@ -158,7 +160,9 @@ struct GpggaData {
Eigen::Matrix<double, 3, 3> 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 {
Expand Down
143 changes: 64 additions & 79 deletions fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -218,14 +218,22 @@ struct GX_GSV {
std::vector<unsigned int> elev;
std::vector<unsigned int> azim;
std::vector<unsigned int> 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;
Expand All @@ -235,7 +243,7 @@ struct GX_GSV {
elev.clear();
azim.clear();
cno.clear();
signal_id = "Unknown";
type = SignalType::Invalid;
}

void ConvertFromTokens(const std::vector<std::string>& tokens);
Expand All @@ -248,7 +256,7 @@ struct GX_GSV {
elev.clear();
azim.clear();
cno.clear();
signal_id = "Unknown";
type = SignalType::Invalid;
}
};

Expand All @@ -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";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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";
Expand All @@ -419,83 +427,60 @@ 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<unsigned int> sat_id;
std::vector<unsigned int> elev;
std::vector<unsigned int> azim;
std::vector<unsigned int> 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;
}
};

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<int> 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<double, 3, 3> 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<std::string, GnssSignalStats> gnss_signals; // GX_GSV

std::unordered_map<std::string, std::string> 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<int> 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<double, 3, 3> 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<SignalType, std::map<unsigned int, GnssSignalStats>> gnss_signals; // GX_GSV

/**
* @brief Check if GNSS epoch is complete
*/
Expand Down
9 changes: 8 additions & 1 deletion fixposition_driver_lib/src/messages/nmea/gxgsv.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,15 @@ void GX_GSV::ConvertFromTokens(const std::vector<std::string>& 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
49 changes: 15 additions & 34 deletions fixposition_driver_lib/src/messages/nmea/nmea.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
Loading

0 comments on commit 6445386

Please sign in to comment.