diff --git a/ethz_piksi_ros/package.xml b/ethz_piksi_ros/package.xml index e82f33b7..c014aad6 100644 --- a/ethz_piksi_ros/package.xml +++ b/ethz_piksi_ros/package.xml @@ -1,7 +1,7 @@ ethz_piksi_ros - 1.8.1 + 1.9.0 Meta-package for the ethz_piksi_ros repository. Marco Tranzatto diff --git a/piksi_multi_rtk_ros/README.md b/piksi_multi_rtk_ros/README.md index 589dbc6b..2d372048 100644 --- a/piksi_multi_rtk_ros/README.md +++ b/piksi_multi_rtk_ros/README.md @@ -7,7 +7,7 @@ The piksi_multi_rtk_ros package has been tested under ROS Indigo and Ubuntu 14.0 **WARNING:** default baud rate of the driver is set to '230400' (default baud rate of Piksi Multi is '115200'). This means you have to set your Piksi Multi baud rate correctly by following [these settings instructions](https://github.com/ethz-asl/ethz_piksi_ros/wiki/Installing-and-Configuring-Your-Piksi-Multi#settings). - + ## Installation and Configuration **WARNING: install __ONLY ONE__ version of SBP library, depending of which Hardware version you are using. This page contains the driver for [Piksi Multi](https://www.swiftnav.com/piksi-multi). If you are using [Piksi V2](http://docs.swiftnav.com/pdfs/piksi_datasheet_v2.3.1.pdf) please check its driver version: [piksi_rtk_gps](https://github.com/ethz-asl/mav_rtk_gps/tree/master/piksi_rtk_gps)** (it is not supported anymore). @@ -22,8 +22,9 @@ source install/install_piksi_multi.sh ### Firmware and SBP Lib Version Please check [here](https://support.swiftnav.com/customer/en/portal/articles/2492810-swift-binary-protocol) which Piksi Multi firmware version based on the current SBP Lib version. -Currently the `install_piksi_multi.sh` will install **SBP Lib 2.3.15** (see [REPO_TAG](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/install/install_piksi_multi.sh#L4)). -This means you are supposed to install **Firmware 1.5.12** from [SwiftNav Firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware) in your Piksi Multi. +Currently the `install_piksi_multi.sh` will install **SBP Lib 2.4.1** (see [REPO_TAG](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/install/install_piksi_multi.sh#L4)). +This means you are supposed to install **Firmware 2.1.14** from [SwiftNav Firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware) in your Piksi Multi. +**WARNING: If upgrading from a firmware below v2.0.0 to a firmware greater than v2.0.0, you must upgrade to v2.0.0 first.** ## Usage **Make sure** you configured your Piksi(s) by following [these instructions](https://github.com/ethz-asl/ethz_piksi_ros/wiki/Installing-and-Configuring-Your-Piksi). @@ -35,7 +36,7 @@ If you have already configured Piksi Multi to act as a base station and sampled roslaunch piksi_multi_rtk_ros piksi_multi_base_station.launch ``` If you have already configured Piksi Multi to act as a base station (and did **NOT** sample its position), then launch the following file. -Once the survey is over, the sampled position will be automatically written in Piksi flash memory. +Once the survey is over, the sampled position will be automatically written in Piksi flash memory. ``` roslaunch piksi_multi_rtk_ros geodetic_survey.launch ``` @@ -66,7 +67,7 @@ The most interesting advertised topics are: this message contains ENU (East-North-Up) coordinate of the receiver in case of SPP fix. Orientation is set to identity quaternion (w=1); - `/piksi/enu_pose_best_fix` ([geometry_msgs/PointStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)) this message contains ENU (East-North-Up) coordinate of the receiver with best available fix at the moment (either RTK or SPP). Orientation is set to identity quaternion (w=1); - + For a complete list of advertised topics please check function [`advertise_topics`](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/src/piksi_multi.py#L264). ### Raw IMU and Magnetometer Measurements @@ -80,7 +81,7 @@ In the former case the following private parameters must be set: - `latitude0_deg`: latitude where the origin of ENU frame is located, in degrees; - `longitude0_deg`: longitude where the origin of ENU frame is located, in degrees; - `altitude0`: altitude where the origin of ENU frame is located, in meters; - + see also configuration file [enu_origin_example.yaml](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/cfg/enu_origin_example.yaml). ## Corrections Over Wifi diff --git a/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-3-15.pdf b/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-3-15.pdf deleted file mode 100644 index 9f3c6e74..00000000 Binary files a/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-3-15.pdf and /dev/null differ diff --git a/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-4-1.pdf b/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-4-1.pdf new file mode 100644 index 00000000..c8b64954 Binary files /dev/null and b/piksi_multi_rtk_ros/docs/lib_sbp_messages_2-4-1.pdf differ diff --git a/piksi_multi_rtk_ros/install/install_piksi_multi.sh b/piksi_multi_rtk_ros/install/install_piksi_multi.sh index beebe0a5..7d2b7aee 100755 --- a/piksi_multi_rtk_ros/install/install_piksi_multi.sh +++ b/piksi_multi_rtk_ros/install/install_piksi_multi.sh @@ -1,7 +1,7 @@ #!/bin/bash GIT_REPO_LIBSBP=https://github.com/swift-nav/libsbp.git -REPO_TAG=v2.3.15 #version you want to checkout before installing +REPO_TAG=v2.4.1 #version you want to checkout before installing # Sort of reg express used to see if there is another verision on SBP library already installed # Adapted from https://stackoverflow.com/questions/6363441/check-if-a-file-exists-with-wildcard-in-shell-script @@ -40,6 +40,7 @@ fi cd ./python echo "Installing SBP dependencies." sudo apt-get install pandoc +sudo apt-get install python-pip sudo pip install tox sudo pip install -r requirements.txt sudo pip install markupsafe diff --git a/piksi_multi_rtk_ros/package.xml b/piksi_multi_rtk_ros/package.xml index e8cc871f..7c5fbf9b 100644 --- a/piksi_multi_rtk_ros/package.xml +++ b/piksi_multi_rtk_ros/package.xml @@ -1,7 +1,7 @@ piksi_multi_rtk_ros - 1.8.1 + 1.9.0 ROS driver for Piksi Multi RTK GPS Receiver. diff --git a/piksi_multi_rtk_ros/src/piksi_multi_rtk_ros/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi_rtk_ros/piksi_multi.py index 3d5d5922..d79c109c 100755 --- a/piksi_multi_rtk_ros/src/piksi_multi_rtk_ros/piksi_multi.py +++ b/piksi_multi_rtk_ros/src/piksi_multi_rtk_ros/piksi_multi.py @@ -13,11 +13,11 @@ import std_srvs.srv # Import message types from sensor_msgs.msg import NavSatFix, NavSatStatus +import piksi_rtk_msgs # TODO(rikba): If we dont have this I get NameError: global name 'piksi_rtk_msgs' is not defined. from piksi_rtk_msgs.msg import (AgeOfCorrections, BaselineEcef, BaselineHeading, BaselineNed, BasePosEcef, BasePosLlh, DeviceMonitor_V2_3_15, DopsMulti, GpsTimeMulti, Heartbeat, ImuRawMulti, - InfoWifiCorrections, Log, MagRaw, Observation, PosEcef, PosLlhMulti, - ReceiverState_V2_3_15, TrackingState_V2_3_15, - UartState_V2_3_15, UtcTimeMulti, VelEcef, VelNed) + InfoWifiCorrections, Log, MagRaw, MeasurementState_V2_4_1, Observation, PosEcef, PosLlhMulti, + ReceiverState_V2_4_1, UartState_V2_3_15, UtcTimeMulti, VelEcef, VelNed) from piksi_rtk_msgs.srv import * from geometry_msgs.msg import (PoseWithCovarianceStamped, PointStamped, PoseWithCovariance, Point, TransformStamped, Transform) @@ -51,7 +51,7 @@ class PiksiMulti: - LIB_SBP_VERSION_MULTI = '2.3.15' # SBP version used for Piksi Multi. + LIB_SBP_VERSION_MULTI = '2.4.1' # SBP version used for Piksi Multi. # Geodetic Constants. kSemimajorAxis = 6378137 @@ -202,7 +202,7 @@ def create_topic_callbacks(self): self.handler.add_callback(self.cb_sbp_obs, msg_type=SBP_MSG_OBS) self.handler.add_callback(self.cb_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP) self.handler.add_callback(self.cb_settings_read_resp, msg_type=SBP_MSG_SETTINGS_READ_RESP) - self.handler.add_callback(self.cb_sbp_tracking_state, msg_type=SBP_MSG_TRACKING_STATE) + self.handler.add_callback(self.cb_sbp_measurement_state, msg_type=SBP_MSG_MEASUREMENT_STATE) self.handler.add_callback(self.cb_sbp_uart_state, msg_type=SBP_MSG_UART_STATE) # Callbacks generated "automatically". @@ -272,7 +272,7 @@ def init_num_corrections_msg(self): return num_wifi_corrections def init_receiver_state_msg(self): - receiver_state_msg = ReceiverState_V2_3_15() + receiver_state_msg = ReceiverState_V2_4_1() receiver_state_msg.num_sat = 0 # Unknown. receiver_state_msg.rtk_mode_fix = False # Unknown. receiver_state_msg.sat = [] # Unknown. @@ -287,7 +287,7 @@ def init_receiver_state_msg(self): receiver_state_msg.cn0_sbas = [] # Unknown. receiver_state_msg.num_glonass_sat = 0 # Unknown. receiver_state_msg.cn0_glonass = [] # Unknown. - receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_UNKNOWN + receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_UNKNOWN return receiver_state_msg @@ -306,10 +306,10 @@ def advertise_topics(self): NavSatFix, queue_size=10) publishers['heartbeat'] = rospy.Publisher(rospy.get_name() + '/heartbeat', Heartbeat, queue_size=10) - publishers['tracking_state'] = rospy.Publisher(rospy.get_name() + '/tracking_state', - TrackingState_V2_3_15, queue_size=10) + publishers['measurement_state'] = rospy.Publisher(rospy.get_name() + '/measurement_state', + piksi_rtk_msgs.msg.MeasurementState_V2_4_1, queue_size=10) publishers['receiver_state'] = rospy.Publisher(rospy.get_name() + '/debug/receiver_state', - ReceiverState_V2_3_15, queue_size=10) + ReceiverState_V2_4_1, queue_size=10) # Do not publish llh message, prefer publishing directly navsatfix_spp or navsatfix_rtk_fix. # publishers['pos_llh'] = rospy.Publisher(rospy.get_name() + '/pos_llh', # PosLlh, queue_size=10) @@ -663,21 +663,21 @@ def cb_sbp_pos_llh(self, msg_raw, **metadata): self.receiver_state_msg.rtk_mode_fix = True if (msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK) else False if msg.flags == PosLlhMulti.FIX_MODE_INVALID: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_INVALID + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_INVALID elif msg.flags == PosLlhMulti.FIX_MODE_SPP: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_SPP + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_SPP elif msg.flags == PosLlhMulti.FIX_MODE_DGNSS: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_DGNSS + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_DGNSS elif msg.flags == PosLlhMulti.FIX_MODE_FLOAT_RTK: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_FLOAT_RTK + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_FLOAT_RTK elif msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_FIXED_RTK + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_FIXED_RTK elif msg.flags == PosLlhMulti.FIX_MODE_DEAD_RECKONING: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.FIX_MODE_DEAD_RECKONING + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.FIX_MODE_DEAD_RECKONING elif msg.flags == PosLlhMulti.FIX_MODE_SBAS: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_SBAS + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_SBAS else: - self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_UNKNOWN + self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_UNKNOWN self.publish_receiver_state_msg() @@ -779,21 +779,14 @@ def cb_sbp_heartbeat(self, msg_raw, **metadata): if self.base_station_mode: self.multicaster.sendSbpPacket(msg_raw) - def cb_sbp_tracking_state(self, msg_raw, **metadata): - msg = MsgTrackingState(msg_raw) + def cb_sbp_measurement_state(self, msg_raw, **metadata): + msg = MsgMeasurementState(msg_raw) - # print "\n\n++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n" - # - # for single_tracking_state in msg.states: - # print single_tracking_state - # print "--------------------\n" - - tracking_state_msg = TrackingState_V2_3_15() - tracking_state_msg.header.stamp = rospy.Time.now() - tracking_state_msg.sat = [] - tracking_state_msg.code = [] - tracking_state_msg.fcn = [] - tracking_state_msg.cn0 = [] + measurement_state_msg = piksi_rtk_msgs.msg.MeasurementState_V2_4_1() + measurement_state_msg.header.stamp = rospy.Time.now() + measurement_state_msg.sat = [] + measurement_state_msg.code = [] + measurement_state_msg.cn0 = [] # Temporary variables for receiver state message. num_gps_sat = 0 @@ -802,52 +795,71 @@ def cb_sbp_tracking_state(self, msg_raw, **metadata): cn0_sbas = [] num_glonass_sat = 0 cn0_glonass = [] + num_bds_sat = 0 + cn0_bds = [] + num_gal_sat = 0 + cn0_gal = [] - for single_tracking_state in msg.states: + for single_measurement_state in msg.states: # Use satellites with valid cn0. - if single_tracking_state.cn0 > 0.0: + if single_measurement_state.cn0 > 0.0: - tracking_state_msg.sat.append(single_tracking_state.sid.sat) - tracking_state_msg.code.append(single_tracking_state.sid.code) - tracking_state_msg.fcn.append(single_tracking_state.fcn) - tracking_state_msg.cn0.append(single_tracking_state.cn0) + measurement_state_msg.sat.append(single_measurement_state.mesid.sat) + measurement_state_msg.code.append(single_measurement_state.mesid.code) + measurement_state_msg.cn0.append(single_measurement_state.cn0) # Receiver state fields. - code = single_tracking_state.sid.code - if code == TrackingState_V2_3_15.CODE_GPS_L1CA or \ - code == TrackingState_V2_3_15.CODE_GPS_L2CM or \ - code == TrackingState_V2_3_15.CODE_GPS_L1P or \ - code == TrackingState_V2_3_15.CODE_GPS_L2P: + code = single_measurement_state.mesid.code + if code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L1CA or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L2CM or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L1P or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L2P: num_gps_sat += 1 - cn0_gps.append(single_tracking_state.cn0) + cn0_gps.append(single_measurement_state.cn0) - if code == TrackingState_V2_3_15.CODE_SBAS_L1CA: + elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_SBAS_L1CA: num_sbas_sat += 1 - cn0_sbas.append(single_tracking_state.cn0) + cn0_sbas.append(single_measurement_state.cn0) - if code == TrackingState_V2_3_15.CODE_GLO_L1CA or \ - code == TrackingState_V2_3_15.CODE_GLO_L1CA: + elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GLO_L1CA or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GLO_L2CA: num_glonass_sat += 1 - cn0_glonass.append(single_tracking_state.cn0) + cn0_glonass.append(single_measurement_state.cn0) + + elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_BDS2_B1 or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_BDS2_B2: + num_bds_sat += 1 + cn0_bds.append(single_measurement_state.cn0) + + elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GAL_E1B or \ + code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GAL_E7I: + num_gal_sat += 1 + cn0_gal.append(single_measurement_state.cn0) + + else: + rospy.logwarn("[cb_sbp_measurement_state]: Unknown satellite code %d.", code) # Publish if there's at least one element in each array. - if len(tracking_state_msg.sat) \ - and len(tracking_state_msg.code) \ - and len(tracking_state_msg.fcn) \ - and len(tracking_state_msg.cn0): - self.publishers['tracking_state'].publish(tracking_state_msg) + if len(measurement_state_msg.sat) \ + and len(measurement_state_msg.code) \ + and len(measurement_state_msg.cn0): + self.publishers['measurement_state'].publish(measurement_state_msg) # Update debug msg and publish. - self.receiver_state_msg.num_sat = num_gps_sat + num_sbas_sat + num_glonass_sat - self.receiver_state_msg.sat = tracking_state_msg.sat - self.receiver_state_msg.cn0 = tracking_state_msg.cn0 + self.receiver_state_msg.num_sat = num_gps_sat + num_sbas_sat + num_glonass_sat + num_bds_sat + num_gal_sat + self.receiver_state_msg.sat = measurement_state_msg.sat + self.receiver_state_msg.cn0 = measurement_state_msg.cn0 self.receiver_state_msg.num_gps_sat = num_gps_sat self.receiver_state_msg.cn0_gps = cn0_gps self.receiver_state_msg.num_sbas_sat = num_sbas_sat self.receiver_state_msg.cn0_sbas = cn0_sbas self.receiver_state_msg.num_glonass_sat = num_glonass_sat self.receiver_state_msg.cn0_glonass = cn0_glonass + self.receiver_state_msg.num_bds_sat = num_bds_sat + self.receiver_state_msg.cn0_bds = cn0_bds + self.receiver_state_msg.num_gal_sat = num_gal_sat + self.receiver_state_msg.cn0_gal = cn0_gal self.publish_receiver_state_msg() diff --git a/piksi_rtk_kml/package.xml b/piksi_rtk_kml/package.xml index 89bd7813..8bf6d78f 100644 --- a/piksi_rtk_kml/package.xml +++ b/piksi_rtk_kml/package.xml @@ -1,7 +1,7 @@ piksi_rtk_kml - 1.8.1 + 1.9.0 ROS node to write KML file from Piksi messages. diff --git a/piksi_rtk_msgs/CMakeLists.txt b/piksi_rtk_msgs/CMakeLists.txt index d461ff7d..b98be069 100644 --- a/piksi_rtk_msgs/CMakeLists.txt +++ b/piksi_rtk_msgs/CMakeLists.txt @@ -35,6 +35,7 @@ add_message_files( InfoWifiCorrections.msg Log.msg MagRaw.msg + MeasurementState_V2_4_1.msg Observation.msg PosEcef.msg PosLlh.msg @@ -42,6 +43,7 @@ add_message_files( ReceiverState.msg ReceiverState_V2_2_15.msg ReceiverState_V2_3_15.msg + ReceiverState_V2_4_1.msg TrackingState.msg TrackingState_V2_2_15.msg TrackingState_V2_3_15.msg diff --git a/piksi_rtk_msgs/msg/MeasurementState_V2_4_1.msg b/piksi_rtk_msgs/msg/MeasurementState_V2_4_1.msg new file mode 100644 index 00000000..2d58b809 --- /dev/null +++ b/piksi_rtk_msgs/msg/MeasurementState_V2_4_1.msg @@ -0,0 +1,23 @@ +# The tracking message returns a variable-length array of tracking channel states. It reports status and +# carrier-to-noise density measurements for all tracked satellites. + +# Message definition based on libsbp v2.4.1 + +Header header + +uint8[] sat # Constellation-specific satellite identifier. +uint8[] code # Signal constellation, band and code. +uint8[] cn0 # Carrier-to-Noise density. Zero implies invalid cn0 [dB Hz / 4]. + + +uint8 CODE_GPS_L1CA = 0 +uint8 CODE_GPS_L2CM = 1 +uint8 CODE_SBAS_L1CA = 2 +uint8 CODE_GLO_L1CA = 3 +uint8 CODE_GLO_L2CA = 4 +uint8 CODE_GPS_L1P = 5 +uint8 CODE_GPS_L2P = 6 +uint8 CODE_BDS2_B1 = 12 +uint8 CODE_BDS2_B2 = 13 +uint8 CODE_GAL_E1B = 14 +uint8 CODE_GAL_E7I = 20 diff --git a/piksi_rtk_msgs/msg/ReceiverState_V2_4_1.msg b/piksi_rtk_msgs/msg/ReceiverState_V2_4_1.msg new file mode 100644 index 00000000..62ec2d26 --- /dev/null +++ b/piksi_rtk_msgs/msg/ReceiverState_V2_4_1.msg @@ -0,0 +1,38 @@ +# Miscellaneous information form different SBP messages + +# Message definition based on libsbp v2.4.1 + + +Header header + +uint8 num_sat # Number of satellites. +bool rtk_mode_fix # 1 = Fixed, 0 = Float. +uint8[] sat # Constellation-specific satellite identifier. +uint8[] cn0 # Carrier-to-Noise density. Zero implies invalid cn0 [dB Hz / 4]. +uint8 system_error # System Error Flag. +uint8 io_error # IO Error Flag. +uint8 swift_nap_error # SwiftNAP Error Flag. +uint8 external_antenna_present # External Antenna Present Flag. +uint8 num_gps_sat # Number of GPS satellites. +uint8[] cn0_gps # Carrier-to-Noise density of GPS satellites. Zero implies invalid cn0 [dB Hz / 4]. +uint8 num_sbas_sat # Number of SBAS satellites. +uint8[] cn0_sbas # Carrier-to-Noise density of SBAS satellites. Zero implies invalid cn0 [dB Hz / 4]. +uint8 num_glonass_sat # Number of GLONASS satellites. +uint8[] cn0_glonass # Carrier-to-Noise density of GLONASS satellites. Zero implies invalid cn0 [dB Hz / 4]. +uint8 num_bds_sat # Number of BEIDOU satellites. +uint8[] cn0_bds # Carrier-to-Noise density of Beidou satellites. Zero implies invalid cn0 [dB Hz / 4]. +uint8 num_gal_sat # Number of GALILEO satellites. +uint8[] cn0_gal # Carrier-to-Noise density of GALILEO satellites. Zero implies invalid cn0 [dB Hz / 4]. +string fix_mode # Invalid, Single Point Position (SPP), Differential GNSS (DGNSS), Float RTK, Fixed RTK. + + +# codes available in message "MeasurementState.msg" + +string STR_FIX_MODE_INVALID = Invalid +string STR_FIX_MODE_SPP = SPP +string STR_FIX_MODE_DGNSS = DGNSS +string STR_FIX_MODE_FLOAT_RTK = FLOAT_RTK +string STR_FIX_MODE_FIXED_RTK = FIXED_RTK +string STR_FIX_MODE_DEAD_RECKONING = DEAD_RECKONING +string STR_FIX_MODE_SBAS = SBAS +string STR_FIX_MODE_UNKNOWN = Unknown diff --git a/piksi_rtk_msgs/package.xml b/piksi_rtk_msgs/package.xml index 64713009..8d237600 100644 --- a/piksi_rtk_msgs/package.xml +++ b/piksi_rtk_msgs/package.xml @@ -1,7 +1,7 @@ piksi_rtk_msgs - 1.8.1 + 1.9.0 Package containing messages for Piksi RTK GPS ROS Driver. diff --git a/piksi_v2_rtk_ros/package.xml b/piksi_v2_rtk_ros/package.xml index 50785006..190a2ca3 100644 --- a/piksi_v2_rtk_ros/package.xml +++ b/piksi_v2_rtk_ros/package.xml @@ -1,7 +1,7 @@ piksi_v2_rtk_ros - 1.8.1 + 1.9.0 ROS driver for Piksi V2 RTK GPS Receiver. diff --git a/rqt_gps_rtk_plugin/include/rqt_gps_rtk_plugin/GpsRtkPlugin.hpp b/rqt_gps_rtk_plugin/include/rqt_gps_rtk_plugin/GpsRtkPlugin.hpp index 648f5b37..ea460d62 100644 --- a/rqt_gps_rtk_plugin/include/rqt_gps_rtk_plugin/GpsRtkPlugin.hpp +++ b/rqt_gps_rtk_plugin/include/rqt_gps_rtk_plugin/GpsRtkPlugin.hpp @@ -19,7 +19,7 @@ #include // messages -#include +#include #include #include #include @@ -75,10 +75,14 @@ Q_OBJECT *pString += "]"; } - template - std::vector scaleSignalStrength(const std::vector &vec_signal_strength) { + template + std::vector scaleSignalStrength( + const std::vector& vec_signal_strength) { auto scaled_signal_strength = vec_signal_strength; - for_each(scaled_signal_strength.begin(), scaled_signal_strength.end(), [](T &signal_strength) {signal_strength /= kSignalStrengthScalingFactor;}); + for_each(scaled_signal_strength.begin(), scaled_signal_strength.end(), + [](T& signal_strength) { + signal_strength /= kSignalStrengthScalingFactor; + }); return scaled_signal_strength; } @@ -93,7 +97,7 @@ Q_OBJECT ros::Subscriber piksiHeartbeatSub_; ros::Subscriber piksiAgeOfCorrectionsSub_; - void piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_3_15& msg); + void piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_4_1& msg); void piksiBaselineNedCb(const piksi_rtk_msgs::BaselineNed& msg); void piksiWifiCorrectionsCb(const piksi_rtk_msgs::InfoWifiCorrections& msg); void piksiNavsatfixRtkFixCb(const sensor_msgs::NavSatFix& msg); diff --git a/rqt_gps_rtk_plugin/package.xml b/rqt_gps_rtk_plugin/package.xml index b659fbb9..27e17acc 100644 --- a/rqt_gps_rtk_plugin/package.xml +++ b/rqt_gps_rtk_plugin/package.xml @@ -1,7 +1,7 @@ rqt_gps_rtk_plugin - 1.8.1 + 1.9.0 The rqt_gps_rtk_plugin package Kai Holtmann diff --git a/rqt_gps_rtk_plugin/resource/gps_rtk_plugin.ui b/rqt_gps_rtk_plugin/resource/gps_rtk_plugin.ui index 330e8f0e..ad56fc0e 100644 --- a/rqt_gps_rtk_plugin/resource/gps_rtk_plugin.ui +++ b/rqt_gps_rtk_plugin/resource/gps_rtk_plugin.ui @@ -132,7 +132,7 @@ Qt::LeftToRight - 0 + 1 @@ -1083,6 +1083,314 @@ + + + + + + + 0 + 0 + + + + + 121 + 21 + + + + BEIDOU satellites: + + + + + + + + 0 + 0 + + + + + 51 + 21 + + + + N/A + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + + + + + + + + + + 0 + 0 + + + + + 111 + 21 + + + + <html><head/><body><p>BEIDOU strength:</p></body></html> + + + + + + + + 0 + 0 + + + + + 81 + 21 + + + + <html><head/><body><p>[dB Hz]</p></body></html> + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + 0 + 21 + + + + N/A + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + + + + + + + + 0 + 0 + + + + + 121 + 21 + + + + GALILEO satellites: + + + + + + + + 51 + 21 + + + + N/A + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + + + + + + + + + + 0 + 0 + + + + + 81 + 21 + + + + <html><head/><body><p>[dB Hz]</p></body></html> + + + + + + + + 0 + 0 + + + + + 111 + 21 + + + + <html><head/><body><p>GALILEO strength:</p></body></html> + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + 0 + 21 + + + + N/A + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + diff --git a/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp b/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp index e2b6f4af..500dc790 100644 --- a/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp +++ b/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp @@ -103,6 +103,10 @@ void GpsRtkPlugin::initLabels() { ui_.label_sbasStrength->setText("N/A"); ui_.label_glonassSatellites->setText("N/A"); ui_.label_glonassStrength->setText("N/A"); + ui_.label_beidouSatellites->setText("N/A"); + ui_.label_beidouStrength->setText("N/A"); + ui_.label_galileoSatellites->setText("N/A"); + ui_.label_galileoStrength->setText("N/A"); ui_.label_ageOfCorrections->setText("N/A"); } @@ -136,6 +140,14 @@ void GpsRtkPlugin::timerCallback(const ros::TimerEvent& e) { QMetaObject::invokeMethod(ui_.label_glonassSatellites, "setText", Q_ARG(QString, na)); // GLONASS signal strength QMetaObject::invokeMethod(ui_.label_glonassStrength, "setText", Q_ARG(QString, na)); + // BEIDOU number of satellites + QMetaObject::invokeMethod(ui_.label_beidouSatellites, "setText", Q_ARG(QString, na)); + // BEIDOU signal strength + QMetaObject::invokeMethod(ui_.label_beidouStrength, "setText", Q_ARG(QString, na)); + // GALILEO number of satellites + QMetaObject::invokeMethod(ui_.label_galileoSatellites, "setText", Q_ARG(QString, na)); + // GALILEO signal strength + QMetaObject::invokeMethod(ui_.label_galileoStrength, "setText", Q_ARG(QString, na)); } if (currentStamp - lastMsgStamps_.baselineNedStamp_ > maxTimeout_) { QMetaObject::invokeMethod(ui_.label_numRtkSatellites_indicator, "setText", Q_ARG(QString, na)); @@ -152,7 +164,7 @@ void GpsRtkPlugin::timerCallback(const ros::TimerEvent& e) { } } -void GpsRtkPlugin::piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_3_15& msg) { +void GpsRtkPlugin::piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_4_1& msg) { // Type of fix const QString fix_mode = QString::fromStdString(msg.fix_mode); QString color_fix_mode(""); @@ -168,6 +180,8 @@ void GpsRtkPlugin::piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_3 color_fix_mode = "QLabel {background-color: rgb(255, 138, 138); color: rgb(191, 0, 191);}"; } else if (msg.fix_mode == msg.STR_FIX_MODE_FIXED_RTK) { color_fix_mode = "QLabel {background-color: lime; color: rgb(255, 166, 2);}"; + } else if (msg.fix_mode == msg.STR_FIX_MODE_DEAD_RECKONING) { + color_fix_mode = "QLabel {background-color: rgb(255, 255, 255); color: rgb(51, 0, 102);}"; } else if (msg.fix_mode == msg.STR_FIX_MODE_SBAS) { color_fix_mode = "QLabel {background-color: rgb(255, 255, 255); color: rgb(43, 255, 223);}"; } @@ -211,6 +225,16 @@ void GpsRtkPlugin::piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_3 // GLONASS signal strength vectorToString(scaleSignalStrength(msg.cn0_glonass), &signal_strength); QMetaObject::invokeMethod(ui_.label_glonassStrength, "setText", Q_ARG(QString, signal_strength)); + // BEIDOU number of satellites + QMetaObject::invokeMethod(ui_.label_beidouSatellites, "setText", Q_ARG(QString, QString::number(msg.num_bds_sat))); + // BEIDOU signal strength + vectorToString(scaleSignalStrength(msg.cn0_bds), &signal_strength); + QMetaObject::invokeMethod(ui_.label_beidouStrength, "setText", Q_ARG(QString, signal_strength)); + // GALILEO number of satellites + QMetaObject::invokeMethod(ui_.label_galileoSatellites, "setText", Q_ARG(QString, QString::number(msg.num_gal_sat))); + // GALILEO signal strength + vectorToString(scaleSignalStrength(msg.cn0_gal), &signal_strength); + QMetaObject::invokeMethod(ui_.label_galileoStrength, "setText", Q_ARG(QString, signal_strength)); //update last msg stamp lastMsgStamps_.receiverStateStamp_ = msg.header.stamp.toSec();