Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion fixposition-sdk
Submodule fixposition-sdk updated 123 files
4 changes: 2 additions & 2 deletions fixposition_driver_lib/src/helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -304,8 +304,8 @@ NmeaEpochData NmeaEpochData::CompleteAndReset() {
else if (rmc_.time.valid) { time_ = rmc_.time; }
else if (zda_.time.valid) { time_ = zda_.time; }
if (gga_.llh.latlon_valid) { llh_ = gga_.llh; }
else if (rmc_.llh.latlon_valid) { llh_ = rmc_.llh; }
else if (gll_.ll.latlon_valid) { llh_ = gll_.ll; } // last as it does not have height
else if (rmc_.ll.latlon_valid) { llh_ = rmc_.ll; }
else if (gll_.ll.latlon_valid) { llh_ = gll_.ll; } // last as it does not have height
// clang-format on

status_ = (gll_.status > rmc_.status ? gll_.status : rmc_.status);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -617,6 +617,8 @@ inline int NmeaSignalIdToMsg(const RosMsgT& msg, const fpsdk::common::parser::nm
case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E1: return msg.consts.SIGNAL_ID_GAL_E1;
case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E5A: return msg.consts.SIGNAL_ID_GAL_E5A;
case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E5B: return msg.consts.SIGNAL_ID_GAL_E5B;
case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E6BC: return msg.consts.SIGNAL_ID_GAL_E5B;
case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E6A: return msg.consts.SIGNAL_ID_GAL_E5B;
case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B1ID: return msg.consts.SIGNAL_ID_BDS_B1ID;
case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B2ID: return msg.consts.SIGNAL_ID_BDS_B2ID;
case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B1C: return msg.consts.SIGNAL_ID_BDS_B1C;
Expand Down
2 changes: 2 additions & 0 deletions fixposition_driver_msgs/msg/NmeaConsts.msg
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,8 @@ int8 SIGNAL_ID_GPS_L5Q = 15 # GPS L5 Q
int8 SIGNAL_ID_GAL_E1 = 31 # Galileo E1
int8 SIGNAL_ID_GAL_E5A = 32 # Galileo E5 A
int8 SIGNAL_ID_GAL_E5B = 33 # Galileo E5 B
int8 SIGNAL_ID_GAL_E6BC = 34 # Galileo E6 B/C
int8 SIGNAL_ID_GAL_E6A = 35 # Galileo E6 A
int8 SIGNAL_ID_BDS_B1ID = 41 # BeiDou B1I D
int8 SIGNAL_ID_BDS_B2ID = 42 # BeiDou B2I D
int8 SIGNAL_ID_BDS_B1C = 43 # BeiDou B1 C
Expand Down
24 changes: 12 additions & 12 deletions fixposition_driver_ros1/src/data_to_ros1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -509,7 +509,7 @@ bool PublishNovbInspvax(const novb::NovbHeader* header, const novb::NovbInspvax*
void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaGga msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -533,7 +533,7 @@ void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaGll msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -553,7 +553,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload,
void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaGsa msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.opmode = NmeaOpModeGsaToMsg(msg, payload.opmode);
msg.navmode = NmeaNavModeGsaToMsg(msg, payload.navmode);
Expand All @@ -574,7 +574,7 @@ void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload,
void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaGst msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -596,8 +596,8 @@ void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload,
void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaGsv msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.signal = NmeaSignalIdToMsg(msg, payload.signal);
msg.num_msgs = payload.num_msgs.value;
Expand All @@ -624,7 +624,7 @@ void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload,
void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaHdt msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.heading = (payload.heading.valid ? payload.heading.value : NAN);
pub.publish(msg);
}
Expand All @@ -635,7 +635,7 @@ void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload,
void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaRmc msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand All @@ -651,8 +651,8 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
msg.mode = NmeaModeRmcGnsToMsg(msg, payload.mode);
msg.navstatus = NmeaNavStatusRmcToMsg(msg, payload.navstatus);
msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN);
msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN);
msg.speed = (payload.speed.valid ? payload.speed.value : NAN);
msg.course = (payload.course.valid ? payload.course.value : NAN);
pub.publish(msg);
Expand All @@ -664,7 +664,7 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaVtg msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.cogt = (payload.cogt.valid ? payload.cogt.value : NAN);
msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN);
msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN);
Expand All @@ -679,7 +679,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload,
void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload, ros::Publisher& pub) {
if (pub.getNumSubscribers() > 0) {
fixposition_driver_msgs::NmeaZda msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand Down
24 changes: 12 additions & 12 deletions fixposition_driver_ros2/src/data_to_ros2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -520,7 +520,7 @@ void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGga msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -545,7 +545,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGll>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGll msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -566,7 +566,7 @@ void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGsa>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGsa msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.opmode = NmeaOpModeGsaToMsg(msg, payload.opmode);
msg.navmode = NmeaNavModeGsaToMsg(msg, payload.navmode);
Expand All @@ -588,7 +588,7 @@ void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGst>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGst msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -611,8 +611,8 @@ void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGsv>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGsv msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.signal = NmeaSignalIdToMsg(msg, payload.signal);
msg.num_msgs = payload.num_msgs.value;
Expand Down Expand Up @@ -640,7 +640,7 @@ void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaHdt>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaHdt msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.heading = (payload.heading.valid ? payload.heading.value : NAN);
pub->publish(msg);
}
Expand All @@ -652,7 +652,7 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaRmc>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaRmc msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand All @@ -668,8 +668,8 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
msg.mode = NmeaModeRmcGnsToMsg(msg, payload.mode);
msg.navstatus = NmeaNavStatusRmcToMsg(msg, payload.navstatus);
msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN);
msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN);
msg.speed = (payload.speed.valid ? payload.speed.value : NAN);
msg.course = (payload.course.valid ? payload.course.value : NAN);
pub->publish(msg);
Expand All @@ -682,7 +682,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaVtg msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.cogt = (payload.cogt.valid ? payload.cogt.value : NAN);
msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN);
msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN);
Expand All @@ -698,7 +698,7 @@ void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaZda msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand Down
Loading