Only released in EOL distros:
Package Summary
The microstrain_3dm_gx5_45 package provides a driver for the LORD/Microstrain 3DM_GXx_45 GPS-aided IMU sensor.
- Maintainer status: developed
- Maintainer: Brian Bingham <briansbingham AT gmail DOT com>
- Author: Brian Bingham <briansbingham AT gmail DOT com>
- License: GPL
- Source: git https://github.com/bsb808/microstrain_3dm_gx5_45.git (branch: master)
Package Summary
The microstrain_3dm_gx5_45 package provides a driver for the LORD/Microstrain 3DM_GXx_45 GPS-aided IMU sensor.
- Maintainer status: developed
- Maintainer: Brian Bingham <briansbingham AT gmail DOT com>
- Author: Brian Bingham <briansbingham AT gmail DOT com>
- License: GPL
- Source: git https://github.com/bsb808/microstrain_3dm_gx5_45.git (branch: master)
Contents
Overview
Interface (driver) software, including ROS node, for Microstrain 3DM-GX5-45.
The interface makes use of the MIP SDK Version 1.1 from Microstrain to communicate with the device. Includes the following applications:
Supported Devices
LORD/Microstrain 3DM-GX5-45: This driver has been mainly tested using the LORD/Microstrain 3DM-GX5-45 sensor. We have done limited testing on the 3DM-GX4-45, and the interface seems to be the same. We have not tested this with the 3DM-GX3-45, but the interface (MIP) should be the same. Please let us know if you find issues.
Limited testing has been done using the 3DM-GX4-25 (AHRS-only) device. See the microstrain_25.launch file for an example of how to configure the driver for use with the -25.
ROS Nodes
microstrain_3dm_gx5_45
ROS node reports the GPS data, AHRS data and Nav Filter estimatePublished Topics
gps/fix (sensor_msgs/NavSatFix)- See [[#navsatmsg]Nav Sat Fix message description] below.
- IMU (AHRS) data is provided
- See Odometry message description below
- See Filter status message description below
Services
reset_kf (std_srvs/Empty)- Resets the Kalman filter on the device
Parameters
port (string, default: /dev/ttyACM0)- Serial port - Linux only
- Baud rate of serial connection
- If true, puts device in idle mode and configures the device.If false, skips configuration. Important: the configuration parameters below are not effective unless this flag is true.
- coming soon
- tbd
- Vehicle dynamics mode 0x01=Portable, 0x02=Automotive, 0x03=Airborne
- Possible declination sources: 0x01=Node, device reports magnetic north, 0x02=Internal World Magnetic Model, 0x03=Manual (see declination parameter)
- Value for the frame_id field in the header of the NavSatFix message published on the gps/fix topic
- Value of the frame_id field in the header of the Imu message publised in the imu/data topic
- Value of the frame_id field in the header of the Odometry message published on the nav/odom topic
- Value of the child_frame_id field in the Odometry message published on the nav/odom topic.
- Sets if ~gps/fix should be advertised/published or not. Note - to maximize performance you may want to only publish the Odometry messages
- Sets if ~imu/data should be advertised/published or not.
- Sets if ~nav/odom should be advertised/published or not.
- Target update (publishing) rate for gps/fix messages. See update [[#rates] Update Rates] below.
- Target update (publishing) rate for imu/data messages. See update [[#rates] Update Rates] below.
- Target update (publishing) rate for nav/odom messages. See update [[#rates] Update Rates] below.
Update Rates
The rates are set as a target value in Hz. The device accepts a decimation value for each output; the packet rate is base_rate/decimation, where decimation is an integer. The program calculates the decimation to get close the the desired rate, based on polling the sensor for its base rate.
For the 3DM-GX4-45 and 3DM-GX5-45 devices tested the base rates were...
- GPS - base rate = 4 Hz
- IMU - base rate = 500 Hz
- Filter - base rate = 500 Hz
Odometry message information
- Currently the pose.position is the longitude (x), latitude (y) and ellipsoid height (z)
- pose.covariance and twist.covariance include diagonal elements for position and attitude
Nav Status message description
- Includes three values - see communication protocol for full documentation.
- filter_state
- 0x00 – Startup
- 0x01 – Initialization (see status flags)
- 0x02 – Running, Solution Valid
- 0x03 – Running, Solution Error (see status flags)
- dynamics mode
- 0x01 – Portable (device default)
- 0x02 – Automotive
- 0x03 – Airborne
- status_flags
- See device documentation
- filter_state
Nav Sat Fix message description
Position covariance is populated with diagonals based on reported horizontal and vertical accuracy. The status.status field is the LLH position data "valid flag"-1. The valid flag mapping from the 3DM protocol is
- 0x0001 – Latitude and Longitude Valid
- 0x0002 – Ellipsoid Height Valid
- 0x0004 – MSL Height Valid
- 0x0008 – Horizontal Accuracy Valid
- 0x0010 – Vertical Accuracy Valid
- E.g., if all valid, then the status.status field should be 30.
Build Instructions
Building from source
DISTRO={hydro|indigo} cd ~/catkin_ws rosdep update rosdep check --from-paths src/microstrain_3dm_gx5_45/ --rosdistro=$DISRO rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO --simulate rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO catkin_make source devel/setup.bash
Dev Notes
- The mip_sdk_user_functions are C functions that need to be called by various parts of the SDK. The main purpose of these functions is to implement the platform-specific serial (RS232) port elements. The prototype serial port open function takes the COM number as an integer input - which is clunky for Linux serial ports. Changed this to take a string defining the port (e.g., /dev/ttyS0), but this necessitated also modifying the mip_sdk_interface.[ch] files, since this is what is called by the application - changed the mip_interface_init function to accept a string argument for specifying the port.
TODO
- Verify order of quaternions
Launch File Examples
There are launch files in the /launch directory to illustrate use of the node.