├── .gitignore ├── src ├── CMakeLists.txt ├── xsens_msgs │ ├── msg │ │ ├── BaroSensorSample.msg │ │ ├── baroSample.msg │ │ ├── XsensQuaternion.msg │ │ ├── velocityEstimate.msg │ │ ├── orientationEstimate.msg │ │ ├── positionEstimate.msg │ │ ├── GnssSensorSample.msg │ │ ├── gnssSample.msg │ │ ├── sensorSample.msg │ │ ├── ImuSensorSample.msg │ │ └── Internal.msg │ ├── package.xml │ └── CMakeLists.txt └── xsens_driver │ ├── launch │ └── xsens.launch │ ├── package.xml │ ├── config │ └── xsens.yaml │ ├── CMakeLists.txt │ └── src │ ├── mtdef.py │ ├── mtnode.py │ └── mtdevice.py ├── CHANGELOG.md ├── mainpage.dox └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | devel/ 3 | *.pyc 4 | *~ 5 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/xsens_msgs/msg/BaroSensorSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data from a baro 2 | 3 | float64 height 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/baroSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data from a baro 2 | 3 | Header header 4 | 5 | float64 height 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/XsensQuaternion.msg: -------------------------------------------------------------------------------- 1 | # This represents an orientation in free space in quaternion form. 2 | 3 | float64 w 4 | float64 x 5 | float64 y 6 | float64 z 7 | 8 | -------------------------------------------------------------------------------- /src/xsens_driver/launch/xsens.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/velocityEstimate.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold filter data 2 | # Supported for MTi Devices with FW 1.4 and above. 3 | 4 | Header header 5 | 6 | float64 velE 7 | float64 velN 8 | float64 velU 9 | 10 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/orientationEstimate.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold filter data 2 | # Supported for MTi Devices with FW 1.4 and above. 3 | 4 | Header header 5 | 6 | float64 roll 7 | float64 pitch 8 | float64 yaw 9 | 10 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/positionEstimate.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold filter data 2 | # Supported for MTi Devices with FW 1.4 and above. 3 | 4 | Header header 5 | 6 | float64 latitude 7 | float64 longitude 8 | float64 hEll 9 | 10 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/GnssSensorSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data a GNSS unit 2 | # Supported for MTi Devices with FW 1.4 and above. 3 | 4 | std_msgs/Float64 itow 5 | std_msgs/Float64 fix 6 | 7 | float64 latitude 8 | float64 longitude 9 | float64 hEll 10 | float64 hMsl 11 | 12 | # ENU velocity 13 | geometry_msgs/Vector3 vel 14 | 15 | float64 hAcc 16 | float64 vAcc 17 | float64 sAcc 18 | 19 | float64 pDop 20 | float64 hDop 21 | float64 vDop 22 | 23 | float64 numSat 24 | float64 heading 25 | float64 headingAcc 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/gnssSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data a GNSS unit 2 | # Supported for MTi Devices with FW 1.4 and above. 3 | 4 | Header header 5 | 6 | float64 itow 7 | float64 fix 8 | 9 | float64 latitude 10 | float64 longitude 11 | float64 hEll 12 | float64 hMsl 13 | 14 | # ENU velocity 15 | geometry_msgs/Vector3 vel 16 | 17 | float64 hAcc 18 | float64 vAcc 19 | float64 sAcc 20 | 21 | float64 pDop 22 | float64 hDop 23 | float64 vDop 24 | 25 | float64 numSat 26 | float64 heading 27 | float64 headingAcc 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/sensorSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data from an IMU (Inertial Measurement Unit) 2 | # 3 | # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec 4 | # 5 | # If the covariance of the measurement is known, it should be filled in (if all you know is the 6 | # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) 7 | # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the 8 | # data a covariance will have to be assumed or gotten from some other source 9 | # 10 | # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 11 | # estimate), please set element 0 of the associated covariance matrix to -1 12 | # If you are interpreting this message, please check for a value of -1 in the first element of each 13 | # covariance matrix, and disregard the associated estimate. 14 | 15 | Header header 16 | 17 | Internal internal 18 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/ImuSensorSample.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data from an IMU (Inertial Measurement Unit) 2 | # 3 | # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec 4 | # 5 | # If the covariance of the measurement is known, it should be filled in (if all you know is the 6 | # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) 7 | # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the 8 | # data a covariance will have to be assumed or gotten from some other source 9 | # 10 | # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 11 | # estimate), please set element 0 of the associated covariance matrix to -1 12 | # If you are interpreting this message, please check for a value of -1 in the first element of each 13 | # covariance matrix, and disregard the associated estimate. 14 | 15 | XsensQuaternion dq 16 | 17 | geometry_msgs/Vector3 dv 18 | 19 | geometry_msgs/Vector3 bGyr 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/xsens_msgs/msg/Internal.msg: -------------------------------------------------------------------------------- 1 | # This is a message to hold data from an IMU (Inertial Measurement Unit) 2 | # 3 | # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec 4 | # 5 | # If the covariance of the measurement is known, it should be filled in (if all you know is the 6 | # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) 7 | # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the 8 | # data a covariance will have to be assumed or gotten from some other source 9 | # 10 | # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 11 | # estimate), please set element 0 of the associated covariance matrix to -1 12 | # If you are interpreting this message, please check for a value of -1 in the first element of each 13 | # covariance matrix, and disregard the associated estimate. 14 | 15 | ImuSensorSample imu 16 | 17 | geometry_msgs/Vector3 mag 18 | 19 | BaroSensorSample baro 20 | 21 | GnssSensorSample gnss 22 | 23 | -------------------------------------------------------------------------------- /src/xsens_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | xsens_driver 3 | 4.0.2 4 | 5 | ROS Driver for publishing mti sensor sample messages (MTi 1-series incl. MTi-7, MTi 10-series and MTi 100-series incl. MTi-G-710). 6 | 7 | Xsens 8 | BSD 9 | 10 | catkin 11 | 12 | rospy 13 | std_msgs 14 | tf 15 | sensor_msgs 16 | geometry_msgs 17 | gps_common 18 | diagnostic_msgs 19 | xsens_msgs 20 | 21 | rospy 22 | std_msgs 23 | tf 24 | sensor_msgs 25 | geometry_msgs 26 | gps_common 27 | diagnostic_msgs 28 | xsens_msgs 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/xsens_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | xsens_msgs 3 | 2.0.2 4 | 5 | This package defines custom Xsens sensor sample format message to be 6 | used only with the MTdata2 format. 7 | 8 | Tully Foote 9 | BSD 10 | 11 | http://ros.org/wiki/sensor_msgs 12 | 13 | catkin 14 | 15 | geometry_msgs 16 | message_generation 17 | std_msgs 18 | sensor_msgs 19 | rospy 20 | roscpp 21 | rosjava_bootstrap 22 | rosjava_messages 23 | 24 | geometry_msgs 25 | message_runtime 26 | std_msgs 27 | rospy 28 | roscpp 29 | sensor_msgs 30 | message_generation 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/xsens_driver/config/xsens.yaml: -------------------------------------------------------------------------------- 1 | # Serial device 2 | device: /dev/ttyUSB0 3 | 4 | # Baudrate. If configure_on_startup is true, will set device to use baudrate 5 | baudrate: 115200 6 | 7 | # Frame id which is assigned to all sensor data messages 8 | frame_id: xsens 9 | 10 | # Will configure the device on startup 11 | configure_on_startup: true 12 | 13 | # Output data rate of device. Valid values: 1, 2, 4, 5, 10, 20, 40, 50, 80, 100, 200, 400, 800 14 | # Refer to MT Low Level Communication Protocol Documentation for supported ODRs for each MTi series 15 | # https://base.xsens.com/hc/en-us/articles/207003759-Online-links-to-manuals-from-the-MT-Software-Suite 16 | odr: 10 17 | 18 | # Filter profiles (xkf-scenario) 19 | # MTi-2 [VRU_general:54] 20 | # MTi-3 [General:50, High_mag_dep:51, Dynamic:52, North_referenced:53, VRU_general:54] 21 | # MTi-7 [General:11, GeneralNoBaro:12, GeneralMag:13] 22 | # MTi-20/200 [VRU_general: 43] 23 | # MTi-30/300 [General:39, High_map_dep:40, Dynamic:41, Low_mag_dep:42, VRU_general:43] 24 | # MTi-G-710 [General:1, GeneralNoBaro:2, GeneralMag:3, Automotive:4 HighPerformanceEDR:5) 25 | xkf_scenario: 11 26 | 27 | # Output data mode 28 | # 1: sensor data 29 | # 2. sensor data w/ rate quantities 30 | # 3: filter estimates 31 | output_mode: 3 32 | 33 | # Use rostime for IMU stamp instead of sensor reported time 34 | use_rostime: true 35 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog for package xsens_mti_ros_node 2 | 3 | ## 4.0.2 (2018-07-31) 4 | * Updated gps package required in prerequisites section of README 5 | 6 | ## 4.0.1 (2018-07-20) 7 | * Fixed typo in xsens.yaml file w.r.t MTi-3 filter profiles 8 | * Changed example message in ReadMe 9 | 10 | ## 4.0.0 (2018-07-04) 11 | * Merge pull request #33: Change custom_msgs to xsens_msgs 12 | * Merge pull request #32: Configure MTi device on startup 13 | * Added support to MTi-7 device 14 | 15 | ## 3.0.2 (2018-04-24) 16 | * Added warning (rospy.logwarn) for timeouts 17 | 18 | ## 3.0.1 (2017-08-09) 19 | * Support added to the node for operation with Kinetic environment 20 | 21 | ## 3.0.0 (2016-05-27) 22 | * Added functionality to detect devices based on Product masks (incl. 1-series and FMT1000 devices) 23 | * Increased the 'additionalTimeOutOffset' from 6ms to 10ms as default 24 | 25 | ## 2.0.2 (2015-11-23) 26 | * Fix Readme 27 | 28 | ## 2.0.1 (2015-03-19) 29 | * Fix documentation, switch to Markdown 30 | 31 | ## 2.0.0 (2015-03-19) 32 | * Updated GNSS data identifiers 33 | * Fixed a timeout issue 34 | * Added new message files 35 | * Add baudrate checks 36 | * Add filter options 37 | * Removed support for legacy devices 38 | 39 | ## 1.0.0 (2014-09-02) 40 | * Improved on MK4 functionality to publish /xsens/sensorSample messages 41 | 42 | Built on ethzasl_xsens_driver developed by previous contributors - Enrique Fernandez, Francis Colas, Paul Mathieu, Sam Pfeiffer, 43 | Benjamin Hitov, Francis Colas, Nikolaus Demmel, Stéphane Magnenat, fcolas. 44 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | The \b xsens_driver package provides \b mtnode.py, a generic ROS node publishing the data streamed by an XSens imu (MT, MTi, MTi-G...). 6 | 7 | \section topics Topics 8 | 9 | The ROS node is a wrapper around the \b mtdevice::MTDevice class. It can publish the 10 | following topics, depending on the configuration of the device: 11 | - \c /imu/data (sensor_msgs::Imu): 13 | orientation, angular velocity, linear acceleration, 14 | - \c /fix (sensor_msgs::NavSatFix): 16 | longitude, latitude, altitude (from GPS only for MTi-G), 17 | - \c /fix_extended (gps_common::GPSFix): 19 | more complete GPS information than \c /fix, 20 | - \c /velocity (geometry_msgs::TwistStamped): linear and angular velocity, 22 | - \c /magnetic (geometry_msgs::Vector3Stamped): direction of magnetic field, 24 | - \c /temperature (std_msgs::Float32): temperature. 26 | 27 | It also publishes diagnostics 28 | information. 29 | 30 | If the IMU is set to raw mode, the values in of the \c /imu/data, \c /velocity and \c 31 | /magnetic topics are the 16 bits output of the AD converters. 32 | 33 | The covariance information in the sensor_msgs::Imu 35 | message are filled with default values from the MTx/MTi/MTi-G documentation but 36 | may not be exact; it also does not correspond to the covariance of the internal XKF. 37 | 38 | \section params Parameters 39 | 40 | The nodes can take the following parameters: 41 | - \a ~device (\c auto): the path of the device file to connect to the imu; \c 42 | auto will look through all serial devices to find the first one. 43 | - \a ~baudrate (0): the baudrate of the IMU (unused for \c auto device); 0 will 44 | try to auto-detect baudrate. 45 | - \a ~frame_id (\c /base_imu): the frame id of the IMU. 46 | 47 | \section dialout Permissions 48 | 49 | It might be necessary to add the user to the \c dialout group so that the node can communicate with the device. 50 | 51 | */ 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **Notice: This ROS node implementation is deprecated. 2 | Please use the new offical ROS node available as part of the Xsens MT Software Suite (version 2019.0 and later). For more information refer to http://wiki.ros.org/xsens_mti_driver** 3 | 4 | # Prerequisites 5 | 6 | * Install the MTi USB Serial Driver 7 | ```sh 8 | $ git clone https://github.com/xsens/xsens_mt.git 9 | $ cd ~/xsens_mt 10 | $ make 11 | $ sudo modprobe usbserial 12 | $ sudo insmod ./xsens_mt.ko 13 | ``` 14 | 15 | * Install gps_common or gps_umd as available based on the ROS distributable 16 | ```sh 17 | $ sudo apt-get install ros-kinetic-gps-umd 18 | 19 | ``` 20 | or 21 | ```sh 22 | $ sudo apt-get install ros-kinetic-gps-common 23 | ``` 24 | 25 | # Running the Xsens MTi ROS Node 26 | 1. Copy the contents of the src folder into your catkin workspace 'src' folder. 27 | Make sure the permissions are set to _o+rw_ on your files and directories. 28 | For details on creating a catkin workspace environment refer to [Creating a catkin ws](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment#Create_a_ROS_Workspace) 29 | 30 | 2. in your catkin_ws ($CATKIN) folder, execute 31 | ```sh 32 | $ catkin_make 33 | ``` 34 | 35 | 3. Source the environment for each terminal you work in. If necessary, add the 36 | line to your .bashrc 37 | ```sh 38 | . $CATKIN/devel/setup.bash 39 | ``` 40 | 41 | 4. Edit the config file to match your specific use case: 42 | ```sh 43 | rosed xsens_driver xsens.yaml 44 | ``` 45 | 46 | 5. To run the node 47 | ```sh 48 | $ roslaunch xsens_driver xsens.launch 49 | ``` 50 | 51 | 6. Open a new terminal (do not forget step 3) 52 | ```sh 53 | $ . $CATKIN/devel/setup.bash 54 | $ rostopic echo /mti/sensor/sample 55 | ``` 56 | or 57 | ```sh 58 | $ . $CATKIN/devel/setup.bash 59 | $ rostopic echo /mti/filter/orientation 60 | ``` 61 | Tested with ROS Kinetic distribution, initially developed for ROS Indigo distribution. 62 | 63 | # Troubleshooting 64 | 65 | * The Mti1 (Motion Tracker Development Board) is not recognized. 66 | Support for the Development Board is present in recent kernels. (Since June 12, 2015).If your kernel does not support the Board, you can add this manually 67 | 68 | $ sudo /sbin/modprobe ftdi_sio 69 | $ echo 2639 0300 | sudo tee /sys/bus/usb-serial/drivers/ftdi_sio/new_id 70 | 71 | 72 | * The device is recognized, but I cannot ever access the device 73 | Make sure you are in the correct group (often dialout or uucp) in order to access the device. You can test this with 74 | 75 | $ ls -l /dev/ttyUSB0 76 | crw-rw---- 1 root dialout 188, 0 May 6 16:21 /dev/ttyUSB0 77 | $ groups 78 | dialout audio video usb users plugdev 79 | 80 | If you aren't in the correct group, you can fix this in two ways. 81 | 82 | 1. Add yourself to the correct group 83 | You can add yourself to it by using your distributions user management 84 | tool, or call 85 | 86 | $ sudo usermod -G dialout -a $USER 87 | 88 | Be sure to replace dialout with the actual group name if it is 89 | different. After adding yourself to the group, either relogin to your 90 | user, or call 91 | 92 | $ newgrp dialout 93 | 94 | to add the current terminal session to the group. 95 | 96 | 2. Use udev rules 97 | Alternatively, put the following rule into /etc/udev/rules.d/99-custom.rules 98 | 99 | SUBSYSTEM=="tty", ATTRS{idVendor}=="2639", ACTION=="add", GROUP="$GROUP", MODE="0660" 100 | 101 | Change $GROUP into your desired group (e.g. adm, plugdev, or usb). 102 | 103 | 104 | * The device is inaccessible for a while after plugging it in 105 | When having problems with the device being busy the first 20 seconds after 106 | plugin, purge the modemmanager application. 107 | -------------------------------------------------------------------------------- /src/xsens_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xsens_driver) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy std_msgs tf sensor_msgs geometry_msgs gps_common diagnostic_msgs xsens_msgs 9 | ) 10 | 11 | ## System dependencies are found with CMake's conventions 12 | # find_package(Boost REQUIRED COMPONENTS system) 13 | 14 | 15 | ## Uncomment this if the package has a setup.py. This macro ensures 16 | ## modules and global scripts declared therein get installed 17 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 18 | # catkin_python_setup() 19 | 20 | ################################################ 21 | ## Declare ROS messages, services and actions ## 22 | ################################################ 23 | 24 | ## To declare and build messages, services or actions from within this 25 | ## package, follow these steps: 26 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 27 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 28 | ## * In the file package.xml: 29 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 30 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 31 | ## pulled in transitively but can be declared for certainty nonetheless: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a run_depend tag for "message_runtime" 34 | ## * In this file (CMakeLists.txt): 35 | ## * add "message_generation" and every package in MSG_DEP_SET to 36 | ## find_package(catkin REQUIRED COMPONENTS ...) 37 | ## * add "message_runtime" and every package in MSG_DEP_SET to 38 | ## catkin_package(CATKIN_DEPENDS ...) 39 | ## * uncomment the add_*_files sections below as needed 40 | ## and list every .msg/.srv/.action file to be processed 41 | ## * uncomment the generate_messages entry below 42 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 43 | 44 | ## Generate messages in the 'msg' folder 45 | # add_message_files( 46 | # FILES 47 | # Message1.msg 48 | # Message2.msg 49 | # ) 50 | 51 | ## Generate services in the 'srv' folder 52 | # add_service_files( 53 | # FILES 54 | # Service1.srv 55 | # Service2.srv 56 | # ) 57 | 58 | ## Generate actions in the 'action' folder 59 | # add_action_files( 60 | # FILES 61 | # Action1.action 62 | # Action2.action 63 | # ) 64 | 65 | ## Generate added messages and services with any dependencies listed here 66 | # generate_messages( 67 | # DEPENDENCIES 68 | # std_msgs # Or other packages containing msgs 69 | # ) 70 | 71 | ################################### 72 | ## catkin specific configuration ## 73 | ################################### 74 | ## The catkin_package macro generates cmake config files for your package 75 | ## Declare things to be passed to dependent projects 76 | ## INCLUDE_DIRS: uncomment this if you package contains header files 77 | ## LIBRARIES: libraries you create in this project that dependent projects also need 78 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 79 | ## DEPENDS: system dependencies of this project that dependent projects also need 80 | catkin_package( 81 | # INCLUDE_DIRS include 82 | # LIBRARIES test_py 83 | CATKIN_DEPENDS rospy std_msgs tf sensor_msgs geometry_msgs gps_common diagnostic_msgs xsens_msgs 84 | # DEPENDS system_lib 85 | ) 86 | 87 | ########### 88 | ## Build ## 89 | ########### 90 | 91 | ## Specify additional locations of header files 92 | ## Your package locations should be listed before other locations 93 | # include_directories(include) 94 | include_directories( 95 | ${catkin_INCLUDE_DIRS} 96 | ) 97 | 98 | ## Declare a cpp library 99 | # add_library(test_py 100 | # src/${PROJECT_NAME}/test_py.cpp 101 | # ) 102 | 103 | ## Declare a cpp executable 104 | # add_executable(test_py_node src/test_py_node.cpp) 105 | 106 | ## Add cmake target dependencies of the executable/library 107 | ## as an example, message headers may need to be generated before nodes 108 | # add_dependencies(test_py_node test_py_generate_messages_cpp) 109 | 110 | ## Specify libraries to link a library or executable target against 111 | # target_link_libraries(test_py_node 112 | # ${catkin_LIBRARIES} 113 | # ) 114 | 115 | ############# 116 | ## Install ## 117 | ############# 118 | 119 | # all install targets should use catkin DESTINATION variables 120 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 121 | 122 | ## Mark executable scripts (Python etc.) for installation 123 | ## in contrast to setup.py, you can choose the destination 124 | # install(PROGRAMS 125 | # scripts/my_python_script 126 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 127 | # ) 128 | install(PROGRAMS 129 | src/mtnode.py 130 | src/mtdef.py 131 | src/mtdevice.py 132 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 133 | ) 134 | 135 | ## Mark executables and/or libraries for installation 136 | # install(TARGETS test_py test_py_node 137 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 139 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 140 | # ) 141 | 142 | ## Mark cpp header files for installation 143 | # install(DIRECTORY include/${PROJECT_NAME}/ 144 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 145 | # FILES_MATCHING PATTERN "*.h" 146 | # PATTERN ".svn" EXCLUDE 147 | # ) 148 | 149 | ## Mark other files for installation (e.g. launch and bag files, etc.) 150 | #install( 151 | # DIRECTORY launch 152 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 153 | #) 154 | 155 | ############# 156 | ## Testing ## 157 | ############# 158 | 159 | ## Add gtest based cpp test target and link libraries 160 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_test_py.cpp) 161 | # if(TARGET ${PROJECT_NAME}-test) 162 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 163 | # endif() 164 | 165 | ## Add folders to be run by python nosetests 166 | # catkin_add_nosetests(test) 167 | -------------------------------------------------------------------------------- /src/xsens_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xsens_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy roscpp std_msgs message_generation sensor_msgs geometry_msgs 9 | ) 10 | 11 | ## System dependencies are found with CMake's conventions 12 | # find_package(Boost REQUIRED COMPONENTS system) 13 | 14 | 15 | ## Uncomment this if the package has a setup.py. This macro ensures 16 | ## modules and global scripts declared therein get installed 17 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 18 | # catkin_python_setup() 19 | 20 | ################################################ 21 | ## Declare ROS messages, services and actions ## 22 | ################################################ 23 | 24 | ## To declare and build messages, services or actions from within this 25 | ## package, follow these steps: 26 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 27 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 28 | ## * In the file package.xml: 29 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 30 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 31 | ## pulled in transitively but can be declared for certainty nonetheless: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a run_depend tag for "message_runtime" 34 | ## * In this file (CMakeLists.txt): 35 | ## * add "message_generation" and every package in MSG_DEP_SET to 36 | ## find_package(catkin REQUIRED COMPONENTS ...) 37 | ## * add "message_runtime" and every package in MSG_DEP_SET to 38 | ## catkin_package(CATKIN_DEPENDS ...) 39 | ## * uncomment the add_*_files sections below as needed 40 | ## and list every .msg/.srv/.action file to be processed 41 | ## * uncomment the generate_messages entry below 42 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 43 | 44 | ## Generate messages in the 'msg' folder 45 | add_message_files( 46 | FILES 47 | XsensQuaternion.msg 48 | ImuSensorSample.msg 49 | BaroSensorSample.msg 50 | GnssSensorSample.msg 51 | Internal.msg 52 | sensorSample.msg 53 | baroSample.msg 54 | gnssSample.msg 55 | orientationEstimate.msg 56 | velocityEstimate.msg 57 | positionEstimate.msg 58 | ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs 78 | sensor_msgs 79 | ) 80 | 81 | ################################### 82 | ## catkin specific configuration ## 83 | ################################### 84 | ## The catkin_package macro generates cmake config files for your package 85 | ## Declare things to be passed to dependent projects 86 | ## INCLUDE_DIRS: uncomment this if you package contains header files 87 | ## LIBRARIES: libraries you create in this project that dependent projects also need 88 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 89 | ## DEPENDS: system dependencies of this project that dependent projects also need 90 | catkin_package( 91 | # INCLUDE_DIRS include 92 | # LIBRARIES test_py 93 | CATKIN_DEPENDS rospy roscpp std_msgs sensor_msgs message_generation message_runtime 94 | # DEPENDS system_lib 95 | ) 96 | 97 | ########### 98 | ## Build ## 99 | ########### 100 | 101 | ## Specify additional locations of header files 102 | ## Your package locations should be listed before other locations 103 | # include_directories(include) 104 | include_directories( 105 | ${catkin_INCLUDE_DIRS} 106 | ) 107 | 108 | ## Declare a cpp library 109 | # add_library(test_py 110 | # src/${PROJECT_NAME}/test_py.cpp 111 | # ) 112 | 113 | ## Declare a cpp executable 114 | # add_executable(test_py_node src/test_py_node.cpp) 115 | 116 | ## Add cmake target dependencies of the executable/library 117 | ## as an example, message headers may need to be generated before nodes 118 | # add_dependencies(test_py_node test_py_generate_messages_cpp) 119 | 120 | ## Specify libraries to link a library or executable target against 121 | # target_link_libraries(test_py_node 122 | # ${catkin_LIBRARIES} 123 | # ) 124 | 125 | ############# 126 | ## Install ## 127 | ############# 128 | 129 | # all install targets should use catkin DESTINATION variables 130 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 131 | 132 | ## Mark executable scripts (Python etc.) for installation 133 | ## in contrast to setup.py, you can choose the destination 134 | # install(PROGRAMS 135 | # scripts/my_python_script 136 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 137 | # ) 138 | 139 | ## Mark executables and/or libraries for installation 140 | # install(TARGETS test_py test_py_node 141 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 142 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 143 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | # ) 145 | 146 | ## Mark cpp header files for installation 147 | # install(DIRECTORY include/${PROJECT_NAME}/ 148 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 149 | # FILES_MATCHING PATTERN "*.h" 150 | # PATTERN ".svn" EXCLUDE 151 | # ) 152 | 153 | ## Mark other files for installation (e.g. launch and bag files, etc.) 154 | install( 155 | DIRECTORY launch 156 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 157 | ) 158 | 159 | ############# 160 | ## Testing ## 161 | ############# 162 | 163 | ## Add gtest based cpp test target and link libraries 164 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_test_py.cpp) 165 | # if(TARGET ${PROJECT_NAME}-test) 166 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 167 | # endif() 168 | 169 | ## Add folders to be run by python nosetests 170 | # catkin_add_nosetests(test) 171 | -------------------------------------------------------------------------------- /src/xsens_driver/src/mtdef.py: -------------------------------------------------------------------------------- 1 | """Constant and messages definition for MT communication.""" 2 | 3 | 4 | class MID: 5 | """Values for the message id (MID)""" 6 | ## Error message, 1 data byte 7 | Error = 0x42 8 | ErrorCodes = { 9 | 0x03: "Invalid period", 10 | 0x04: "Invalid message", 11 | 0x1E: "Timer overflow", 12 | 0x20: "Invalid baudrate", 13 | 0x21: "Invalid parameter" 14 | } 15 | 16 | # State MID 17 | ## Wake up procedure 18 | WakeUp = 0x3E 19 | ## Switch to config state 20 | GoToConfig = 0x30 21 | ## Switch to measurement state 22 | GoToMeasurement = 0x10 23 | ## Reset device 24 | Reset = 0x40 25 | 26 | # Informational messages 27 | ## Request device id 28 | ReqDID = 0x00 29 | ## DeviceID, 4 bytes: HH HL LH LL 30 | DeviceID = 0x01 31 | ## Compatibility for XBus Master users 32 | InitMT = 0x02 33 | InitMTResults = 0x03 34 | ## Request product code in plain text 35 | ReqProductCode = 0x1C 36 | ## Product code (max 20 bytes data) 37 | ProductCode = 0x1D 38 | ## Request firmware revision 39 | ReqFWRev = 0x12 40 | ## Firmware revision, 3 bytes: major minor rev 41 | FirmwareRev = 0x13 42 | ## Request data length according to current configuration 43 | ReqDataLength = 0x0A 44 | ## Data Length, 2 bytes 45 | DataLength = 0x0B 46 | ## Request GPS status (MTi-G only) 47 | ReqGPSStatus = 0xA6 48 | ## GPS status (MTi-G only) 49 | GPSStatus = 0xA7 50 | 51 | # Device specific messages 52 | ## Baudrate, 1 byte 53 | SetBaudrate = 0x18 54 | ## Error mode, 2 bytes, 0000, 0001, 0002, 0003 (default 0001) 55 | SetErrorMode = 0xDA 56 | ## Location ID, 2 bytes, arbitrary, default is 0 57 | SetLocationID = 0x84 58 | ## Restore factory defaults 59 | RestoreFactoryDef = 0x0E 60 | ## Transmit delay (RS485), 2 bytes, number of clock ticks (1/29.4912 MHz) 61 | SetTransmitDelay = 0xDC 62 | 63 | # Synchronization messages 64 | ## Synchronization settings (MTi-10/100 series only), N*12 bytes 65 | SetSyncSettings = 0x2C 66 | ## SyncIn setting (MTi only), (1+) 2 or 4 bytes depending on request 67 | SetSyncInSettings = 0xD6 68 | ## SyncOut setting (MTi/MTi-G only), (1+) 2 or 4 bytes depending on request 69 | SetSyncOutSettings = 0xD8 70 | 71 | # Configuration messages 72 | ## Request configuration 73 | ReqConfiguration = 0x0C 74 | ## Configuration, 118 bytes 75 | Configuration = 0x0D 76 | ## Output configuration (MTi-10/100 series only), N*4 bytes 77 | SetOutputConfiguration = 0xC0 78 | ## Sampling period (MTi/MTi-G only), 2 bytes 79 | SetPeriod = 0x04 80 | ## Skip factor (MTi/MTi-G only), 2 bytes 81 | SetOutputSkipFactor = 0xD4 82 | ## Object alignment matrix, 9*4 bytes 83 | SetObjectAlignment = 0xE0 84 | ## Output mode (MTi/MTi-G only), 2 bytes 85 | SetOutputMode = 0xD0 86 | ## Output settings (MTi/MTi-G only), 4 bytes 87 | SetOutputSettings = 0xD2 88 | 89 | # Data messages 90 | ## Request MTData message (for 65535 skip factor) 91 | ReqData = 0x34 92 | ## Legacy data packet 93 | MTData = 0x32 94 | ## Newer data packet (MTi-10/100 series only) 95 | MTData2 = 0x36 96 | 97 | # XKF Filter messages 98 | ## Heading (MTi only), 4 bytes 99 | SetHeading = 0x82 100 | ## Reset orientation, 2 bytes 101 | ResetOrientation = 0xA4 102 | ## Request UTC time from sensor (MTI-G and MTi-10/100 series) 103 | ReqUTCTime = 0x60 104 | ## UTC Time (MTI-G and MTi-10/100 series), 12 bytes 105 | UTCTime = 0x61 106 | ## Request the available XKF scenarios on the device 107 | ReqAvailableScenarios = 0x62 108 | ## Available Scenarios 109 | AvailableScenarios = 0x63 110 | ## Current XKF scenario, 2 bytes 111 | SetCurrentScenario = 0x64 112 | ## Magnitude of the gravity used for the sensor fusion mechanism, 4 bytes 113 | SetGravityMagnitude = 0x66 114 | ## Lever arm of the GPSin sensor coordinates (MTi-G and MTi-700 only), 3*4 bytes 115 | SetLeverArmGPS = 0x68 116 | ## Magnetic declination (MTi-G only), 4 bytes 117 | SetMagneticDeclination = 0x6A 118 | ## Latitude, Longitude and Altitude for local declination and gravity 119 | # (MTi-10/100 series only), 24 bytes 120 | SetLatLonAlt = 0x6E 121 | ## Processing flags (not on firmware 2.2 or lower for MTi/MTi-g), 1 byte 122 | SetProcessingFlags = 0x20 123 | ## Initiate No Rotation procedure (not on MTi-G), 2 bytes 124 | SetNoRotation = 0x22 125 | 126 | ## Some timeout related stuff 127 | additionalTimeOutOffset = 0.010 # 6ms 128 | 129 | 130 | def getName(cls, value): 131 | '''Return the name of the first found member of class cls with given 132 | value.''' 133 | for k, v in cls.__dict__.iteritems(): 134 | if v==value: 135 | return k 136 | return '' 137 | 138 | 139 | def getMIDName(mid): 140 | '''Return the name of a message given the message id.''' 141 | name = getName(MID, mid) 142 | if name: 143 | return name 144 | if mid&1: 145 | name = getName(MID, mid-1) 146 | if name: 147 | return name+'Ack' 148 | return 'unknown MID' 149 | 150 | 151 | class Baudrates(object): 152 | """Baudrate information and conversion.""" 153 | ## Baudrate mapping between ID and value 154 | Baudrates = [ 155 | (0x80, 921600), 156 | (0x0A, 921600), 157 | (0x00, 460800), 158 | (0x01, 230400), 159 | (0x02, 115200), 160 | (0x03, 76800), 161 | (0x04, 57600), 162 | (0x05, 38400), 163 | (0x06, 28800), 164 | (0x07, 19200), 165 | (0x08, 14400), 166 | (0x09, 9600), 167 | (0x0B, 4800), 168 | (0x80, 921600)] 169 | @classmethod 170 | def get_BRID(cls, baudrate): 171 | """Get baudrate id for a given baudrate.""" 172 | for brid, br in cls.Baudrates: 173 | if baudrate==br: 174 | return brid 175 | raise MTException("unsupported baudrate.") 176 | @classmethod 177 | def get_BR(cls, baudrate_id): 178 | """Get baudrate for a given baudrate id.""" 179 | for brid, br in cls.Baudrates: 180 | if baudrate_id==brid: 181 | return br 182 | raise MTException("unknown baudrate id.") 183 | 184 | 185 | class XDIGroup: 186 | """Values for the XDI groups.""" 187 | Temperature = 0x0800 188 | Timestamp = 0x1000 189 | OrientationData = 0x2000 190 | Pressure = 0x3000 191 | Acceleration = 0x4000 192 | Position = 0x5000 193 | AngularVelocity = 0x8000 194 | GNSS = 0x7000 195 | SensorComponentReadout = 0xA000 196 | AnalogIn = 0xB000 197 | Magnetic = 0xC000 198 | Velocity = 0xD000 199 | Status = 0xE000 200 | 201 | class XDIMessage: 202 | """Values for the MKIV output data presets.""" 203 | PacketCounter = 0x00001020 204 | PaddedFs = 0x0000FFFF 205 | SampleTimeFine = 0x00001060 206 | DeltaV = 0x00004010 207 | DeltaVFs = 0x00000190 # 400Hz 208 | FsModule = 0x00000064 # 100Hz for 1-series and FMT1000 series 209 | Acceleration = 0x00004020 210 | AccelerationFs = 0x00000190 # 400Hz 211 | DeltaQ = 0x00008030 212 | DeltaQFs = 0x00000190 # 400Hz 213 | RateOfTurn = 0x00008020 214 | RateOfTurnFs = 0x00000190 # 400Hz 215 | MagneticField = 0x0000C020 216 | MagneticFieldFs = 0x00000064 # 100Hz 217 | Pressure = 0x00003010 218 | PressureFs = 0x00000032 # 50Hz 219 | StatusWord = 0x0000E020 220 | GnssPvtData = 0x00007010 # 4Hz TBD 221 | GnssSatInfo = 0x00007020 # 4Hz TBD 222 | GnssFs = 0x00000004 223 | PositionLatLon = 0x00005040 # Latitude and longitude 224 | PositionHeight = 0x00005020 # Ellipsoidal height 225 | Velocity = 0x0000D010 # Velocity in ENU 226 | Orientation = 0x00002030 # Euler orientation ENU 227 | OrientationQuat = 0x00002010 # Quaternion orientation ENU 228 | 229 | class XDIProductMask: 230 | """Product masks for the Xsens MTi series devices.""" 231 | MTi1Series = "8" # 0x08 232 | MTi7Device = "7" # 0x07 233 | MTi10Series = "6" # 0x06 234 | MTi100Series = "7" # 0x07 235 | MTi700Device = "7" # this is a subset of the 100-series 236 | 237 | 238 | 239 | class MTException(Exception): 240 | def __init__(self, message): 241 | self.message = message 242 | def __str__(self): 243 | return "MT error: " + self.message 244 | 245 | -------------------------------------------------------------------------------- /src/xsens_driver/src/mtnode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib; roslib.load_manifest('xsens_driver') 3 | import rospy 4 | import select 5 | import mtdevice 6 | import math 7 | import pdb 8 | 9 | from std_msgs.msg import Header, Float32, Float64 10 | from sensor_msgs.msg import Imu 11 | from geometry_msgs.msg import TwistStamped, Vector3Stamped, QuaternionStamped 12 | from gps_common.msg import GPSFix, GPSStatus 13 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 14 | from xsens_msgs.msg import sensorSample, baroSample, gnssSample 15 | from xsens_msgs.msg import positionEstimate, velocityEstimate, orientationEstimate 16 | 17 | # transform Euler angles or matrix into quaternions 18 | from math import pi, radians 19 | from tf.transformations import quaternion_from_matrix, quaternion_from_euler, identity_matrix 20 | 21 | import numpy 22 | 23 | def get_param(name, default): 24 | try: 25 | v = rospy.get_param(name) 26 | rospy.loginfo("Found parameter: %s, value: %s"%(name, str(v))) 27 | except KeyError: 28 | v = default 29 | rospy.logwarn("Cannot find value for parameter: %s, assigning " 30 | "default: %s"%(name, str(v))) 31 | return v 32 | 33 | class XSensDriver(object): 34 | 35 | ENU = numpy.identity(3) 36 | NED = numpy.array([[0, 1, 0], [ 1, 0, 0], [0, 0, -1]]) 37 | NWU = numpy.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) 38 | 39 | def __init__(self): 40 | 41 | device = get_param('~device', 'auto') 42 | baudrate = get_param('~baudrate', 0) 43 | if device=='auto': 44 | devs = mtdevice.find_devices() 45 | if devs: 46 | device, baudrate = devs[0] 47 | rospy.loginfo("Detected MT device on port %s @ %d bps"%(device, 48 | baudrate)) 49 | else: 50 | rospy.logerr("Fatal: could not find proper MT device.") 51 | rospy.signal_shutdown("Could not find proper MT device.") 52 | return 53 | if not baudrate: 54 | baudrate = mtdevice.find_baudrate(device) 55 | if not baudrate: 56 | rospy.logerr("Fatal: could not find proper baudrate.") 57 | rospy.signal_shutdown("Could not find proper baudrate.") 58 | return 59 | 60 | rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate)) 61 | self.mt = mtdevice.MTDevice(device, baudrate) 62 | 63 | configure_on_startup = get_param('~configure_on_startup', False) 64 | odr = get_param('~odr', 40) 65 | output_mode = get_param('~output_mode', 2) 66 | xkf_scenario = get_param('~xkf_scenario', 43) 67 | self.use_rostime = get_param('~use_rostime', False) 68 | 69 | if configure_on_startup: 70 | rospy.loginfo('Setting ODR (%d) and output mode (%d)' % (odr, output_mode)) 71 | if odr not in [1, 2, 5, 10, 20, 40, 50, 80, 100, 200, 400]: 72 | raise Exception('Invalid ODR configuraton requested') 73 | if output_mode not in [1, 2, 3]: 74 | raise Exception('Invalid output mode requested') 75 | self.mt.configureMti(odr, output_mode) 76 | self.mt.ChangeBaudrate(baudrate) 77 | self.mt.SetCurrentScenario(xkf_scenario) 78 | self.mt.GoToMeasurement() 79 | else: 80 | rospy.loginfo('Using saved odr and output configuration') 81 | 82 | self.frame_id = get_param('~frame_id', 'mti/data') 83 | 84 | frame_local = get_param('~frame_local' , 'ENU') 85 | frame_local_imu = get_param('~frame_local_imu', 'ENU') 86 | 87 | if frame_local == 'ENU': 88 | R = XSensDriver.ENU 89 | elif frame_local == 'NED': 90 | R = XSensDriver.NED 91 | elif frame_local == 'NWU': 92 | R = XSensDriver.NWU 93 | 94 | if frame_local_imu == 'ENU': 95 | R_IMU = XSensDriver.ENU 96 | elif frame_local_imu == 'NED': 97 | R_IMU = XSensDriver.NED 98 | elif frame_local_imu == 'NWU': 99 | R_IMU = XSensDriver.NWU 100 | 101 | self.R = R.dot(R_IMU.transpose()) 102 | 103 | self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10) 104 | self.diag_msg = DiagnosticArray() 105 | self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1, 106 | message='No status information') 107 | self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1, 108 | message='No status information') 109 | self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1, 110 | message='No status information') 111 | self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat] 112 | 113 | self.imu_pub = rospy.Publisher('mti/sensor/imu', Imu, queue_size=10) #IMU message 114 | self.ss_pub = rospy.Publisher('mti/sensor/sample', sensorSample, queue_size=10) # sensorSample 115 | self.mag_pub = rospy.Publisher('mti/sensor/magnetic', Vector3Stamped, queue_size=10) # magnetic 116 | self.baro_pub = rospy.Publisher('mti/sensor/pressure', baroSample, queue_size=10) # baro 117 | self.gnssPvt_pub = rospy.Publisher('mti/sensor/gnssPvt', gnssSample, queue_size=10) # GNSS PVT 118 | #self.gnssSatinfo_pub = rospy.Publisher('mti/sensor/gnssStatus', GPSStatus, queue_size=10) # GNSS SATINFO 119 | self.ori_pub = rospy.Publisher('mti/filter/orientation', orientationEstimate, queue_size=10) # XKF/XEE orientation 120 | self.vel_pub = rospy.Publisher('mti/filter/velocity', velocityEstimate, queue_size=10) # XKF/XEE velocity 121 | self.pos_pub = rospy.Publisher('mti/filter/position', positionEstimate, queue_size=10) # XKF/XEE position 122 | 123 | self.temp_pub = rospy.Publisher('temperature', Float32, queue_size=10) # decide type 124 | 125 | 126 | 127 | 128 | def spin(self): 129 | try: 130 | while not rospy.is_shutdown(): 131 | self.spin_once() 132 | # Ctrl-C signal interferes with select with the ROS signal handler 133 | # should be OSError in python 3.? 134 | except select.error: 135 | pass 136 | 137 | def spin_once(self): 138 | 139 | def baroPressureToHeight(value): 140 | c1 = 44330.0 141 | c2 = 9.869232667160128300024673081668e-6 142 | c3 = 0.1901975534856 143 | intermediate = 1-math.pow(c2*value, c3) 144 | height = c1*intermediate 145 | return height 146 | 147 | # get data 148 | data = self.mt.read_measurement() 149 | # common header 150 | h = Header() 151 | h.stamp = rospy.Time.now() 152 | h.frame_id = self.frame_id 153 | 154 | # get data (None if not present) 155 | #temp = data.get('Temp') # float 156 | orient_data = data.get('Orientation Data') 157 | velocity_data = data.get('Velocity') 158 | position_data = data.get('Latlon') 159 | altitude_data = data.get('Altitude') 160 | acc_data = data.get('Acceleration') 161 | gyr_data = data.get('Angular Velocity') 162 | mag_data = data.get('Magnetic') 163 | pressure_data = data.get('Pressure') 164 | time_data = data.get('Timestamp') 165 | gnss_data = data.get('Gnss PVT') 166 | status = data.get('Status') # int 167 | 168 | # create messages and default values 169 | "Imu message supported with Modes 1 & 2" 170 | imu_msg = Imu() 171 | pub_imu = False 172 | "SensorSample message supported with Mode 2" 173 | ss_msg = sensorSample() 174 | pub_ss = False 175 | "Mag message supported with Modes 1 & 2" 176 | mag_msg = Vector3Stamped() 177 | pub_mag = False 178 | "Baro in meters" 179 | baro_msg = baroSample() 180 | pub_baro = False 181 | "GNSS message supported only with MTi-G-7xx devices" 182 | "Valid only for modes 1 and 2" 183 | gnssPvt_msg = gnssSample() 184 | pub_gnssPvt = False 185 | #gnssSatinfo_msg = GPSStatus() 186 | #pub_gnssSatinfo = False 187 | # All filter related outputs 188 | "Supported in mode 3" 189 | ori_msg = orientationEstimate() 190 | pub_ori = False 191 | "Supported in mode 3 for MTi-G-7xx devices" 192 | vel_msg = velocityEstimate() 193 | pub_vel = False 194 | "Supported in mode 3 for MTi-G-7xx devices" 195 | pos_msg = positionEstimate() 196 | pub_pos = False 197 | 198 | secs = 0 199 | nsecs = 0 200 | 201 | if time_data: 202 | # first getting the sampleTimeFine 203 | time = time_data['SampleTimeFine'] 204 | secs = 100e-6*time 205 | nsecs = 1e5*time - 1e9*math.floor(secs) 206 | 207 | if acc_data: 208 | if 'Delta v.x' in acc_data: # found delta-v's 209 | pub_ss = True 210 | ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] 211 | ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] 212 | ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] 213 | elif 'accX' in acc_data: # found acceleration 214 | pub_imu = True 215 | imu_msg.linear_acceleration.x = acc_data['accX'] 216 | imu_msg.linear_acceleration.y = acc_data['accY'] 217 | imu_msg.linear_acceleration.z = acc_data['accZ'] 218 | else: 219 | raise MTException("Unsupported message in XDI_AccelerationGroup.") 220 | 221 | if gyr_data: 222 | if 'Delta q0' in gyr_data: # found delta-q's 223 | pub_ss = True 224 | ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] 225 | ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] 226 | ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] 227 | ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] 228 | elif 'gyrX' in gyr_data: # found rate of turn 229 | pub_imu = True 230 | imu_msg.angular_velocity.x = gyr_data['gyrX'] 231 | imu_msg.angular_velocity.y = gyr_data['gyrY'] 232 | imu_msg.angular_velocity.z = gyr_data['gyrZ'] 233 | else: 234 | raise MTException("Unsupported message in XDI_AngularVelocityGroup.") 235 | 236 | if mag_data: 237 | # magfield 238 | ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] 239 | ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] 240 | ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] 241 | pub_mag = True 242 | 243 | if pressure_data: 244 | pub_baro = True 245 | height = baroPressureToHeight(pressure_data['Pressure']) 246 | baro_msg.height = ss_msg.internal.baro.height = height 247 | 248 | if gnss_data: 249 | pub_gnssPvt = True 250 | gnssPvt_msg.itow = gnss_data['iTOW'] 251 | gnssPvt_msg.fix = gnss_data['fix'] 252 | gnssPvt_msg.latitude = gnss_data['lat'] 253 | gnssPvt_msg.longitude = gnss_data['lon'] 254 | gnssPvt_msg.hEll = gnss_data['hEll'] 255 | gnssPvt_msg.hMsl = gnss_data['hMsl'] 256 | gnssPvt_msg.vel.x = gnss_data['velE'] 257 | gnssPvt_msg.vel.y = gnss_data['velN'] 258 | gnssPvt_msg.vel.z = -gnss_data['velD'] 259 | gnssPvt_msg.hAcc = gnss_data['horzAcc'] 260 | gnssPvt_msg.vAcc = gnss_data['vertAcc'] 261 | gnssPvt_msg.sAcc = gnss_data['speedAcc'] 262 | gnssPvt_msg.pDop = gnss_data['PDOP'] 263 | gnssPvt_msg.hDop = gnss_data['HDOP'] 264 | gnssPvt_msg.vDop = gnss_data['VDOP'] 265 | gnssPvt_msg.numSat = gnss_data['nSat'] 266 | gnssPvt_msg.heading = gnss_data['heading'] 267 | gnssPvt_msg.headingAcc = gnss_data['headingAcc'] 268 | 269 | if orient_data: 270 | if 'Q0' in orient_data: 271 | pub_imu = True 272 | imu_msg.orientation.w = orient_data['Q0'] 273 | imu_msg.orientation.x = orient_data['Q1'] 274 | imu_msg.orientation.y = orient_data['Q2'] 275 | imu_msg.orientation.z = orient_data['Q3'] 276 | elif 'Roll' in orient_data: 277 | pub_ori = True 278 | ori_msg.roll = orient_data['Roll'] 279 | ori_msg.pitch = orient_data['Pitch'] 280 | ori_msg.yaw = orient_data['Yaw'] 281 | else: 282 | raise MTException('Unsupported message in XDI_OrientationGroup') 283 | 284 | if velocity_data: 285 | pub_vel = True 286 | vel_msg.velE = velocity_data['velX'] 287 | vel_msg.velN = velocity_data['velY'] 288 | vel_msg.velU = velocity_data['velZ'] 289 | 290 | if position_data: 291 | pub_pos = True 292 | pos_msg.latitude = position_data['lat'] 293 | pos_msg.longitude = position_data['lon'] 294 | 295 | if altitude_data: 296 | pub_pos = True 297 | tempData = altitude_data['ellipsoid'] 298 | pos_msg.hEll = tempData[0] 299 | 300 | #if status is not None: 301 | # if status & 0b0001: 302 | # self.stest_stat.level = DiagnosticStatus.OK 303 | # self.stest_stat.message = "Ok" 304 | # else: 305 | # self.stest_stat.level = DiagnosticStatus.ERROR 306 | # self.stest_stat.message = "Failed" 307 | # if status & 0b0010: 308 | # self.xkf_stat.level = DiagnosticStatus.OK 309 | # self.xkf_stat.message = "Valid" 310 | # else: 311 | # self.xkf_stat.level = DiagnosticStatus.WARN 312 | # self.xkf_stat.message = "Invalid" 313 | # if status & 0b0100: 314 | # self.gps_stat.level = DiagnosticStatus.OK 315 | # self.gps_stat.message = "Ok" 316 | # else: 317 | # self.gps_stat.level = DiagnosticStatus.WARN 318 | # self.gps_stat.message = "No fix" 319 | # self.diag_msg.header = h 320 | # self.diag_pub.publish(self.diag_msg) 321 | 322 | 323 | # publish available information 324 | if pub_imu: 325 | imu_msg.header = h 326 | if not self.use_rostime: 327 | #all time assignments (overwriting ROS time) 328 | imu_msg.header.stamp.secs = secs 329 | imu_msg.header.stamp.nsecs = nsecs 330 | self.imu_pub.publish(imu_msg) 331 | #if pub_gps: 332 | # xgps_msg.header = gps_msg.header = h 333 | # self.gps_pub.publish(gps_msg) 334 | # self.xgps_pub.publish(xgps_msg) 335 | if pub_mag: 336 | mag_msg.header = h 337 | self.mag_pub.publish(mag_msg) 338 | #if pub_temp: 339 | # self.temp_pub.publish(temp_msg) 340 | if pub_ss: 341 | ss_msg.header = h 342 | if not self.use_rostime: 343 | #all time assignments (overwriting ROS time) 344 | ss_msg.header.stamp.secs = secs 345 | ss_msg.header.stamp.nsecs = nsecs 346 | self.ss_pub.publish(ss_msg) 347 | if pub_baro: 348 | baro_msg.header = h 349 | if not self.use_rostime: 350 | #all time assignments (overwriting ROS time) 351 | baro_msg.header.stamp.secs = secs 352 | baro_msg.header.stamp.nsecs = nsecs 353 | self.baro_pub.publish(baro_msg) 354 | if pub_gnssPvt: 355 | gnssPvt_msg.header = h 356 | if not self.use_rostime: 357 | #all time assignments (overwriting ROS time) 358 | baro_msg.header.stamp.secs = secs 359 | baro_msg.header.stamp.nsecs = nsecs 360 | self.gnssPvt_pub.publish(gnssPvt_msg) 361 | if pub_ori: 362 | ori_msg.header = h 363 | if not self.use_rostime: 364 | #all time assignments (overwriting ROS time) 365 | ori_msg.header.stamp.secs = secs 366 | ori_msg.header.stamp.nsecs = nsecs 367 | self.ori_pub.publish(ori_msg) 368 | if pub_vel: 369 | vel_msg.header = h 370 | if not self.use_rostime: 371 | #all time assignments (overwriting ROS time) 372 | vel_msg.header.stamp.secs = secs 373 | vel_msg.header.stamp.nsecs = nsecs 374 | self.vel_pub.publish(vel_msg) 375 | if pub_pos: 376 | pos_msg.header = h 377 | if not self.use_rostime: 378 | #all time assignments (overwriting ROS time) 379 | pos_msg.header.stamp.secs = secs 380 | pos_msg.header.stamp.nsecs = nsecs 381 | self.pos_pub.publish(pos_msg) 382 | 383 | 384 | def main(): 385 | '''Create a ROS node and instantiate the class.''' 386 | rospy.init_node('xsens_driver', anonymous=True, log_level=rospy.INFO) 387 | driver = XSensDriver() 388 | driver.spin() 389 | 390 | 391 | if __name__== '__main__': 392 | main() 393 | 394 | 395 | -------------------------------------------------------------------------------- /src/xsens_driver/src/mtdevice.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import serial 3 | import struct 4 | 5 | import sys, getopt, time, glob, math, pdb, numpy, rospy 6 | 7 | from mtdef import MID, MTException, Baudrates, XDIGroup, getName, getMIDName, XDIMessage, XDIProductMask 8 | 9 | # Verbose flag for debugging 10 | verbose = False 11 | 12 | ################################################################ 13 | # MTDevice class 14 | ################################################################ 15 | ## Xsens MT device communication object. 16 | class MTDevice(object): 17 | """Xsens MT device communication object.""" 18 | 19 | def __init__(self, port, baudrate=115200, timeout=0.1, autoconf=True, 20 | config_mode=False): 21 | """Open device.""" 22 | self.device = serial.Serial(port, baudrate, timeout=timeout, 23 | writeTimeout=timeout, rtscts=True, dsrdtr=True) 24 | self.device.flushInput() # flush to make sure the port is ready TODO 25 | self.device.flushOutput() # flush to make sure the port is ready TODO 26 | ## timeout for communication 27 | self.timeout = timeout 28 | if autoconf: 29 | self.auto_config() 30 | else: 31 | ## mode parameter of the IMU 32 | self.mode = None 33 | ## settings parameter of the IMU 34 | self.settings = None 35 | ## length of the MTData message 36 | self.length = None 37 | ## header of the MTData message 38 | self.header = None 39 | if config_mode: 40 | self.GoToConfig() 41 | 42 | ############################################################ 43 | # Low-level communication 44 | ############################################################ 45 | 46 | ## Low-level message sending function. 47 | def write_msg(self, mid, data=[]): 48 | """Low-level message sending function.""" 49 | length = len(data) 50 | if length>254: 51 | lendat = [0xFF, 0xFF&length, 0xFF&(length>>8)] 52 | else: 53 | lendat = [length] 54 | packet = [0xFA, 0xFF, mid] + lendat + list(data) 55 | packet.append(0xFF&(-(sum(packet[1:])))) 56 | msg = struct.pack('%dB'%len(packet), *packet) 57 | start = time.time() 58 | while (time.time()-start)254: 70 | lendat = [0xFF, 0xFF&length, 0xFF&(length>>8)] 71 | else: 72 | lendat = [length] 73 | packet = [0xFA, 0xFF, mid] + lendat + list(data) 74 | packet.append(0xFF&(-(sum(packet[1:])))) 75 | print packet 76 | msg = struct.pack('%dB'%len(packet), *packet) 77 | start = time.time() 78 | while (time.time()-start)254: 93 | totlength = 7 + self.length 94 | else: 95 | totlength = 5 + self.length 96 | while (time.time()-start)= self.timeout: 134 | rospy.logwarn("timeout waiting for message") 135 | 136 | c = self.device.read() 137 | while (not c) and ((time.time()-new_start)0xFA: 142 | continue 143 | # second part of preamble 144 | waitfor(3) 145 | if ord(self.device.read())<>0xFF: # we assume no timeout anymore 146 | continue 147 | # read message id and length of message 148 | #msg = self.device.read(2) 149 | mid, length = struct.unpack('!BB', self.device.read(2)) 150 | if length==255: # extended length 151 | waitfor(2) 152 | length, = struct.unpack('!H', self.device.read(2)) 153 | # read contents and checksum 154 | 155 | waitfor(length+1) 156 | buf = self.device.read(length+1) 157 | while (len(buf)>8, mode&0x00FF 242 | self.write_ack(MID.SetOutputMode, (H, L)) 243 | self.mode = mode 244 | 245 | 246 | ## Get current output mode. 247 | # Assume the device is in Config state. 248 | def GetOutputSettings(self): 249 | """Get current output mode. 250 | Assume the device is in Config state.""" 251 | data = self.write_ack(MID.SetOutputSettings) 252 | self.settings, = struct.unpack('!I', data) 253 | return self.settings 254 | 255 | 256 | ## Select how to output the information. 257 | # Assume the device is in Config state. 258 | def SetOutputSettings(self, settings): 259 | """Select how to output the information. 260 | Assume the device is in Config state.""" 261 | HH, HL = (settings&0xFF000000)>>24, (settings&0x00FF0000)>>16 262 | LH, LL = (settings&0x0000FF00)>>8, settings&0x000000FF 263 | self.write_ack(MID.SetOutputSettings, (HH, HL, LH, LL)) 264 | self.settings = settings 265 | 266 | 267 | ## Set the period of sampling. 268 | # Assume the device is in Config state. 269 | def SetPeriod(self, period): 270 | """Set the period of sampling. 271 | Assume the device is in Config state.""" 272 | H, L = (period&0xFF00)>>8, period&0x00FF 273 | self.write_ack(MID.SetPeriod, (H, L)) 274 | 275 | 276 | ## Set the output skip factor. 277 | # Assume the device is in Config state. 278 | def SetOutputSkipFactor(self, skipfactor): 279 | """Set the output skip factor. 280 | Assume the device is in Config state.""" 281 | H, L = (skipfactor&0xFF00)>>8, skipfactor&0x00FF 282 | self.write_ack(MID.SetOutputSkipFactor, (H, L)) 283 | 284 | 285 | ## Get data length. 286 | # Assume the device is in Config state. 287 | def ReqDataLength(self): 288 | """Get data length. 289 | Assume the device is in Config state.""" 290 | data = self.write_ack(MID.ReqDataLength) 291 | self.length, = struct.unpack('!H', data) 292 | self.header = '\xFA\xFF\x32'+chr(self.length) 293 | return self.length 294 | 295 | 296 | ## Ask for the current configuration of the MT device. 297 | # Assume the device is in Config state. 298 | def ReqConfiguration(self): 299 | """Ask for the current configuration of the MT device. 300 | Assume the device is in Config state.""" 301 | data_ack = self.write_ack(MID.SetOutputConfiguration,()) 302 | config = [] 303 | try: 304 | for i in range(len(data_ack)/4): 305 | config.append(struct.unpack('!HH', data_ack[i*4:i*4+4])) 306 | except struct.error: 307 | raise MTException("could not parse configuration.") 308 | return config 309 | 310 | ## Set the baudrate of the device using the baudrate id. 311 | # Assume the device is in Config state. 312 | def SetBaudrate(self, brid): 313 | """Set the baudrate of the device using the baudrate id. 314 | Assume the device is in Config state.""" 315 | baudRateAck = False 316 | self.write_msg(MID.SetBaudrate, (brid,)) 317 | dataAck = self.write_ack(MID.SetBaudrate, ()) 318 | bridAck = struct.unpack('!B',dataAck) 319 | if bridAck[0] == brid: 320 | baudRateAck = True 321 | return baudRateAck 322 | 323 | ## Request the available XKF scenarios on the device. 324 | # Assume the device is in Config state. 325 | def ReqAvailableScenarios(self): 326 | """Request the available XKF scenarios on the device. 327 | Assume the device is in Config state.""" 328 | scenarios_dat = self.write_ack(MID.ReqAvailableScenarios) 329 | scenarios = [] 330 | try: 331 | for i in range(len(scenarios_dat)/22): 332 | scenario_type, version, label =\ 333 | struct.unpack('!BB20s', scenarios_dat[22*i:22*(i+1)]) 334 | scenarios.append((scenario_type, version, label.strip())) 335 | ## available XKF scenarios 336 | self.scenarios = scenarios 337 | except struct.error: 338 | raise MTException("could not parse the available XKF scenarios.") 339 | return scenarios 340 | 341 | 342 | ## Request the ID of the currently used XKF scenario. 343 | # Assume the device is in Config state. 344 | def ReqCurrentScenario(self): 345 | """Request the ID of the currently used XKF scenario. 346 | Assume the device is in Config state.""" 347 | data = self.write_ack(MID.SetCurrentScenario) 348 | ## current XKF id 349 | self.scenario_id, = struct.unpack('!H', data) 350 | try: 351 | scenarios = self.scenarios 352 | except AttributeError: 353 | scenarios = self.ReqAvailableScenarios() 354 | for t, _, label in scenarios: 355 | if t==self.scenario_id: 356 | ## current XKF label 357 | self.scenario_label = label 358 | break 359 | else: 360 | self.scenario_label = "" 361 | return self.scenario_id, self.scenario_label 362 | 363 | 364 | ## Sets the XKF scenario to use. 365 | # Assume the device is in Config state. 366 | def SetCurrentScenario(self, scenario_id): 367 | """Sets the XKF scenario to use. 368 | Assume the device is in Config state.""" 369 | data = self.ReqAvailableScenarios() 370 | availableSc = numpy.array(data) 371 | validateSc = availableSc == str(scenario_id) 372 | if validateSc.any(): 373 | self.write_ack(MID.SetCurrentScenario, (0x00, scenario_id&0xFF)) 374 | print "Set to scenario:%2d"%scenario_id 375 | else: 376 | raise MTException("not an available XKF scenario") 377 | 378 | 379 | ############################################################ 380 | # High-level utility functions 381 | ############################################################ 382 | ## Configure the mode and settings of the MT MK4 device. 383 | # this configures the MK4 devices to publish Xsens sensorSample format 384 | def configureMti(self, mtiSampleRate, mtiMode): 385 | """Configure the mode and settings of the MTMk4 device.""" 386 | self.GoToConfig() 387 | self.timeout = math.pow(mtiSampleRate,-1)+MID.additionalTimeOutOffset # additional 5ms leeway 388 | print "Timeout changed to %1.3fs based on current settings."%(self.timeout) 389 | mid = MID.SetOutputConfiguration 390 | midReqDID = MID.ReqDID 391 | dataReqDID = (0x00, 0x00) 392 | dataDID = self.write_ack(midReqDID, dataReqDID) 393 | try: 394 | masterID = struct.unpack('!L', dataDID) 395 | except struct.error: 396 | raise MTException("could not parse configuration.") 397 | # to have a clear distinction between MTi-G-700 and 100-series devices 398 | deviceIDProductMask = hex(masterID[0]&0x00f00000) 399 | deviceTypeMask = hex(masterID[0]&0x0f000000) 400 | # check for user input for the delta q and delta v quantities 401 | new_imu_period = XDIMessage.DeltaQFs 402 | if mtiSampleRate < int(new_imu_period): 403 | new_imu_period = mtiSampleRate 404 | # check for user input for the rate IMU quantities 405 | rate_imu_period = XDIMessage.RateOfTurnFs 406 | if mtiSampleRate < int(rate_imu_period): 407 | rate_imu_period = mtiSampleRate 408 | # check for user input for the mag quantities 409 | new_mag_period = XDIMessage.MagneticFieldFs 410 | 411 | if mtiSampleRate < int(new_mag_period): 412 | new_mag_period = mtiSampleRate 413 | # check for user input for the baro samples 414 | new_pressure_period = XDIMessage.PressureFs 415 | if mtiSampleRate < int(new_pressure_period): 416 | new_pressure_period = mtiSampleRate 417 | 418 | if (deviceIDProductMask[2] == XDIProductMask.MTi1Series): 419 | new_imu_period = rate_imu_period = new_mag_period = XDIMessage.FsModule 420 | if mtiSampleRate < int(new_imu_period): 421 | new_imu_period = rate_imu_period = new_mag_period = mtiSampleRate 422 | # All messages with corresponding output data rates 423 | "Packet couter, SampleTimeFine" 424 | # mPc = self.getMtiConfigBytes(XDIMessage.PacketCounter, XDIMessage.PaddedFs) 425 | mStf = self.getMtiConfigBytes(XDIMessage.SampleTimeFine, XDIMessage.PaddedFs) 426 | "Sensor data" 427 | mImuDq = self.getMtiConfigBytes(XDIMessage.DeltaQ, new_imu_period) 428 | mImuDv = self.getMtiConfigBytes(XDIMessage.DeltaV, new_imu_period) 429 | "Sensor data (rate quantities)" 430 | mImuMag = self.getMtiConfigBytes(XDIMessage.MagneticField, new_mag_period) 431 | mImuGyr = self.getMtiConfigBytes(XDIMessage.RateOfTurn, rate_imu_period) 432 | mImuAcc = self.getMtiConfigBytes(XDIMessage.Acceleration, rate_imu_period) 433 | "Baro data" 434 | mImuP = self.getMtiConfigBytes(XDIMessage.Pressure, new_pressure_period) 435 | "GNSS data" 436 | mGnssPvt = self.getMtiConfigBytes(XDIMessage.GnssPvtData, XDIMessage.GnssFs) 437 | mGnssSat = self.getMtiConfigBytes(XDIMessage.GnssSatInfo, XDIMessage.GnssFs) 438 | "Status word" 439 | mSw = self.getMtiConfigBytes(XDIMessage.StatusWord, XDIMessage.PaddedFs) 440 | # Filter related messages 441 | "Filter estimate" 442 | mOrientationQuat = self.getMtiConfigBytes(XDIMessage.OrientationQuat, rate_imu_period) 443 | mOrientation = self.getMtiConfigBytes(XDIMessage.Orientation,rate_imu_period) 444 | mVelocity = self.getMtiConfigBytes(XDIMessage.Velocity,rate_imu_period) 445 | mPosition = self.getMtiConfigBytes(XDIMessage.PositionLatLon,rate_imu_period) 446 | mHeight = self.getMtiConfigBytes(XDIMessage.PositionHeight,rate_imu_period) 447 | 448 | # Output configuration set based on the product ID and user specification 449 | if (deviceIDProductMask[2] == XDIProductMask.MTi100Series) & (deviceTypeMask[2] == XDIProductMask.MTi700Device): 450 | print "MTi-G-700/710 (GNSS/INS) device detected." 451 | if mtiMode == 1: 452 | print "Enabled publishing all sensor data" 453 | data = mStf+mImuDq+mImuDv+mImuMag+mImuP+mGnssPvt+mGnssSat+mSw+mOrientationQuat 454 | elif mtiMode == 2: 455 | print "Enabled publishing all sensor data (rate quantities)" 456 | data = mStf+mImuGyr+mImuAcc+mImuMag+mImuP+mGnssPvt+mGnssSat+mSw+mOrientationQuat 457 | elif mtiMode == 3: 458 | print "Enabled publishing all filter estimates" 459 | data = mStf+mSw+mOrientation+mVelocity+mPosition+mHeight 460 | else: 461 | raise MTException("unknown mtiMode: (%d)."% (mtiMode)) 462 | elif deviceIDProductMask[2] == XDIProductMask.MTi100Series: 463 | print "MTi-100/200/300 device detected." 464 | if mtiMode == 1: 465 | print "Enabled publishing all sensor data" 466 | data = mStf+mImuDq+mImuDv+mImuMag+mImuP+mSw+mOrientationQuat 467 | elif mtiMode == 2: 468 | print "Enabled publishing all sensor data (rate quantities)" 469 | data = mStf+mImuGyr+mImuAcc+mImuMag+mImuP+mSw+mOrientationQuat 470 | elif mtiMode == 3: 471 | print "Enabled publishing all filter estimates" 472 | data = mStf+mSw+mOrientation 473 | else: 474 | raise MTException("unknown mtiMode: (%d)."% (mtiMode)) 475 | elif deviceIDProductMask[2] == XDIProductMask.MTi10Series: 476 | print "MTi-10/20/30 device detected" 477 | if mtiMode == 1: 478 | print "Enabled publishing all sensor data" 479 | data = mStf+mImuDq+mImuDv+mImuMag+mSw+mOrientationQuat 480 | elif mtiMode == 2: 481 | print "Enabled publishing all sensor data (rate quantities)" 482 | data = mStf+mImuGyr+mImuAcc+mImuMag+mSw+mOrientationQuat 483 | elif mtiMode == 3: 484 | print "Enabled publishing all filter estimates" 485 | data = mStf+mSw+mOrientation 486 | else: 487 | raise MTException("unknown mtiMode: (%d)."% (mtiMode)) 488 | elif (deviceIDProductMask[2] == XDIProductMask.MTi1Series) & (deviceTypeMask[2] == XDIProductMask.MTi7Device): 489 | print "MTi-7 device detected" 490 | if mtiMode == 1: 491 | print "Enabled publishing all sensor data" 492 | data = mStf+mImuDq+mImuDv+mImuMag+mImuP+mGnssPvt+mSw+mOrientationQuat 493 | elif mtiMode == 2: 494 | print "Enabled publishing all sensor data (rate quantities)" 495 | data = mStf+mImuGyr+mImuAcc+mImuMag+mImuP+mGnssPvt+mSw+mOrientationQuat 496 | elif mtiMode == 3: 497 | print "Enabled publishing all filter estimates" 498 | data = mStf+mSw+mOrientation+mVelocity+mPosition+mHeight 499 | else: 500 | raise MTException("unknown mtiMode: (%d)."% (mtiMode)) 501 | elif deviceIDProductMask[2] == XDIProductMask.MTi1Series: 502 | print "MTi-1/2/3 device detected" 503 | if mtiMode == 1: 504 | print "Enabled publishing all sensor data" 505 | data = mStf+mImuDq+mImuDv+mImuMag+mSw+mOrientationQuat 506 | elif mtiMode == 2: 507 | print "Enabled publishing all sensor data (rate quantities)" 508 | data = mStf+mImuGyr+mImuAcc+mImuMag+mSw+mOrientationQuat 509 | elif mtiMode == 3: 510 | print "Enabled publishing all filter estimates" 511 | data = mStf+mSw+mOrientation 512 | else: 513 | raise MTException("unknown mtiMode: (%d)."% (mtiMode)) 514 | else: 515 | raise MTException("Unknown device") 516 | 517 | self.write_msg(mid, data) 518 | # check for the set baudrate 519 | dataAck = self.write_ack(MID.SetBaudrate, ()) 520 | bridAck = struct.unpack('!B',dataAck) 521 | brSettings = Baudrates.get_BR(bridAck[0]) 522 | print "Device configured at %1.0f bps"%(brSettings) 523 | self.GoToMeasurement() 524 | 525 | def getMtiConfigBytes(self, dataMessage, dataFs): 526 | H, L = (dataMessage&0xFF00)>>8, dataMessage&0x00FF 527 | HFs, LFs = (dataFs&0xFF00)>>8, dataFs&0x00FF 528 | message = [H, L, HFs, LFs] 529 | return message 530 | 531 | 532 | ## Read configuration from device. 533 | def auto_config(self): 534 | """Read configuration from device.""" 535 | self.GoToConfig() 536 | config =self.ReqConfiguration() 537 | Config = numpy.array(config) 538 | if Config.any(): 539 | configuredMtiFs = numpy.max(Config[Config[:,1]!=65535,1]) 540 | self.timeout = math.pow(configuredMtiFs,-1)+MID.additionalTimeOutOffset # additional 5ms leeway 541 | print "Timeout defaults to %1.3fs based on output settings."%(self.timeout) 542 | else: 543 | self.timeout = 1+MID.additionalTimeOutOffset 544 | print "Timeout defaults to %1.3fs based on output settings."%(self.timeout) 545 | self.GoToMeasurement() 546 | return self.timeout 547 | 548 | 549 | ## Read and parse a measurement packet 550 | def read_measurement(self, mode=None, settings=None): 551 | # getting data 552 | mid, data = self.read_msg() 553 | if mid==MID.MTData2: 554 | return self.parse_MTData2(data) 555 | else: 556 | raise MTException("Only MTData2 supported, use -f and -m to configure MTi.\n unknown data message: mid=0x%02X (%s)."% (mid, getMIDName(mid))) 557 | 558 | ## Parse a new MTData2 message 559 | def parse_MTData2(self, data): 560 | # Functions to parse each type of packet 561 | def parse_temperature(data_id, content, ffmt): 562 | o = {} 563 | if (data_id&0x00F0) == 0x10: # Temperature 564 | o['Temp'], = struct.unpack('!'+ffmt, content) 565 | else: 566 | raise MTException("unknown packet: 0x%04X."%data_id) 567 | return o 568 | def parse_timestamp(data_id, content, ffmt): 569 | o = {} 570 | if (data_id&0x00F0) == 0x10: # UTC Time 571 | o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\ 572 | o['Minute'], o['Second'], o['Flags'] =\ 573 | struct.unpack('!LHBBBBBB', content) 574 | elif (data_id&0x00F0) == 0x20: # Packet Counter 575 | o['PacketCounter'], = struct.unpack('!H', content) 576 | elif (data_id&0x00F0) == 0x30: # Integer Time of Week 577 | o['TimeOfWeek'], = struct.unpack('!L', content) 578 | elif (data_id&0x00F0) == 0x40: # GPS Age 579 | o['gpsAge'], = struct.unpack('!B', content) 580 | elif (data_id&0x00F0) == 0x50: # Pressure Age 581 | o['pressureAge'], = struct.unpack('!B', content) 582 | elif (data_id&0x00F0) == 0x60: # Sample Time Fine 583 | o['SampleTimeFine'], = struct.unpack('!L', content) 584 | elif (data_id&0x00F0) == 0x70: # Sample Time Coarse 585 | o['SampleTimeCoarse'], = struct.unpack('!L', content) 586 | elif (data_id&0x00F0) == 0x80: # Frame Range 587 | o['startFrame'], o['endFrame'] = struct.unpack('!HH', content) 588 | else: 589 | raise MTException("unknown packet: 0x%04X."%data_id) 590 | return o 591 | def parse_orientation_data(data_id, content, ffmt): 592 | o = {} 593 | if (data_id&0x00F0) == 0x10: # Quaternion 594 | o['Q0'], o['Q1'], o['Q2'], o['Q3'] = struct.unpack('!'+4*ffmt, 595 | content) 596 | elif (data_id&0x00F0) == 0x20: # Rotation Matrix 597 | o['a'], o['b'], o['c'], o['d'], o['e'], o['f'], o['g'], o['h'],\ 598 | o['i'] = struct.unpack('!'+9*ffmt, content) 599 | elif (data_id&0x00F0) == 0x30: # Euler Angles 600 | o['Roll'], o['Pitch'], o['Yaw'] = struct.unpack('!'+3*ffmt, 601 | content) 602 | else: 603 | raise MTException("unknown packet: 0x%04X."%data_id) 604 | return o 605 | def parse_pressure(data_id, content, ffmt): 606 | o = {} 607 | if (data_id&0x00F0) == 0x10: # Baro pressure 608 | # FIXME is it really U4 as in the doc and not a float/double? 609 | o['Pressure'], = struct.unpack('!L', content) 610 | else: 611 | raise MTException("unknown packet: 0x%04X."%data_id) 612 | return o 613 | def parse_acceleration(data_id, content, ffmt): 614 | o = {} 615 | if (data_id&0x00F0) == 0x10: # Delta V 616 | o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] = \ 617 | struct.unpack('!'+3*ffmt, content) 618 | elif (data_id&0x00F0) == 0x20: # Acceleration 619 | o['accX'], o['accY'], o['accZ'] = \ 620 | struct.unpack('!'+3*ffmt, content) 621 | elif (data_id&0x00F0) == 0x30: # Free Acceleration 622 | o['freeAccX'], o['freeAccY'], o['freeAccZ'] = \ 623 | struct.unpack('!'+3*ffmt, content) 624 | else: 625 | raise MTException("unknown packet: 0x%04X."%data_id) 626 | return o 627 | def parse_position(data_id, content, ffmt): 628 | o = {} 629 | heightFlag = False 630 | if (data_id&0x00F0) == 0x40: # LatLon 631 | o['lat'], o['lon'] = struct.unpack('!'+2*ffmt, content) 632 | elif (data_id&0x00F0) == 0x20: # Altitude Ellipsoid 633 | o['ellipsoid'] = struct.unpack('!'+1*ffmt, content) 634 | heightFlag = True 635 | else: 636 | raise MTException("unknown packet: 0x%04X."%data_id) 637 | return o, heightFlag 638 | def parse_angular_velocity(data_id, content, ffmt): 639 | o = {} 640 | # FIXME is it really 802y and 803y as in the doc? 641 | if (data_id&0x00F0) == 0x20: # Rate of Turn 642 | o['gyrX'], o['gyrY'], o['gyrZ'] = \ 643 | struct.unpack('!'+3*ffmt, content) 644 | elif (data_id&0x00F0) == 0x30: # Delta Q 645 | o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3'] = \ 646 | struct.unpack('!'+4*ffmt, content) 647 | else: 648 | raise MTException("unknown packet: 0x%04X."%data_id) 649 | return o 650 | def parse_GNSS(data_id, content, ffmt): 651 | o = {} 652 | pvtFlag = False 653 | if (data_id&0x00F0) == 0x10: # GNSS PVT DATA 654 | o['iTOW'],x1,x2,x3,x4,x5,x6,x7,x8,x9,o['fix'],o['flag'],o['nSat'],x10,lon,lat,h,a,hAcc, \ 655 | vAcc,vN,vE,vD,x11,x12,sAcc,headAcc,headVeh,gDop,pDop,tDop,vDop,hDop,nDop,eDop = \ 656 | struct.unpack('!LHBBBBBBLiBBBBiiiiLLiiiiiLLIHHHHHHH', content) 657 | o['lat'], o['lon'], o['hEll'], o['hMsl'], o['velN'], o['velE'], o['velD'], \ 658 | o['horzAcc'], o['vertAcc'], o['speedAcc'], o['GDOP'], o['PDOP'], o['TDOP'],\ 659 | o['VDOP'], o['HDOP'], o['NDOP'], o['EDOP'], o['heading'], o['headingAcc'] = 1e-7*lat, 1e-7*lon, 1e-3*h, \ 660 | 1e-3*a, 1e-3*vN, 1e-3*vE, 1e-3*vD, 1e-3*hAcc, 1e-3*vAcc, 1e-3*sAcc, 1e-2*gDop, \ 661 | 1e-2*pDop, 1e-2*tDop, 1e-2*vDop, 1e-2*hDop, 1e-2*nDop, 1e-2*eDop, 1e-5*headVeh, 1e-5*headAcc 662 | pvtFlag = True 663 | elif (data_id&0x00F0) == 0x20: # GNSS SAT Info 664 | o['iTOW'], o['numCh'] = struct.unpack('!LBxxx', content[:8]) 665 | channels = [] 666 | ch = {} 667 | for i in range(o['numCh']): 668 | ch['gnssId'], ch['svId'], ch['cno'], ch['flags'] = \ 669 | struct.unpack('!BBBB', content[8+4*i:12+4*i]) 670 | channels.append(ch) 671 | ch = {} # empty 672 | o['channels'] = channels 673 | else: 674 | raise MTException("unknown packet: 0x%04X."%data_id) 675 | return o, pvtFlag 676 | def parse_SCR(data_id, content, ffmt): 677 | o = {} 678 | if (data_id&0x00F0) == 0x10: # ACC+GYR+MAG+Temperature 679 | o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \ 680 | o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['Temp']=\ 681 | struct.unpack("!9Hh", content) 682 | elif (data_id&0x00F0) == 0x20: # Gyro Temperature 683 | o['tempGyrX'], o['tempGyrY'], o['tempGyrZ'] = \ 684 | struct.unpack("!hhh", content) 685 | else: 686 | raise MTException("unknown packet: 0x%04X."%data_id) 687 | return o 688 | def parse_analog_in(data_id, content, ffmt): 689 | o = {} 690 | if (data_id&0x00F0) == 0x10: # Analog In 1 691 | o['analogIn1'], = struct.unpack("!H", content) 692 | elif (data_id&0x00F0) == 0x20: # Analog In 2 693 | o['analogIn2'], = struct.unpack("!H", content) 694 | else: 695 | raise MTException("unknown packet: 0x%04X."%data_id) 696 | return o 697 | def parse_magnetic(data_id, content, ffmt): 698 | o = {} 699 | if (data_id&0x00F0) == 0x20: # Magnetic Field 700 | o['magX'], o['magY'], o['magZ'] = \ 701 | struct.unpack("!3"+ffmt, content) 702 | else: 703 | raise MTException("unknown packet: 0x%04X."%data_id) 704 | return o 705 | def parse_velocity(data_id, content, ffmt): 706 | o = {} 707 | if (data_id&0x00F0) == 0x10: # Velocity XYZ 708 | o['velX'], o['velY'], o['velZ'] = \ 709 | struct.unpack("!3"+ffmt, content) 710 | else: 711 | raise MTException("unknown packet: 0x%04X."%data_id) 712 | return o 713 | def parse_status(data_id, content, ffmt): 714 | o = {} 715 | if (data_id&0x00F0) == 0x10: # Status Byte 716 | o['StatusByte'], = struct.unpack("!B", content) 717 | elif (data_id&0x00F0) == 0x20: # Status Word 718 | o['StatusWord'], = struct.unpack("!L", content) 719 | elif (data_id&0x00F0) == 0x40: # RSSI 720 | o['RSSI'], = struct.unpack("!b", content) 721 | else: 722 | raise MTException("unknown packet: 0x%04X."%data_id) 723 | return o 724 | 725 | # data object 726 | output = {} 727 | while data: 728 | try: 729 | data_id, size = struct.unpack('!HB', data[:3]) 730 | if (data_id&0x0003) == 0x3: 731 | float_format = 'd' 732 | elif (data_id&0x0003) == 0x0: 733 | float_format = 'f' 734 | else: 735 | raise MTException("fixed point precision not supported.") 736 | content = data[3:3+size] 737 | data = data[3+size:] 738 | group = data_id&0xFF00 739 | ffmt = float_format 740 | if group == XDIGroup.Temperature: 741 | output['Temperature'] = parse_temperature(data_id, content, ffmt) 742 | elif group == XDIGroup.Timestamp: 743 | output['Timestamp'] = parse_timestamp(data_id, content, ffmt) 744 | elif group == XDIGroup.OrientationData: 745 | output['Orientation Data'] = parse_orientation_data(data_id, content, ffmt) 746 | elif group == XDIGroup.Pressure: 747 | output['Pressure'] = parse_pressure(data_id, content, ffmt) 748 | elif group == XDIGroup.Acceleration: 749 | output['Acceleration'] = parse_acceleration(data_id, content, ffmt) 750 | elif group == XDIGroup.Position: 751 | temp, dataFlagPos = parse_position(data_id, content, ffmt) 752 | if dataFlagPos: 753 | output['Altitude'] = temp 754 | else: 755 | output['Latlon'] = temp 756 | elif group == XDIGroup.AngularVelocity: 757 | output['Angular Velocity'] = parse_angular_velocity(data_id, content, ffmt) 758 | elif group == XDIGroup.GNSS: 759 | temp, dataFlagGnss = parse_GNSS(data_id, content, ffmt) 760 | if dataFlagGnss: 761 | output['Gnss PVT'] = temp 762 | else: 763 | output['Gnss SATINFO'] = temp 764 | elif group == XDIGroup.SensorComponentReadout: 765 | output['SCR'] = parse_SCR(data_id, content, ffmt) 766 | elif group == XDIGroup.AnalogIn: 767 | output['Analog In'] = parse_analog_in(data_id, content, ffmt) 768 | elif group == XDIGroup.Magnetic: 769 | output['Magnetic'] = parse_magnetic(data_id, content, ffmt) 770 | elif group == XDIGroup.Velocity: 771 | output['Velocity'] = parse_velocity(data_id, content, ffmt) 772 | elif group == XDIGroup.Status: 773 | output['Status'] = parse_status(data_id, content, ffmt) 774 | else: 775 | raise MTException("unknown XDI group: 0x%04X."%group) 776 | except struct.error, e: 777 | raise MTException("couldn't parse MTData2 message.") 778 | return output 779 | 780 | ## Parse a legacy MTData message 781 | ## Change the baudrate, reset the device and reopen communication. 782 | def ChangeBaudrate(self, baudrate): 783 | """Change the baudrate, reset the device and reopen communication.""" 784 | self.GoToConfig() 785 | brid = Baudrates.get_BRID(baudrate) 786 | bridAck = self.SetBaudrate(brid) 787 | if bridAck: # Testing if the BR was set correctly 788 | self.device.baudrate=baudrate 789 | print "Baudrate set to %d bps"%baudrate 790 | time.sleep(0.01) 791 | else: 792 | print "NOK:Baudrate not configured." 793 | 794 | 795 | 796 | ################################################################ 797 | # Auto detect port 798 | ################################################################ 799 | def find_devices(): 800 | mtdev_list = [] 801 | for port in glob.glob("/dev/tty*S*"): 802 | try: 803 | br = find_baudrate(port) 804 | if br: 805 | mtdev_list.append((port, br)) 806 | except MTException: 807 | pass 808 | return mtdev_list 809 | 810 | 811 | ################################################################ 812 | # Auto detect baudrate 813 | ################################################################ 814 | def find_baudrate(port): 815 | baudrates = (115200, 460800, 921600, 230400, 57600, 38400, 19200, 9600) 816 | for br in baudrates: 817 | try: 818 | mt = MTDevice(port, br) 819 | except serial.SerialException: 820 | raise MTException("unable to open %s"%port) 821 | try: 822 | mt.GoToConfig() 823 | mt.GoToMeasurement() 824 | return br 825 | except MTException: 826 | pass 827 | 828 | 829 | 830 | ################################################################ 831 | # Documentation for stand alone usage 832 | ################################################################ 833 | def usage(): 834 | print """MT device driver. 835 | Usage: 836 | ./mtdevice.py [commands] [opts] 837 | 838 | Commands: 839 | -h, --help 840 | Print this help and quit. 841 | -r, --reset 842 | Reset device to factory defaults. 843 | -f, --mtiSampleRate=SAMPLERATE 844 | Configures the device to the specified Output Data Rate (ODR).Possible 845 | ODR's are 1,2,4,5,10,20,40,50,80,100,200 & 400 (the maximum output rate 846 | for mag, baro and GNSS sensor is 100Hz, 50Hz and 4Hz respectively) 847 | -m, --sensorMode=SENSORMODE 848 | Configures the device to a particular sensor mode. The values can be 1 849 | (for sensor data),2 (for sensor data with rate quantities) or 850 | 3(for filter estimates). Use it in conjunction with -f command 851 | -a, --change-baudrate=NEW_BAUD 852 | Change baudrate from BAUD (see below) to NEW_BAUD. 853 | -e, --echo 854 | Print MTData. It is the default if no other command is supplied. 855 | -i, --inspect 856 | Print current MT device configuration. 857 | -x, --xkf-scenario=ID 858 | Change the current XKF scenario. 859 | Options: 860 | -d, --device=DEV 861 | Serial interface of the device (default: /dev/ttyUSB0). If 'auto', then 862 | all serial ports are tested at all baudrates and the first 863 | suitable device is used. 864 | -b, --baudrate=BAUD 865 | Baudrate of serial interface (default: 115200). If 0, then all 866 | rates are tried until a suitable one is found. 867 | -s, --skip-factor=SKIPFACTOR 868 | Number of samples to skip before sending MTData2 message 869 | (default: 0). 870 | The frequency at which MTData message is send is: 871 | 115200/(PERIOD * (SKIPFACTOR + 1)) 872 | If the value is 0xffff, no data is send unless a ReqData request 873 | is made. 874 | """ 875 | ################################################################ 876 | # Main function 877 | ################################################################ 878 | def main(): 879 | # parse command line 880 | shopts = 'hra:eid:b:s:x:f:m:' 881 | lopts = ['help', 'reset', 'change-baudrate=', 'echo', 882 | 'inspect', 'device=', 'baudrate=', 'skip-factor=', 883 | 'xkf-scenario=', 'mti-odr=','mti-mode='] 884 | try: 885 | opts, args = getopt.gnu_getopt(sys.argv[1:], shopts, lopts) 886 | #pdb.set_trace() 887 | except getopt.GetoptError, e: 888 | print e 889 | usage() 890 | return 1 891 | # default values 892 | device = '/dev/ttyUSB0' 893 | baudrate = 115200 894 | mode = 1 895 | settings = None 896 | period = None 897 | skipfactor = None 898 | new_baudrate = None 899 | new_xkf = None 900 | sampleRate = 100 901 | actions = [] 902 | # filling in arguments 903 | for o, a in opts: 904 | if o in ('-h', '--help'): 905 | usage() 906 | return 907 | if o in ('-r', '--reset'): 908 | actions.append('reset') 909 | if o in ('-a', '--change-baudrate'): 910 | try: 911 | new_baudrate = int(a) 912 | except ValueError: 913 | print "change-baudrate argument must be integer." 914 | return 1 915 | actions.append('change-baudrate') 916 | if o in ('-e', '--echo'): 917 | actions.append('echo') 918 | if o in ('-i', '--inspect'): 919 | actions.append('inspect') 920 | if o in ('-x', '--xkf-scenario'): 921 | try: 922 | new_xkf = int(a) 923 | except ValueError: 924 | print "xkf-scenario argument must be integer." 925 | return 1 926 | actions.append('xkf-scenario') 927 | if o in ('-d', '--device'): 928 | device = a 929 | if o in ('-b', '--baudrate'): 930 | try: 931 | baudrate = int(a) 932 | except ValueError: 933 | print "Baudrate argument must be integer." 934 | return 1 935 | if o in ('-s', '--skip-factor'): 936 | try: 937 | skipfactor = int(a) 938 | except ValueError: 939 | print "skip-factor argument must be integer." 940 | return 1 941 | if o in ('-f', '--mti-odr'): 942 | try: 943 | sampleRate = int(a) 944 | actions.append('setMtiOutputConfiguration') 945 | except ValueError: 946 | print "MTi sample rate argument must be integer." 947 | return 1 948 | if o in ('-m','--mti-mode'): 949 | try: 950 | mode = int(a) 951 | actions.append('setMtiOutputConfiguration') 952 | except ValueError: 953 | print "MTi mode argument must be integer." 954 | return 1 955 | 956 | 957 | # if nothing else: echo 958 | if len(actions) == 0: 959 | actions.append('echo') 960 | try: 961 | if device=='auto': 962 | devs = find_devices() 963 | if devs: 964 | print "Detected devices:","".join('\n\t%s @ %d'%(d,p) for d,p in 965 | devs) 966 | print "Using %s @ %d"%devs[0] 967 | device, baudrate = devs[0] 968 | else: 969 | print "No suitable device found." 970 | return 1 971 | # find baudrate 972 | if not baudrate: 973 | baudrate = find_baudrate(device) 974 | if not baudrate: 975 | print "No suitable baudrate found." 976 | return 1 977 | # open device 978 | try: 979 | mt = MTDevice(device, baudrate) 980 | except serial.SerialException: 981 | raise MTException("unable to open %s"%device) 982 | # execute actions 983 | if 'inspect' in actions: 984 | mt.GoToConfig() 985 | print "Device: %s at %d bps:"%(device, baudrate) 986 | print "General configuration:", mt.ReqConfiguration() 987 | print "Available scenarios:", mt.ReqAvailableScenarios() 988 | print "Current scenario: %s (id: %d)"%mt.ReqCurrentScenario()[::-1] 989 | mt.GoToMeasurement() 990 | if 'change-baudrate' in actions: 991 | print "Changing baudrate from %d to %d bps\n"%(baudrate, new_baudrate), 992 | sys.stdout.flush() 993 | mt.ChangeBaudrate(new_baudrate) 994 | if 'reset' in actions: 995 | print "Restoring factory defaults", 996 | sys.stdout.flush() 997 | mt.RestoreFactoryDefaults() 998 | print " Ok" # should we test it was actually ok? 999 | if 'xkf-scenario' in actions: 1000 | print "Changing XKF scenario..." 1001 | sys.stdout.flush() 1002 | mt.GoToConfig() 1003 | mt.SetCurrentScenario(new_xkf) 1004 | mt.GoToMeasurement() 1005 | print "Ok" 1006 | if 'setMtiOutputConfiguration' in actions: 1007 | sys.stdout.flush() 1008 | mt.GoToConfig() 1009 | print "Device intiated at %d Hz"%(sampleRate) 1010 | mt.configureMti(sampleRate,mode) 1011 | if 'echo' in actions: 1012 | try: 1013 | while True: 1014 | print mt.read_measurement(mode, settings) 1015 | except KeyboardInterrupt: 1016 | pass 1017 | except MTException as e: 1018 | #traceback.print_tb(sys.exc_info()[2]) 1019 | print e 1020 | 1021 | 1022 | 1023 | if __name__=='__main__': 1024 | main() 1025 | 1026 | --------------------------------------------------------------------------------