diff --git a/fixposition-sdk b/fixposition-sdk index ecb0b9ce..8c37c260 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit ecb0b9cefa80722a98da3634a635fc5a55e6f423 +Subproject commit 8c37c260beeebe0fe26738c469f1806430f9f19d diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp index 787c3aeb..466d903a 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp @@ -235,7 +235,8 @@ struct NmeaEpochData { fpsdk::common::parser::nmea::NmeaNavStatusRmc navstatus_ = fpsdk::common::parser::nmea::NmeaNavStatusRmc::UNSPECIFIED; fpsdk::common::parser::nmea::NmeaModeRmcGns mode1_ = fpsdk::common::parser::nmea::NmeaModeRmcGns::UNSPECIFIED; - fpsdk::common::parser::nmea::NmeaModeGllVtg mode2_ = fpsdk::common::parser::nmea::NmeaModeGllVtg::UNSPECIFIED; + fpsdk::common::parser::nmea::NmeaModeGll mode2_ = fpsdk::common::parser::nmea::NmeaModeGll::UNSPECIFIED; + fpsdk::common::parser::nmea::NmeaModeVtg mode3_ = fpsdk::common::parser::nmea::NmeaModeVtg::UNSPECIFIED; fpsdk::common::parser::nmea::NmeaQualityGga quality_ = fpsdk::common::parser::nmea::NmeaQualityGga::UNSPECIFIED; fpsdk::common::parser::nmea::NmeaOpModeGsa opmode_ = fpsdk::common::parser::nmea::NmeaOpModeGsa::UNSPECIFIED; fpsdk::common::parser::nmea::NmeaNavModeGsa navmode_ = fpsdk::common::parser::nmea::NmeaNavModeGsa::UNSPECIFIED; diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 759deea7..232c4a8e 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -311,7 +311,8 @@ NmeaEpochData NmeaEpochData::CompleteAndReset() { status_ = (gll_.status > rmc_.status ? gll_.status : rmc_.status); navstatus_ = rmc_.navstatus; mode1_ = rmc_.mode; - mode2_ = (gll_.mode > vtg_.mode ? gll_.mode : vtg_.mode); + mode2_ = gll_.mode; + mode3_ = vtg_.mode; quality_ = gga_.quality; opmode_ = gsa_.opmode; navmode_ = gsa_.navmode; 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 1ace8375..9fcf3c2a 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 @@ -67,6 +67,8 @@ inline int FpaGnssFixToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa: case fpsdk::common::parser::fpa::FpaGnssFix::S3D_DR: return msg.consts.GNSS_FIX_S3D_DR; case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FLOAT: return msg.consts.GNSS_FIX_RTK_FLOAT; case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FIXED: return msg.consts.GNSS_FIX_RTK_FIXED; + case fpsdk::common::parser::fpa::FpaGnssFix::PPP_FLOAT: return msg.consts.GNSS_FIX_PPP_FLOAT; + case fpsdk::common::parser::fpa::FpaGnssFix::PPP_FIXED: return msg.consts.GNSS_FIX_PPP_FIXED; } // clang-format on return msg.consts.GNSS_FIX_UNSPECIFIED; @@ -85,7 +87,9 @@ inline int FpaGnssFixToNavSatStatusStatus(const RosMsgT& msg, const fpsdk::commo case fpsdk::common::parser::fpa::FpaGnssFix::S3D: /* FALLTHROUGH */ case fpsdk::common::parser::fpa::FpaGnssFix::S3D_DR: return msg.STATUS_FIX; case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FLOAT: /* FALLTHROUGH */ - case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FIXED: return msg.STATUS_GBAS_FIX; + case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FIXED: /* FALLTHROUGH */ + case fpsdk::common::parser::fpa::FpaGnssFix::PPP_FLOAT: /* FALLTHROUGH */ + case fpsdk::common::parser::fpa::FpaGnssFix::PPP_FIXED: return msg.STATUS_GBAS_FIX; } // clang-format on return msg.STATUS_NO_FIX; @@ -218,10 +222,10 @@ inline int FpaGnssStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::f case fpsdk::common::parser::fpa::FpaGnssStatus::NO_FIX: return msg.consts.GNSS_STATUS_NO_FIX; case fpsdk::common::parser::fpa::FpaGnssStatus::SPP: return msg.consts.GNSS_STATUS_SPP; case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_MB: return msg.consts.GNSS_STATUS_RTK_MB; - case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED3: return msg.consts.GNSS_STATUS_RESERVED3; + case fpsdk::common::parser::fpa::FpaGnssStatus::PPP_FLOAT: return msg.consts.GNSS_STATUS_PPP_FLOAT; case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED4: return msg.consts.GNSS_STATUS_RESERVED4; case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_FLOAT: return msg.consts.GNSS_STATUS_RTK_FLOAT; - case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED6: return msg.consts.GNSS_STATUS_RESERVED6; + case fpsdk::common::parser::fpa::FpaGnssStatus::PPP_FIXED: return msg.consts.GNSS_STATUS_PPP_FIXED; case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED7: return msg.consts.GNSS_STATUS_RESERVED7; case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_FIXED: return msg.consts.GNSS_STATUS_RTK_FIXED; case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED9: return msg.consts.GNSS_STATUS_RESERVED9; @@ -507,20 +511,38 @@ inline int NmeaStatusGllRmcToMsg(const RosMsgT& msg, const fpsdk::common::parser } template -inline int NmeaModeGllVtgToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaModeGllVtg mode) { +inline int NmeaModeGllToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaModeGll mode) { // clang-format off switch (mode) { - case fpsdk::common::parser::nmea::NmeaModeGllVtg::UNSPECIFIED: return msg.consts.MODE_GLL_VTG_UNSPECIFIED; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::INVALID: return msg.consts.MODE_GLL_VTG_INVALID; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::AUTONOMOUS: return msg.consts.MODE_GLL_VTG_AUTONOMOUS; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::DGNSS: return msg.consts.MODE_GLL_VTG_DGNSS; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::ESTIMATED: return msg.consts.MODE_GLL_VTG_ESTIMATED; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::MANUAL: return msg.consts.MODE_GLL_VTG_MANUAL; - case fpsdk::common::parser::nmea::NmeaModeGllVtg::SIM: return msg.consts.MODE_GLL_VTG_SIM; + case fpsdk::common::parser::nmea::NmeaModeGll::UNSPECIFIED: return msg.consts.MODE_GLL_UNSPECIFIED; + case fpsdk::common::parser::nmea::NmeaModeGll::INVALID: return msg.consts.MODE_GLL_INVALID; + case fpsdk::common::parser::nmea::NmeaModeGll::AUTONOMOUS: return msg.consts.MODE_GLL_AUTONOMOUS; + case fpsdk::common::parser::nmea::NmeaModeGll::DGNSS: return msg.consts.MODE_GLL_DGNSS; + case fpsdk::common::parser::nmea::NmeaModeGll::ESTIMATED: return msg.consts.MODE_GLL_ESTIMATED; + case fpsdk::common::parser::nmea::NmeaModeGll::MANUAL: return msg.consts.MODE_GLL_MANUAL; + case fpsdk::common::parser::nmea::NmeaModeGll::SIM: return msg.consts.MODE_GLL_SIM; } // clang-format on - return msg.consts.MODE_GLL_VTG_UNSPECIFIED; + return msg.consts.MODE_GLL_UNSPECIFIED; +} + +template +inline int NmeaModeVtgToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaModeVtg mode) { + // clang-format off + switch (mode) + { + case fpsdk::common::parser::nmea::NmeaModeVtg::UNSPECIFIED: return msg.consts.MODE_VTG_UNSPECIFIED; + case fpsdk::common::parser::nmea::NmeaModeVtg::INVALID: return msg.consts.MODE_VTG_INVALID; + case fpsdk::common::parser::nmea::NmeaModeVtg::AUTONOMOUS: return msg.consts.MODE_VTG_AUTONOMOUS; + case fpsdk::common::parser::nmea::NmeaModeVtg::DGNSS: return msg.consts.MODE_VTG_DGNSS; + case fpsdk::common::parser::nmea::NmeaModeVtg::ESTIMATED: return msg.consts.MODE_VTG_ESTIMATED; + case fpsdk::common::parser::nmea::NmeaModeVtg::MANUAL: return msg.consts.MODE_VTG_MANUAL; + case fpsdk::common::parser::nmea::NmeaModeVtg::SIM: return msg.consts.MODE_VTG_SIM; + case fpsdk::common::parser::nmea::NmeaModeVtg::PRECISE: return msg.consts.MODE_VTG_PRECISE; + } + // clang-format on + return msg.consts.MODE_VTG_UNSPECIFIED; } template diff --git a/fixposition_driver_msgs/msg/FpaConsts.msg b/fixposition_driver_msgs/msg/FpaConsts.msg index b5f13523..fcce6646 100644 --- a/fixposition_driver_msgs/msg/FpaConsts.msg +++ b/fixposition_driver_msgs/msg/FpaConsts.msg @@ -67,10 +67,10 @@ int8 GNSS_STATUS_UNSPECIFIED = -1 # Unspecified int8 GNSS_STATUS_NO_FIX = 0 # No fix int8 GNSS_STATUS_SPP = 1 # Single-point positioning (SPP) int8 GNSS_STATUS_RTK_MB = 2 # RTK moving baseline -int8 GNSS_STATUS_RESERVED3 = 3 # Reserved +int8 GNSS_STATUS_PPP_FLOAT = 3 # PPP float int8 GNSS_STATUS_RESERVED4 = 4 # Reserved int8 GNSS_STATUS_RTK_FLOAT = 5 # RTK float -int8 GNSS_STATUS_RESERVED6 = 6 # Reserved +int8 GNSS_STATUS_PPP_FIXED = 6 # PPP fixed int8 GNSS_STATUS_RESERVED7 = 7 # Reserved int8 GNSS_STATUS_RTK_FIXED = 8 # RTK fixed int8 GNSS_STATUS_RESERVED9 = 9 # Reserved @@ -166,6 +166,8 @@ int8 GNSS_FIX_S3D = 5 # Single 3D fix int8 GNSS_FIX_S3D_DR = 6 # Single 3D fix with dead-reckoning int8 GNSS_FIX_RTK_FLOAT = 7 # RTK float fix int8 GNSS_FIX_RTK_FIXED = 8 # RTK fixed fix +int8 GNSS_FIX_PPP_FLOAT = 11 # PPP float fix +int8 GNSS_FIX_PPP_FIXED = 12 # PPP fixed fix # FpaTextLevel int8 TEXT_LEVEL_UNSPECIFIED = -1 # Unspecified diff --git a/fixposition_driver_msgs/msg/NmeaConsts.msg b/fixposition_driver_msgs/msg/NmeaConsts.msg index 28bd9479..451195cf 100644 --- a/fixposition_driver_msgs/msg/NmeaConsts.msg +++ b/fixposition_driver_msgs/msg/NmeaConsts.msg @@ -31,14 +31,24 @@ int8 STATUS_GLL_RMC_UNSPECIFIED = -1 # Unspecified int8 STATUS_GLL_RMC_INVALID = 0 # Data invalid int8 STATUS_GLL_RMC_VALID = 1 # Data valid -# NmeaModeGllVtg -int8 MODE_GLL_VTG_UNSPECIFIED = -1 # Unspecified -int8 MODE_GLL_VTG_INVALID = 0 # Invalid (no fix) -int8 MODE_GLL_VTG_AUTONOMOUS = 1 # Autonomous mode (SPP) -int8 MODE_GLL_VTG_DGNSS = 2 # Differential GNSS fix -int8 MODE_GLL_VTG_ESTIMATED = 3 # Estimated (dead reckoning only) -int8 MODE_GLL_VTG_MANUAL = 4 # Manual input mode -int8 MODE_GLL_VTG_SIM = 5 # Simulator mode +# NmeaModeGll +int8 MODE_GLL_UNSPECIFIED = -1 # Unspecified +int8 MODE_GLL_INVALID = 0 # Invalid (no fix) +int8 MODE_GLL_AUTONOMOUS = 1 # Autonomous mode (SPP) +int8 MODE_GLL_DGNSS = 2 # Differential GNSS fix +int8 MODE_GLL_ESTIMATED = 3 # Estimated (dead reckoning only) +int8 MODE_GLL_MANUAL = 4 # Manual input mode +int8 MODE_GLL_SIM = 5 # Simulator mode + +# NmeaModeVtg +int8 MODE_VTG_UNSPECIFIED = -1 # Unspecified +int8 MODE_VTG_INVALID = 0 # Invalid (no fix) +int8 MODE_VTG_AUTONOMOUS = 1 # Autonomous mode (SPP) +int8 MODE_VTG_DGNSS = 2 # Differential GNSS fix +int8 MODE_VTG_ESTIMATED = 3 # Estimated (dead reckoning only) +int8 MODE_VTG_MANUAL = 4 # Manual input mode +int8 MODE_VTG_SIM = 5 # Simulator mode +int8 MODE_VTG_PRECISE = 6 # Precise # NmeaModeRmcGns int8 MODE_RMC_GNS_UNSPECIFIED = -1 # Unspecified diff --git a/fixposition_driver_msgs/msg/NmeaEpoch.msg b/fixposition_driver_msgs/msg/NmeaEpoch.msg index e15a0a29..14d575bb 100644 --- a/fixposition_driver_msgs/msg/NmeaEpoch.msg +++ b/fixposition_driver_msgs/msg/NmeaEpoch.msg @@ -18,7 +18,8 @@ float32 time_s # Time seconds int8 status # Positioning system status, see consts.STATUS_GLL_RMC_... int8 navstatus # Navigational status (optional), see consts.NAV_STATUS_RMC_... int8 mode1 # Positioning system mode indicator, see consts.MODE_RMC_GNS_... -int8 mode2 # Positioning system mode, see consts.MODE_GLL_VTG_... +int8 mode2 # Positioning system mode, see consts.MODE_GLL_... +int8 mode3 # Positioning system mode, see consts.MODE_VTG_... int8 quality # Fix quality, see consts.QUALITY_GGA_... int8 opmode # Operation mode, see consts.OP_MODE_GSA_... int8 navmode # Nav mode, see consts.NAV_MODE_GSA_... diff --git a/fixposition_driver_msgs/msg/NmeaGll.msg b/fixposition_driver_msgs/msg/NmeaGll.msg index 0c57bb81..b13c4190 100644 --- a/fixposition_driver_msgs/msg/NmeaGll.msg +++ b/fixposition_driver_msgs/msg/NmeaGll.msg @@ -14,6 +14,6 @@ float64 latitude # Latitude [deg], NaN if not available float64 longitude # Longitude [deg], NaN if not available int8 status # Positioning system status, see consts.STATUS_GLL_RMC_... -int8 mode # Positioning system mode, see consts.MODE_GLL_VTG_... +int8 mode # Positioning system mode, see consts.MODE_GLL_... fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA diff --git a/fixposition_driver_msgs/msg/NmeaVtg.msg b/fixposition_driver_msgs/msg/NmeaVtg.msg index b3ba3f46..173319ac 100644 --- a/fixposition_driver_msgs/msg/NmeaVtg.msg +++ b/fixposition_driver_msgs/msg/NmeaVtg.msg @@ -10,6 +10,6 @@ float32 cogm # Course over ground (magnetic) [deg], NaN if float32 sogn # Speed over ground [knots], NaN if not available float32 sogk # Speed over ground [km/h], NaN if not available -int8 mode # Positioning system mode, see consts.MODE_GLL_VTG_... +int8 mode # Positioning system mode, see consts.MODE_VTG_... fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index a82cbdc9..942ba1bd 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -589,7 +589,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN); msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN); msg.status = NmeaStatusGllRmcToMsg(msg, payload.status); - msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode); + msg.mode = NmeaModeGllToMsg(msg, payload.mode); pub.publish(msg); } } @@ -715,7 +715,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN); msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN); msg.sogk = (payload.sogk.valid ? payload.sogk.value : NAN); - msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode); + msg.mode = NmeaModeVtgToMsg(msg, payload.mode); pub.publish(msg); } } @@ -781,7 +781,8 @@ void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub) { msg.status = NmeaStatusGllRmcToMsg(msg, data.status_); msg.navstatus = NmeaNavStatusRmcToMsg(msg, data.navstatus_); msg.mode1 = NmeaModeRmcGnsToMsg(msg, data.mode1_); - msg.mode2 = NmeaModeGllVtgToMsg(msg, data.mode2_); + msg.mode2 = NmeaModeGllToMsg(msg, data.mode2_); + msg.mode3 = NmeaModeVtgToMsg(msg, data.mode3_); msg.quality = NmeaQualityGgaToMsg(msg, data.quality_); msg.opmode = NmeaOpModeGsaToMsg(msg, data.opmode_); msg.navmode = NmeaNavModeGsaToMsg(msg, data.navmode_); diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 4ecaa879..e9952743 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -601,7 +601,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN); msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN); msg.status = NmeaStatusGllRmcToMsg(msg, payload.status); - msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode); + msg.mode = NmeaModeGllToMsg(msg, payload.mode); pub->publish(msg); } } @@ -733,7 +733,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN); msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN); msg.sogk = (payload.sogk.valid ? payload.sogk.value : NAN); - msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode); + msg.mode = NmeaModeVtgToMsg(msg, payload.mode); pub->publish(msg); } } @@ -801,7 +801,8 @@ void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher