Skip to content

Commit

Permalink
Improve NMEA message with all NMEA topics
Browse files Browse the repository at this point in the history
  • Loading branch information
Facundo Garcia committed Jun 28, 2024
1 parent 90ebd66 commit 64463c3
Show file tree
Hide file tree
Showing 11 changed files with 560 additions and 390 deletions.
1 change: 1 addition & 0 deletions fixposition_driver_lib/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ add_library(
src/messages/nmea/gprmc.cpp
src/messages/nmea/gpvtg.cpp
src/messages/nmea/gpzda.cpp
src/messages/nmea/nmea.cpp
src/fixposition_driver.cpp
src/helper.cpp
src/parser.cpp
Expand Down
131 changes: 121 additions & 10 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 @@ -386,8 +386,8 @@ struct GP_ZDA {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// Message fields
std::string date_str;
std::string time_str;
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;
Expand All @@ -400,8 +400,8 @@ struct GP_ZDA {

GP_ZDA() {
stamp = fixposition::times::GpsTime();
time_str = "hhmmss.ss(ss)";
date_str = "dd/mm/yyyy";
time_str = "";
date_str = "";
local_hr = 0;
local_min = 0;
}
Expand All @@ -410,29 +410,140 @@ struct GP_ZDA {

void ResetData() {
stamp = fixposition::times::GpsTime();
time_str = "hhmmss.ss(ss)";
date_str = "dd/mm/yyyy";
time_str = "";
date_str = "";
local_hr = 0;
local_min = 0;
}
};

struct NmeaMessage {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GP_GGA gpgga;
GP_ZDA gpzda;
GP_RMC gprmc;

// 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)
uint8_t num_sats; // GX_GSV
std::vector<unsigned int> sat_id; // GX_GSV
std::vector<unsigned int> elev; // GX_GSV
std::vector<unsigned int> azim; // GX_GSV
std::vector<unsigned int> cno; // GX_GSV
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

/**
* @brief Check if GNSS epoch is complete
*/
bool checkEpoch() {
if ((gpgga.time_str.compare(gpzda.time_str) == 0) && (gpgga.time_str.compare(gprmc.time_str) == 0)) {
if ((gpgga_time_str.compare(gpzda_time_str) == 0)) {
return true;
} else {
return false;
}
}

NmeaMessage() {
gpgga_time_str = "";
gpzda_time_str = "";
time_str = "";
date_str = "";
stamp = fixposition::times::GpsTime();
llh.setZero();
quality = -1;
diff_age = 0.0;
diff_sta = "";
ids.clear();
hdop_receiver = 0.0;
pdop = 0.0;
hdop = 0.0;
vdop = 0.0;
rms_range = 0.0;
std_major = 0.0;
std_minor = 0.0;
angle_major = 0.0;
std_lat = 0.0;
std_lon = 0.0;
std_alt = 0.0;
cov.setZero();
cov_type = 0;
num_sv = 0;
num_sats = 0;
sat_id.clear();
elev.clear();
azim.clear();
cno.clear();
heading = 0.0;
speed = 0.0;
course = 0.0;
}

void ResetData() {
gpgga_time_str = "";
gpzda_time_str = "";
time_str = "";
date_str = "";
stamp = fixposition::times::GpsTime();
llh.setZero();
quality = -1;
diff_age = 0.0;
diff_sta = "";
ids.clear();
hdop_receiver = 0.0;
pdop = 0.0;
hdop = 0.0;
vdop = 0.0;
rms_range = 0.0;
std_major = 0.0;
std_minor = 0.0;
angle_major = 0.0;
std_lat = 0.0;
std_lon = 0.0;
std_alt = 0.0;
cov.setZero();
cov_type = 0;
num_sv = 0;
num_sats = 0;
sat_id.clear();
elev.clear();
azim.clear();
cno.clear();
heading = 0.0;
speed = 0.0;
course = 0.0;
}

void AddNmeaEpoch(const GP_GGA& msg);
void AddNmeaEpoch(const GP_GLL& msg);
void AddNmeaEpoch(const GN_GSA& msg);
void AddNmeaEpoch(const GP_GST& msg);
void AddNmeaEpoch(const GX_GSV& msg);
void AddNmeaEpoch(const GP_HDT& msg);
void AddNmeaEpoch(const GP_RMC& msg);
void AddNmeaEpoch(const GP_VTG& msg);
void AddNmeaEpoch(const GP_ZDA& msg);
};

} // namespace fixposition
Expand Down
135 changes: 135 additions & 0 deletions fixposition_driver_lib/src/messages/nmea/nmea.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,135 @@
/**
* @file
* @brief Implementation of NMEA-GX-GSV parser
*
* \verbatim
* ___ ___
* \ \ / /
* \ \/ / Fixposition AG
* / /\ \ All right reserved.
* /__/ \__\
* \endverbatim
*
*/

/* PACKAGE */
#include <fixposition_driver_lib/messages/nmea_type.hpp>
#include <fixposition_driver_lib/messages/base_converter.hpp>

namespace fixposition {

void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) {
// Populate time if empty
if (time_str == "") {
time_str = msg.time_str;
}
gpgga_time_str = msg.time_str;

// Populate LLH position
llh = msg.llh;

// Populate quality
quality = msg.quality;

// Populate total number of satellites
num_sv = msg.num_sv;

// Populate GNSS receiver HDOP
hdop_receiver = msg.hdop;

// Populate differential data information
diff_age = msg.diff_age;
diff_sta = msg.diff_sta;

// Populate position covariance [m^2]
cov(0, 0) = msg.hdop * msg.hdop;
cov(1, 1) = msg.hdop * msg.hdop;
cov(2, 2) = 4 * msg.hdop * msg.hdop;
cov(0, 1) = cov(1, 0) = 0.0;
cov(0, 2) = cov(2, 0) = 0.0;
cov(1, 2) = cov(2, 1) = 0.0;
}

void NmeaMessage::AddNmeaEpoch(const GP_GLL& msg) {
// Populate time if empty
if (time_str == "") {
time_str = msg.time_str;
}

// Populate latitude and longitude if the vector is empty
if (llh.isZero()) {
llh(0) = msg.latlon(0);
llh(1) = msg.latlon(1);
}
}

void NmeaMessage::AddNmeaEpoch(const GN_GSA& msg) {
// Populate DOP values (priority)
pdop = msg.pdop;
hdop = msg.hdop;
vdop = msg.vdop;

// Populate satellite information (priority)
ids = msg.ids;
}

void NmeaMessage::AddNmeaEpoch(const GP_GST& msg) {
// Populate time if empty
if (time_str == "") {
time_str = msg.time_str;
}

// Populate GNSS pseudorange error statistics (priority)
rms_range = msg.rms_range;
std_major = msg.std_major;
std_minor = msg.std_minor;
angle_major = msg.angle_major;
std_lat = msg.std_lat;
std_lon = msg.std_lon;
std_alt = msg.std_alt;
}

void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) {
// Populate GNSS satellites in view (priority)
num_sats = msg.num_sats;
sat_id = msg.sat_id;
elev = msg.elev;
azim = msg.azim;
cno = msg.cno;
}

void NmeaMessage::AddNmeaEpoch(const GP_HDT& msg) {
// Populate true heading (priority)
heading = msg.heading;
}

void NmeaMessage::AddNmeaEpoch(const GP_RMC& msg) {
// Populate time if empty
if (time_str == "") {
time_str = msg.time_str;
}

// Populate time if empty
if (date_str == "") {
date_str = msg.date_str;
}

// Populate SOG and COG (priority)
speed = msg.speed_ms;
course = msg.course;

// Populate latitude and longitude if the vector is empty
if (llh.isZero()) {
llh(0) = msg.latlon(0);
llh(1) = msg.latlon(1);
}
}

void NmeaMessage::AddNmeaEpoch(const GP_ZDA& msg) {
// Populate time and date
time_str = msg.time_str;
date_str = msg.date_str;
gpzda_time_str = msg.time_str;
}

} // namespace fixposition
1 change: 0 additions & 1 deletion fixposition_driver_ros1/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS
)
add_message_files(
FILES
VRTK.msg
Speed.msg
WheelSensor.msg
NMEA.msg
Expand Down
Loading

0 comments on commit 64463c3

Please sign in to comment.