├── ntrip_client ├── resource │ └── ntrip_client ├── src │ └── ntrip_client │ │ ├── __init__.py │ │ └── nmea_parser.py ├── setup.cfg ├── package.xml ├── setup.py ├── LICENSE └── CHANGELOG.rst ├── ublox_msgs ├── msg │ ├── MonVERExtension.msg │ ├── Inf.msg │ ├── EsfSTATUS_Sens.msg │ ├── EsfSTATUSSens.msg │ ├── Ack.msg │ ├── EsfRAWBlock.msg │ ├── CfgMSG.msg │ ├── MonVER.msg │ ├── NavCLOCK.msg │ ├── NavSBASSV.msg │ ├── NavPOSECEF.msg │ ├── NavVELECEF.msg │ ├── NavDGPSSV.msg │ ├── NavSAT.msg │ ├── EsfRAW.msg │ ├── NavSVINFO.msg │ ├── RxmSVSI.msg │ ├── CfgHNR.msg │ ├── RxmRAWSV.msg │ ├── NavTIMEGPS.msg │ ├── CfgDGNSS.msg │ ├── NavDOP.msg │ ├── NavVELNED.msg │ ├── RxmRAW.msg │ ├── CfgINF.msg │ ├── RxmRTCM.msg │ ├── NavDGPS.msg │ ├── NavPOSLLH.msg │ ├── CfgINFBlock.msg │ ├── NavATT.msg │ ├── RxmSVSISV.msg │ ├── RxmSVSI_SV.msg │ ├── NavSBAS.msg │ ├── RxmSFRB.msg │ ├── RxmSFRBX.msg │ ├── MonGNSS.msg │ ├── UpdSOS.msg │ ├── RxmALM.msg │ ├── CfgANT.msg │ ├── CfgUSB.msg │ ├── EsfSTATUS.msg │ ├── RxmEPH.msg │ ├── AidALM.msg │ ├── TimTM2.msg │ ├── CfgDAT.msg │ ├── CfgSBAS.msg │ ├── CfgRST.msg │ ├── NavSOL.msg │ ├── NavTIMEUTC.msg │ ├── CfgNMEA6.msg │ ├── CfgRATE.msg │ ├── UpdSOSAck.msg │ ├── AidEPH.msg │ ├── AidHUI.msg │ ├── CfgCFG.msg │ ├── EsfINS.msg │ ├── RxmRAWX.msg │ ├── RxmRAWXMeas.msg │ ├── CfgGNSS.msg │ ├── NavSVIN.msg │ ├── NavSVINFOSV.msg │ ├── MonHW.msg │ ├── MonHW6.msg │ ├── CfgGNSSBlock.msg │ ├── HnrPVT.msg │ ├── MgaGAL.msg │ ├── NavSTATUS.msg │ ├── CfgNAVX5.msg │ ├── CfgNMEA7.msg │ ├── CfgTMODE3.msg │ └── CfgNAV5.msg ├── package.xml └── CMakeLists.txt ├── rtcm_msgs ├── msg │ └── Message.msg ├── CMakeLists.txt ├── package.xml └── LICENSE ├── ublox ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── ublox_gps ├── cmake │ └── Findasio.cmake ├── src │ ├── gnss.cpp │ ├── node_main.cpp │ ├── ublox_firmware9.cpp │ ├── ublox_firmware.cpp │ ├── logger_node_pa.cpp │ ├── hp_pos_rec_product.cpp │ ├── raw_data_product.cpp │ └── tim_product.cpp ├── include │ └── ublox_gps │ │ ├── rtcm.hpp │ │ ├── gnss.hpp │ │ ├── ublox_firmware9.hpp │ │ ├── hp_pos_rec_product.hpp │ │ ├── ublox_firmware.hpp │ │ ├── tim_product.hpp │ │ ├── ublox_firmware7.hpp │ │ ├── mkgmtime.h │ │ ├── raw_data_product.hpp │ │ ├── fix_diagnostic.hpp │ │ ├── hpg_rov_product.hpp │ │ ├── ublox_firmware8.hpp │ │ ├── fts_product.hpp │ │ ├── adr_udr_product.hpp │ │ ├── component_interface.hpp │ │ ├── ublox_topic_diagnostic.hpp │ │ └── worker.hpp ├── config │ ├── nmea.yaml │ ├── c94_m8t_base.yaml │ ├── zed_f9p.yaml │ ├── neo_m8u_rover.yaml │ ├── c94_m8t_rover.yaml │ ├── c94_m8p_rover.yaml │ └── c94_m8p_base.yaml ├── package.xml ├── launch │ ├── ublox_gps_node-launch.py │ └── ublox_gps_node-composed-launch.py └── CMakeLists.txt ├── ublox_serialization ├── package.xml ├── CMakeLists.txt ├── CHANGELOG.rst └── include │ └── ublox_serialization │ └── checksum.hpp ├── fix2nmea ├── package.xml └── CMakeLists.txt └── README.md /ntrip_client/resource/ntrip_client: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ntrip_client/src/ntrip_client/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MonVERExtension.msg: -------------------------------------------------------------------------------- 1 | # see MonVER message 2 | # 3 | 4 | char[30] field -------------------------------------------------------------------------------- /ntrip_client/setup.cfg: -------------------------------------------------------------------------------- 1 | 2 | [develop] 3 | script_dir=$base/lib/ntrip_client 4 | [install] 5 | install_scripts=$base/lib/ntrip_client -------------------------------------------------------------------------------- /rtcm_msgs/msg/Message.msg: -------------------------------------------------------------------------------- 1 | # A message representing a single RTCM message. 2 | std_msgs/Header header 3 | 4 | uint8[] message 5 | -------------------------------------------------------------------------------- /ublox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ublox) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /ublox_msgs/msg/Inf.msg: -------------------------------------------------------------------------------- 1 | # UBX-INF (0x04, 0x00...0x04) 2 | # ASCII output 3 | # 4 | # This message has a variable length payload, representing an ASCII string. 5 | # 6 | 7 | uint8 CLASS_ID = 4 8 | 9 | char[] str -------------------------------------------------------------------------------- /ublox_gps/cmake/Findasio.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | find_path(ASIO_INCLUDE_DIR NAMES asio.hpp) 4 | find_package_handle_standard_args(asio DEFAULT_MSG ASIO_INCLUDE_DIR) 5 | mark_as_advanced(ASIO_INCLUDE_DIR) 6 | -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfSTATUS_Sens.msg: -------------------------------------------------------------------------------- 1 | # See Esf-STATUS 2 | # 3 | 4 | uint8 sensStatus1 # Sensor status, part 1 (see graphic below) 5 | uint8 sensStatus2 # Sensor status, part 2 (see graphic below) 6 | uint8 freq # Observation frequency [Hz] 7 | uint8 faults # Sensor faults (see graphic below) -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfSTATUSSens.msg: -------------------------------------------------------------------------------- 1 | # See Esf-STATUS 2 | # 3 | 4 | uint8 sens_status1 # Sensor status, part 1 (see graphic below) 5 | uint8 sens_status2 # Sensor status, part 2 (see graphic below) 6 | uint8 freq # Observation frequency [Hz] 7 | uint8 faults # Sensor faults (see graphic below) 8 | -------------------------------------------------------------------------------- /ublox_gps/src/gnss.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace ublox_node { 6 | 7 | void Gnss::add(const std::string & gnss) 8 | { 9 | supported_.insert(gnss); 10 | } 11 | 12 | bool Gnss::isSupported(const std::string & gnss) 13 | { 14 | return supported_.count(gnss) > 0; 15 | } 16 | 17 | } // namespace ublox_node 18 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/rtcm.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_RTCM_HPP 2 | #define UBLOX_GPS_RTCM_HPP 3 | 4 | #include 5 | 6 | namespace ublox_gps { 7 | 8 | struct Rtcm { 9 | //! ID of RTCM out message to configure. 10 | uint8_t id; 11 | //! Rate of RTCM out message. 12 | uint8_t rate; 13 | }; 14 | 15 | } // namespace ublox_gps 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /ublox_msgs/msg/Ack.msg: -------------------------------------------------------------------------------- 1 | # ACK (0x05) 2 | # Message Acknowledged / Not-Acknowledged 3 | # 4 | # Output upon processing of an input message 5 | # 6 | 7 | uint8 CLASS_ID = 5 8 | uint8 NACK_MESSAGE_ID = 0 9 | uint8 ACK_MESSAGE_ID = 1 10 | 11 | uint8 cls_id # Class ID of the (Not-)Acknowledged Message 12 | uint8 msg_id # Message ID of the (Not-)Acknowledged Message 13 | -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfRAWBlock.msg: -------------------------------------------------------------------------------- 1 | # See ESF-RAW 2 | 3 | uint32 data # Its scaling and unit depends on the type and is 4 | # the same as in ESF-MEAS 5 | uint32 DATA_FIELD_MASK = 16777215 6 | uint32 DATA_TYPE_MASK = 4278190080 # type of data 7 | # (0 = no data; 1..255 = data type) 8 | uint32 s_t_tag # sensor time tag 9 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgMSG.msg: -------------------------------------------------------------------------------- 1 | # CFG-MSG (0x06 0x01) 2 | # Message Rate(s) 3 | # 4 | # Set message rate for the current port 5 | 6 | uint8 CLASS_ID = 6 7 | uint8 MESSAGE_ID = 1 8 | 9 | uint8 msg_class # Message Class 10 | uint8 msg_id # Message Identifier 11 | uint8 rate # Send rate on current port 12 | # [number of navigation solutions] 13 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MonVER.msg: -------------------------------------------------------------------------------- 1 | # MON-VER (0x0A 0x04) 2 | # 3 | # Receiver/Software Version 4 | # Returned when the version is polled. 5 | 6 | uint8 CLASS_ID = 10 7 | uint8 MESSAGE_ID = 4 8 | 9 | char[30] sw_version # Zero-terminated software version string. 10 | char[10] hw_version # Zero-terminated hardware version string. 11 | 12 | # Start of repeated block (N times) 13 | MonVERExtension[] extension 14 | # End of repeated block 15 | -------------------------------------------------------------------------------- /ublox_gps/src/node_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | int main(int argc, char** argv) { 8 | // Force flush of the stdout buffer. 9 | setvbuf(stdout, nullptr, _IONBF, BUFSIZ); 10 | 11 | rclcpp::init(argc, argv); 12 | 13 | rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); 14 | 15 | rclcpp::shutdown(); 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavCLOCK.msg: -------------------------------------------------------------------------------- 1 | # NAV-CLOCK (0x01 0x22) 2 | # Clock Solution 3 | # 4 | 5 | uint8 CLASS_ID = 1 6 | uint8 MESSAGE_ID = 34 7 | 8 | uint32 i_tow # GPS Millisecond Time of week [ms] 9 | 10 | int32 clk_b # Clock bias in nanoseconds [ns] 11 | int32 clk_d # Clock drift in nanoseconds per second [ns/s] 12 | uint32 t_acc # Time Accuracy Estimate [ns] 13 | uint32 f_acc # Frequency Accuracy Estimate [ps/s] 14 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSBASSV.msg: -------------------------------------------------------------------------------- 1 | # see message NavSBAS 2 | # 3 | 4 | uint8 svid # SV Id 5 | uint8 flags # Flags for this SV 6 | uint8 udre # Monitoring status 7 | uint8 sv_sys # System (WAAS/EGNOS/...), same as SYS 8 | uint8 sv_service # Services available, same as SERVICE 9 | uint8 reserved1 # Reserved 10 | int16 prc # Pseudo Range correction in [cm] 11 | uint16 reserved2 # Reserved 12 | int16 ic # Ionosphere correction in [cm] 13 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavPOSECEF.msg: -------------------------------------------------------------------------------- 1 | # NAV-POSECEF (0x01 0x01) 2 | # Position Solution in ECEF 3 | # 4 | # See important comments concerning validity of position given in section 5 | # Navigation Output Filters. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 1 10 | 11 | uint32 i_tow # GPS Millisecond Time of Week [ms] 12 | 13 | int32 ecef_x # ECEF X coordinate [cm] 14 | int32 ecef_y # ECEF Y coordinate [cm] 15 | int32 ecef_z # ECEF Z coordinate [cm] 16 | uint32 p_acc # Position Accuracy Estimate [cm] 17 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavVELECEF.msg: -------------------------------------------------------------------------------- 1 | # NAV-VELECEF (0x01 0x11) 2 | # Velocity Solution in ECEF 3 | # 4 | # See important comments concerning validity of velocity given in section 5 | # Navigation Output Filters. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 17 10 | 11 | uint32 i_tow # GPS Millisecond time of week [ms] 12 | 13 | int32 ecef_vx # ECEF X velocity [cm/s] 14 | int32 ecef_vy # ECEF Y velocity [cm/s] 15 | int32 ecef_vz # ECEF Z velocity [cm/s] 16 | uint32 s_acc # Speed Accuracy Estimate [cm/s] 17 | -------------------------------------------------------------------------------- /ublox_gps/config/nmea.yaml: -------------------------------------------------------------------------------- 1 | # Sample NMEA parameter configuration 2 | 3 | nmea: 4 | set: true 5 | version: 65 6 | num_sv: 8 7 | sv_numbering: 1 8 | compat: true 9 | consider: true 10 | limit82: true 11 | high_prec: true 12 | filter: 13 | pos: true 14 | msk_pos: true 15 | time: true 16 | date: true 17 | gps_only: true 18 | track: true 19 | gnssToFilter: 20 | gps: false 21 | sbas: true 22 | qzss: true 23 | glonass: false 24 | beidou: true 25 | main_talker_id: 1 26 | gsv_talker_id: 1 27 | bds_talker_id: [0,0] 28 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavDGPSSV.msg: -------------------------------------------------------------------------------- 1 | # see message NavDGPS 2 | 3 | uint8 svid # Satellite ID 4 | 5 | uint8 flags # Bitmask / Channel Number and Usage: 6 | uint8 FLAGS_CHANNEL_MASK = 15 # Bitmask for channel number, range 0..15 7 | # Channel numbers > 15 marked as 15 8 | uint8 FLAGS_DGPS = 16 # DGPS Used for this SV 9 | 10 | uint16 age_c # Age of latest correction data [ms] 11 | float32 prc # Pseudo Range Correction [m] 12 | float32 prrc # Pseudo Range Rate Correction [m/s] 13 | 14 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSAT.msg: -------------------------------------------------------------------------------- 1 | # NAV-SAT (0x01 0x35) 2 | # Satellite Information 3 | # 4 | # This message displays information about SVs which are either known to be 5 | # visible or currently tracked by the receiver. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 53 10 | 11 | uint32 i_tow # GPS time of week of the navigation epoch. [ms] 12 | uint8 version # Message version (1 for this version) 13 | uint8 num_svs # Number of satellites 14 | uint8[2] reserved0 # Reserved 15 | 16 | # start of repeated block (numSvs times) 17 | NavSATSV[] sv 18 | # end of repeated block 19 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/gnss.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_GNSS_HPP 2 | #define UBLOX_GPS_GNSS_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ublox_node { 8 | 9 | class Gnss final { 10 | public: 11 | Gnss() = default; 12 | ~Gnss() = default; 13 | 14 | Gnss(Gnss &&c) = delete; 15 | Gnss &operator=(Gnss &&c) = delete; 16 | Gnss(const Gnss &c) = delete; 17 | Gnss &operator=(const Gnss &c) = delete; 18 | 19 | void add(const std::string & gnss); 20 | bool isSupported(const std::string & gnss); 21 | 22 | private: 23 | std::set supported_; 24 | }; 25 | 26 | } // namespace ublox_node 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfRAW.msg: -------------------------------------------------------------------------------- 1 | # ESF-RAW (0x10 0x03) 2 | # Raw sensor measurements 3 | # 4 | # The message contains measurements from the active inertial sensors connected 5 | # to the GNSS chip. Possible data types for the data field are accelerometer, 6 | # gyroscope and temperature readings as described in the ESF Measurement Data 7 | # section. Note that the rate selected in CFG-MSG is not respected. If a 8 | # positive rate is selected then all raw measurements will be output. 9 | # 10 | # Supported on ADR/UDR products. 11 | # 12 | 13 | uint8 CLASS_ID = 16 14 | uint8 MESSAGE_ID = 3 15 | 16 | uint8[4] reserved0 # Reserved 17 | 18 | EsfRAWBlock[] blocks 19 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSVINFO.msg: -------------------------------------------------------------------------------- 1 | # NAV-SVINFO (0x01 0x30) 2 | # Space Vehicle Information 3 | # 4 | 5 | uint8 CLASS_ID = 1 6 | uint8 MESSAGE_ID = 48 7 | 8 | uint32 i_tow # GPS Millisecond time of week [ms] 9 | 10 | uint8 num_ch # Number of channels 11 | 12 | uint8 global_flags # Bitmask 13 | # Chip Hardware generation flags 14 | uint8 CHIPGEN_ANTARIS = 0 # Antaris, Antaris 4 15 | uint8 CHIPGEN_UBLOX5 = 1 # u-blox 5 16 | uint8 CHIPGEN_UBLOX6 = 2 # u-blox 6 17 | uint8 CHIPGEN_UBLOX7 = 3 # u-blox 7 18 | uint8 CHIPGEN_UBLOX8 = 4 # u-blox 8 / u-blox M8 19 | 20 | uint16 reserved2 # Reserved 21 | 22 | NavSVINFOSV[] sv 23 | -------------------------------------------------------------------------------- /ublox_serialization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ublox_serialization 4 | 2.3.0 5 | 6 | 7 | ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. 8 | 9 | 10 | Johannes Meyer 11 | Veronica Lane 12 | BSD 13 | http://ros.org/wiki/ublox 14 | 15 | ament_cmake 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmSVSI.msg: -------------------------------------------------------------------------------- 1 | # RXM-SVSI (0x02 0x20) 2 | # SV Status Info 3 | # 4 | # Status of the receiver manager knowledge about GPS Orbit Validity 5 | # 6 | # This message has only been retained for backwards compatibility; users are 7 | # recommended to use the UBX-NAV-ORB message in preference. 8 | 9 | uint8 CLASS_ID = 2 10 | uint8 MESSAGE_ID = 32 11 | 12 | int32 i_tow # GPS time of week of the navigation epoch [ms] 13 | int16 week # GPS week number of the navigation epoch [weeks] 14 | 15 | uint8 num_vis # Number of visible satellites 16 | uint8 num_sv # Number of per-SV data blocks following 17 | 18 | RxmSVSISV[] sv 19 | -------------------------------------------------------------------------------- /ublox_serialization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ublox_serialization) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | add_library(${PROJECT_NAME} INTERFACE) 8 | target_include_directories(${PROJECT_NAME} INTERFACE 9 | "$" 10 | "$") 11 | 12 | install(DIRECTORY include/ 13 | DESTINATION include/${PROJECT_NAME} 14 | ) 15 | install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) 16 | 17 | ament_export_include_directories("include/${PROJECT_NAME}") 18 | 19 | ament_export_targets(export_${PROJECT_NAME}) 20 | 21 | ament_package() 22 | -------------------------------------------------------------------------------- /ublox_gps/src/ublox_firmware9.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ublox_node { 13 | 14 | UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) 15 | : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) 16 | { 17 | } 18 | 19 | } // namespace ublox_node 20 | -------------------------------------------------------------------------------- /ublox_gps/src/ublox_firmware.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace ublox_node { 10 | 11 | // 12 | // U-Blox Firmware (all versions) 13 | // 14 | UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node) : updater_(updater), gnss_(gnss), node_(node) 15 | { 16 | } 17 | 18 | void UbloxFirmware::initializeRosDiagnostics() { 19 | updater_->add("fix", this, &UbloxFirmware::fixDiagnostic); 20 | updater_->force_update(); 21 | } 22 | 23 | } // namespace ublox_node 24 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgHNR.msg: -------------------------------------------------------------------------------- 1 | # CFG-HNR (0x06 0x5C) 2 | # High Navigation Rate Settings 3 | # 4 | # The u-blox receivers support high rates of navigation update up to 30 Hz. 5 | # The navigation solution output (NAV-HNR) will not be aligned to the top of a 6 | # second. 7 | # The update rate has a direct influence on the power consumption. The more 8 | # fixes that are required, the more CPU power and communication resources are 9 | # required. 10 | # For most applications a 1 Hz update rate would be sufficient. 11 | # 12 | # (only with ADR or UDR products) 13 | # 14 | 15 | uint8 CLASS_ID = 6 16 | uint8 MESSAGE_ID = 92 17 | 18 | uint8 high_nav_rate # Rate of navigation solution output [Hz] 19 | uint8[3] reserved0 # Reserved 20 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmRAWSV.msg: -------------------------------------------------------------------------------- 1 | # see message RxmRAW 2 | # 3 | 4 | float64 cp_mes # Carrier phase measurement [L1 cycles] 5 | float64 pr_mes # Pseudorange measurement [m] 6 | float32 do_mes # Doppler measurement [Hz] 7 | 8 | uint8 sv # Space Vehicle Number 9 | int8 mes_qi # Nav Measurements Quality Indicator 10 | # >=4 : PR+DO OK 11 | # >=5 : PR+DO+CP OK 12 | # <6 : likely loss of carrier lock in previous 13 | # interval 14 | int8 cno # Signal strength C/No. [dbHz] 15 | uint8 lli # Loss of lock indicator (RINEX definition) 16 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavTIMEGPS.msg: -------------------------------------------------------------------------------- 1 | # NAV-TIMEGPS (0x01 0x20) 2 | # GPS Time Solution 3 | # 4 | 5 | uint8 CLASS_ID = 1 6 | uint8 MESSAGE_ID = 32 7 | 8 | uint32 i_tow # GPS Millisecond time of week [ms] 9 | int32 f_tow # Fractional Nanoseconds remainder of rounded 10 | # ms above, range -500000 .. 500000 [ns] 11 | int16 week # GPS week (GPS time) 12 | 13 | int8 leap_s # Leap Seconds (GPS-UTC) [s] 14 | 15 | uint8 valid # Validity Flags 16 | uint8 VALID_TOW = 1 # Valid Time of Week 17 | uint8 VALID_WEEK = 2 # Valid Week Number 18 | uint8 VALID_LEAP_S = 4 # Valid Leap Seconds 19 | 20 | uint32 t_acc # Time Accuracy Estimate [ns] 21 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgDGNSS.msg: -------------------------------------------------------------------------------- 1 | # CFG-DGNSS (0x06 0x70) 2 | # DGNSS configuration 3 | # 4 | # This message allows the user to configure the DGNSS configuration of the 5 | # receiver. 6 | # Supported on: 7 | # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 (only 8 | # with High Precision GNSS products) 9 | 10 | uint8 CLASS_ID = 6 11 | uint8 MESSAGE_ID = 112 12 | 13 | uint8 dgnss_mode # Specifies differential mode: 14 | uint8 DGNSS_MODE_RTK_FLOAT = 2 # RTK float: No attempts are made to fix 15 | # ambiguities. 16 | uint8 DGNSS_MODE_RTK_FIXED = 3 # RTK fixed: Ambiguities are fixed whenever 17 | # possible. 18 | uint8[3] reserved0 # Reserved 19 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavDOP.msg: -------------------------------------------------------------------------------- 1 | # NAV-DOP (0x01 0x04) 2 | # Dilution of precision 3 | # 4 | # - DOP values are dimensionless. 5 | # - All DOP values are scaled by a factor of 100. If the unit transmits a value 6 | # of e.g. 156, the DOP value is 1.56. 7 | # 8 | 9 | uint8 CLASS_ID = 1 10 | uint8 MESSAGE_ID = 4 11 | 12 | uint32 i_tow # GPS Millisecond Time of Week [ms] 13 | 14 | uint16 g_dop # Geometric DOP [1 / 0.01] 15 | uint16 p_dop # Position DOP [1 / 0.01] 16 | uint16 t_dop # Time DOP [1 / 0.01] 17 | uint16 v_dop # Vertical DOP [1 / 0.01] 18 | uint16 h_dop # Horizontal DOP [1 / 0.01] 19 | uint16 n_dop # Northing DOP [1 / 0.01] 20 | uint16 e_dop # Easting DOP [1 / 0.01] 21 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavVELNED.msg: -------------------------------------------------------------------------------- 1 | # NAV-VELNED (0x01 0x12) 2 | # Velocity Solution in NED 3 | # 4 | # See important comments concerning validity of velocity given in section 5 | # Navigation Output Filters. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 18 10 | 11 | uint32 i_tow # GPS Millisecond time of week [ms] 12 | 13 | int32 vel_n # NED north velocity [cm/s] 14 | int32 vel_e # NED east velocity [cm/s] 15 | int32 vel_d # NED down velocity [cm/s] 16 | uint32 speed # Speed (3-D) [cm/s] 17 | uint32 g_speed # Ground Speed (2-D) [cm/s] 18 | int32 heading # Heading of motion 2-D [deg / 1e-5] 19 | uint32 s_acc # Speed Accuracy Estimate [cm/s] 20 | uint32 c_acc # Course / Heading Accuracy Estimate [deg / 1e-5] 21 | -------------------------------------------------------------------------------- /ublox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ublox 4 | 2.3.0 5 | Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. 6 | Johannes Meyer 7 | Veronica Lane 8 | BSD 9 | 10 | http://wiki.ros.org/ublox 11 | 12 | ament_cmake 13 | ublox_serialization 14 | ublox_msgs 15 | ublox_gps 16 | 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmRAW.msg: -------------------------------------------------------------------------------- 1 | # RXM-RAW (0x02 0x10) 2 | # Raw Measurement Data 3 | # 4 | # Supported up to ublox 7 firmware. See RxmRAWX for ublox 8 5 | # This message contains all information needed to be able to generate a RINEX 6 | # observation file. 7 | # This message outputs pseudorange, doppler and carrier phase measurements for 8 | # GPS satellites once signals have been synchronised. No other GNSS types are 9 | # currently supported. 10 | # 11 | 12 | uint8 CLASS_ID = 2 13 | uint8 MESSAGE_ID = 16 14 | 15 | int32 rcv_tow # Measurement time of week in receiver local time [s] 16 | int16 week # Measurement week number in receiver local time [weeks] 17 | 18 | uint8 num_sv # # of satellites following 19 | uint8 reserved1 # Reserved 20 | 21 | RxmRAWSV[] sv # numSV times 22 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgINF.msg: -------------------------------------------------------------------------------- 1 | # CFG-INF (0x06 0x02) 2 | # Information message configuration 3 | # 4 | # The value of infMsgMask[x] below are that each bit represents one of the INF 5 | # class messages (Bit 0 for ERROR, Bit 1 for WARNING and so on.). For a complete 6 | # list, see the Message Class INF. Several configurations can be concatenated to 7 | # one input message. 8 | # In this case the payload length can be a multiple of the normal length. Output 9 | # messages from the module contain only one configuration unit. Note that I/O 10 | # Ports 1 and 2 correspond to serial ports 1 and 2. I/O port 0 is DDC. I/O port 11 | # 3 is USB. I/O port 4 is SPI. I/O port 5 is reserved for future use 12 | # 13 | 14 | uint8 CLASS_ID = 6 15 | uint8 MESSAGE_ID = 2 16 | 17 | # start of repeated block 18 | CfgINFBlock[] blocks 19 | # end of repeated block 20 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmRTCM.msg: -------------------------------------------------------------------------------- 1 | # RXM-RTCM (0x02 0x32) 2 | # RTCM input status 3 | # 4 | # Output upon processing of an RTCM input message 5 | # Supported on: 6 | # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 7 | # 8 | 9 | uint8 CLASS_ID = 2 10 | uint8 MESSAGE_ID = 50 11 | 12 | uint8 version # Message version (0x02 for this version) 13 | uint8 flags # RTCM input status flags 14 | uint8 FLAGS_CRC_FAILED = 1 # 0 when RTCM message received and passed CRC 15 | # check, 1 when failed in which case refStation 16 | # and msgType might be corrupted and misleading 17 | 18 | uint8[2] reserved0 # Reserved 19 | 20 | uint16 ref_station # Reference station ID 21 | uint16 msg_type # Message type 22 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavDGPS.msg: -------------------------------------------------------------------------------- 1 | # NAV-DGPS (0x01 0x31) 2 | # DGPS Data Used for NAV 3 | # 4 | # This message outputs the Correction data as it has been applied to the current 5 | # NAV Solution. See also the notes on the RTCM protocol. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 49 10 | 11 | uint32 i_tow # GPS Millisecond time of week [ms] 12 | 13 | int32 age # Age of newest correction data [ms] 14 | int16 base_id # DGPS Base Station ID 15 | int16 base_health # DGPS Base Station Health Status 16 | int8 num_ch # Number of channels for which correction data is 17 | # following 18 | 19 | uint8 status # DGPS Correction Type Status 20 | uint8 DGPS_CORRECTION_NONE = 0 21 | uint8 DGPS_CORRECTION_PR_PRR = 1 22 | 23 | uint16 reserved1 # Reserved 24 | 25 | NavDGPSSV[] sv 26 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavPOSLLH.msg: -------------------------------------------------------------------------------- 1 | # NAV-POSLLH (0x01 0x02) 2 | # Geodetic Position Solution 3 | # 4 | # See important comments concerning validity of position given in section 5 | # Navigation Output Filters. 6 | # This message outputs the Geodetic position in the currently selected 7 | # Ellipsoid. The default is the WGS84 Ellipsoid, but can be changed with the 8 | # message CFG-DAT. 9 | # 10 | 11 | uint8 CLASS_ID = 1 12 | uint8 MESSAGE_ID = 2 13 | 14 | uint32 i_tow # GPS Millisecond Time of Week [ms] 15 | 16 | int32 lon # Longitude [deg / 1e-7] 17 | int32 lat # Latitude [deg / 1e-7] 18 | int32 height # Height above Ellipsoid [mm] 19 | int32 h_msl # Height above mean sea level [mm] 20 | uint32 h_acc # Horizontal Accuracy Estimate [mm] 21 | uint32 v_acc # Vertical Accuracy Estimate [mm] 22 | -------------------------------------------------------------------------------- /ntrip_client/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ntrip_client 4 | 1.2.0 5 | NTRIP client that will publish RTCM corrections to a ROS topic, and optionally subscribe to NMEA messages to send to an NTRIP server 6 | Parker Hannifin Corp 7 | Rob Fisher 8 | Melissa Gill 9 | MIT 10 | https://github.com/LORD-MicroStrain/ntrip_client 11 | 12 | rclpy 13 | std_msgs 14 | mavros_msgs 15 | nmea_msgs 16 | 17 | 18 | 19 | ament_python 20 | 21 | 22 | -------------------------------------------------------------------------------- /fix2nmea/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fix2nmea 5 | 0.0.0 6 | ROS2 executable to parse /fix type message containing Lat Lon to /nmea type message required by NTRIP Client. 7 | olivia 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | nmea_msgs 14 | sensor_msgs 15 | ublox_msgs 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgINFBlock.msg: -------------------------------------------------------------------------------- 1 | # See CfgINF message 2 | # 3 | 4 | uint8 protocol_id # Protocol Identifier, identifying for which 5 | # protocol the configuration is set/get. The 6 | # following are valid Protocol Identifiers: 7 | # 0: UBX Protocol 8 | # 1: NMEA Protocol 9 | # 2-255: Reserved 10 | uint8 PROTOCOL_ID_UBX = 0 11 | uint8 PROTOCOL_ID_NMEA = 1 12 | 13 | uint8[3] reserved1 # Reserved 14 | 15 | uint8[6] inf_msg_mask # A bit mask, saying which information messages 16 | # are enabled on each I/O port 17 | uint8 INF_MSG_ERROR = 1 # enable ERROR 18 | uint8 INF_MSG_WARNING = 2 # enable WARNING 19 | uint8 INF_MSG_NOTICE = 4 # enable NOTICE 20 | uint8 INF_MSG_TEST = 8 # enable TEST 21 | uint8 INF_MSG_DEBUG = 16 # enable DEBUG 22 | -------------------------------------------------------------------------------- /ublox_gps/config/c94_m8t_base.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for C94-M8T device 2 | ublox_gps_node: 3 | ros__parameters: 4 | debug: 2 # Range 0-4 (0 means no debug statements will print) 5 | 6 | device: /dev/ttyACM0 7 | frame_id: m8t_base 8 | dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics 9 | # assumed. 10 | fix_mode: auto 11 | 12 | rate: 1 # Measurement rate in Hz 13 | nav_rate: 1 # in number of measurement cycles 14 | 15 | uart1: 16 | baudrate: 9600 # C94-M8P specific 17 | in: 0 # No UART in for base 18 | out: 0 # RTCM 3 19 | 20 | inf: 21 | all: true # Whether to display all INF messages in console 22 | 23 | # Enable u-blox message publishers 24 | publish: 25 | tim: 26 | tm2: true 27 | -------------------------------------------------------------------------------- /ublox_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ublox_msgs 5 | 2.3.0 6 | 7 | 8 | ublox_msgs contains raw messages for u-blox GNSS devices. 9 | 10 | 11 | Johannes Meyer 12 | Veronica Lane 13 | BSD 14 | http://ros.org/wiki/ublox 15 | 16 | ament_cmake_ros 17 | 18 | rosidl_default_generators 19 | 20 | std_msgs 21 | sensor_msgs 22 | ublox_serialization 23 | 24 | rosidl_interface_packages 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavATT.msg: -------------------------------------------------------------------------------- 1 | # NAV-ATT (0x01 0x05) 2 | # Attitude Solution 3 | # 4 | # This message outputs the attitude solution as roll, pitch and heading angles. 5 | # Supported on ADR and UDR products. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 5 10 | 11 | uint32 i_tow # GPS time of week of the navigation epoch [ms] 12 | uint8 version # Message version (0 for this version) 13 | 14 | uint8[3] reserved0 # Reserved 15 | 16 | int32 roll # Vehicle roll. [deg / 1e-5] 17 | int32 pitch # Vehicle pitch. [deg / 1e-5] 18 | int32 heading # Vehicle heading. [deg / 1e-5] 19 | uint32 acc_roll # Vehicle roll accuracy (if null, roll angle is not 20 | # available). [deg / 1e-5] 21 | uint32 acc_pitch # Vehicle pitch accuracy (if null, pitch angle is not 22 | # available). [deg / 1e-5] 23 | uint32 acc_heading # Vehicle heading accuracy [deg / 1e-5] 24 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/ublox_firmware9.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_UBLOX_FIRMWARE9_HPP 2 | #define UBLOX_GPS_UBLOX_FIRMWARE9_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace ublox_node { 15 | 16 | /** 17 | * @brief Implements functions for firmware version 9. 18 | * For now it simply re-uses the firmware version 8 class 19 | * but allows for future expansion of functionality 20 | */ 21 | class UbloxFirmware9 final : public UbloxFirmware8 { 22 | public: 23 | explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); 24 | }; 25 | 26 | } // namespace ublox_node 27 | 28 | #endif // UBLOX_GPS_UBLOX_FIRMWARE9_HPP 29 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmSVSISV.msg: -------------------------------------------------------------------------------- 1 | # see message RxmSVSI 2 | # 3 | 4 | uint8 svid # Satellite ID 5 | 6 | uint8 sv_flag # Information Flags 7 | uint8 FLAG_URA_MASK = 15 # Figure of Merit (URA) range 0..15 8 | uint8 FLAG_HEALTHY = 16 # SV healthy flag 9 | uint8 FLAG_EPH_VAL = 32 # Ephemeris valid 10 | uint8 FLAG_ALM_VAL = 64 # Almanac valid 11 | uint8 FLAG_NOT_AVAIL = 128 # SV not available 12 | 13 | int16 azim # Azimuth 14 | int8 elev # Elevation 15 | 16 | uint8 age # Age of Almanac and Ephemeris 17 | uint8 AGE_ALM_MASK = 15 # Age of ALM in days offset by 4 18 | # i.e. the reference time may be in the future: 19 | # ageOfAlm = (age & 0x0f) - 4 20 | uint8 AGE_EPH_MASK = 240 # Age of EPH in hours offset by 4. 21 | # i.e. the reference time may be in the future: 22 | # ageOfEph = ((age & 0xf0) >> 4) - 4 23 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmSVSI_SV.msg: -------------------------------------------------------------------------------- 1 | # see message RxmSVSI 2 | # 3 | 4 | uint8 svid # Satellite ID 5 | 6 | uint8 svFlag # Information Flags 7 | uint8 FLAG_URA_MASK = 15 # Figure of Merit (URA) range 0..15 8 | uint8 FLAG_HEALTHY = 16 # SV healthy flag 9 | uint8 FLAG_EPH_VAL = 32 # Ephemeris valid 10 | uint8 FLAG_ALM_VAL = 64 # Almanac valid 11 | uint8 FLAG_NOT_AVAIL = 128 # SV not available 12 | 13 | int16 azim # Azimuth 14 | int8 elev # Elevation 15 | 16 | uint8 age # Age of Almanac and Ephemeris 17 | uint8 AGE_ALM_MASK = 15 # Age of ALM in days offset by 4 18 | # i.e. the reference time may be in the future: 19 | # ageOfAlm = (age & 0x0f) - 4 20 | uint8 AGE_EPH_MASK = 240 # Age of EPH in hours offset by 4. 21 | # i.e. the reference time may be in the future: 22 | # ageOfEph = ((age & 0xf0) >> 4) - 4 23 | -------------------------------------------------------------------------------- /rtcm_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rtcm_msgs) 3 | 4 | # ros_environment is needed to have ROS_VERSION 5 | find_package(ros_environment REQUIRED) 6 | set(ROS_VERSION $ENV{ROS_VERSION}) 7 | 8 | find_package(std_msgs REQUIRED) 9 | 10 | if(${ROS_VERSION} EQUAL 1) 11 | 12 | find_package(catkin REQUIRED 13 | COMPONENTS 14 | message_generation 15 | ) 16 | 17 | add_message_files(DIRECTORY msg 18 | FILES 19 | Message.msg 20 | ) 21 | 22 | generate_messages(DEPENDENCIES std_msgs) 23 | 24 | catkin_package(CATKIN_DEPENDS message_runtime std_msgs) 25 | 26 | elseif(${ROS_VERSION} EQUAL 2) 27 | find_package(ament_cmake REQUIRED) 28 | find_package(builtin_interfaces REQUIRED) 29 | find_package(rosidl_default_generators REQUIRED) 30 | 31 | rosidl_generate_interfaces(${PROJECT_NAME} ADD_LINTER_TESTS 32 | msg/Message.msg 33 | DEPENDENCIES builtin_interfaces std_msgs 34 | ) 35 | 36 | ament_export_dependencies(rosidl_default_runtime) 37 | ament_package() 38 | 39 | endif() 40 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSBAS.msg: -------------------------------------------------------------------------------- 1 | # NAV-SBAS (0x01 0x32) 2 | # SBAS Status Data 3 | # 4 | # This message outputs the status of the SBAS sub system 5 | # 6 | 7 | uint8 CLASS_ID = 1 8 | uint8 MESSAGE_ID = 50 9 | 10 | uint32 i_tow # GPS Millisecond time of week [ms] 11 | 12 | uint8 geo # PRN Number of the GEO where correction and integrity 13 | # data is used from 14 | 15 | uint8 mode # SBAS Mode 16 | uint8 MODE_DISABLED = 0 17 | uint8 MODE_ENABLED_INTEGRITY = 1 18 | uint8 MODE_ENABLED_TESTMODE = 3 19 | 20 | int8 sys # SBAS System (WAAS/EGNOS/...) 21 | int8 SYS_UNKNOWN = -1 22 | int8 SYS_WAAS = 0 23 | int8 SYS_EGNOS = 1 24 | int8 SYS_MSAS = 2 25 | int8 SYS_GAGAN = 3 26 | int8 SYS_GPS = 16 27 | 28 | uint8 service # SBAS Services available 29 | uint8 SERVICE_RANGING = 1 30 | uint8 SERVICE_CORRECTIONS = 2 31 | uint8 SERVICE_INTEGRITY = 4 32 | uint8 SERVICE_TESTMODE = 8 33 | 34 | uint8 cnt # Number of SV data following 35 | uint8[3] reserved0 # Reserved 36 | 37 | NavSBASSV[] sv 38 | -------------------------------------------------------------------------------- /ublox_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ublox_gps 4 | 2.3.0 5 | 6 | Driver for u-blox GPS devices. 7 | 8 | Johannes Meyer 9 | Gareth Cross 10 | Chao Qu 11 | Veronica Lane 12 | BSD 13 | http://ros.org/wiki/ublox 14 | 15 | ament_cmake_ros 16 | 17 | asio 18 | diagnostic_msgs 19 | diagnostic_updater 20 | geometry_msgs 21 | rcl_interfaces 22 | rclcpp 23 | rclcpp_components 24 | sensor_msgs 25 | std_msgs 26 | tf2 27 | ublox_msgs 28 | ublox_serialization 29 | rtcm_msgs 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmSFRB.msg: -------------------------------------------------------------------------------- 1 | # RXM-SFRB (0x02 0x11) 2 | # Subframe Buffer 3 | # 4 | # The content of one single subframe buffer 5 | # For GPS satellites, the 10 dwrd values contain the parity checked subframe 6 | # data for 10 Words. Each dwrd has 24 Bits with valid data (Bits 23 to 0). The 7 | # remaining 8 bits (31 to 24) have an undefined value. The direction within the 8 | # Word is that the higher order bits are received from the SV first. Example: 9 | # The Preamble can be found in dwrd[0], at bit position 23 down to 16. For more 10 | # details on the data format please refer to the ICD-GPS-200C 11 | # Interface document. 12 | # For SBAS satellites, the 250 Bit message block can be found in dwrd[0] to 13 | # dwrd[6] for the first 224 bits. The remaining 26 bits are in dwrd[7], whereas 14 | # Bits 25 and 24 are the last two data bits, and Bits 23 down to 0 are the 15 | # parity bits. For more information on SBAS data format, please refer to 16 | # RTCA/DO-229C (MOPS), Appendix A. 17 | # 18 | 19 | uint8 CLASS_ID = 2 20 | uint8 MESSAGE_ID = 17 21 | 22 | uint8 chn # Channel Number 23 | uint8 svid # ID of Satellite transmitting Subframe 24 | uint32[10] dwrd # Words of Data 25 | -------------------------------------------------------------------------------- /ntrip_client/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import glob 3 | from setuptools import setup 4 | 5 | package_name = 'ntrip_client' 6 | 7 | setup( 8 | name=package_name, 9 | version='1.2.0', 10 | packages=[package_name], 11 | package_dir={'': 'src'}, 12 | data_files=[ 13 | ('share/ament_index/resource_index/packages', ['resource/' + package_name]), 14 | (os.path.join('share', package_name), ['package.xml', *glob.glob('launch/*')]), 15 | ], 16 | install_requires=['setuptools'], 17 | zip_safe=True, 18 | author='Rob Fisher', 19 | author_email='rob.fisher@parker.com', 20 | maintainer='Rob Fisher', 21 | maintainer_email='rob.fisher@parker.com', 22 | keywords=['ROS'], 23 | classifiers=[ 24 | 'Intended Audience :: Developers', 25 | 'License :: OSI Approved :: MIT Software License', 26 | 'Programming Language :: Python', 27 | 'Topic :: Software Development', 28 | ], 29 | description='NTRIP client that will publish RTCM corrections to a ROS topic, and optionally subscribe to NMEA messages to send to an NTRIP server', 30 | license='MIT License', 31 | tests_require=['pytest'], 32 | scripts=[ 33 | 'scripts/ntrip_ros.py' 34 | ] 35 | ) -------------------------------------------------------------------------------- /ntrip_client/LICENSE: -------------------------------------------------------------------------------- 1 | ROS NTRIP Client is licensed under the MIT License 2 | : 3 | 4 | Copyright (c) 2013-2017 Niels Lohmann 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 10 | of the Software, and to permit persons to whom the Software is furnished to do 11 | so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. -------------------------------------------------------------------------------- /ublox_gps/config/zed_f9p.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for C94-M8P device 2 | ublox_gps_node: 3 | ros__parameters: 4 | debug: 1 # Range 0-4 (0 means no debug statements will print) 5 | device: /dev/ttyACM0 6 | 7 | frame_id: gps 8 | rate: 1.0 # in Hz 9 | nav_rate: 1 10 | 11 | dynamic_model: portable 12 | 13 | uart1: 14 | baudrate: 57600 15 | 16 | gnss: 17 | glonass: true 18 | beidou: true 19 | gps: true 20 | qzss: true 21 | galileo: true 22 | imes: false 23 | 24 | # TMODE3 Config 25 | tmode3: 0 # Survey-In Mode 26 | sv_in: 27 | reset: false # True: disables and re-enables survey-in (resets) 28 | # False: Disables survey-in only if TMODE3 is 29 | # disabled 30 | min_dur: 300 # Survey-In Minimum Duration [s] 31 | acc_lim: 3.0 # Survey-In Accuracy Limit [m] 32 | 33 | inf: 34 | all: true # Whether to display all INF messages in console 35 | 36 | publish: 37 | aid/all: false 38 | 39 | -------------------------------------------------------------------------------- /fix2nmea/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(fix2nmea) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(nmea_msgs REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(ublox_msgs REQUIRED) 14 | 15 | add_executable(fix2nmea src/fix2nmea.cpp) 16 | ament_target_dependencies(fix2nmea rclcpp sensor_msgs ublox_msgs nmea_msgs) 17 | 18 | install(TARGETS 19 | fix2nmea 20 | DESTINATION lib/${PROJECT_NAME}) 21 | 22 | if(BUILD_TESTING) 23 | find_package(ament_lint_auto REQUIRED) 24 | # the following line skips the linter which checks for copyrights 25 | # comment the line when a copyright and license is added to all source files 26 | set(ament_cmake_copyright_FOUND TRUE) 27 | # the following line skips cpplint (only works in a git repo) 28 | # comment the line when this package is in a git repo and when 29 | # a copyright and license is added to all source files 30 | set(ament_cmake_cpplint_FOUND TRUE) 31 | ament_lint_auto_find_test_dependencies() 32 | endif() 33 | 34 | ament_package() 35 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmSFRBX.msg: -------------------------------------------------------------------------------- 1 | # RXM-SFRB (0x02 0x13) 2 | # Subframe Buffer 3 | # 4 | # This message reports a complete subframe of broadcast navigation data decoded 5 | # from a single signal. The number of data words reported in each message 6 | # depends on the nature of the signal. See the section on Broadcast Navigation 7 | # Data for further details. 8 | # 9 | 10 | uint8 CLASS_ID = 2 11 | uint8 MESSAGE_ID = 19 12 | 13 | uint8 gnss_id # GNSS identifier (see Cfg GNSS for constants) 14 | 15 | uint8 sv_id # Satellite identifier within corresponding GNSS system 16 | uint8 reserved0 # Reserved 17 | uint8 freq_id # Only used for GLONASS: This is the frequency 18 | # slot + 7 (range from 0 to 13) 19 | uint8 num_words # The number of data words contained in this message (up 20 | # to 10, for currently supported signals) 21 | uint8 chn # The tracking channel number the message was received 22 | # on 23 | uint8 version # Message version, (0x02 for this version) 24 | uint8 reserved1 # Reserved 25 | 26 | # Start of repeated block (numWords times) 27 | uint32[] dwrd # The data words 28 | # End of repeated block 29 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MonGNSS.msg: -------------------------------------------------------------------------------- 1 | # MON-GNSS (0x0A 0x28) 2 | # Information message major GNSS selection 3 | # 4 | # This message reports major GNSS selection. Augmentation systems are not 5 | # reported. 6 | # 7 | 8 | uint8 CLASS_ID = 10 9 | uint8 MESSAGE_ID = 40 10 | 11 | uint8 version # Message version (0x01 for this version) 12 | 13 | uint8 BIT_MASK_GPS = 1 14 | uint8 BIT_MASK_GLONASS = 2 15 | uint8 BIT_MASK_BEIDOU = 4 16 | uint8 BIT_MASK_GALILEO = 8 17 | 18 | uint8 supported # The major GNSS that can be supported by this receiver 19 | uint8 default_gnss # Default major GNSS selection. If the default major GNSS 20 | # selection is currently configured in the efuse for this 21 | # receiver, it takes precedence over the default major 22 | # GNSS selection configured in the executing firmware of 23 | # this receiver. 24 | # see bit mask constants 25 | uint8 enabled # Current major GNSS selection enabled for this receiver 26 | # see bit mask constants 27 | 28 | uint8 simultaneous # Maximum number of concurrent major GNSS that can be 29 | # supported by this receiver 30 | 31 | uint8[3] reserved1 # Reserved 32 | -------------------------------------------------------------------------------- /ublox_msgs/msg/UpdSOS.msg: -------------------------------------------------------------------------------- 1 | # UPD-SOS (0x09 0x14) 2 | # 3 | # Firmware Supported on: 4 | # u-blox 8 / u-blox M8 from protocol version 15 up to version 23.01 5 | # 6 | 7 | uint8 CLASS_ID = 9 8 | uint8 MESSAGE_ID = 20 9 | 10 | uint8 cmd # Command 11 | # The host can send this message in order to save part of the BBR memory in a 12 | # file in flash file system. The feature is designed in order to emulate the 13 | # presence of the backup battery even if it is not present; the host can issue 14 | # the save on shutdown command before switching off the device supply. It is 15 | # recommended to issue a GNSS stop command before, in order to keep the BBR 16 | # memory content consistent. 17 | uint8 CMD_FLASH_BACKUP_CREATE = 0 # Create Backup File in Flash 18 | # The host can send this message in order to erase the backup file present in 19 | # flash. It is recommended that the clear operation is issued after the host has 20 | # received the notification that the memory has been restored after a reset. 21 | # Alternatively the host can parse the startup string 'Restored data saved on 22 | # shutdown' or poll the UBX-UPD-SOS message for getting the status. 23 | uint8 CMD_FLASH_BACKUP_CLEAR = 1 # Clear Backup File in Flash 24 | 25 | uint8[3] reserved1 # Reserved 26 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmALM.msg: -------------------------------------------------------------------------------- 1 | # RXM-ALM (0x02 0x30) 2 | # GPS Aiding Almanach Input/Output Message 3 | # 4 | # This message is provided considered obsolete, please use AID-ALM instead! 5 | # - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not 6 | # available for the given SV. 7 | # - DWORD0 to DWORD7 contain the 8 words following the Hand-Over Word ( HOW ) 8 | # from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or 9 | # pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the 10 | # contents of the Almanac pages. 11 | # - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of 12 | # data are located in Bits 0 to 23. Bits 24 to 31 shall be ignored. 13 | # - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, 14 | # Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas 15 | # Bit 0 is the LSB. 16 | # 17 | 18 | uint8 CLASS_ID = 2 19 | uint8 MESSAGE_ID = 48 20 | 21 | uint32 svid # SV ID for which this Almanach Data is 22 | # (Valid Range: 1 .. 32 or 51, 56, 63). 23 | uint32 week # Issue Date of Almanach (GPS week number) 24 | 25 | # Start of optional block 26 | uint32[] dwrd # Almanach Words 27 | # End of optional block 28 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgANT.msg: -------------------------------------------------------------------------------- 1 | # CFG-ANT (0x06 0x13) 2 | # Antenna Control Settings 3 | # 4 | 5 | uint8 CLASS_ID = 6 6 | uint8 MESSAGE_ID = 19 7 | 8 | uint16 flags # Antenna Flag Mask 9 | uint16 FLAGS_SVCS = 1 # Enable Antenna Supply Voltage Control Signal 10 | uint16 FLAGS_SCD = 2 # Enable Short Circuit Detection 11 | uint16 FLAGS_OCD = 4 # Enable Open Circuit Detection 12 | uint16 FLAGS_PDWN_ON_SCD = 8 # Power Down Antenna supply if Short Circuit is 13 | # detected. (only in combination with Bit 1) 14 | uint16 FLAGS_RECOVERY = 16 # Enable automatic recovery from short state 15 | 16 | uint16 pins # Antenna Pin Configuration 17 | uint16 PIN_SWITCH_MASK = 31 # PIO-Pin used for switching antenna supply 18 | # (internal to TIM-LP/TIM-LF) 19 | uint16 PIN_SCD_MASK = 992 # PIO-Pin used for detecting a short in the 20 | # antenna supply 21 | uint16 PIN_OCD_MASK = 31744 # PIO-Pin used for detecting open/not connected 22 | # antenna 23 | uint16 PIN_RECONFIG = 32678 # if set to one, and this command is sent to the 24 | # receiver, the receiver will reconfigure the 25 | # pins as specified. 26 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgUSB.msg: -------------------------------------------------------------------------------- 1 | # UBX-CFG-USB (0x06 0x1B) 2 | # USB Configuration 3 | # 4 | 5 | uint8 CLASS_ID = 6 6 | uint8 MESSAGE_ID = 27 7 | 8 | uint16 vendor_id # Only set to registered Vendor IDs. 9 | # Changing this field requires special Host drivers. 10 | 11 | uint16 product_id # Product ID. Changing this field requires special 12 | # Host drivers. 13 | 14 | uint8[2] reserved1 # Reserved 15 | uint8[2] reserved2 # Reserved 16 | 17 | uint16 power_consumption # Power consumed by the device [mA] 18 | 19 | uint16 flags # various configuration flags (see graphic below) 20 | uint16 FLAGS_RE_ENUM = 0 # force re-enumeration 21 | uint16 FLAGS_POWER_MODE = 2 # self-powered (1), bus-powered (0) 22 | 23 | int8[32] vendor_string # String containing the vendor name. 24 | # 32 ASCII bytes including 0-termination. 25 | int8[32] product_string # String containing the product name. 26 | # 32 ASCII bytes including 0-termination. 27 | int8[32] serial_number # String containing the serial number. 28 | # 32 ASCII bytes including 0-termination. 29 | # Changing the String fields requires special Host 30 | # drivers. 31 | -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfSTATUS.msg: -------------------------------------------------------------------------------- 1 | # ESF-STATUS (0x10 0x10) 2 | # External Sensor Fusion (ESF) status information 3 | # 4 | # Supported on UDR/ADR products 5 | # 6 | 7 | uint8 CLASS_ID = 16 8 | uint8 MESSAGE_ID = 16 9 | 10 | uint32 i_tow # GPS time of week of the navigation epoch [ms] 11 | uint8 version # Message version (2 for this version) 12 | 13 | uint8[7] reserved1 # Reserved 14 | 15 | uint8 fusion_mode # Fusion mode: 16 | uint8 FUSION_MODE_INIT = 0 # receiver is initializing some unknown values 17 | # required for doing sensor fusion 18 | uint8 FUSION_MODE_FUSION = 1 # GNSS and sensor data are 19 | # used for navigation solution computation 20 | uint8 FUSION_MODE_SUSPENDED = 2 # sensor fusion is temporarily disabled 21 | # due to e.g. invalid sensor data or detected 22 | # ferry 23 | uint8 FUSION_MODE_DISABLED = 3 # sensor fusion is permanently disabled 24 | # until receiver reset due e.g. to sensor 25 | # error 26 | 27 | uint8[2] reserved2 # Reserved 28 | 29 | uint8 num_sens # Number of sensors 30 | 31 | 32 | # Start of repeated block (numSens times) 33 | EsfSTATUSSens[] sens 34 | # End of repeated block 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RTK_GPS_NTRIP 2 | System built on ROS2 Humble distribution to utilize NTRIP for a RTK Global Navigation Satelite System (GNSS). 3 | 4 | RTCM corrections obtained through an NTRIP Client are passed through a ROS Topic and provided to the u-blox driver to obtain high accuracy in GPS/GNSS positioning. 5 | 6 | ### 1. u-blox Driver 7 | u-blox driver by KumarRobotics is used and can be found here https://github.com/KumarRobotics/ublox.git. The current configuration set is utilized for a ZED F9P receiver acting as a rover. 8 | 9 | To activate the ublox node, run the following command in the terminal: `ros2 launch ublox_gps ublox_gps_node-launch.py` 10 | 11 | ### 2. NTRIP Client 12 | NTRIP Client package by LORD-MicroStrain is used and can be found here https://github.com/LORD-MicroStrain/ntrip_client.git. Prior to running the command below, provide the NTRIP credentials (for access, if required) in the ntrip_client_launch.py file including the host, port, mountpoint, username, and password. 13 | 14 | To activate the NTRIP Client node, run the following command in the terminal: `ros2 launch ntrip_client ntrip_client_launch.py` 15 | 16 | ### 3. /fix to /nmea Parser 17 | As the NTRIP Client requires an NMEA message, the /fix type message is parsed into an /nmea type message by running the fix2nmea executable. It can be executed by running the following command in the terminal: `ros2 run fix2nmea fix2nmea` 18 | 19 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmEPH.msg: -------------------------------------------------------------------------------- 1 | # RXM-EPH (0x02 0x31) 2 | # GPS Aiding Ephemeris Input/Output Message 3 | # 4 | # This message is provided considered obsolete, please use AID-EPH instead! 5 | # - SF1D0 to SF3D7 is only sent if ephemeris is available for this SV. If not, 6 | # the payload may be reduced to 8 Bytes, or all bytes are set to zero, 7 | # indicating that this SV Number does not have valid ephemeris for the moment. 8 | # - SF1D0 to SF3D7 contain the 24 words following the Hand-Over Word ( HOW ) 9 | # from the GPS navigation message, subframes 1 to 3. See IS-GPS-200 for a 10 | # full description of the contents of the Subframes. 11 | # - In SF1D0 to SF3D7, the parity bits have been removed, and the 24 bits of 12 | # data are located in Bits 0 to 23. Bits 24 to 31 shall be ignored. 13 | # 14 | 15 | uint8 CLASS_ID = 2 16 | uint8 MESSAGE_ID = 49 17 | 18 | uint32 svid # SV ID for which this ephemeris data is (Valid Range: 1 .. 32). 19 | uint32 how # Hand-Over Word of first Subframe. This is 20 | # required if data is sent to the receiver. 21 | # 0 indicates that no Ephemeris Data is following. 22 | 23 | # Start of optional block 24 | uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) 25 | uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) 26 | uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) 27 | # End of optional block 28 | -------------------------------------------------------------------------------- /ublox/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ublox 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.0 (2022-04-13) 6 | ------------------ 7 | 8 | 2.0.0 (2020-10-13) 9 | ------------------ 10 | * Initial ROS 2 port to Dashing 11 | * Port the ublox package to ROS 2. 12 | * Start ROS 2 port by COLCON_IGNORE everything. 13 | * Contributors: Chao Qu, Chris Lalancette 14 | 15 | 1.2.0 (2019-11-19) 16 | ------------------ 17 | 18 | 1.1.2 (2017-08-02) 19 | ------------------ 20 | * README and package xml updates 21 | * Contributors: Veronica Lane 22 | 23 | 1.1.0 (2017-07-17) 24 | ------------------ 25 | * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter 26 | * README and ublox package version 27 | * Contributors: Veronica Lane 28 | 29 | 1.0.0 (2017-06-23) 30 | ------------------ 31 | * added myself as maintainer to package xmls and updated version numbers of modified packages. 32 | * Contributors: Veronica Lane 33 | 34 | 0.0.5 (2016-08-06) 35 | ------------------ 36 | 37 | 0.0.4 (2014-12-08) 38 | ------------------ 39 | 40 | 0.0.3 (2014-10-18) 41 | ------------------ 42 | * Updated readme to reflect changes 43 | * Contributors: Gareth Cross 44 | 45 | 0.0.2 (2014-10-03) 46 | ------------------ 47 | 48 | 0.0.1 (2014-08-15) 49 | ------------------ 50 | 51 | 0.0.0 (2014-06-23) 52 | ------------------ 53 | * ublox: first commit 54 | * Contributors: Chao Qu 55 | -------------------------------------------------------------------------------- /ublox_msgs/msg/AidALM.msg: -------------------------------------------------------------------------------- 1 | # AID-ALM (0x0B 0x30) 2 | # GPS Aiding Almanach Input/Output Message 3 | # 4 | # All UBX-AID messages are deprecated; use UBX-MGA messages instead 5 | # - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not 6 | # available for the given SV. This may happen even if NAV-SVINFO and RXM-SVSI 7 | # are indicating almanac availability as the internal data may not represent 8 | # the content of an original broadcast almanac (or only parts thereof). 9 | # - DWORD0 to DWORD7 contain the 8 words following the Hand-Over Word ( HOW ) 10 | # from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or 11 | # pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the 12 | # contents of the Almanac pages. 13 | # - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of 14 | # data are located in Bits 0 to 23. Bits 24 to 31 shall be ignored. 15 | # - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, 16 | # Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas 17 | # Bit 0 is the LSB. 18 | 19 | uint8 CLASS_ID = 11 20 | uint8 MESSAGE_ID = 48 21 | 22 | uint32 svid # SV ID for which the receiver shall return its 23 | # Almanac Data (Valid Range: 1 .. 32 or 51, 56, 63). 24 | uint32 week # Issue Date of Almanach (GPS week number) 25 | 26 | # Start of optional block 27 | uint32[] dwrd # Almanac Words 28 | # End of optional block 29 | -------------------------------------------------------------------------------- /ublox_msgs/msg/TimTM2.msg: -------------------------------------------------------------------------------- 1 | # TIM-TM2 (0x0D, 0x03) 2 | # Time mark data 3 | # 4 | # Description for details. 5 | # 6 | # Supported on: 7 | # - u-blox 8 / u-blox M8 with protocol version 22 (only with Timing Products) 8 | # 9 | 10 | uint8 CLASS_ID = 13 11 | uint8 MESSAGE_ID = 3 12 | 13 | uint8 ch # Channel (i.e. EXTINT) upon which the pulse was measured 14 | 15 | uint8 flags # Bitmask [newRisingEdge, time, utc, timeBase, , newFallingEdge, run, mode] 16 | uint8 FLAGS_MODE_RUNNING = 1 # single = 0, running = 1 17 | uint8 FLAGS_RUN = 2 # armed = 0, stopped = 1 18 | uint8 FLAGS_NEWFALLINGEDGE = 4 # new falling edge detected 19 | uint8 FLAGS_TIMEBASE_GNSS = 8 # 0 = time base is receiver time, 1 = time base is GNSS Time (according to the configuration in CFG-TP5 for tpldx= 0) 20 | uint8 FLAGS_TIMEBASE_UTC = 16 # Time Base is UTC (the variant according to the configuration in CFG-NAV5 21 | uint8 FLAGS_UTC_AVAIL = 32 # 0 = utc not available, 1 = utc available 22 | uint8 FLAGS_TIME_VALID = 64 # 0 = time is not valid, 1 time is valid 23 | uint8 FLAGS_NEWRISINGEDGE = 128 # new rising edge detected 24 | 25 | uint16 rising_edge_count # rising edge count 26 | uint16 wn_r # week number of last rising edge 27 | uint16 wn_f # week number of last falling edge 28 | uint32 tow_ms_r # Tow of rising edge 29 | uint32 tow_sub_ms_r # Millisecond Fraction of Tow of rising edge in nanoseconds 30 | uint32 tow_ms_f # tow of falling edge 31 | uint32 tow_sub_ms_f # millisecond fraction of tow of falling edge in nanoseconds 32 | uint32 acc_est # Accuracy estimate 33 | -------------------------------------------------------------------------------- /rtcm_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | rtcm_msgs 7 | 1.1.6 8 | The rtcm_msgs package contains messages related to data in the RTCM format. 9 | Marek Materzok 10 | BSD 11 | Marek Materzok 12 | 13 | catkin 14 | ament_cmake 15 | rosidl_default_generators 16 | 17 | ros_environment 18 | message_generation 19 | builtin_interfaces 20 | 21 | std_msgs 22 | 23 | message_runtime 24 | rosidl_default_runtime 25 | 26 | ament_lint_common 27 | 28 | rosidl_interface_packages 29 | 30 | 31 | catkin 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgDAT.msg: -------------------------------------------------------------------------------- 1 | # CFG-DAT (0x06 0x06) 2 | # Set User-defined Datum 3 | # 4 | # For more information see the description of Geodetic Systems and Frames. 5 | # 6 | 7 | uint8 CLASS_ID = 6 8 | uint8 MESSAGE_ID = 6 9 | 10 | # Only for GET, these values are not used for write 11 | uint16 datum_num # Datum Number: 0 = WGS84, 0xFFFF = user-defined 12 | uint16 DATUM_NUM_WGS84 = 0 13 | uint16 DATUM_NUM_USER = 65535 14 | 15 | uint8[6] datum_name # ASCII String: WGS84 or USER 16 | 17 | float64 maj_a # Semi-major Axis [m] 18 | # accepted range = 6,300,000.0 to 6,500,000.0 meters 19 | float64 flat # 1.0 / Flattening 20 | # accepted range is 0.0 to 500.0 21 | 22 | float32 d_x # X Axis shift at the origin [m] 23 | # accepted range is +/- 5000.0 meters 24 | float32 d_y # Y Axis shift at the origin [m] 25 | # accepted range is +/- 5000.0 meters 26 | float32 d_z # Z Axis shift at the origin [m] 27 | # accepted range is +/- 5000.0 meters 28 | 29 | float32 rot_x # Rotation about the X Axis [s] 30 | # accepted range is +/- 20.0 milli-arc seconds 31 | float32 rot_y # Rotation about the Y Axis [s] 32 | # accepted range is +/- 20.0 milli-arc seconds 33 | float32 rot_z # Rotation about the Z Axis [s] 34 | # accepted range is +/- 20.0 milli-arc seconds 35 | 36 | float32 scale # Scale change [ppm] 37 | # accepted range is 0.0 to 50.0 parts per million 38 | -------------------------------------------------------------------------------- /rtcm_msgs/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Marek Materzok 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /ntrip_client/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ntrip_client 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2022-08-11) 6 | ------------------ 7 | * Adds the ability to configure the NTRIP client to connect using SSL (`#23 `_) 8 | * ROS automatic reconnect (`#18 `_) 9 | * Adds ability to reconnect to ROS 10 | * Allows the timeouts and other parameters to be modified via the launch file 11 | * Moves optional config out of constructor, and declares constnats in class definition 12 | * Contributors: Rob 13 | 14 | 1.1.0 (2022-04-22) 15 | ------------------ 16 | * ROS Ntrip Version Configuration (`#8 `_) 17 | * No longer sends Ntrip-Version header if not specified 18 | * Adds some hopefully helpful debug logging 19 | * Properly handles responses that have both a failure response and success response 20 | * Adds ability to print debug logs via launch parameter 21 | * Contributors: robbiefish 22 | 23 | 1.0.2 (2022-02-10) 24 | ------------------ 25 | * Checks if there is a * character in the string before parsing fully 26 | * Contributors: robbiefish 27 | 28 | 1.0.1 (2022-01-20) 29 | ------------------ 30 | * Replaces ROS timer destroy methods with ROS2 methods and removes extra destroy 31 | * Contributors: robbiefish 32 | 33 | 1.0.0 (2021-12-09) 34 | ------------------ 35 | * Initial implementation of ROS2 NTRIP client 36 | * Adds ability to cache packets if they do contain some of a mesage but not the whole thing 37 | * Contributors: drobb257, nathanmillerparker, robbiefish 38 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_HP_POS_REC_PRODUCT_HPP 2 | #define UBLOX_GPS_HP_POS_REC_PRODUCT_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace ublox_node { 19 | 20 | class HpPosRecProduct final : public virtual HpgRefProduct { 21 | public: 22 | explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node); 23 | 24 | /** 25 | * @brief Subscribe to Rover messages, such as NavRELPOSNED. 26 | */ 27 | void subscribe(std::shared_ptr gps) override; 28 | 29 | private: 30 | 31 | /** 32 | * @brief Set the last received message and call rover diagnostic updater 33 | * 34 | * @details Publish received NavRELPOSNED messages if enabled 35 | */ 36 | void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m); 37 | 38 | sensor_msgs::msg::Imu imu_; 39 | 40 | //! Last relative position (used for diagnostic updater) 41 | ublox_msgs::msg::NavRELPOSNED9 last_rel_pos_; 42 | 43 | rclcpp::Publisher::SharedPtr nav_relposned_pub_; 44 | rclcpp::Publisher::SharedPtr imu_pub_; 45 | 46 | std::string frame_id_; 47 | }; 48 | 49 | } // namespace ublox_node 50 | 51 | #endif // UBLOX_GPS_HP_POS_REC_PRODUCT_HPP 52 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgSBAS.msg: -------------------------------------------------------------------------------- 1 | # CFG-SBAS (0x06 0x16) 2 | # SBAS Configuration 3 | # 4 | # This message configures the SBAS receiver subsystem (i.e. WAAS, EGNOS, MSAS). 5 | # See the SBAS Configuration Settings Description for a detailed description of 6 | # how these settings affect receiver operation 7 | # 8 | 9 | uint8 CLASS_ID = 6 10 | uint8 MESSAGE_ID = 22 11 | 12 | uint8 mode # SBAS Mode 13 | uint8 MODE_ENABLED = 1 # SBAS Enabled (1) / Disabled (0) 14 | # This field is deprecated; use UBX-CFG-GNSS to 15 | # enable/disable SBAS operation 16 | uint8 MODE_TEST = 2 # SBAS Testbed: Use data anyhow (1) / Ignore data when 17 | # in Test Mode (SBAS Msg 0) 18 | 19 | uint8 usage # SBAS Usage 20 | uint8 USAGE_RANGE = 1 # Use SBAS GEOs as a ranging source (for navigation) 21 | uint8 USAGE_DIFF_CORR = 2 # Use SBAS Differential Corrections 22 | uint8 USAGE_INTEGRITY = 4 # Use SBAS Integrity Information 23 | 24 | uint8 max_sbas # Maximum Number of SBAS prioritized tracking 25 | # channels (valid range: 0 - 3) to use 26 | # (obsolete and superseeded by UBX-CFG-GNSS in protocol 27 | # versions 14+). 28 | 29 | 30 | uint8 scanmode2 # Continuation of scanmode bitmask below 31 | # PRN 152...158 32 | uint32 scanmode1 # Which SBAS PRN numbers to search for (Bitmask) 33 | # If all Bits are set to zero, auto-scan (i.e. all valid 34 | # PRNs) are searched. Every bit corresponds to a PRN 35 | # number. 36 | # PRN 120..151 37 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/ublox_firmware.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_UBLOX_FIRMWARE_HPP 2 | #define UBLOX_GPS_UBLOX_FIRMWARE_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | // This file declares UbloxFirmware is an abstract class which implements 13 | // ComponentInterface and functions generic to all firmware (such as the 14 | // initializing the fix diagnostics). 15 | 16 | namespace ublox_node { 17 | 18 | /** 19 | * @brief This abstract class represents a firmware component. 20 | * 21 | * @details The Firmware components update the fix diagnostics. 22 | */ 23 | class UbloxFirmware : public virtual ComponentInterface { 24 | public: 25 | //! Subscribe Rate for u-blox SV Info messages 26 | constexpr static uint32_t kNavSvInfoSubscribeRate = 20; 27 | 28 | explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node); 29 | 30 | /** 31 | * @brief Add the fix diagnostics to the updater. 32 | */ 33 | void initializeRosDiagnostics() override; 34 | 35 | protected: 36 | /** 37 | * @brief Handle to send fix status to ROS diagnostics. 38 | */ 39 | virtual void fixDiagnostic( 40 | diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; 41 | 42 | std::shared_ptr updater_; 43 | std::shared_ptr gnss_; 44 | //! The fix status service type, set in the Firmware Component 45 | //! based on the enabled GNSS 46 | int fix_status_service_{0}; 47 | rclcpp::Node* node_; 48 | }; 49 | 50 | } // namespace ublox_node 51 | 52 | #endif // UBLOX_GPS_UBLOX_FIRMWARE_HPP 53 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgRST.msg: -------------------------------------------------------------------------------- 1 | # CFG-RST (0x06 0x04) 2 | # Reset Receiver / Clear Backup Data Structures 3 | # 4 | # Don't expect this message to be acknowledged by the receiver. 5 | # - Newer FW version won't acknowledge this message at all. 6 | # - Older FW version will acknowledge this message but the acknowledge may not 7 | # be sent completely before the receiver is reset. 8 | # 9 | 10 | uint8 CLASS_ID = 6 11 | uint8 MESSAGE_ID = 4 12 | 13 | uint16 nav_bbr_mask # BBR Sections to clear. 14 | # The following Special Sets apply: 15 | uint16 NAV_BBR_HOT_START = 0 # Hot start the device 16 | uint16 NAV_BBR_WARM_START = 1 # Warm start the device 17 | uint16 NAV_BBR_COLD_START = 65535 # Cold start the device 18 | uint16 NAV_BBR_EPH = 1 # Ephemeris 19 | uint16 NAV_BBR_ALM = 2 # Almanac 20 | uint16 NAV_BBR_HEALTH = 4 # Health 21 | uint16 NAV_BBR_KLOB = 8 # Klobuchar parameters 22 | uint16 NAV_BBR_POS = 16 # Position 23 | uint16 NAV_BBR_CLKD = 32 # Clock Drift 24 | uint16 NAV_BBR_OSC = 64 # Oscillator Parameter 25 | uint16 NAV_BBR_UTC = 128 # UTC Correction + GPS Leap Seconds Parameters 26 | uint16 NAV_BBR_RTC = 256 # RTC 27 | uint16 NAV_BBR_AOP = 32768 # Autonomous Orbit Parameters 28 | 29 | uint8 reset_mode # Reset Type: 30 | uint8 RESET_MODE_HW_IMMEDIATE = 0 # Hardware reset (Watchdog) immediately 31 | uint8 RESET_MODE_SW = 1 # Controlled Software reset 32 | uint8 RESET_MODE_GNSS = 2 # Controlled Software reset (GNSS only) 33 | uint8 RESET_MODE_HW_AFTER_SHUTDOWN = 4 # Hardware reset (Watchdog) after 34 | # shutdown 35 | uint8 RESET_MODE_GNSS_STOP = 8 # Controlled GNSS stop 36 | uint8 RESET_MODE_GNSS_START = 9 # Controlled GNSS start 37 | 38 | uint8 reserved1 # Reserved 39 | -------------------------------------------------------------------------------- /ublox_gps/config/neo_m8u_rover.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for NEO-M8U device 2 | 3 | ublox_gps_node: 4 | ros__parameters: 5 | debug: 1 # Range 0-4 (0 means no debug statements will print) 6 | 7 | save: 8 | mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver 9 | # Manager, Antenna, and Logging Configuration 10 | device: 4 # Save to EEPROM 11 | 12 | device: /dev/ttyACM0 13 | frame_id: m8u 14 | rate: 4 # in Hz 15 | nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may 16 | # be either 5 Hz (Dual constellation) or 17 | # 8 Hz (GPS only) 18 | dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), 19 | # Max Alt: 50km 20 | # Max Horizontal Velocity: 250 m/s, 21 | # Max Vertical Velocity: 100 m/s 22 | fix_mode: 3 23 | enable_ppp: true 24 | dr_limit: 1 25 | 26 | uart1: 27 | baudrate: 115200 # NEO-M8U specific 28 | in: 16 # RTCM 3 29 | out: 16 # No UART out for rover 30 | 31 | gnss: 32 | glonass: true # Supported by NEO-M8U 33 | beidou: false # Supported by NEO-M8U 34 | qzss: false # Supported by NEO-M8U 35 | 36 | dgnss_mode: 3 # Fixed mode 37 | 38 | inf: 39 | all: true # Whether to display all INF messages in console 40 | 41 | # Enable u-blox message publishers 42 | publish: 43 | all: true 44 | esf: true 45 | aid: 46 | hui: false 47 | nav: 48 | posecef: false 49 | -------------------------------------------------------------------------------- /ublox_gps/config/c94_m8t_rover.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for C94-M8T device 2 | ublox_gps_node: 3 | ros__parameters: 4 | 5 | debug: 1 # Range 0-4 (0 means no debug statements will print) 6 | 7 | save: 8 | mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver 9 | # Manager, Antenna, and Logging Configuration 10 | device: 4 # Save to EEPROM 11 | 12 | device: /dev/ttyACM1 13 | frame_id: gps 14 | rate: 4 # in Hz 15 | nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may 16 | # be either 5 Hz (Dual constellation) or 17 | # 8 Hz (GPS only) 18 | dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), 19 | # Max Alt: 50km 20 | # Max Horizontal Velocity: 250 m/s, 21 | # Max Vertical Velocity: 100 m/s 22 | fix_mode: auto 23 | enable_ppp: false # Not supported by C94-M8T 24 | dr_limit: 0 25 | 26 | uart1: 27 | baudrate: 19200 # C94-M8P specific 28 | in: 32 # RTCM 3 29 | out: 0 # No UART out for rover 30 | 31 | gnss: 32 | glonass: true # Supported by C94-M8P 33 | beidou: false # Supported by C94-M8P 34 | qzss: false # Supported by C94-M8P 35 | 36 | dgnss_mode: 3 # Fixed mode 37 | 38 | inf: 39 | all: true # Whether to display all INF messages in console 40 | 41 | # Enable u-blox message publishers 42 | publish: 43 | all: true 44 | aid: 45 | hui: false 46 | 47 | nav: 48 | posecef: false 49 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSOL.msg: -------------------------------------------------------------------------------- 1 | # NAV-SOL (0x01 0x06) 2 | # Navigation Solution Information 3 | # 4 | # This message combines Position, velocity and time solution in ECEF, including 5 | # accuracy figures 6 | # This message has only been retained for backwards compatibility; users are 7 | # recommended to use the UBX-NAV-PVT message in preference. 8 | # 9 | 10 | uint8 CLASS_ID = 1 11 | uint8 MESSAGE_ID = 6 12 | 13 | uint32 i_tow # GPS Millisecond time of week [ms] 14 | int32 f_tow # Fractional Nanoseconds remainder of rounded 15 | # ms above, range -500000 .. 500000 [ns] 16 | int16 week # GPS week (GPS time) 17 | 18 | uint8 gps_fix # GPSfix Type, range 0..5 19 | uint8 GPS_NO_FIX = 0 20 | uint8 GPS_DEAD_RECKONING_ONLY = 1 21 | uint8 GPS_2D_FIX = 2 22 | uint8 GPS_3D_FIX = 3 23 | uint8 GPS_GPS_DEAD_RECKONING_COMBINED = 4 24 | uint8 GPS_TIME_ONLY_FIX = 5 25 | 26 | uint8 flags # Fix Status Flags 27 | uint8 FLAGS_GPS_FIX_OK = 1 # Fix within limits i.e. within DOP & ACC Masks 28 | uint8 FLAGS_DIFF_SOLN = 2 # DGPS used 29 | uint8 FLAGS_WKNSET = 4 # Week Number valid 30 | uint8 FLAGS_TOWSET = 8 # Time of Week valid 31 | 32 | int32 ecef_x # ECEF X coordinate [cm] 33 | int32 ecef_y # ECEF Y coordinate [cm] 34 | int32 ecef_z # ECEF Z coordinate [cm] 35 | uint32 p_acc # 3D Position Accuracy Estimate [cm] 36 | int32 ecef_vx # ECEF X velocity [cm/s] 37 | int32 ecef_vy # ECEF Y velocity [cm/s] 38 | int32 ecef_vz # ECEF Z velocity [cm/s] 39 | uint32 s_acc # Speed Accuracy Estimate [cm/s] 40 | uint16 p_dop # Position DOP [1 / 0.01] 41 | uint8 reserved1 # Reserved 42 | uint8 num_sv # Number of SVs used in Nav Solution 43 | uint32 reserved2 # Reserved 44 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavTIMEUTC.msg: -------------------------------------------------------------------------------- 1 | # NAV-TIMEUTC (0x01 0x21) 2 | # UTC Time Solution 3 | # 4 | 5 | uint8 CLASS_ID = 1 6 | uint8 MESSAGE_ID = 33 7 | 8 | uint32 i_tow # GPS Millisecond time of week [ms] 9 | 10 | uint32 t_acc # Time Accuracy Estimate [ns] 11 | int32 nano # Fraction of second, range -1e9 .. 1e9 (UTC) [ns] 12 | uint16 year # Year, range 1999..2099 (UTC) [y] 13 | uint8 month # Month, range 1..12 (UTC) [month] 14 | uint8 day # Day of Month, range 1..31 (UTC) [d] 15 | uint8 hour # Hour of Day, range 0..23 (UTC) [h] 16 | uint8 min # Minute of Hour, range 0..59 (UTC) [min] 17 | uint8 sec # Seconds of Minute, range 0..60 (UTC) [s] (60 for 18 | # leap second) 19 | 20 | uint8 valid # Validity Flags 21 | uint8 VALID_TOW = 1 # Valid Time of Week 22 | uint8 VALID_WKN = 2 # Valid Week Number 23 | uint8 VALID_UTC = 4 # Valid Leap Seconds, i.e. Leap Seconds already known 24 | uint8 VALID_UTC_STANDARD_MASK = 240 # UTC standard Identifier Bit mask: 25 | uint8 UTC_STANDARD_NOT_AVAILABLE = 0 # Information not available 26 | uint8 UTC_STANDARD_CRL = 16 # Communications Research Labratory 27 | uint8 UTC_STANDARD_NIST = 32 # National Institute of Standards and 28 | # Technology (NIST) 29 | uint8 UTC_STANDARD_USNO = 48 # U.S. Naval Observatory (USNO) 30 | uint8 UTC_STANDARD_BIPM = 64 # International Bureau of Weights and 31 | # Measures (BIPM) 32 | uint8 UTC_STANDARD_EL = 80 # European Laboratory (tbd) 33 | uint8 UTC_STANDARD_SU = 96 # Former Soviet Union (SU) 34 | uint8 UTC_STANDARD_NTSC = 112 # National Time Service Center, China 35 | uint8 UTC_STANDARD_UNKNOWN = 240 36 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgNMEA6.msg: -------------------------------------------------------------------------------- 1 | # CFG-NMEA (0x06 0x17) 2 | # NMEA protocol configuration 3 | # 4 | # Set/Get the NMEA protocol configuration. See section NMEA Protocol 5 | # Configuration for a detailed description of the configuration effects on 6 | # NMEA output 7 | # 8 | # Supported on u-blox 6 from firmware version 6.00 up to version 7.03. 9 | # 10 | 11 | uint8 CLASS_ID = 6 12 | uint8 MESSAGE_ID = 23 13 | 14 | uint8 filter # filter flags 15 | uint8 FILTER_POS = 1 # Disable position filtering 16 | uint8 FILTER_MSK_POS = 2 # Disable masked position filtering 17 | uint8 FILTER_TIME = 4 # Disable time filtering 18 | uint8 FILTER_DATE = 8 # Disable date filtering 19 | uint8 FILTER_SBAS_FILT = 16 # Enable SBAS filtering 20 | uint8 FILTER_TRACK = 32 # Disable track filtering 21 | 22 | uint8 version # NMEA version 23 | uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 24 | uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 25 | 26 | uint8 num_sv # Maximum Number of SVs to report in NMEA 27 | # protocol. 28 | # This does not affect the receiver's operation. 29 | # It only limits the number of SVs reported in 30 | # NMEA mode (this might be needed with older 31 | # mapping applications which only support 8- or 32 | # 12-channel receivers) 33 | 34 | uint8 flags # flags 35 | uint8 FLAGS_COMPAT = 1 # enable compatibility mode. 36 | # This might be needed for certain applications 37 | # when customer's NMEA parser expects a fixed 38 | # number of digits in position coordinates 39 | uint8 FLAGS_CONSIDER = 2 # enable considering mode 40 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgRATE.msg: -------------------------------------------------------------------------------- 1 | # CFG-RATE (0x06 0x08) 2 | # Navigation/Measurement Rate Settings 3 | # 4 | # This message allows the user to alter the rate at which navigation solutions 5 | # (and the measurements that they depend on) are generated by the receiver. The 6 | # calculation of the navigation solution will always be aligned to the top of a 7 | # second zero (first second of the week) of the configured reference time 8 | # system. For protocol version 18 and later the navigation period is an integer 9 | # multiple of the measurement period. 10 | # - Each measurement triggers the measurements generation and raw data output. 11 | # - The navRate value defines that every nth measurement triggers a navigation 12 | # epoch. 13 | # - The update rate has a direct influence on the power consumption. The more 14 | # fixes that are required, the more CPU power and communication resources 15 | # are required. 16 | # - For most applications a 1 Hz update rate would be sufficient. 17 | # - When using Power Save Mode, measurement and navigation rate can differ from 18 | # the values configured here 19 | 20 | uint8 CLASS_ID = 6 21 | uint8 MESSAGE_ID = 8 22 | 23 | uint16 meas_rate # Measurement Rate, GPS measurements are 24 | # taken every measRate milliseconds [ms] 25 | uint16 nav_rate # Navigation Rate, in number of measurement 26 | # cycles. On u-blox 5 and u-blox 6, this parameter 27 | # cannot be changed, and always equals 1. 28 | uint16 time_ref # The time system to which measurements are aligned 29 | uint16 TIME_REF_UTC = 0 30 | uint16 TIME_REF_GPS = 1 31 | uint16 TIME_REF_GLONASS = 2 # not supported in protocol versions less than 18 32 | uint16 TIME_REF_BEIDOU = 3 # not supported in protocol versions less than 18 33 | uint16 TIME_REF_GALILEO = 4 # not supported in protocol versions less than 18 34 | -------------------------------------------------------------------------------- /ublox_msgs/msg/UpdSOSAck.msg: -------------------------------------------------------------------------------- 1 | # UPD-SOS (0x09 0x14) 2 | # 3 | # Backup File Creation Acknowledge / System Restored from Backup 4 | # 5 | # Firmware Supported on: 6 | # u-blox 8 / u-blox M8 from protocol version 15 up to version 23.01 7 | # 8 | 9 | uint8 CLASS_ID = 9 10 | uint8 MESSAGE_ID = 20 11 | 12 | uint8 cmd # Command 13 | uint8 CMD_BACKUP_CREATE_ACK = 2 # Backup File Creation Acknowledge 14 | # The message is sent from the device as 15 | # confirmation of creation of a backup file 16 | # in flash. The host can safely shut down the 17 | # device after received this message. 18 | uint8 CMD_SYSTEM_RESTORED = 3 # System Restored from Backup 19 | # The message is sent from the device to 20 | # notify the host the BBR has been restored 21 | # from a backup file in flash. The host 22 | # should clear the backup file after 23 | # receiving this message. If the UBX-UPD-SOS 24 | # message is polled, this message will be 25 | # present. 26 | 27 | uint8[3] reserved0 # Reserved 28 | 29 | uint8 response # Response: 30 | uint8 BACKUP_CREATE_NACK = 0 # Not acknowledged 31 | uint8 BACKUP_CREATE_ACK = 1 # Acknowledged 32 | uint8 SYSTEM_RESTORED_RESPONSE_UNKNOWN = 0 # Unknown 33 | uint8 SYSTEM_RESTORED_RESPONSE_FAILED = 1 # Failed restoring from backup 34 | # file 35 | uint8 SYSTEM_RESTORED_RESPONSE_RESTORED = 2 # Restored from backup file 36 | uint8 SYSTEM_RESTORED_RESPONSE_NOT_RESTORED = 3 # Not restored (no backup) 37 | 38 | uint8[3] reserved1 # Reserved 39 | -------------------------------------------------------------------------------- /ublox_msgs/msg/AidEPH.msg: -------------------------------------------------------------------------------- 1 | # AID-EPH (0x0B 0x31) 2 | # GPS Aiding Ephemeris Input/Output Message 3 | # 4 | # All UBX-AID messages are deprecated; use UBX-MGA messages instead 5 | # - SF1D0 to SF3D7 is only sent if ephemeris is available for this SV. If not, the payload may 6 | # be reduced to 8 Bytes, or all bytes are set to zero, indicating that this SV Number does 7 | # not have valid ephemeris for the moment. This may happen even if NAV-SVINFO and 8 | # RXM-SVSI are indicating ephemeris availability as the internal data may not represent the 9 | # content of an original broadcast ephemeris (or only parts thereof). 10 | # - SF1D0 to SF3D7 contain the 24 words following the Hand-Over Word ( HOW ) from the 11 | # GPS navigation message, subframes 1 to 3. The Truncated TOW Count is not valid and 12 | # cannot be used. See IS-GPS-200 for a full description of the contents of the Subframes. 13 | # - In SF1D0 to SF3D7, the parity bits have been removed, and the 24 bits of data are 14 | # located in Bits 0 to 23. Bits 24 to 31 shall be ignored. 15 | # - When polled, the data contained in this message does not represent the full original 16 | # ephemeris broadcast. Some fields that are irrelevant to u-blox receivers may be missing. 17 | # The week number in Subframe 1 has already been modified to match the Time Of 18 | # Ephemeris (TOE). 19 | 20 | uint8 CLASS_ID = 11 21 | uint8 MESSAGE_ID = 49 22 | 23 | uint32 svid # SV ID for which this ephemeris data is 24 | # (Valid Range: 1 .. 32). 25 | uint32 how # Hand-Over Word of first Subframe. This is 26 | # required if data is sent to the receiver. 27 | # 0 indicates that no Ephemeris Data is following. 28 | 29 | # Start of optional block 30 | uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) 31 | uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) 32 | uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) 33 | # End of optional block 34 | -------------------------------------------------------------------------------- /ublox_msgs/msg/AidHUI.msg: -------------------------------------------------------------------------------- 1 | # AID-HUI (0x0B 0x02) 2 | # GPS Health, UTC and ionosphere parameters 3 | # 4 | # All UBX-AID messages are deprecated; use UBX-MGA messages instead. 5 | # This message contains a health bit mask, UTC time and Klobuchar parameters. For more 6 | # information on these parameters, please see the ICD-GPS-200 documentation. 7 | 8 | uint8 CLASS_ID = 11 9 | uint8 MESSAGE_ID = 2 10 | 11 | uint32 health # Bitmask, every bit represents a GPS SV (1-32). 12 | # If the bit is set the SV is healthy. 13 | float64 utc_a0 # UTC - parameter A0 14 | float64 utc_a1 # UTC - parameter A1 15 | int32 utc_tow # UTC - reference time of week 16 | int16 utc_wnt # UTC - reference week number 17 | int16 utc_ls # UTC - time difference due to leap seconds before event 18 | int16 utc_wnf # UTC - week number when next leap second event occurs 19 | int16 utc_dn # UTC - day of week when next leap second event occurs 20 | int16 utc_lsf # UTC - time difference due to leap seconds after event 21 | int16 utc_spare # UTC - Spare to ensure structure is a multiple of 4 22 | # bytes 23 | float32 klob_a0 # Klobuchar - alpha 0 [s] 24 | float32 klob_a1 # Klobuchar - alpha 1 [s/semicircle] 25 | float32 klob_a2 # Klobuchar - alpha 2 [s/semicircle^2] 26 | float32 klob_a3 # Klobuchar - alpha 3 [s/semicircle^3] 27 | float32 klob_b0 # Klobuchar - beta 0 [s] 28 | float32 klob_b1 # Klobuchar - beta 1 [s/semicircle] 29 | float32 klob_b2 # Klobuchar - beta 2 [s/semicircle^2] 30 | float32 klob_b3 # Klobuchar - beta 3 [s/semicircle^3] 31 | uint32 flags # flags 32 | uint32 FLAGS_HEALTH = 1 # Healthmask field in this message is valid 33 | uint32 FLAGS_UTC = 2 # UTC parameter fields in this message are valid 34 | uint32 FLAGS_KLOB = 4 # Klobuchar parameter fields in this message are 35 | # valid 36 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgCFG.msg: -------------------------------------------------------------------------------- 1 | # CFG-CFG (0x06 0x09) 2 | # Clear, Save and Load configurations 3 | # 4 | 5 | uint8 CLASS_ID = 6 6 | uint8 MESSAGE_ID = 9 7 | 8 | uint32 clear_mask # Mask with configuration sub-sections to Clear 9 | # (=Load Default Configurations to Permanent 10 | # Configurations in non-volatile memory) 11 | uint32 save_mask # Mask with configuration sub-section to Save 12 | # (=Save Current Configuration to Non-volatile 13 | # Memory) 14 | uint32 load_mask # Mask with configuration sub-sections to Load 15 | # (=Load Permanent Configurations from 16 | # Non-volatile Memory to Current Configurations) 17 | 18 | uint32 MASK_IO_PORT = 1 # Communications port settings. Modifying this 19 | # sub-section results in an IO system reset. 20 | # Because of this undefined data may be output 21 | # for a short period of time after receiving the 22 | # message. 23 | uint32 MASK_MSG_CONF = 2 # Message Configuration 24 | uint32 MASK_INF_MSG = 4 # INF Message Configuration 25 | uint32 MASK_NAV_CONF = 8 # Navigation Configuration 26 | uint32 MASK_RXM_CONF = 16 # Receiver Manager Configuration 27 | uint32 MASK_SEN_CONF = 256 # Sensor Interface Configuration, protocol >= 19 28 | uint32 MASK_RINV_CONF = 512 # Remote Inventory Configuration 29 | uint32 MASK_ANT_CONF = 1024 # Antenna Configuration 30 | uint32 MASK_LOG_CONF = 2048 # Logging Configuration 31 | uint32 MASK_FTS_CONF = 4096 # FTS Configuration. Only applicable to the 32 | # FTS product variant. 33 | 34 | uint8 device_mask # Mask which selects the devices for this command 35 | uint8 DEV_BBR = 1 # device battery backed RAM 36 | uint8 DEV_FLASH = 2 # device Flash 37 | uint8 DEV_EEPROM = 4 # device EEPROM 38 | uint8 DEV_SPI_FLASH = 16 # device SPI Flash 39 | -------------------------------------------------------------------------------- /ublox_msgs/msg/EsfINS.msg: -------------------------------------------------------------------------------- 1 | # ESF-INS (0x10 0x15) 2 | # Vehicle dynamics information 3 | # 4 | # This message outputs information about vehicle dynamics computed by the 5 | # Inertial Navigation System (INS) during ESF-based navigation. 6 | # For ADR products, the output dynamics information (angular rates and 7 | # accelerations) is expressed with respect to the vehicle-frame. 8 | # For UDR products, the output dynamics information (angular rates and 9 | # accelerations) is expressed with respect to the body-frame. 10 | # 11 | 12 | uint8 CLASS_ID = 16 13 | uint8 MESSAGE_ID = 21 14 | 15 | uint32 bitfield0 # Bitfield (see graphic below) 16 | uint32 BITFIELD0_VERSION = 255 # Message version (1 for this version). 17 | uint32 BITFIELD0_X_ANG_RATE_VALID = 256 # Compensated x-axis angular rate data 18 | # validity flag 19 | uint32 BITFIELD0_Y_ANG_RATE_VALID = 512 # Compensated y-axis angular rate data 20 | # validity flag 21 | uint32 BITFIELD0_Z_ANG_RATE_VALID = 1024 # Compensated z-axis angular rate data 22 | # validity flag 23 | uint32 BITFIELD0_X_ACCEL_VALID = 2048 # Compensated x-axis acceleration data 24 | # validity flag 25 | uint32 BITFIELD0_Y_ACCEL_VALID = 4096 # Compensated y-axis acceleration data 26 | # validity flag 27 | uint32 BITFIELD0_Z_ACCEL_VALID = 8192 # Compensated z-axis acceleration data 28 | # validity flag 29 | 30 | uint8[4] reserved1 # Reserved 31 | 32 | uint32 i_tow # GPS time of week of the navigation epoch [ms] 33 | int32 x_ang_rate # Compensated x-axis angular rate [deg/s / 1e-3] 34 | int32 y_ang_rate # Compensated y-axis angular rate [deg/s / 1e-3] 35 | int32 z_ang_rate # Compensated z-axis angular rate [deg/s / 1e-3] 36 | int32 x_accel # Compensated x-axis acceleration (gravity-free) [mg] 37 | int32 y_accel # Compensated y-axis acceleration (gravity-free) [mg] 38 | int32 z_accel # Compensated z-axis acceleration (gravity-free) [mg] 39 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmRAWX.msg: -------------------------------------------------------------------------------- 1 | # RXM-RAWX (0x02 0x15) 2 | # Multi-GNSS Raw Measurement Data 3 | # 4 | # This message contains the information needed to be able to generate a RINEX 3 5 | # multi-GNSS observation file. 6 | # This message contains pseudorange, Doppler, carrier phase, phase lock and 7 | # signal quality information for GNSS satellites once signals have been 8 | # synchronized. This message supports all active GNSS. 9 | # 10 | 11 | uint8 CLASS_ID = 2 12 | uint8 MESSAGE_ID = 21 13 | 14 | float64 rcv_tow # Measurement time of week in receiver local time [s] 15 | # approximately aligned to the GPS time system. The 16 | # receiver local time of week number and leap second 17 | # information can be used to translate the time to other 18 | # time systems. More information about the difference in 19 | # time systems can be found in RINEX 3 documentation. 20 | # For a receiver operating in GLONASS only mode, UTC 21 | # time can be determined by subtracting the leapS field 22 | # from GPS time regardless of whether the GPS leap 23 | # seconds are valid. 24 | uint16 week # GPS week number in receiver local time. [weeks] 25 | int8 leap_s # GPS leap seconds (GPS-UTC). [s] 26 | # This field represents the receiver's best knowledge of 27 | # the leap seconds offset. A flag is given in the 28 | # recStat bitfield to indicate if the leap seconds 29 | # are known. 30 | uint8 num_meas # # of measurements to follow 31 | uint8 rec_stat # Receiver tracking status bitfield 32 | uint8 REC_STAT_LEAP_SEC = 1 # Leap seconds have been determined 33 | uint8 REC_STAT_CLK_RESET = 2 # Clock reset applied. Typically the receiver 34 | # clock is changed in increments of integer 35 | # milliseconds. 36 | uint8 version # Message version (0x01 for this version). 37 | uint8[2] reserved1 # Reserved 38 | 39 | RxmRAWXMeas[] meas 40 | -------------------------------------------------------------------------------- /ublox_gps/config/c94_m8p_rover.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for C94-M8P device 2 | ublox_gps_node: 3 | ros__parameters: 4 | debug: 1 # Range 0-4 (0 means no debug statements will print) 5 | 6 | save: 7 | mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver 8 | # Manager, Antenna, and Logging Configuration 9 | device: 4 # Save to EEPROM 10 | 11 | device: /dev/ttyACM0 12 | frame_id: gps 13 | rate: 1.0 # in Hz 14 | nav_rate: 1 # [# of measurement cycles], recommended 1 Hz, may 15 | # be either 5 Hz (Dual constellation) or 16 | # 8 Hz (GPS only) 17 | dynamic_model: portable # Airborne < 2G, 2D fix not supported (3D only), 18 | # Max Alt: 50km 19 | # Max Horizontal Velocity: 250 m/s, 20 | # Max Vertical Velocity: 100 m/s 21 | fix_mode: auto 22 | enable_ppp: false # Not supported by C94-M8P 23 | dr_limit: 0 24 | 25 | # TMODE3 Config 26 | tmode3: 0 # Survey-In Mode 27 | sv_in: 28 | reset: false # True: disables and re-enables survey-in (resets) 29 | # False: Disables survey-in only if TMODE3 is 30 | # disabled 31 | min_dur: 300 # Survey-In Minimum Duration [s] 32 | acc_lim: 3.0 # Survey-In Accuracy Limit [m] 33 | 34 | uart1: 35 | baudrate: 19200 # C94-M8P specific 36 | in: 32 # RTCM 3 37 | out: 0 # No UART out for rover 38 | 39 | gnss: 40 | glonass: true # Supported by C94-M8P 41 | beidou: false # Supported by C94-M8P 42 | qzss: true # Supported by C94-M8P 43 | 44 | dgnss_mode: 3 # Fixed mode 45 | 46 | inf: 47 | all: true # Whether to display all INF messages in console 48 | 49 | # Enable u-blox message publishers 50 | publish: 51 | all: true 52 | aid: 53 | hui: false 54 | 55 | nav: 56 | posecef: false 57 | -------------------------------------------------------------------------------- /ublox_msgs/msg/RxmRAWXMeas.msg: -------------------------------------------------------------------------------- 1 | # see message RxmRAWX 2 | # 3 | 4 | float64 pr_mes # Pseudorange measurement [m]. GLONASS inter frequency 5 | # channel delays are compensated with an internal 6 | # calibration table. 7 | float64 cp_mes # Carrier phase measurement [L1 cycles]. The carrier 8 | # phase initial ambiguity is initialized using an 9 | # approximate value to make the magnitude of 10 | # the phase close to the pseudorange 11 | # measurement. Clock resets are applied to both 12 | # phase and code measurements in accordance 13 | # with the RINEX specification. 14 | float32 do_mes # Doppler measurement [Hz] (positive sign for 15 | # approaching satellites) 16 | uint8 gnss_id # GNSS identifier (see CfgGNSS for constants) 17 | 18 | uint8 sv_id # Satellite identifier (see Satellite Numbering) 19 | 20 | uint8 reserved0 # Reserved 21 | 22 | uint8 freq_id # Only used for GLONASS: This is the frequency 23 | # slot + 7 (range from 0 to 13) 24 | uint16 locktime # Carrier phase locktime counter [ms] 25 | # (maximum 64500 ms) 26 | int8 cno # Carrier-to-noise density ratio (signal strength) 27 | # [dB-Hz] 28 | uint8 pr_stdev # Estimated pseudorange measurement standard 29 | # deviation [m / 0.01*2^n] 30 | uint8 cp_stdev # Estimated carrier phase measurement standard 31 | # deviation (note a raw value of 0x0F indicates the 32 | # value is invalid) [cycles / 0.004] 33 | uint8 do_stdev # Estimated Doppler measurement standard deviation 34 | # [Hz / 0.002*2^n] 35 | 36 | uint8 trk_stat # Tracking status bitfield 37 | uint8 TRK_STAT_PR_VALID = 1 # Pseudorange valid 38 | uint8 TRK_STAT_CP_VALID = 2 # Carrier phase valid 39 | uint8 TRK_STAT_HALF_CYC = 4 # Half cycle valid 40 | uint8 TRK_STAT_SUB_HALF_CYC = 8 # Half cycle subtracted from phase 41 | 42 | uint8 reserved1 # Reserved 43 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/tim_product.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_TIM_PRODUCT_HPP 2 | #define UBLOX_GPS_TIM_PRODUCT_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace ublox_node { 19 | 20 | /** 21 | * @brief Implements functions for Time Sync products. 22 | * @todo partially implemented 23 | */ 24 | class TimProduct final : public virtual ComponentInterface { 25 | public: 26 | explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); 27 | 28 | /** 29 | * @brief Get the Time Sync parameters. 30 | * @todo Currently unimplemented. 31 | */ 32 | void getRosParams() override; 33 | 34 | /** 35 | * @brief Configure Time Sync settings. 36 | * @todo Currently unimplemented. 37 | */ 38 | bool configureUblox(std::shared_ptr gps) override; 39 | 40 | /** 41 | * @brief Adds diagnostic updaters for Time Sync status. 42 | * @todo Currently unimplemented. 43 | */ 44 | void initializeRosDiagnostics() override; 45 | 46 | /** 47 | * @brief Subscribe to Time Sync messages. 48 | * 49 | * @details Subscribes to RxmRAWX & RxmSFRBX messages. 50 | */ 51 | void subscribe(std::shared_ptr gps) override; 52 | 53 | private: 54 | /** 55 | * @brief 56 | * @details Publish recieved TimTM2 messages if enabled 57 | */ 58 | void callbackTimTM2(const ublox_msgs::msg::TimTM2 &m); 59 | 60 | sensor_msgs::msg::TimeReference t_ref_; 61 | 62 | rclcpp::Publisher::SharedPtr timtm2_pub_; 63 | rclcpp::Publisher::SharedPtr interrupt_time_pub_; 64 | rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; 65 | rclcpp::Publisher::SharedPtr rxm_raw_pub_; 66 | 67 | std::string frame_id_; 68 | std::shared_ptr updater_; 69 | 70 | rclcpp::Node* node_; 71 | }; 72 | 73 | } // namespace ublox_node 74 | 75 | #endif // UBLOX_GPS_TIM_PRODUCT_HPP 76 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/ublox_firmware7.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_UBLOX_FIRMWARE7_HPP 2 | #define UBLOX_GPS_UBLOX_FIRMWARE7_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | namespace ublox_node { 20 | 21 | /** 22 | * @brief Implements functions for firmware version 7. 23 | */ 24 | class UbloxFirmware7 final : public UbloxFirmware7Plus { 25 | public: 26 | explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) 27 | : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { 28 | if (getRosBoolean(node_, "publish.nav.svinfo")) { 29 | nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); 30 | } 31 | if (getRosBoolean(node_, "publish.mon.hw")) { 32 | mon_hw_pub_ = node->create_publisher("monhw", 1); 33 | } 34 | } 35 | 36 | /** 37 | * @brief Get the parameters specific to firmware version 7. 38 | * 39 | * @details Get the GNSS and NMEA settings. 40 | */ 41 | void getRosParams() override; 42 | 43 | /** 44 | * @brief Configure GNSS individually. Only configures GLONASS. 45 | */ 46 | bool configureUblox(std::shared_ptr gps) override; 47 | 48 | /** 49 | * @brief Subscribe to messages which are not generic to all firmware. 50 | * 51 | * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. 52 | */ 53 | void subscribe(std::shared_ptr gps) override; 54 | 55 | private: 56 | //! Used to configure NMEA (if set_nmea_) 57 | /*! 58 | * Filled from ROS parameters 59 | */ 60 | ublox_msgs::msg::CfgNMEA7 cfg_nmea_; 61 | 62 | rclcpp::Publisher::SharedPtr nav_svinfo_pub_; 63 | rclcpp::Publisher::SharedPtr mon_hw_pub_; 64 | }; 65 | 66 | } // namespace ublox_node 67 | 68 | #endif // UBLOX_GPS_UBLOX_FIRMWARE7_HPP 69 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgGNSS.msg: -------------------------------------------------------------------------------- 1 | # CFG-GNSS (0x06 0x3E) 2 | # GNSS Configuration 3 | # 4 | # Gets or sets the GNSS system channel sharing configuration. 5 | # If the receiver is sent a valid new configuration, it will respond with a 6 | # UBX-ACK-ACK message and immediately change to the new configuration. Otherwise 7 | # the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing 8 | # operation with the previous configuration. 9 | # Configuration requirements: 10 | # - It is necessary for at least one major GNSS to be enabled, after applying 11 | # the new configuration to the current one. 12 | # - It is also required that at least 4 tracking channels are available to each 13 | # enabled major GNSS, i.e. maxTrkCh must have a minimum value of 4 for each 14 | # enabled major GNSS. 15 | # - The number of tracking channels in use must not exceed the number of 16 | # tracking channels available in hardware, and the sum of all reserved 17 | # tracking channels needs to be less than or equal to the number of tracking 18 | # channels in use. 19 | # Notes: 20 | # - To avoid cross-correlation issues, it is recommended that GPS and QZSS are 21 | # always both enabled or both disabled. 22 | # - Polling this message returns the configuration of all supported GNSS, 23 | # whether enabled or not; it may also include GNSS unsupported by the 24 | # particular product, but in such cases the enable flag will always be unset. 25 | # - See section GNSS Configuration for a discussion of the use of this message 26 | # and section Satellite Numbering for a description of the GNSS IDs available 27 | # - Configuration specific to the GNSS system can be done via other messages 28 | # (e.g. UBX-CFG-SBAS). 29 | # 30 | 31 | uint8 CLASS_ID = 6 32 | uint8 MESSAGE_ID = 62 33 | 34 | uint8 msg_ver # Message version (= 0 for this version) 35 | uint8 num_trk_ch_hw # Number of tracking channels in hardware (read only) 36 | uint8 num_trk_ch_use # (Read only in protocol versions greater than 23) 37 | # Number of tracking channels to use (<= numTrkChHw) 38 | # If 0xFF, then number of tracking channels to use will 39 | # be set to numTrkChHw 40 | uint8 num_config_blocks # Number of configuration blocks following 41 | 42 | # Start of repeated block (numConfigBlocks times) 43 | CfgGNSSBlock[] blocks 44 | # End of repeated block 45 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/mkgmtime.h: -------------------------------------------------------------------------------- 1 | /* mkgmtime.h -- make a time_t from a gmtime struct tm 2 | $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $ 3 | 4 | * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 18 | * 3. The name "Carnegie Mellon University" must not be used to 19 | * endorse or promote products derived from this software without 20 | * prior written permission. For permission or any other legal 21 | * details, please contact 22 | * Office of Technology Transfer 23 | * Carnegie Mellon University 24 | * 5000 Forbes Avenue 25 | * Pittsburgh, PA 15213-3890 26 | * (412) 268-4387, fax: (412) 268-7395 27 | * tech-transfer@andrew.cmu.edu 28 | * 29 | * 4. Redistributions of any form whatsoever must retain the following 30 | * acknowledgment: 31 | * "This product includes software developed by Computing Services 32 | * at Carnegie Mellon University (http://www.cmu.edu/computing/)." 33 | * 34 | * CARNEGIE MELLON UNIVERSITY DISCLAIMS ALL WARRANTIES WITH REGARD TO 35 | * THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY 36 | * AND FITNESS, IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE 37 | * FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 38 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 39 | * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING 40 | * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 41 | * 42 | */ 43 | 44 | #ifndef UBLOX_GPS_MKGMTIME_H 45 | #define UBLOX_GPS_MKGMTIME_H 46 | 47 | #include 48 | 49 | #ifdef __cplusplus 50 | extern "C" { 51 | #endif 52 | 53 | /** 54 | * @brief Get the UTC time in seconds and nano-seconds from a time struct in 55 | * GM time. 56 | */ 57 | time_t mkgmtime(struct tm * const tmp); 58 | 59 | #ifdef __cplusplus 60 | } 61 | #endif 62 | 63 | #endif // UBLOX_GPS_MKGMTIME_H 64 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/raw_data_product.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_RAW_DATA_PRODUCT_HPP 2 | #define UBLOX_GPS_RAW_DATA_PRODUCT_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | namespace ublox_node { 20 | 21 | /** 22 | * @brief Implements functions for Raw Data products. 23 | */ 24 | class RawDataProduct final : public virtual ComponentInterface { 25 | public: 26 | const double kRtcmFreqTol = 0.15; 27 | const int kRtcmFreqWindow = 25; 28 | 29 | explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node); 30 | 31 | /** 32 | * @brief Does nothing since there are no Raw Data product specific settings. 33 | */ 34 | void getRosParams() override {} 35 | 36 | /** 37 | * @brief Does nothing since there are no Raw Data product specific settings. 38 | * @return always returns true 39 | */ 40 | bool configureUblox(std::shared_ptr gps) override { 41 | (void)gps; 42 | return true; 43 | } 44 | 45 | /** 46 | * @brief Adds frequency diagnostics for RTCM topics. 47 | */ 48 | void initializeRosDiagnostics() override; 49 | 50 | /** 51 | * @brief Subscribe to Raw Data Product messages and set up ROS publishers. 52 | * 53 | * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. 54 | */ 55 | void subscribe(std::shared_ptr gps) override; 56 | 57 | private: 58 | //! Topic diagnostic updaters 59 | std::vector > freq_diagnostics_; 60 | 61 | rclcpp::Publisher::SharedPtr rxm_raw_pub_; 62 | rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; 63 | rclcpp::Publisher::SharedPtr rxm_eph_pub_; 64 | rclcpp::Publisher::SharedPtr rxm_alm_pub_; 65 | 66 | uint16_t nav_rate_; 67 | uint16_t meas_rate_; 68 | std::shared_ptr updater_; 69 | rclcpp::Node* node_; 70 | }; 71 | 72 | } // namespace ublox_node 73 | 74 | #endif // UBLOX_GPS_RAW_DATA_PRODUCT_HPP 75 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSVIN.msg: -------------------------------------------------------------------------------- 1 | # NAV-SVIN (0x01 0x3B) 2 | # Survey-in data 3 | # 4 | # This message contains information about survey-in parameters. 5 | # Supported on: 6 | # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision 7 | # GNSS products) 8 | 9 | uint8 CLASS_ID = 1 10 | uint8 MESSAGE_ID = 59 11 | 12 | uint8 version # Message version (0x00 for this version) 13 | uint8[3] reserved0 # Reserved 14 | 15 | uint32 i_tow # GPS time of week of the navigation epoch [ms] 16 | 17 | uint32 dur # Passed survey-in observation time [s] 18 | 19 | int32 mean_x # Current survey-in mean position ECEF X coordinate [cm] 20 | int32 mean_y # Current survey-in mean position ECEF Y coordinate [cm] 21 | int32 mean_z # Current survey-in mean position ECEF Z coordinate [cm] 22 | 23 | int8 mean_xhp # Current high-precision survey-in mean position 24 | # ECEF X coordinate. 0.1_mm 25 | # Must be in the range -99..+99. 26 | # The current survey-in mean position ECEF X 27 | # coordinate, in units of cm, is given by 28 | # meanX + (0.01 * meanXHP) 29 | int8 mean_yhp # Current high-precision survey-in mean position 30 | # ECEF Y coordinate. [0.1 mm] 31 | # Must be in the range -99..+99. 32 | # The current survey-in mean position ECEF Y 33 | # coordinate, in units of cm, is given by 34 | # meanY + (0.01 * meanYHP) 35 | int8 mean_zhp # Current high-precision survey-in mean position 36 | # ECEF Z coordinate. [0.1 mm] 37 | # Must be in the range -99..+99. 38 | # The current survey-in mean position ECEF Z 39 | # coordinate, in units of cm, is given by 40 | # meanZ + (0.01 * meanZHP) 41 | 42 | uint8 reserved1 # Reserved 43 | 44 | uint32 mean_acc # Current survey-in mean position accuracy [0.1 mm] 45 | uint32 obs # Number of position observations used during survey-in 46 | uint8 valid # Survey-in position validity flag, 1 = valid 47 | # otherwise 0 48 | uint8 active # Survey-in in progress flag, 1 = in-progress 49 | # otherwise 0 50 | 51 | uint8[2] reserved3 # Reserved 52 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSVINFOSV.msg: -------------------------------------------------------------------------------- 1 | # see message NavSVINFO 2 | # 3 | 4 | uint8 chn # Channel number, 255 for SVs not assigned to a channel 5 | uint8 svid # Satellite ID 6 | 7 | uint8 flags # Bitmask 8 | uint8 FLAGS_SV_USED = 1 # SV is used for navigation 9 | uint8 FLAGS_DIFF_CORR = 2 # Differential correction data 10 | # is available for this SV 11 | uint8 FLAGS_ORBIT_AVAIL = 4 # Orbit information is available for 12 | # this SV (Ephemeris or Almanach) 13 | uint8 FLAGS_ORBIT_EPH = 8 # Orbit information is Ephemeris 14 | uint8 FLAGS_UNHEALTHY = 16 # SV is unhealthy / shall not be 15 | # used 16 | uint8 FLAGS_ORBIT_ALM = 32 # Orbit information is Almanac Plus 17 | uint8 FLAGS_ORBIT_AOP = 64 # Orbit information is AssistNow 18 | # Autonomous 19 | uint8 FLAGS_SMOOTHED = 128 # Carrier smoothed pseudorange used 20 | 21 | uint8 quality # Bitfield 22 | # qualityInd: Signal Quality indicator (range 0..7). The following list shows 23 | # the meaning of the different QI values: 24 | # Note: Since IMES signals are not time synchronized, a channel tracking an IMES 25 | # signal can never reach a quality indicator value of higher than 3. 26 | uint8 QUALITY_IDLE = 0 # This channel is idle 27 | uint8 QUALITY_SEARCHING = 1 # Channel is searching 28 | uint8 QUALITY_ACQUIRED = 2 # Signal acquired 29 | uint8 QUALITY_DETECTED = 3 # Signal detected but unusable 30 | uint8 QUALITY_CODE_LOCK = 4 # Code Lock on Signal 31 | uint8 QUALITY_CODE_AND_CARRIER_LOCKED1 = 5 # Code and Carrier locked 32 | # and time synchronized 33 | uint8 QUALITY_CODE_AND_CARRIER_LOCKED2 = 6 # Code and Carrier locked 34 | # and time synchronized 35 | uint8 QUALITY_CODE_AND_CARRIER_LOCKED3 = 7 # Code and Carrier locked 36 | # and time synchronized 37 | 38 | uint8 cno # Carrier to Noise Ratio (Signal Strength) [dBHz] 39 | int8 elev # Elevation in integer degrees [deg] 40 | int16 azim # Azimuth in integer degrees [deg] 41 | int32 pr_res # Pseudo range residual in centimetres [cm] 42 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/fix_diagnostic.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_FIX_DIAGNOSTIC_HPP 2 | #define UBLOX_GPS_FIX_DIAGNOSTIC_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace ublox_node { 11 | 12 | //! Topic diagnostics for fix / fix_velocity messages 13 | class FixDiagnostic { 14 | public: 15 | /** 16 | * @brief Add a topic diagnostic to the diagnostic updater for fix topics. 17 | * 18 | * @details The minimum and maximum frequency are equal to the nav rate in Hz. 19 | * @param name the ROS topic 20 | * @param freq_tol the tolerance [%] for the topic frequency 21 | * @param freq_window the number of messages to use for diagnostic statistics 22 | * @param stamp_min the minimum allowed time delay 23 | */ 24 | explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, 25 | double stamp_min, uint16_t nav_rate, uint16_t meas_rate, 26 | std::shared_ptr updater) { 27 | const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz 28 | min_freq = target_freq; 29 | max_freq = target_freq; 30 | diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, 31 | freq_tol, freq_window); 32 | double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); 33 | diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); 34 | diagnostic = std::make_shared(name, 35 | *updater, 36 | freq_param, 37 | time_param); 38 | } 39 | 40 | // Must not copy this struct (would confuse FrequencyStatusParam pointers) 41 | FixDiagnostic(FixDiagnostic &&c) = delete; 42 | FixDiagnostic &operator=(FixDiagnostic &&c) = delete; 43 | FixDiagnostic(const FixDiagnostic &c) = delete; 44 | FixDiagnostic &operator=(const FixDiagnostic &c) = delete; 45 | 46 | ~FixDiagnostic() = default; 47 | 48 | //! Topic frequency diagnostic updater 49 | std::shared_ptr diagnostic; 50 | 51 | private: 52 | //! Minimum allow frequency of topic 53 | double min_freq; 54 | //! Maximum allow frequency of topic 55 | double max_freq; 56 | }; 57 | 58 | } // namespace ublox_node 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /ublox_gps/src/logger_node_pa.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================== 2 | // Copyright (c) 2019, Peter Weissig, TU Chemnitz 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND 19 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 22 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | //============================================================================== 29 | 30 | // This file is an addon from TUC-ProAut (https://github.com/TUC-ProAut/). 31 | // It was created to log all data from the ublox to a logfile and to publish 32 | // the data as ros messages. This is used by our group to also evaluate the 33 | // measured data with the rtklib. 34 | 35 | // ROS includes 36 | #include 37 | // Ublox GPS includes 38 | #include 39 | 40 | // 41 | // Raw Data Stream (feature from TUC-ProAut) 42 | // 43 | 44 | int main(int argc, char** argv) { 45 | 46 | rclcpp::init(argc, argv); 47 | 48 | auto node = std::make_shared(true); 49 | node->getRosParams(); 50 | node->initialize(); 51 | 52 | rclcpp::spin(node); 53 | 54 | rclcpp::shutdown(); 55 | 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /ntrip_client/src/ntrip_client/nmea_parser.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | NMEA_DEFAULT_MAX_LENGTH = 150 4 | #82 5 | NMEA_DEFAULT_MIN_LENGTH = 3 6 | _NMEA_CHECKSUM_SEPERATOR = "*" 7 | 8 | class NMEAParser: 9 | 10 | def __init__(self, logerr=logging.error, logwarn=logging.warning, loginfo=logging.info, logdebug=logging.debug): 11 | # Bit of a strange pattern here, but save the log functions so we can be agnostic of ROS 12 | self._logerr = logerr 13 | self._logwarn = logwarn 14 | self._loginfo = loginfo 15 | self._logdebug = logdebug 16 | 17 | # Save some other config 18 | self.nmea_max_length = NMEA_DEFAULT_MAX_LENGTH 19 | self.nmea_min_length = NMEA_DEFAULT_MIN_LENGTH 20 | 21 | def is_valid_sentence(self, sentence): 22 | # Simple sanity checks 23 | if len(sentence) > self.nmea_max_length: 24 | self._logwarn('Received invalid NMEA sentence. Max length is {}, but sentence was {} bytes'.format(self.nmea_max_length, len(sentence))) 25 | self._logwarn('Sentence: {}'.format(sentence)) 26 | return False 27 | if len(sentence) < self.nmea_min_length: 28 | self._logwarn('Received invalid NMEA sentence. We need at least {} bytes to parse but got {} bytes'.format(self.nmea_min_length, len(sentence))) 29 | self._logwarn('Sentence: {}'.format(sentence)) 30 | return False 31 | if sentence[0] != '$' and sentence[0] != '!': 32 | self._logwarn('Received invalid NMEA sentence. Sentence should begin with "$" or "!", but instead begins with {}'.format(sentence[0])) 33 | self._logwarn('Sentence: {}'.format(sentence)) 34 | return False 35 | if sentence[-2:] != '\r\n': 36 | self._logwarn('Received invalid NMEA sentence. Sentence should end with \\r\\n, but instead ends with {}'.format(sentence[-2:])) 37 | self._logwarn('Sentence: {}'.format(sentence)) 38 | return False 39 | if _NMEA_CHECKSUM_SEPERATOR not in sentence: 40 | self._logwarn('Received invalid NMEA sentence. Sentence should have a "{}" character to seperate the checksum, but we could not find it.'.format(_NMEA_CHECKSUM_SEPERATOR)) 41 | self._logwarn('Sentence: {}'.format(sentence)) 42 | return False 43 | 44 | # Checksum check 45 | data, expected_checksum_str = sentence.rsplit(_NMEA_CHECKSUM_SEPERATOR, 1) 46 | expected_checksum = int(expected_checksum_str, 16) 47 | calculated_checksum = 0 48 | for char in data[1:]: 49 | calculated_checksum ^= ord(char) 50 | if expected_checksum != calculated_checksum: 51 | self._logwarn('Received invalid NMEA sentence. Checksum mismatch'); 52 | self._logwarn('Expected Checksum: 0x{:X}'.format(expected_checksum)) 53 | self._logwarn('Calculated Checksum: 0x{:X}'.format(calculated_checksum)) 54 | return False 55 | 56 | # Passed all checks 57 | return True 58 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MonHW.msg: -------------------------------------------------------------------------------- 1 | # MON-HW (0x0A 0x09) 2 | # Hardware Status 3 | # 4 | # Status of different aspect of the hardware, such as Antenna, PIO/Peripheral 5 | # Pins, Noise Level, Automatic Gain Control (AGC) 6 | # 7 | # WARNING: this message is a different length than the MonHW message for 8 | # firmware version 6 9 | 10 | uint8 CLASS_ID = 10 11 | uint8 MESSAGE_ID = 9 12 | 13 | uint32 pin_sel # Mask of Pins Set as Peripheral/PIO 14 | uint32 pin_bank # Mask of Pins Set as Bank A/B 15 | uint32 pin_dir # Mask of Pins Set as Input/Output 16 | uint32 pin_val # Mask of Pins Value Low/High 17 | uint16 noise_per_ms # Noise Level as measured by the GPS Core 18 | uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, 19 | # range 0 to 8191) 20 | uint8 a_status # Status of the Antenna Supervisor State Machine 21 | uint8 A_STATUS_INIT = 0 22 | uint8 A_STATUS_UNKNOWN = 1 23 | uint8 A_STATUS_OK = 2 24 | uint8 A_STATUS_SHORT = 3 25 | uint8 A_STATUS_OPEN = 4 26 | 27 | uint8 a_power # Current PowerStatus of Antenna 28 | uint8 A_POWER_OFF = 0 29 | uint8 A_POWER_ON = 1 30 | uint8 A_POWER_UNKNOWN = 2 31 | 32 | uint8 flags # Flags: 33 | uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated 34 | uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) 35 | uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: 36 | uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled 37 | uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming 38 | uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK 39 | uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix 40 | uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent 41 | # (not supported in protocol versions < 18) 42 | uint8 reserved0 # Reserved 43 | uint32 used_mask # Mask of Pins that are used by the Virtual Pin 44 | # Manager 45 | uint8[17] vp # Array of Pin Mappings for each of the 17 46 | # Physical Pins 47 | uint8 jam_ind # CW Jamming indicator, scaled: 48 | uint8 JAM_IND_NONE = 0 # No CW Jamming 49 | uint8 JAM_IND_STRONG = 255 # Strong CW Jamming 50 | 51 | uint8[2] reserved1 # Reserved 52 | 53 | uint32 pin_irq # Mask of Pins Value using the PIO Irq 54 | uint32 pull_h # Mask of Pins Value using the PIO Pull High 55 | # Resistor 56 | uint32 pull_l # Mask of Pins Value using the PIO Pull Low 57 | # Resistor 58 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MonHW6.msg: -------------------------------------------------------------------------------- 1 | # MON-HW (0x0A 0x09) 2 | # Hardware Status 3 | # Firmware 6 4 | # 5 | # Status of different aspect of the hardware, such as Antenna, PIO/Peripheral 6 | # Pins, Noise Level, Automatic Gain Control (AGC) 7 | # 8 | # WARNING: this message is a different length than the MonHW message for 9 | # firmware version 7 & 8 10 | 11 | uint8 CLASS_ID = 10 12 | uint8 MESSAGE_ID = 9 13 | 14 | uint32 pin_sel # Mask of Pins Set as Peripheral/PIO 15 | uint32 pin_bank # Mask of Pins Set as Bank A/B 16 | uint32 pin_dir # Mask of Pins Set as Input/Output 17 | uint32 pin_val # Mask of Pins Value Low/High 18 | uint16 noise_per_ms # Noise Level as measured by the GPS Core 19 | uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, 20 | # range 0 to 8191) 21 | uint8 a_status # Status of the Antenna Supervisor State Machine 22 | uint8 A_STATUS_INIT = 0 23 | uint8 A_STATUS_UNKNOWN = 1 24 | uint8 A_STATUS_OK = 2 25 | uint8 A_STATUS_SHORT = 3 26 | uint8 A_STATUS_OPEN = 4 27 | 28 | uint8 a_power # Current PowerStatus of Antenna 29 | uint8 A_POWER_OFF = 0 30 | uint8 A_POWER_ON = 1 31 | uint8 A_POWER_UNKNOWN = 2 32 | 33 | uint8 flags # Flags: 34 | uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated 35 | uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) 36 | uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: 37 | uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled 38 | uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming 39 | uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK 40 | uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix 41 | uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent 42 | # (not supported in protocol versions < 18) 43 | uint8 reserved0 # Reserved 44 | uint32 used_mask # Mask of Pins that are used by the Virtual Pin 45 | # Manager 46 | uint8[25] vp # Array of Pin Mappings for each of the 25 47 | # Physical Pins 48 | uint8 jam_ind # CW Jamming indicator, scaled: 49 | uint8 JAM_IND_NONE = 0 # No CW Jamming 50 | uint8 JAM_IND_STRONG = 255 # Strong CW Jamming 51 | 52 | uint8[2] reserved1 # Reserved 53 | 54 | uint32 pin_irq # Mask of Pins Value using the PIO Irq 55 | uint32 pull_h # Mask of Pins Value using the PIO Pull High 56 | # Resistor 57 | uint32 pull_l # Mask of Pins Value using the PIO Pull Low 58 | # Resistor 59 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgGNSSBlock.msg: -------------------------------------------------------------------------------- 1 | # see Cfg-GNSS message 2 | # 3 | 4 | uint8 gnss_id # System identifier (see Satellite Numbering) 5 | 6 | uint8 GNSS_ID_GPS = 0 7 | uint8 GNSS_ID_SBAS = 1 8 | uint8 GNSS_ID_GALILEO = 2 9 | uint8 GNSS_ID_BEIDOU = 3 10 | uint8 GNSS_ID_IMES = 4 11 | uint8 GNSS_ID_QZSS = 5 12 | uint8 GNSS_ID_GLONASS = 6 13 | 14 | uint8 res_trk_ch # (Read only in protocol versions greater than 23) 15 | # Number of reserved (minimum) tracking channels 16 | # for this GNSS system 17 | uint8 RES_TRK_CH_GPS = 8 18 | uint8 RES_TRK_CH_QZSS = 0 19 | uint8 RES_TRK_CH_SBAS = 0 20 | uint8 RES_TRK_CH_GLONASS = 8 21 | uint8 max_trk_ch # (Read only in protocol versions greater than 23) 22 | # Maximum number of tracking channels used for this 23 | # system. Must be > 0, >= resTrkChn, <= numTrkChUse and 24 | # <= maximum number of tracking channels supported for 25 | # this system 26 | uint8 MAX_TRK_CH_MAJOR_MIN = 4 # max_trk_ch must have this minimum value 27 | # for each enabled major GNSS 28 | uint8 MAX_TRK_CH_GPS = 16 29 | uint8 MAX_TRK_CH_GLONASS = 14 30 | uint8 MAX_TRK_CH_QZSS = 3 31 | uint8 MAX_TRK_CH_SBAS = 3 32 | 33 | uint8 reserved1 # Reserved 34 | 35 | uint32 flags # Bitfield of flags. At least one signal must be 36 | # configured in every enabled system. 37 | uint32 FLAGS_ENABLE = 1 # Enable this system 38 | uint32 FLAGS_SIG_CFG_MASK = 16711680 # Signal configuration mask 39 | uint32 SIG_CFG_GPS_L1CA = 65536 # When gnssId is 0 (GPS) 40 | # * 0x01 = GPS L1C/A 41 | uint32 SIG_CFG_SBAS_L1CA = 65536 # When gnssId is 1 (SBAS) 42 | # * 0x01 = SBAS L1C/A 43 | uint32 SIG_CFG_GALILEO_E1OS = 65536 # When gnssId is 2 (Galileo) 44 | # * 0x01 = Galileo E1OS (not supported in 45 | # protocol versions less than 18) 46 | uint32 SIG_CFG_BEIDOU_B1I = 65536 # When gnssId is 3 (BeiDou) 47 | # * 0x01 = BeiDou B1I 48 | uint32 SIG_CFG_IMES_L1 = 65536 # When gnssId is 4 (IMES) 49 | # * 0x01 = IMES L1 50 | uint32 SIG_CFG_QZSS_L1CA = 65536 # When gnssId is 5 (QZSS) 51 | # * 0x01 = QZSS L1C/A 52 | uint32 SIG_CFG_QZSS_L1SAIF = 262144 # * 0x04 = QZSS L1SAIF 53 | uint32 SIG_CFG_GLONASS_L1OF = 65536 # When gnssId is 6 (GLONASS) 54 | # * 0x01 = GLONASS L1OF 55 | -------------------------------------------------------------------------------- /ublox_serialization/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ublox_serialization 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.0 (2022-04-13) 6 | ------------------ 7 | * Revamp the building of the driver for modern ROS 2 practices. 8 | * Contributors: Chao Qu, Chris Lalancette 9 | 10 | 2.0.0 (2020-10-13) 11 | ------------------ 12 | * Initial ROS 2 port to Dashing 13 | * Fixes pointed out by clang-tidy. 14 | * Make sure to initialize checksum properly. 15 | * Port of ublox_serialization to ROS 2. 16 | * Start ROS 2 port by COLCON_IGNORE everything. 17 | * Remove unused vector serialization. 18 | * Move serialization into the ublox_serialization module. 19 | * Move the directory structure around just a bit. 20 | * Remove boost from serialization. 21 | * Remove trailing whitespace in all files. 22 | * Rename header files to have .hpp extension. 23 | * Contributors: Chao Qu, Chris Lalancette 24 | 25 | 1.2.0 (2019-11-19) 26 | ------------------ 27 | 28 | 1.1.2 (2017-08-02) 29 | ------------------ 30 | * README and package xml updates 31 | * Fixed bug with enabling INF messages. Changed how messages which update fix are enabled and changed name of subscribe param namespace to publish. 32 | * Added doxygen comments and made minor cleanup changes. 33 | * Added doxygen comments 34 | * Contributors: Veronica Lane 35 | 36 | 1.1.0 (2017-07-17) 37 | ------------------ 38 | * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter 39 | * changed name of macro for clarity 40 | * changed receive message error print statements to only print in debug mode 41 | * Added additional MON messages. Received INF messages are now printed to the ROS console. Also added constants and comments to serialization and a new macro so 1 message can have multiple class, message ID pairs. 42 | * Contributors: Veronica Lane 43 | 44 | 1.0.0 (2017-06-23) 45 | ------------------ 46 | * added myself as maintainer to package xmls and updated version numbers of modified packages. 47 | * formatting 48 | * Formatting of copyright so it's <80 char and changed std::cout in Async worker to ROS_INFO messages 49 | * Update CfgGNSS message and serialization which now publishes and receives blocks and reads and configures all GNSS settings at once. Updated MonVER message and serialization, MonVER settings are displayed during initialization, including extension chars. Changed various std::cout messages to ROS_INFO and ROS_ERROR messages. 50 | * Contributors: Veronica Lane 51 | 52 | 0.0.5 (2016-08-06) 53 | ------------------ 54 | 55 | 0.0.4 (2014-12-08) 56 | ------------------ 57 | * Add install targets 58 | * Contributors: Kartik Mohta 59 | 60 | 0.0.3 (2014-10-18) 61 | ------------------ 62 | 63 | 0.0.2 (2014-10-03) 64 | ------------------ 65 | 66 | 0.0.1 (2014-08-15) 67 | ------------------ 68 | 69 | 0.0.0 (2014-06-23) 70 | ------------------ 71 | * ublox: first commit 72 | * Contributors: Chao Qu 73 | -------------------------------------------------------------------------------- /ublox_msgs/msg/HnrPVT.msg: -------------------------------------------------------------------------------- 1 | # HNR-PVT (0x28 0x00) 2 | # High Rate Output of PVT Solution 3 | # 4 | # Note that during a leap second there may be more (or less) than 60 seconds in 5 | # a minute; see the description of leap seconds for details. 6 | # 7 | # This message provides the position, velocity and time solution with high 8 | # output rate. 9 | # 10 | # Supported on ADR and UDR products. 11 | # 12 | uint8 CLASS_ID = 40 13 | uint8 MESSAGE_ID = 0 14 | 15 | uint32 i_tow # GPS Millisecond time of week [ms] 16 | uint16 year # Year (UTC) 17 | uint8 month # Month, range 1..12 (UTC) 18 | uint8 day # Day of month, range 1..31 (UTC) 19 | uint8 hour # Hour of day, range 0..23 (UTC) 20 | uint8 min # Minute of hour, range 0..59 (UTC) 21 | uint8 sec # Seconds of minute, range 0..60 (UTC) 22 | 23 | uint8 valid # Validity flags 24 | uint8 VALID_DATE = 1 # Valid UTC Date 25 | uint8 VALID_TIME = 2 # Valid 26 | uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved 27 | # (no seconds uncertainty) 28 | uint8 VALID_MAG = 8 # Valid Magnetic Declination 29 | 30 | int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) 31 | 32 | uint8 gps_fix # GPS fix Type, range 0..5 33 | uint8 FIX_TYPE_NO_FIX = 0 34 | uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 35 | uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, 36 | # constant altitude assumed 37 | uint8 FIX_TYPE_3D = 3 38 | uint8 FIX_TYPE_GPS_DEAD_RECKONING_COMBINED = 4 # GPS + Dead reckoning 39 | uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix 40 | 41 | uint8 flags # Fix Status Flags 42 | uint8 FLAGS_GNSS_FIX_OK = 1 # i.e. within DOP & accuracy masks 43 | uint8 FLAGS_DIFF_SOLN = 2 # DGPS used 44 | uint8 FLAGS_WKN_SET = 4 # Valid GPS week number 45 | uint8 FLAGS_TOW_SET = 8 # Valid GPS time of week (iTOW & fTOW) 46 | uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid 47 | 48 | uint8[2] reserved0 # Reserved 49 | 50 | int32 lon # Longitude [deg / 1e-7] 51 | int32 lat # Latitude [deg / 1e-7] 52 | int32 height # Height above Ellipsoid [mm] 53 | int32 h_msl # Height above mean sea level [mm] 54 | 55 | int32 g_speed # Ground Speed (2-D) [mm/s] 56 | int32 speed # Speed (3-D) [mm/s] 57 | 58 | int32 head_mot # Heading of motion (2-D) [deg / 1e-5] 59 | int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] 60 | 61 | uint32 h_acc # Horizontal Accuracy Estimate [mm] 62 | uint32 v_acc # Vertical Accuracy Estimate [mm] 63 | uint32 s_acc # Speed Accuracy Estimate [mm/s] 64 | uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) 65 | # [deg / 1e-5] 66 | 67 | uint8[4] reserved1 # Reserved 68 | -------------------------------------------------------------------------------- /ublox_gps/launch/ublox_gps_node-launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Open Source Robotics Foundation, Inc. 2 | # All rights reserved. 3 | # 4 | # Software License Agreement (BSD License 2.0) 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of {copyright_holder} nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | """Launch the ublox gps node with c94-m8p configuration.""" 34 | 35 | import os 36 | 37 | import ament_index_python.packages 38 | import launch 39 | import launch_ros.actions 40 | 41 | 42 | def generate_launch_description(): 43 | config_directory = os.path.join( 44 | ament_index_python.packages.get_package_share_directory('ublox_gps'), 45 | 'config') 46 | params = os.path.join(config_directory, 'zed_f9p.yaml') 47 | ublox_gps_node = launch_ros.actions.Node(package='ublox_gps', 48 | executable='ublox_gps_node', 49 | output='both', 50 | parameters=[params]) 51 | 52 | return launch.LaunchDescription([ublox_gps_node, 53 | 54 | launch.actions.RegisterEventHandler( 55 | event_handler=launch.event_handlers.OnProcessExit( 56 | target_action=ublox_gps_node, 57 | on_exit=[launch.actions.EmitEvent( 58 | event=launch.events.Shutdown())], 59 | )), 60 | ]) 61 | -------------------------------------------------------------------------------- /ublox_gps/launch/ublox_gps_node-composed-launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Open Source Robotics Foundation, Inc. 2 | # All rights reserved. 3 | # 4 | # Software License Agreement (BSD License 2.0) 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of {copyright_holder} nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | """Launch the ublox gps node in a composable container with c94-m8p configuration.""" 34 | 35 | import os 36 | 37 | import ament_index_python.packages 38 | 39 | from launch import LaunchDescription 40 | from launch_ros.actions import ComposableNodeContainer 41 | from launch_ros.descriptions import ComposableNode 42 | 43 | import yaml 44 | 45 | 46 | def generate_launch_description(): 47 | config_directory = os.path.join( 48 | ament_index_python.packages.get_package_share_directory('ublox_gps'), 49 | 'config') 50 | param_config = os.path.join(config_directory, 'c94_m8p_rover.yaml') 51 | with open(param_config, 'r') as f: 52 | params = yaml.safe_load(f)['ublox_gps_node']['ros__parameters'] 53 | container = ComposableNodeContainer( 54 | name='ublox_gps_container', 55 | namespace='', 56 | package='rclcpp_components', 57 | executable='component_container', 58 | composable_node_descriptions=[ 59 | ComposableNode( 60 | package='ublox_gps', 61 | plugin='ublox_node::UbloxNode', 62 | name='ublox_gps_node', 63 | parameters=[params]), 64 | ], 65 | output='both', 66 | ) 67 | 68 | return LaunchDescription([container]) 69 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/hpg_rov_product.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_HPG_ROV_PRODUCT_HPP 2 | #define UBLOX_GPS_HPG_ROV_PRODUCT_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace ublox_node { 16 | 17 | /** 18 | * @brief Implements functions for High Precision GNSS Rover devices. 19 | */ 20 | class HpgRovProduct final : public virtual ComponentInterface { 21 | public: 22 | // Constants for diagnostic updater 23 | //! Diagnostic updater: RTCM topic frequency min [Hz] 24 | const double kRtcmFreqMin = 1; 25 | //! Diagnostic updater: RTCM topic frequency max [Hz] 26 | const double kRtcmFreqMax = 10; 27 | //! Diagnostic updater: RTCM topic frequency tolerance [%] 28 | const double kRtcmFreqTol = 0.1; 29 | //! Diagnostic updater: RTCM topic frequency window [num messages] 30 | const int kRtcmFreqWindow = 25; 31 | 32 | explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node); 33 | 34 | /** 35 | * @brief Get the ROS parameters specific to the Rover configuration. 36 | * 37 | * @details Get the DGNSS mode. 38 | */ 39 | void getRosParams() override; 40 | 41 | /** 42 | * @brief Configure rover settings. 43 | * 44 | * @details Configure the DGNSS mode. 45 | * @return true if configured correctly, false otherwise 46 | */ 47 | bool configureUblox(std::shared_ptr gps) override; 48 | 49 | /** 50 | * @brief Add diagnostic updaters for rover GNSS status, including 51 | * status of RTCM messages. 52 | */ 53 | void initializeRosDiagnostics() override; 54 | 55 | /** 56 | * @brief Subscribe to Rover messages, such as NavRELPOSNED. 57 | */ 58 | void subscribe(std::shared_ptr gps) override; 59 | 60 | private: 61 | /** 62 | * @brief Update the rover diagnostics, including the carrier phase solution 63 | * status (float or fixed). 64 | */ 65 | void carrierPhaseDiagnostics( 66 | diagnostic_updater::DiagnosticStatusWrapper& stat); 67 | 68 | /** 69 | * @brief Set the last received message and call rover diagnostic updater 70 | * 71 | * @details Publish received NavRELPOSNED messages if enabled 72 | */ 73 | void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m); 74 | 75 | 76 | //! Last relative position (used for diagnostic updater) 77 | ublox_msgs::msg::NavRELPOSNED last_rel_pos_; 78 | 79 | //! The DGNSS mode 80 | /*! see CfgDGNSS message for possible values */ 81 | uint8_t dgnss_mode_; 82 | 83 | //! The RTCM topic frequency diagnostic updater 84 | std::unique_ptr freq_rtcm_; 85 | 86 | rclcpp::Publisher::SharedPtr nav_rel_pos_ned_pub_; 87 | 88 | uint16_t nav_rate_; 89 | std::shared_ptr updater_; 90 | rclcpp::Node* node_; 91 | }; 92 | 93 | } // namespace ublox_node 94 | 95 | #endif // UBLOX_GPS_HPG_ROV_PRODUCT_HPP 96 | -------------------------------------------------------------------------------- /ublox_gps/src/hp_pos_rec_product.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace ublox_node { 19 | 20 | // 21 | // U-Blox High Precision Positioning Receiver 22 | // 23 | HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) 24 | : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) 25 | { 26 | if (getRosBoolean(node_, "publish.nav.relposned")) { 27 | nav_relposned_pub_ = 28 | node_->create_publisher("navrelposned", 1); 29 | } 30 | 31 | if (getRosBoolean(node_, "publish.nav.heading")) { 32 | imu_pub_ = 33 | node_->create_publisher("navheading", 1); 34 | } 35 | } 36 | 37 | void HpPosRecProduct::subscribe(std::shared_ptr gps) { 38 | // Whether to publish Nav Relative Position NED 39 | // Subscribe to Nav Relative Position NED messages (also updates diagnostics) 40 | gps->subscribe(std::bind( 41 | &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); 42 | } 43 | 44 | void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m) { 45 | if (getRosBoolean(node_, "publish.nav.relposned")) { 46 | nav_relposned_pub_->publish(m); 47 | } 48 | 49 | if (getRosBoolean(node_, "publish.nav.heading")) { 50 | imu_.header.stamp = node_->now(); 51 | imu_.header.frame_id = frame_id_; 52 | 53 | imu_.linear_acceleration_covariance[0] = -1; 54 | imu_.angular_velocity_covariance[0] = -1; 55 | 56 | // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). 57 | // Alos convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id). 58 | double heading = (static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI) - M_PI_2; 59 | tf2::Quaternion orientation; 60 | orientation.setRPY(0, 0, heading); 61 | imu_.orientation.x = orientation[0]; 62 | imu_.orientation.y = orientation[1]; 63 | imu_.orientation.z = orientation[2]; 64 | imu_.orientation.w = orientation[3]; 65 | imu_.orientation_covariance[0] = 1000.0; 66 | imu_.orientation_covariance[4] = 1000.0; 67 | imu_.orientation_covariance[8] = 1000.0; 68 | // When heading is reported to be valid, use accuracy reported in 1e-5 deg units 69 | if (m.flags & ublox_msgs::msg::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID) { 70 | imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2); 71 | } 72 | 73 | imu_pub_->publish(imu_); 74 | } 75 | 76 | last_rel_pos_ = m; 77 | } 78 | 79 | } // namespace ublox_node 80 | -------------------------------------------------------------------------------- /ublox_gps/config/c94_m8p_base.yaml: -------------------------------------------------------------------------------- 1 | # Configuration Settings for C94-M8P device 2 | ublox_gps_node: 3 | ros__parameters: 4 | debug: 1 # Range 0-4 (0 means no debug statements will print) 5 | 6 | save: 7 | mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver 8 | # Manager, Antenna, and Logging Configuration 9 | device: 4 # Save to EEPROM 10 | 11 | device: /dev/ttyACM0 12 | frame_id: gps_base 13 | dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics 14 | # assumed. 15 | fix_mode: auto 16 | dr_limit: 0 17 | enable_ppp: false # Not supported by C94-M8P 18 | 19 | rate: 1.0 # Measurement rate in Hz 20 | nav_rate: 1 # in number of measurement cycles 21 | 22 | uart1: 23 | baudrate: 19200 # C94-M8P specific 24 | in: 0 # No UART in for base 25 | out: 32 # RTCM 3 26 | 27 | # TMODE3 Config 28 | tmode3: 1 # Survey-In Mode 29 | sv_in: 30 | reset: true # True: disables and re-enables survey-in (resets) 31 | # False: Disables survey-in only if TMODE3 is 32 | # disabled 33 | min_dur: 300 # Survey-In Minimum Duration [s] 34 | acc_lim: 3.0 # Survey-In Accuracy Limit [m] 35 | 36 | # RTCM out config 37 | rtcm: 38 | ids: [5, 87, 77, 230] # RTCM Messages to configure for Base station 39 | # Enabled: GPS MSM7, GLONASS MSM7, 40 | # GLONASS CP bias, Stationary RTK ref 41 | # 0xF5 0x05 Stationary RTK reference station 42 | # ARP 43 | # 0xF5 0x4A GPS MSM4 44 | # 0xF5 0x4D GPS MSM7 45 | # 0xF5 0x54 GLONASS MSM4 46 | # 0xF5 0x57 GLONASS MSM7 47 | # 0xF5 0x7C BeiDou MSM4 48 | # 0xF5 0x7F BeiDou MSM7 49 | # 0xF5 0xE6 GLONASS code-phase biases 50 | # 0xF5 0xFE Reference station PVT 51 | #(u-blox proprietary RTCM Message) 52 | rates: [1, 1, 1, 10] # in number of navigation solutions, must match 53 | # nav_rate, except for 0xE6 54 | 55 | dat: 56 | set: false 57 | 58 | # GNSS Config 59 | gnss: 60 | glonass: true # Supported by C94-M8P 61 | beidou: false # Supported by C94-M8P 62 | qzss: true # Supported by C94-M8P 63 | 64 | inf: 65 | all: true # Whether to display all INF messages in console 66 | 67 | # Enable u-blox message publishers 68 | publish: 69 | all: true 70 | aid: 71 | hui: false 72 | nav: 73 | posecef: false 74 | -------------------------------------------------------------------------------- /ublox_gps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ublox_gps) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") 14 | 15 | find_package(ament_cmake_ros REQUIRED) 16 | find_package(asio REQUIRED) 17 | find_package(diagnostic_msgs REQUIRED) 18 | find_package(diagnostic_updater REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(rcl_interfaces REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(rclcpp_components REQUIRED) 23 | find_package(sensor_msgs REQUIRED) 24 | find_package(std_msgs REQUIRED) 25 | find_package(tf2 REQUIRED) 26 | find_package(ublox_msgs REQUIRED) 27 | find_package(ublox_serialization REQUIRED) 28 | find_package(rtcm_msgs REQUIRED) 29 | 30 | # build node 31 | add_library(ublox_gps 32 | src/adr_udr_product.cpp 33 | src/gnss.cpp 34 | src/gps.cpp 35 | src/hp_pos_rec_product.cpp 36 | src/hpg_ref_product.cpp 37 | src/hpg_rov_product.cpp 38 | src/mkgmtime.c 39 | src/node.cpp 40 | src/raw_data_pa.cpp 41 | src/raw_data_product.cpp 42 | src/tim_product.cpp 43 | src/ublox_firmware.cpp 44 | src/ublox_firmware6.cpp 45 | src/ublox_firmware7.cpp 46 | src/ublox_firmware8.cpp 47 | src/ublox_firmware9.cpp) 48 | target_include_directories(ublox_gps PUBLIC 49 | "$" 50 | "$" 51 | ${diagnostic_updater_INCLUDE_DIRS} 52 | ) 53 | target_link_libraries(ublox_gps PUBLIC 54 | ${asio_LIBRARIES} 55 | ${diagnostic_updater_LIBRARIES} 56 | ${diagnostic_msgs_TARGETS} 57 | ${geometry_msgs_TARGETS} 58 | ${rcl_interfaces_TARGETS} 59 | rclcpp::rclcpp 60 | rclcpp_components::component 61 | ${rtcm_msgs_TARGETS} 62 | ${sensor_msgs_TARGETS} 63 | ${std_msgs_TARGETS} 64 | tf2::tf2 65 | ${ublox_msgs_TARGETS} 66 | ublox_serialization::ublox_serialization 67 | ) 68 | 69 | install(TARGETS ublox_gps EXPORT export_${PROJECT_NAME} 70 | ARCHIVE DESTINATION lib 71 | LIBRARY DESTINATION lib 72 | RUNTIME DESTINATION bin 73 | ) 74 | 75 | add_executable(ublox_gps_node src/node_main.cpp) 76 | target_link_libraries(ublox_gps_node PRIVATE 77 | rclcpp::rclcpp 78 | ublox_gps 79 | ) 80 | 81 | # build logger node 82 | add_executable(ublox_logger src/logger_node_pa.cpp src/raw_data_pa.cpp) 83 | target_link_libraries(ublox_logger PRIVATE 84 | rclcpp::rclcpp 85 | ${std_msgs_TARGETS} 86 | ublox_gps 87 | ) 88 | 89 | install(TARGETS 90 | ublox_gps_node ublox_logger 91 | DESTINATION lib/${PROJECT_NAME} 92 | ) 93 | 94 | rclcpp_components_register_nodes(ublox_gps 95 | "ublox_node::UbloxNode") 96 | 97 | install(DIRECTORY include/ 98 | DESTINATION include/${PROJECT_NAME} 99 | ) 100 | 101 | install( 102 | DIRECTORY launch config 103 | DESTINATION share/${PROJECT_NAME} 104 | ) 105 | 106 | ament_export_include_directories("include/${PROJECT_NAME}") 107 | ament_export_targets(export_${PROJECT_NAME}) 108 | 109 | ament_package() 110 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/ublox_firmware8.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_UBLOX_FIRMWARE8_HPP 2 | #define UBLOX_GPS_UBLOX_FIRMWARE8_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace ublox_node { 20 | 21 | /** 22 | * @brief Implements functions for firmware version 8. 23 | */ 24 | class UbloxFirmware8 : public UbloxFirmware7Plus { 25 | public: 26 | explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) 27 | : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { 28 | if (getRosBoolean(node_, "publish.nav.sat")) { 29 | nav_sat_pub_ = node->create_publisher("navstate", 1); 30 | } 31 | if (getRosBoolean(node_, "publish.mon.hw")) { 32 | mon_hw_pub_ = node->create_publisher("monhw", 1); 33 | } 34 | if (getRosBoolean(node_, "publish.rxm.rtcm")) { 35 | rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); 36 | } 37 | } 38 | 39 | /** 40 | * @brief Get the ROS parameters specific to firmware version 8. 41 | * 42 | * @details Get the GNSS, NMEA, and UPD settings. 43 | */ 44 | void getRosParams() override; 45 | 46 | /** 47 | * @brief Configure settings specific to firmware 8 based on ROS parameters. 48 | * 49 | * @details Configure GNSS, if it is different from current settings. 50 | * Configure the NMEA if desired by the user. It also may clear the 51 | * flash memory based on the ROS parameters. 52 | */ 53 | bool configureUblox(std::shared_ptr gps) override; 54 | 55 | /** 56 | * @brief Subscribe to u-blox messages which are not generic to all firmware 57 | * versions. 58 | * 59 | * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based 60 | * on user settings. 61 | */ 62 | void subscribe(std::shared_ptr gps) override; 63 | 64 | private: 65 | // Set from ROS parameters 66 | //! Whether or not to enable the Galileo GNSS 67 | bool enable_galileo_{false}; 68 | //! Whether or not to enable the BeiDuo GNSS 69 | bool enable_beidou_{false}; 70 | //! Whether or not to enable the IMES GNSS 71 | bool enable_imes_{false}; 72 | //! Desired NMEA configuration. 73 | ublox_msgs::msg::CfgNMEA cfg_nmea_; 74 | //! Whether to clear the flash memory during configuration 75 | bool clear_bbr_{false}; 76 | bool save_on_shutdown_{false}; 77 | 78 | rclcpp::Publisher::SharedPtr nav_sat_pub_; 79 | rclcpp::Publisher::SharedPtr mon_hw_pub_; 80 | rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; 81 | }; 82 | 83 | } // namespace ublox_node 84 | 85 | #endif // UBLOX_GPS_UBLOX_FIRMWARE8_HPP 86 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/fts_product.hpp: -------------------------------------------------------------------------------- 1 | //============================================================================== 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND 19 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 22 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | //============================================================================== 29 | 30 | #ifndef UBLOX_GPS_FTS_PRODUCT_HPP 31 | #define UBLOX_GPS_FTS_PRODUCT_HPP 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace ublox_node { 39 | 40 | /** 41 | * @brief Implements functions for FTS products. Currently unimplemented. 42 | * @todo Unimplemented. 43 | */ 44 | class FtsProduct final : public virtual ComponentInterface { 45 | /** 46 | * @brief Get the FTS parameters. 47 | * @todo Currently unimplemented. 48 | */ 49 | void getRosParams() override { 50 | // RCLCPP_WARN("Functionality specific to u-blox FTS devices is %s", 51 | // "unimplemented. See FtsProduct class in node.hpp & node.cpp."); 52 | } 53 | 54 | /** 55 | * @brief Configure FTS settings. 56 | * @todo Currently unimplemented. 57 | */ 58 | bool configureUblox(std::shared_ptr gps) override { 59 | (void)gps; 60 | return false; 61 | } 62 | 63 | /** 64 | * @brief Adds diagnostic updaters for FTS status. 65 | * @todo Currently unimplemented. 66 | */ 67 | void initializeRosDiagnostics() override {} 68 | 69 | /** 70 | * @brief Subscribe to FTS messages. 71 | * @todo Currently unimplemented. 72 | */ 73 | void subscribe(std::shared_ptr gps) override { 74 | (void)gps; 75 | } 76 | }; 77 | 78 | } // namespace ublox_node 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /ublox_msgs/msg/MgaGAL.msg: -------------------------------------------------------------------------------- 1 | # MGA-GAL (0x13 0x02) 2 | # Galileo Ephemeris Assistance 3 | # 4 | # This message allows the delivery of Galileo ephemeris assistance to a 5 | # receiver. See the description of AssistNow Online for details. 6 | # 7 | 8 | uint8 CLASS_ID = 19 9 | uint8 MESSAGE_ID = 2 10 | 11 | uint8 type # Message type (0x01 for this type) 12 | uint8 version # Message version (0x00 for this version) 13 | uint8 svid # Galileo Satellite identifier 14 | 15 | uint8 reserved0 # Reserved 16 | 17 | uint16 iod_nav # Ephemeris and clock correction issue of Data 18 | int16 delta_n # Mean motion difference from computed value 19 | # [semi-cir cles/s * 2^-43] 20 | int32 m0 # Mean anomaly at reference time [semi-cir cles 2^-31] 21 | uint32 e # Eccentricity [2^-33] 22 | uint32 sqrt_a # Square root of the semi-major axis [m^0.5 * 2^-19] 23 | int32 omega0 # Longitude of ascending node of orbital plane at weekly 24 | # epoch [semi-cir cles 2^-31] 25 | int32 i0 # inclination angle at reference time 26 | # [semi-cir cles 2^-31] 27 | int32 omega # Argument of perigee [semi-cir cles 2^-31] 28 | int32 omega_dot # Rate of change of right ascension 29 | # [semi-cir cles/s 2^-43] 30 | int16 i_dot # Rate of change of inclination angle 31 | # [semi-cir cles/s 2^-43] 32 | int16 cuc # Amplitude of the cosine harmonic correction term to 33 | # the argument of latitude [radians * 2^-29] 34 | int16 cus # Amplitude of the sine harmonic correction term to 35 | # the argument of latitude [radians * 2^-29] 36 | int16 crc # Amplitude of the cosine harmonic correction term 37 | # to the orbit radius [radians * 2^-5] 38 | int16 crs # Amplitude of the sine harmonic correction term to the 39 | # orbit radius [radians * 2^-5] 40 | int16 cic # Amplitude of the cosine harmonic correction term to 41 | # the angle of inclination [radians * 2^-29] 42 | int16 cis # Amplitude of the sine harmonic correction term to the 43 | # angle of inclination [radians * 2^-29] 44 | uint16 toe # Ephemeris reference time [60 * s] 45 | int32 af0 # clock bias correction coefficient [s * 2^-34] 46 | int32 af1 # SV clock drift correction coefficient [s/s * 2^-46] 47 | int8 af2 # SV clock drift rate correction coefficient 48 | # [s/s^2 * 2^-59] 49 | uint8 sisaindex_e1_e5b # Signal-in-Space Accuracy index for dual frequency 50 | # E1-E5b 51 | uint16 toc # Clock correction data reference Time of Week [60 * s] 52 | int16 bgd_e1_e5b # E1-E5b Broadcast Group Delay 53 | 54 | uint8[2] reserved1 # Reserved 55 | 56 | uint8 health_e1b # E1-B Signal Health Status 57 | uint8 data_validity_e1b # E1-B Data Validity Status 58 | uint8 health_e5b # E5b Signal Health Status 59 | uint8 data_validity_e5b # E5b Data Validity Status 60 | 61 | uint8[4] reserved2 # Reserved 62 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/adr_udr_product.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_ADR_UDR_PRODUCT_HPP 2 | #define UBLOX_GPS_ADR_UDR_PRODUCT_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace ublox_node { 24 | 25 | /** 26 | * @brief Implements functions for Automotive Dead Reckoning (ADR) and 27 | * Untethered Dead Reckoning (UDR) Devices. 28 | */ 29 | class AdrUdrProduct final : public virtual ComponentInterface { 30 | public: 31 | explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); 32 | 33 | /** 34 | * @brief Get the ADR/UDR parameters. 35 | * 36 | * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. 37 | */ 38 | void getRosParams() override; 39 | 40 | /** 41 | * @brief Configure ADR/UDR settings. 42 | * @details Configure the use_adr setting. 43 | * @return true if configured correctly, false otherwise 44 | */ 45 | bool configureUblox(std::shared_ptr gps) override; 46 | 47 | /** 48 | * @brief Initialize the ROS diagnostics for the ADR/UDR device. 49 | * @todo unimplemented 50 | */ 51 | void initializeRosDiagnostics() override { 52 | // RCLCPP_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", 53 | // "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); 54 | } 55 | 56 | /** 57 | * @brief Subscribe to ADR/UDR messages. 58 | * 59 | * @details Subscribe to NavATT, ESF and HNR messages based on user 60 | * parameters. 61 | */ 62 | void subscribe(std::shared_ptr gps) override; 63 | 64 | private: 65 | //! Whether or not to enable dead reckoning 66 | bool use_adr_; 67 | 68 | sensor_msgs::msg::Imu imu_; 69 | sensor_msgs::msg::TimeReference t_ref_; 70 | 71 | void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); 72 | 73 | rclcpp::Publisher::SharedPtr imu_pub_; 74 | rclcpp::Publisher::SharedPtr time_ref_pub_; 75 | rclcpp::Publisher::SharedPtr nav_att_pub_; 76 | rclcpp::Publisher::SharedPtr esf_ins_pub_; 77 | rclcpp::Publisher::SharedPtr esf_meas_pub_; 78 | rclcpp::Publisher::SharedPtr esf_raw_pub_; 79 | rclcpp::Publisher::SharedPtr esf_status_pub_; 80 | rclcpp::Publisher::SharedPtr hnr_pvt_pub_; 81 | 82 | uint16_t nav_rate_; 83 | uint16_t meas_rate_; 84 | 85 | std::string frame_id_; 86 | std::shared_ptr updater_; 87 | rclcpp::Node* node_; 88 | }; 89 | 90 | } // namespace ublox_node 91 | 92 | #endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP 93 | -------------------------------------------------------------------------------- /ublox_serialization/include/ublox_serialization/checksum.hpp: -------------------------------------------------------------------------------- 1 | //============================================================================== 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //============================================================================== 28 | 29 | #ifndef UBLOX_SERIALIZATION_CHECKSUM_HPP 30 | #define UBLOX_SERIALIZATION_CHECKSUM_HPP 31 | 32 | #include 33 | 34 | namespace ublox { 35 | 36 | /** 37 | * @brief calculate the checksum of a u-blox_message 38 | * @param data the start of the u-blox message 39 | * @param data the size of the u-blox message 40 | * @param ck_a the checksum a output 41 | * @param ck_b the checksum b output 42 | */ 43 | static inline void calculateChecksum(const uint8_t *data, 44 | uint32_t size, 45 | uint8_t &ck_a, 46 | uint8_t &ck_b) { 47 | ck_a = 0; ck_b = 0; 48 | for(uint32_t i = 0; i < size; ++i) 49 | { 50 | ck_a = ck_a + data[i]; 51 | ck_b = ck_b + ck_a; 52 | } 53 | } 54 | 55 | /** 56 | * @brief calculate the checksum of a u-blox_message. 57 | * @param data the start of the u-blox message 58 | * @param data the size of the u-blox message 59 | * @param checksum the checksum output 60 | * @return the checksum 61 | */ 62 | static inline uint16_t calculateChecksum(const uint8_t *data, 63 | uint32_t size, 64 | uint16_t &checksum) { 65 | uint8_t *byte = reinterpret_cast(&checksum); 66 | calculateChecksum(data, size, byte[0], byte[1]); 67 | return checksum; 68 | } 69 | 70 | } // namespace ublox 71 | 72 | #endif // UBLOX_SERIALIZATION_CHECKSUM_HPP 73 | -------------------------------------------------------------------------------- /ublox_msgs/msg/NavSTATUS.msg: -------------------------------------------------------------------------------- 1 | # NAV-STATUS (0x01 0x03) 2 | # Receiver Navigation Status 3 | # 4 | # See important comments concerning validity of position and velocity given in 5 | # section Navigation Output Filters. 6 | # 7 | 8 | uint8 CLASS_ID = 1 9 | uint8 MESSAGE_ID = 3 10 | 11 | uint32 i_tow # GPS Millisecond time of week [ms] 12 | 13 | uint8 gps_fix # GPSfix Type, this value does not qualify a fix as 14 | # valid and within the limits. See note on flag gpsFixOk 15 | # below 16 | uint8 GPS_NO_FIX = 0 17 | uint8 GPS_DEAD_RECKONING_ONLY = 1 18 | uint8 GPS_2D_FIX = 2 19 | uint8 GPS_3D_FIX = 3 20 | uint8 GPS_GPS_DEAD_RECKONING_COMBINED = 4 21 | uint8 GPS_TIME_ONLY_FIX = 5 22 | 23 | uint8 flags # Navigation Status Flags 24 | uint8 FLAGS_GPS_FIX_OK = 1 # position & velocity valid & within DOP & ACC 25 | # Masks 26 | uint8 FLAGS_DIFF_SOLN = 2 # Differential corrections were applied 27 | uint8 FLAGS_WKNSET = 4 # Week Number valid 28 | uint8 FLAGS_TOWSET = 8 # Time of Week valid 29 | 30 | uint8 fix_stat # Fix Status Information 31 | uint8 FIX_STAT_DIFF_CORR_MASK = 1 # 1 = differential corrections available 32 | # map matching status: 33 | uint8 FIX_STAT_MAP_MATCHING_MASK = 192 34 | uint8 MAP_MATCHING_NONE = 0 # none 35 | uint8 MAP_MATCHING_VALID = 64 # valid but not used, i.e. map matching data 36 | # was received, but was too old 37 | uint8 MAP_MATCHING_USED = 128 # valid and used, map matching data was applied 38 | uint8 MAP_MATCHING_DR = 192 # valid and used, map matching data was 39 | # applied. In case of sensor unavailability map 40 | # matching data enables dead reckoning. 41 | # This requires map matched latitude/longitude 42 | # or heading data. 43 | 44 | uint8 flags2 # further information about navigation output 45 | # power safe mode state (Only for FW version >= 7.01; undefined otherwise) 46 | uint8 FLAGS2_PSM_STATE_MASK = 3 47 | uint8 PSM_STATE_ACQUISITION = 0 # ACQUISITION 48 | # [or when psm disabled] 49 | uint8 PSM_STATE_TRACKING = 1 # TRACKING 50 | uint8 PSM_STATE_POWER_OPTIMIZED_TRACKING = 2 # POWER OPTIMIZED TRACKING 51 | uint8 PSM_STATE_INACTIVE = 3 # INACTIVE 52 | # Note that the spoofing state value only reflects the detector state for the 53 | # current navigation epoch. As spoofing can be detected most easily at the 54 | # transition from real signal to spoofing signal, this is also where the 55 | # detector is triggered the most. I.e. a value of 1 - No spoofing indicated does 56 | # not mean that the receiver is not spoofed, it #simply states that the detector 57 | # was not triggered in this epoch. 58 | uint8 FLAGS2_SPOOF_DET_STATE_MASK = 24 59 | uint8 SPOOF_DET_STATE_UNKNOWN = 0 # Unknown or deactivated 60 | uint8 SPOOF_DET_STATE_NONE = 8 # No spoofing indicated 61 | uint8 SPOOF_DET_STATE_SPOOFING = 16 # Spoofing indicated 62 | uint8 SPOOF_DET_STATE_MULTIPLE = 24 # Multiple spoofing indication 63 | 64 | uint32 ttff # Time to first fix (millisecond time tag) [ms] 65 | uint32 msss # Milliseconds since Startup / Reset [ms] 66 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/component_interface.hpp: -------------------------------------------------------------------------------- 1 | //============================================================================== 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND 19 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 22 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | //============================================================================== 29 | 30 | #ifndef UBLOX_GPS_COMPONENT_INTERFACE_HPP 31 | #define UBLOX_GPS_COMPONENT_INTERFACE_HPP 32 | 33 | #include 34 | 35 | #include 36 | 37 | // This file declares the ComponentInterface which acts as a high level 38 | // interface for u-blox firmware, product categories, etc. It contains methods 39 | // to configure the u-blox and subscribe to u-blox messages. 40 | // 41 | 42 | namespace ublox_node { 43 | 44 | /** 45 | * @brief This interface is used to add functionality to the main node. 46 | * 47 | * @details This interface is generic and can be implemented for other features 48 | * besides the main node, hardware versions, and firmware versions. 49 | */ 50 | class ComponentInterface { 51 | public: 52 | /** 53 | * @brief Get the ROS parameters. 54 | * @throws std::runtime_error if a parameter is invalid or required 55 | * parameters are not set. 56 | */ 57 | virtual void getRosParams() = 0; 58 | 59 | /** 60 | * @brief Configure the U-Blox settings. 61 | * @return true if configured correctly, false otherwise 62 | */ 63 | virtual bool configureUblox(std::shared_ptr gps) = 0; 64 | 65 | /** 66 | * @brief Initialize the diagnostics. 67 | * 68 | * @details Function may be empty. 69 | */ 70 | virtual void initializeRosDiagnostics() = 0; 71 | 72 | /** 73 | * @brief Subscribe to u-blox messages and publish to ROS topics. 74 | */ 75 | virtual void subscribe(std::shared_ptr gps) = 0; 76 | }; 77 | 78 | } // namespace ublox_node 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /ublox_gps/src/raw_data_product.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace ublox_node { 17 | 18 | // 19 | // Raw Data Products 20 | // 21 | RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node) 22 | : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { 23 | if (getRosBoolean(node_, "publish.rxm.raw")) { 24 | rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); 25 | } 26 | if (getRosBoolean(node_, "publish.rxm.sfrb")) { 27 | rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); 28 | } 29 | if (getRosBoolean(node_, "publish.rxm.eph")) { 30 | rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); 31 | } 32 | if (getRosBoolean(node_, "publish.rxm.almRaw")) { 33 | rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); 34 | } 35 | } 36 | 37 | void RawDataProduct::subscribe(std::shared_ptr gps) { 38 | // Subscribe to RXM Raw 39 | if (getRosBoolean(node_, "publish.rxm.raw")) { 40 | gps->subscribe([this](const ublox_msgs::msg::RxmRAW &m) { rxm_raw_pub_->publish(m); }, 41 | 1); 42 | } 43 | 44 | // Subscribe to RXM SFRB 45 | if (getRosBoolean(node_, "publish.rxm.sfrb")) { 46 | gps->subscribe([this](const ublox_msgs::msg::RxmSFRB &m) { rxm_sfrb_pub_->publish(m); }, 47 | 1); 48 | } 49 | 50 | // Subscribe to RXM EPH 51 | if (getRosBoolean(node_, "publish.rxm.eph")) { 52 | gps->subscribe([this](const ublox_msgs::msg::RxmEPH &m) { rxm_eph_pub_->publish(m); }, 53 | 1); 54 | } 55 | 56 | // Subscribe to RXM ALM 57 | if (getRosBoolean(node_, "publish.rxm.almRaw")) { 58 | gps->subscribe([this](const ublox_msgs::msg::RxmALM &m) { rxm_alm_pub_->publish(m); }, 59 | 1); 60 | } 61 | } 62 | 63 | void RawDataProduct::initializeRosDiagnostics() { 64 | if (getRosBoolean(node_, "publish.rxm.raw")) { 65 | freq_diagnostics_.push_back(std::make_shared( 66 | "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); 67 | } 68 | if (getRosBoolean(node_, "publish.rxm.sfrb")) { 69 | freq_diagnostics_.push_back(std::make_shared( 70 | "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); 71 | } 72 | if (getRosBoolean(node_, "publish.rxm.eph")) { 73 | freq_diagnostics_.push_back(std::make_shared( 74 | "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); 75 | } 76 | if (getRosBoolean(node_, "publish.rxm.almRaw")) { 77 | freq_diagnostics_.push_back(std::make_shared( 78 | "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); 79 | } 80 | } 81 | 82 | } // namespace ublox_node 83 | -------------------------------------------------------------------------------- /ublox_gps/src/tim_product.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace ublox_node { 19 | 20 | // 21 | // U-Blox Time Sync Products, partially implemented. 22 | // 23 | TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : frame_id_(frame_id), updater_(updater), node_(node) 24 | { 25 | timtm2_pub_ = 26 | node_->create_publisher("timtm2", 1); 27 | interrupt_time_pub_ = 28 | node_->create_publisher("interrupt_time", 1); 29 | 30 | if (getRosBoolean(node_, "publish.rxm.sfrb")) { 31 | rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); 32 | } 33 | if (getRosBoolean(node_, "publish.rxm.raw")) { 34 | rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); 35 | } 36 | } 37 | 38 | void TimProduct::getRosParams() { 39 | } 40 | 41 | bool TimProduct::configureUblox(std::shared_ptr gps) { 42 | uint8_t r = 1; 43 | // Configure the reciever 44 | if (!gps->setUTCtime()) { 45 | throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); 46 | } 47 | 48 | if (!gps->setTimtm2(r)) { 49 | throw std::runtime_error(std::string("Failed to Configure TIM Product")); 50 | } 51 | 52 | return true; 53 | } 54 | 55 | void TimProduct::subscribe(std::shared_ptr gps) { 56 | gps->subscribe(std::bind( 57 | &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); 58 | 59 | // RCLCPP_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); 60 | 61 | // Subscribe to SFRBX messages 62 | if (getRosBoolean(node_, "publish.rxm.sfrb")) { 63 | gps->subscribe([this](const ublox_msgs::msg::RxmSFRBX &m) { rxm_sfrb_pub_->publish(m); }, 64 | 1); 65 | } 66 | 67 | // Subscribe to RawX messages 68 | if (getRosBoolean(node_, "publish.rxm.raw")) { 69 | gps->subscribe([this](const ublox_msgs::msg::RxmRAWX &m) { rxm_raw_pub_->publish(m); }, 70 | 1); 71 | } 72 | } 73 | 74 | void TimProduct::callbackTimTM2(const ublox_msgs::msg::TimTM2 &m) { 75 | if (getRosBoolean(node_, "publish.tim.tm2")) { 76 | // create time ref message and put in the data 77 | t_ref_.header.stamp = node_->now(); 78 | t_ref_.header.frame_id = frame_id_; 79 | 80 | t_ref_.time_ref = rclcpp::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); 81 | 82 | std::ostringstream src; 83 | src << "TIM" << int(m.ch); 84 | t_ref_.source = src.str(); 85 | 86 | t_ref_.header.stamp = node_->now(); // create a new timestamp 87 | t_ref_.header.frame_id = frame_id_; 88 | 89 | timtm2_pub_->publish(m); 90 | interrupt_time_pub_->publish(t_ref_); 91 | } 92 | 93 | } 94 | 95 | void TimProduct::initializeRosDiagnostics() { 96 | updater_->force_update(); 97 | } 98 | 99 | } // namespace ublox_node 100 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgNAVX5.msg: -------------------------------------------------------------------------------- 1 | # CFG-NAVX5 (0x06 0x23) 2 | # Navigation Engine Expert Settings 3 | # 4 | # Warning: Refer to u-blox protocol spec before changing these settings. 5 | 6 | uint8 CLASS_ID = 6 7 | uint8 MESSAGE_ID = 35 8 | 9 | uint16 version # Message version (set to 0) 10 | 11 | uint16 mask1 # First parameters bitmask (possible values below) 12 | uint16 MASK1_MIN_MAX = 4 # apply min/max SVs settings 13 | uint16 MASK1_MIN_CNO = 8 # apply minimum C/N0 setting 14 | uint16 MASK1_INITIAL_FIX_3D = 64 # apply initial 3D fix settings 15 | uint16 MASK1_WKN_ROLL = 512 # apply GPS week number rollover settings 16 | uint16 MASK1_ACK_AID = 1024 # apply assistance acknowledgment 17 | # settings 18 | uint16 MASK1_PPP = 8192 # apply usePPP flag 19 | uint16 MASK1_AOP = 16384 # apply aopCfg (useAOP flag) and 20 | # aopOrbMaxErr settings 21 | # (AssistNow Autonomous) 22 | 23 | uint32 mask2 # Second parameters bitmask (possible values below) 24 | # Firmware >=8 only 25 | uint32 MASK2_ADR = 64 # Apply ADR sensor fusion on/off 26 | # setting 27 | uint32 MASK2_SIG_ATTEN_COMP_MODE = 128 # Apply signal attenuation 28 | # compensation feature settings 29 | 30 | uint8[2] reserved1 # Always set to zero 31 | 32 | uint8 min_svs # Minimum number of satellites for navigation 33 | uint8 max_svs # Maximum number of satellites for navigation 34 | uint8 min_cno # Minimum satellite signal level for navigation [dBHz] 35 | 36 | uint8 reserved2 # Always set to zero 37 | 38 | uint8 ini_fix3d # If set to 1, initial fix must be 3D 39 | 40 | uint8[2] reserved3 # Always set to zero 41 | 42 | uint8 ack_aiding # If set to 1, issue acknowledgments for assistance 43 | uint16 wkn_rollover # GPS week rollover number, GPS week numbers will be set 44 | # correctly from this week up to 1024 weeks after this 45 | # week 46 | uint8 sig_atten_comp_mode # Permanently attenuated signal compensation [dBHz] 47 | # 0 = disabled, 255 = automatic 48 | # 1..63 = maximum expected C/N0 value 49 | # Firmware 8 only 50 | 51 | uint8[5] reserved4 # Always set to zero 52 | 53 | uint8 use_ppp # Enable/disable PPP (on supported units) 54 | uint8 aop_cfg # AssistNow Autonomous configuration, 1: enabled 55 | 56 | uint8[2] reserved5 # Always set to zero 57 | 58 | uint16 aop_orb_max_err # Maximum acceptable (modeled) autonomous orbit 59 | # error [m] 60 | # valid range = 5..1000 61 | # 0 = reset to firmware default 62 | 63 | uint8[7] reserved6 # Always set to zero 64 | 65 | uint8 use_adr # Enable/disable ADR sensor fusion 66 | # 1: enabled, 0: disabled 67 | # Only supported on certain products 68 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP 2 | #define UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ublox_node { 12 | 13 | //! Topic diagnostics for u-blox messages 14 | struct UbloxTopicDiagnostic { 15 | UbloxTopicDiagnostic() = default; 16 | 17 | // Must not copy this struct (would confuse FrequencyStatusParam pointers) 18 | UbloxTopicDiagnostic(UbloxTopicDiagnostic &&c) = delete; 19 | UbloxTopicDiagnostic &operator=(UbloxTopicDiagnostic &&c) = delete; 20 | UbloxTopicDiagnostic(const UbloxTopicDiagnostic &c) = delete; 21 | UbloxTopicDiagnostic &operator=(const UbloxTopicDiagnostic &c) = delete; 22 | 23 | ~UbloxTopicDiagnostic() = default; 24 | 25 | /** 26 | * @brief Add a topic diagnostic to the diagnostic updater for 27 | * 28 | * @details The minimum and maximum frequency are equal to the nav rate in Hz. 29 | * @param name the ROS topic 30 | * @param freq_tol the tolerance [%] for the topic frequency 31 | * @param freq_window the number of messages to use for diagnostic statistics 32 | */ 33 | explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, 34 | uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { 35 | const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz 36 | min_freq = target_freq; 37 | max_freq = target_freq; 38 | diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, 39 | freq_tol, freq_window); 40 | diagnostic = std::make_shared(topic, 41 | *updater, 42 | freq_param); 43 | } 44 | 45 | /** 46 | * @brief Add a topic diagnostic to the diagnostic updater for 47 | * 48 | * @details The minimum and maximum frequency are equal to the nav rate in Hz. 49 | * @param name the ROS topic 50 | * @param freq_min the minimum acceptable frequency for the topic 51 | * @param freq_max the maximum acceptable frequency for the topic 52 | * @param freq_tol the tolerance [%] for the topic frequency 53 | * @param freq_window the number of messages to use for diagnostic statistics 54 | */ 55 | explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, 56 | double freq_tol, int freq_window, std::shared_ptr updater) { 57 | min_freq = freq_min; 58 | max_freq = freq_max; 59 | diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, 60 | freq_tol, freq_window); 61 | diagnostic = std::make_shared(topic, 62 | *updater, 63 | freq_param); 64 | } 65 | 66 | //! Topic frequency diagnostic updater 67 | std::shared_ptr diagnostic; 68 | //! Minimum allow frequency of topic 69 | double min_freq{0.0}; 70 | //! Maximum allow frequency of topic 71 | double max_freq{0.0}; 72 | }; 73 | 74 | } // namespace ublox_node 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /ublox_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ublox_msgs) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake_ros REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | find_package(ublox_serialization REQUIRED) 18 | 19 | set(msg_files 20 | "msg/Ack.msg" 21 | "msg/AidALM.msg" 22 | "msg/AidEPH.msg" 23 | "msg/AidHUI.msg" 24 | "msg/CfgANT.msg" 25 | "msg/CfgCFG.msg" 26 | "msg/CfgDAT.msg" 27 | "msg/CfgDGNSS.msg" 28 | "msg/CfgGNSSBlock.msg" 29 | "msg/CfgGNSS.msg" 30 | "msg/CfgHNR.msg" 31 | "msg/CfgINFBlock.msg" 32 | "msg/CfgINF.msg" 33 | "msg/CfgMSG.msg" 34 | "msg/CfgNAV5.msg" 35 | "msg/CfgNAVX5.msg" 36 | "msg/CfgNMEA6.msg" 37 | "msg/CfgNMEA7.msg" 38 | "msg/CfgNMEA.msg" 39 | "msg/CfgPRT.msg" 40 | "msg/CfgRATE.msg" 41 | "msg/CfgRST.msg" 42 | "msg/CfgSBAS.msg" 43 | "msg/CfgTMODE3.msg" 44 | "msg/CfgUSB.msg" 45 | "msg/EsfINS.msg" 46 | "msg/EsfMEAS.msg" 47 | "msg/EsfRAWBlock.msg" 48 | "msg/EsfRAW.msg" 49 | "msg/EsfSTATUS.msg" 50 | "msg/EsfSTATUSSens.msg" 51 | "msg/HnrPVT.msg" 52 | "msg/Inf.msg" 53 | "msg/MgaGAL.msg" 54 | "msg/MonGNSS.msg" 55 | "msg/MonHW6.msg" 56 | "msg/MonHW.msg" 57 | "msg/MonVERExtension.msg" 58 | "msg/MonVER.msg" 59 | "msg/NavATT.msg" 60 | "msg/NavCLOCK.msg" 61 | "msg/NavDGPS.msg" 62 | "msg/NavDGPSSV.msg" 63 | "msg/NavDOP.msg" 64 | "msg/NavPOSECEF.msg" 65 | "msg/NavPOSLLH.msg" 66 | "msg/NavPVT7.msg" 67 | "msg/NavPVT.msg" 68 | "msg/NavRELPOSNED9.msg" 69 | "msg/NavRELPOSNED.msg" 70 | "msg/NavSAT.msg" 71 | "msg/NavSATSV.msg" 72 | "msg/NavSBAS.msg" 73 | "msg/NavSBASSV.msg" 74 | "msg/NavSOL.msg" 75 | "msg/NavSTATUS.msg" 76 | "msg/NavSVINFO.msg" 77 | "msg/NavSVINFOSV.msg" 78 | "msg/NavSVIN.msg" 79 | "msg/NavTIMEGPS.msg" 80 | "msg/NavTIMEUTC.msg" 81 | "msg/NavVELECEF.msg" 82 | "msg/NavVELNED.msg" 83 | "msg/RxmALM.msg" 84 | "msg/RxmEPH.msg" 85 | "msg/RxmRAW.msg" 86 | "msg/RxmRAWSV.msg" 87 | "msg/RxmRAWXMeas.msg" 88 | "msg/RxmRAWX.msg" 89 | "msg/RxmRTCM.msg" 90 | "msg/RxmSFRB.msg" 91 | "msg/RxmSFRBX.msg" 92 | "msg/RxmSVSI.msg" 93 | "msg/RxmSVSISV.msg" 94 | "msg/TimTM2.msg" 95 | "msg/UpdSOSAck.msg" 96 | "msg/UpdSOS.msg" 97 | ) 98 | 99 | rosidl_generate_interfaces(${PROJECT_NAME} 100 | ${msg_files} 101 | DEPENDENCIES 102 | sensor_msgs 103 | std_msgs 104 | ) 105 | 106 | rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") 107 | if(cpp_typesupport_target) 108 | add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) 109 | target_include_directories(${PROJECT_NAME}_lib PRIVATE 110 | "$" 111 | "$") 112 | target_link_libraries(${PROJECT_NAME}_lib 113 | ${cpp_typesupport_target} 114 | ublox_serialization::ublox_serialization 115 | ) 116 | 117 | install(TARGETS ${PROJECT_NAME}_lib EXPORT ${PROJECT_NAME}_lib 118 | ARCHIVE DESTINATION lib 119 | LIBRARY DESTINATION lib 120 | RUNTIME DESTINATION bin 121 | ) 122 | 123 | install(DIRECTORY include/ 124 | DESTINATION "include/${PROJECT_NAME}" 125 | ) 126 | 127 | ament_export_include_directories("include/${PROJECT_NAME}") 128 | ament_export_libraries(${PROJECT_NAME}_lib) 129 | ament_export_targets(${PROJECT_NAME}_lib) 130 | endif() 131 | 132 | ament_export_dependencies(rosidl_default_runtime sensor_msgs std_msgs ublox_serialization) 133 | 134 | ament_package() 135 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgNMEA7.msg: -------------------------------------------------------------------------------- 1 | # CFG-NMEA (0x06 0x17) 2 | # NMEA protocol configuration V0 3 | # 4 | # Set/Get the NMEA protocol configuration. See section NMEA Protocol 5 | # Configuration for a detailed description of the configuration effects on 6 | # NMEA output 7 | # 8 | # Supported on: u-blox 7 firmware version 1.00 9 | # 10 | 11 | uint8 CLASS_ID = 6 12 | uint8 MESSAGE_ID = 23 13 | 14 | uint8 filter # filter flags 15 | uint8 FILTER_POS = 1 # Enable position output for failed or 16 | # invalid fixes 17 | uint8 FILTER_MSK_POS = 2 # Enable position output for invalid fixes 18 | uint8 FILTER_TIME = 4 # Enable time output for invalid times 19 | uint8 FILTER_DATE = 8 # Enable date output for invalid dates 20 | uint8 FILTER_GPS_ONLY = 16 # Restrict output to GPS satellites only 21 | uint8 FILTER_TRACK = 32 # Enable COG output even if COG is frozen 22 | 23 | uint8 nmea_version # NMEA version 24 | uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 25 | uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 26 | 27 | uint8 num_sv # Maximum Number of SVs to report per TalkerId: 28 | # unlimited (0) or 8, 12, 16 29 | uint8 NUM_SV_UNLIMITED = 0 30 | 31 | uint8 flags # flags 32 | uint8 FLAGS_COMPAT = 1 # enable compatibility mode. 33 | # This might be needed for certain applications 34 | # when customer's NMEA parser expects a fixed 35 | # number of digits in position coordinates 36 | uint8 FLAGS_CONSIDER = 2 # enable considering mode 37 | 38 | uint32 gnss_to_filter # Filters out satellites based on their GNSS. 39 | # If a bitfield is enabled, the corresponding 40 | # satellites will be not output. 41 | uint32 GNSS_TO_FILTER_GPS = 1 # Disable reporting of GPS satellites 42 | uint32 GNSS_TO_FILTER_SBAS = 2 # Disable reporting of SBAS satellites 43 | uint32 GNSS_TO_FILTER_QZSS = 16 # Disable reporting of QZSS satellites 44 | uint32 GNSS_TO_FILTER_GLONASS = 32 # Disable reporting of GLONASS satellites 45 | 46 | uint8 sv_numbering # Configures the display of satellites that do not 47 | # have an NMEA-defined value. Note: this does not 48 | # apply to satellites with an unknown ID. 49 | uint8 SV_NUMBERING_STRICT = 0 # Strict - Satellites are not output 50 | uint8 SV_NUMBERING_EXTENDED = 1 # Extended - Use proprietary numbering 51 | 52 | uint8 main_talker_id # By default the main Talker ID (i.e. the Talker 53 | # ID used for all messages other than GSV) is 54 | # determined by the GNSS assignment of the 55 | # receiver's channels (see CfgGNSS). 56 | # This field enables the main Talker ID to be 57 | # overridden 58 | uint8 MAIN_TALKER_ID_NOT_OVERRIDDEN = 0 # Main Talker ID is not overridden 59 | uint8 MAIN_TALKER_ID_GP = 1 # Set main Talker ID to 'GP' 60 | uint8 MAIN_TALKER_ID_GL = 2 # Set main Talker ID to 'GL' 61 | uint8 MAIN_TALKER_ID_GN = 3 # Set main Talker ID to 'GN' 62 | 63 | uint8 gsv_talker_id # By default the Talker ID for GSV messages is 64 | # GNSS specific (as defined by NMEA). This field 65 | # enables the GSV Talker ID to be overridden. 66 | uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID 67 | # (as defined by NMEA) 68 | uint8 GSV_TALKER_ID_MAIN = 1 # Use the main Talker ID 69 | 70 | uint8 reserved # Reserved 71 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgTMODE3.msg: -------------------------------------------------------------------------------- 1 | # CFG-TMODE3 (0x06, 0x71) 2 | # Time Mode Settings 3 3 | # 4 | # Configures the receiver to be in Time Mode. The position referred to in this 5 | # message is that of the Antenna Reference Point (ARP). See the Time Mode 6 | # Description for details. 7 | # 8 | # Supported on: 9 | # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision 10 | # GNSS products) 11 | # 12 | 13 | uint8 CLASS_ID = 6 14 | uint8 MESSAGE_ID = 113 15 | 16 | uint8 version # Message version (0x00 for this version) 17 | uint8 reserved1 # Reserved 18 | 19 | uint16 flags 20 | uint16 FLAGS_MODE_MASK = 255 # Receiver Mode: 21 | uint16 FLAGS_MODE_DISABLED = 0 # Disabled 22 | uint16 FLAGS_MODE_SURVEY_IN = 1 # Survey In 23 | uint16 FLAGS_MODE_FIXED = 2 # Fixed Mode (true ARP position required) 24 | uint16 FLAGS_LLA = 256 # Position is given in LAT/LON/ALT 25 | # (default is ECEF) 26 | 27 | int32 ecef_x_or_lat # WGS84 ECEF X coordinate (or latitude) of 28 | # the ARP position, depending on flags above 29 | # [cm] or [deg / 1e-7] 30 | int32 ecef_y_or_lon # WGS84 ECEF Y coordinate (or longitude) of 31 | # the ARP position, depending on flags above 32 | # [cm] or [deg / 1e-7] 33 | int32 ecef_z_or_alt # WGS84 ECEF Z coordinate (or altitude) of 34 | # the ARP position, depending on flags above 35 | # [cm] 36 | int8 ecef_x_or_lat_hp # High-precision WGS84 ECEF X coordinate (or 37 | # latitude) of the ARP position, depending on 38 | # flags above. Must be in the range -99..+99. 39 | # The precise WGS84 ECEF X coordinate in units 40 | # of cm, or the precise WGS84 ECEF latitude in 41 | # units of 1e-7 degrees, is given by 42 | # ecefXOrLat + (ecefXOrLatHP * 1e-2) 43 | # [0.1 mm] or [deg * 1e-9] 44 | int8 ecef_y_or_lon_hp # High-precision WGS84 ECEF Y coordinate (or 45 | # longitude) of the ARP position, depending on 46 | # flags above. Must be in the range -99..+99. 47 | # The precise WGS84 ECEF Y coordinate in units 48 | # of cm, or the precise WGS84 ECEF longitude 49 | # in units of 1e-7 degrees, is given by 50 | # ecefYOrLon + (ecefYOrLonHP * 1e-2) 51 | # [0.1 mm] or [deg * 1e-9] 52 | int8 ecef_z_or_alt_hp # High-precision WGS84 ECEF Z coordinate (or 53 | # altitude) of the ARP position, depending on 54 | # flags above. Must be in the range -99..+99. 55 | # The precise WGS84 ECEF Z coordinate, or 56 | # altitude coordinate, in units of cm is given 57 | # by ecefZOrAlt + (ecefZOrAltHP * 1e-2) 58 | # [0.1 mm] 59 | uint8 reserved2 # Reserved 60 | 61 | uint32 fixed_pos_acc # Fixed position 3D accuracy 62 | # [0.1 mm] 63 | uint32 svin_min_dur # Survey-in minimum duration 64 | # [s] 65 | uint32 svin_acc_limit # Survey-in position accuracy limit 66 | # [0.1 mm] 67 | 68 | uint8[8] reserved3 # Reserved 69 | -------------------------------------------------------------------------------- /ublox_gps/include/ublox_gps/worker.hpp: -------------------------------------------------------------------------------- 1 | //============================================================================== 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //============================================================================== 28 | 29 | #ifndef UBLOX_GPS_WORKER_HPP 30 | #define UBLOX_GPS_WORKER_HPP 31 | 32 | #include 33 | #include 34 | 35 | namespace ublox_gps { 36 | 37 | /** 38 | * @brief Handles I/O reading and writing. 39 | */ 40 | // clang-tidy insists that we follow rule-of-5 for this abstract base class. 41 | // Generally we'd define the copy and move constructors as delete, but once 42 | // we do that we also have to explicitly define a default constructor (otherwise 43 | // it fails to compile). That all works, but harms the readability of this 44 | // completely abstract base class (basically an interface), so we just disable 45 | // the checks for this class. 46 | class Worker { // NOLINT(hicpp-special-member-functions, cppcoreguidelines-special-member-functions) 47 | public: 48 | using WorkerCallback = std::function; 49 | using WorkerRawCallback = std::function; 50 | 51 | virtual ~Worker() = default; 52 | 53 | /** 54 | * @brief Set the callback function for received messages. 55 | * @param callback the callback function which process messages in the buffer 56 | */ 57 | virtual void setCallback(const WorkerCallback& callback) = 0; 58 | 59 | /** 60 | * @brief Set the callback function which handles raw data. 61 | * @param callback the write callback which handles raw data 62 | */ 63 | virtual void setRawDataCallback(const WorkerRawCallback& callback) = 0; 64 | 65 | /** 66 | * @brief Send the data in the buffer. 67 | * @param data the bytes to send 68 | * @param size the size of the buffer 69 | */ 70 | virtual bool send(const unsigned char* data, const unsigned int size) = 0; 71 | 72 | /** 73 | * @brief Wait for an incoming message. 74 | * @param timeout the maximum time to wait. 75 | */ 76 | virtual void wait(const std::chrono::milliseconds& timeout) = 0; 77 | 78 | /** 79 | * @brief Whether or not the I/O stream is open. 80 | */ 81 | virtual bool isOpen() const = 0; 82 | }; 83 | 84 | } // namespace ublox_gps 85 | 86 | #endif // UBLOX_GPS_WORKER_HPP 87 | -------------------------------------------------------------------------------- /ublox_msgs/msg/CfgNAV5.msg: -------------------------------------------------------------------------------- 1 | # CFG-NAV5 (0x06 0x24) 2 | # Navigation Engine Settings 3 | 4 | uint8 CLASS_ID = 6 5 | uint8 MESSAGE_ID = 36 6 | 7 | uint16 mask # Parameters Bitmask. Only the masked 8 | # parameters will be applied. 9 | uint16 MASK_DYN = 1 # Apply dynamic model settings 10 | uint16 MASK_MIN_EL = 2 # Apply minimum elevation settings 11 | uint16 MASK_FIX_MODE = 4 # Apply fix mode settings 12 | uint16 MASK_DR_LIM = 8 # Apply DR limit settings 13 | uint16 MASK_POS_MASK = 16 # Apply position mask settings 14 | uint16 MASK_TIME_MASK = 32 # Apply time mask settings 15 | uint16 MASK_STATIC_HOLD_MASK = 64 # Apply static hold settings 16 | uint16 MASK_DGPS_MASK = 128 # Apply DGPS settings, firmware >= 7 only 17 | uint16 MASK_CNO = 256 # Apply CNO threshold settings 18 | uint16 MASK_UTC = 1024 # Apply UTC settings, protocol >= 16 only 19 | 20 | uint8 dyn_model # Dynamic Platform model: 21 | uint8 DYN_MODEL_PORTABLE = 0 # Portable 22 | uint8 DYN_MODEL_STATIONARY = 2 # Stationary 23 | uint8 DYN_MODEL_PEDESTRIAN = 3 # Pedestrian 24 | uint8 DYN_MODEL_AUTOMOTIVE = 4 # Automotive 25 | uint8 DYN_MODEL_SEA = 5 # Sea 26 | uint8 DYN_MODEL_AIRBORNE_1G = 6 # Airborne with <1g Acceleration 27 | uint8 DYN_MODEL_AIRBORNE_2G = 7 # Airborne with <2g Acceleration 28 | uint8 DYN_MODEL_AIRBORNE_4G = 8 # Airborne with <4g Acceleration 29 | uint8 DYN_MODEL_WRIST_WATCH = 9 # Wrist watch, protocol >= 18 30 | 31 | uint8 fix_mode # Position Fixing Mode. 32 | uint8 FIX_MODE_2D_ONLY = 1 # 2D only 33 | uint8 FIX_MODE_3D_ONLY = 2 # 3D only 34 | uint8 FIX_MODE_AUTO = 3 # Auto 2D/3D 35 | 36 | int32 fixed_alt # Fixed altitude (mean sea level) for 2D fix mode. 37 | # [m / 0.01] 38 | uint32 fixed_alt_var # Fixed altitude variance for 2D mode. [m^2 / 0.0001] 39 | int8 min_elev # Minimum Elevation for a GNSS satellite to be used in 40 | # NAV [deg] 41 | uint8 dr_limit # Maximum time to perform dead reckoning [s] 42 | # (linear extrapolation) in case of GPS signal loss 43 | uint16 p_dop # Position DOP Mask to use [1 / 0.1] 44 | uint16 t_dop # Time DOP Mask to use [1 / 0.1] 45 | uint16 p_acc # Position Accuracy Mask [m] 46 | uint16 t_acc # Time Accuracy Mask [m] 47 | uint8 static_hold_thresh # Static hold threshold [cm/s] 48 | uint8 dgnss_time_out # DGNSS timeout, firmware 7 and newer only [s] 49 | 50 | uint8 cno_thresh_num_svs # Number of satellites required to have C/N0 above 51 | # cnoThresh for a fix to be attempted 52 | uint8 cno_thresh # C/N0 threshold for deciding whether to attempt a fix 53 | # [dBHz] 54 | uint8[2] reserved1 # Reserved 55 | 56 | uint16 static_hold_max_dist # Static hold distance threshold (before quitting 57 | # static hold) [m] 58 | uint8 utc_standard # UTC standard to be used: 59 | uint8 UTC_STANDARD_AUTOMATIC = 0 # receiver selects based on GNSS configuration 60 | uint8 UTC_STANDARD_GPS = 3 # UTC as operated by the U.S. Naval Observatory 61 | # (USNO); derived from GPS time 62 | uint8 UTC_STANDARD_GLONASS = 6 # UTC as operated by the former Soviet Union; 63 | # derived from GLONASS time 64 | uint8 UTC_STANDARD_BEIDOU = 7 # UTC as operated by the National Time Service 65 | # Center, China; derived from BeiDou time 66 | uint8[5] reserved2 # Reserved 67 | --------------------------------------------------------------------------------