diff --git a/fixposition-sdk b/fixposition-sdk index 53fa5b5c..81984507 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit 53fa5b5ce523614bacec31a36a65388e45a69258 +Subproject commit 819845072bae11a27f8742d54ddb0a5517704fef diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 781b7725..c293c209 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -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); diff --git a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp index 324ba7dc..06bd3ade 100644 --- a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp +++ b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp @@ -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; diff --git a/fixposition_driver_msgs/msg/NmeaConsts.msg b/fixposition_driver_msgs/msg/NmeaConsts.msg index 876ab37f..0b39457b 100644 --- a/fixposition_driver_msgs/msg/NmeaConsts.msg +++ b/fixposition_driver_msgs/msg/NmeaConsts.msg @@ -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 diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 36ac5e1c..44e52225 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -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; @@ -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; @@ -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); @@ -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; @@ -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; @@ -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); } @@ -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; @@ -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); @@ -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); @@ -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; diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index d60f8449..13f590cf 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -520,7 +520,7 @@ void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, rclcpp::Publisher::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; @@ -545,7 +545,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, rclcpp::Publisher::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; @@ -566,7 +566,7 @@ void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload, rclcpp::Publisher::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); @@ -588,7 +588,7 @@ void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload, rclcpp::Publisher::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; @@ -611,8 +611,8 @@ void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload, rclcpp::Publisher::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; @@ -640,7 +640,7 @@ void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload, rclcpp::Publisher::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); } @@ -652,7 +652,7 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload, rclcpp::Publisher::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; @@ -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); @@ -682,7 +682,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, rclcpp::Publisher::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); @@ -698,7 +698,7 @@ void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload, rclcpp::Publisher::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;