├── .clang-format ├── .github └── workflows │ ├── arduino-compile.yml │ ├── arduino-lint.yml │ └── ceedling.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── docs ├── basic-charts.png ├── command_table.ods └── mtmanager_packets.md ├── examples ├── basics │ └── basics.ino └── write_config │ └── write_config.ino ├── keywords.txt ├── library.properties ├── project.yml ├── src ├── xsens_constants.h ├── xsens_mdata2.c ├── xsens_mdata2.h ├── xsens_mti.c ├── xsens_mti.h ├── xsens_mti_private.h ├── xsens_utility.c └── xsens_utility.h └── test ├── test_xsens_mti.c ├── test_xsens_mti_macros.c ├── test_xsens_mti_parse_messages.c ├── test_xsens_mti_send.c ├── test_xsens_mti_table_handling.c └── test_xsens_utility.c /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: WebKit 3 | AlignAfterOpenBracket: Align 4 | AlignConsecutiveMacros: true 5 | AlignConsecutiveAssignments: true 6 | AlignConsecutiveDeclarations: true 7 | AlignEscapedNewlines: Left 8 | AlignTrailingComments: true 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | AllowShortBlocksOnASingleLine: true 11 | AllowShortFunctionsOnASingleLine: Inline 12 | AllowShortIfStatementsOnASingleLine: Never 13 | AllowShortLoopsOnASingleLine: false 14 | AlwaysBreakBeforeMultilineStrings: false 15 | IndentCaseLabels: true 16 | SpacesInCStyleCastParentheses: false 17 | SpacesBeforeTrailingComments: 4 18 | SpacesInParentheses: true 19 | SpaceBeforeParens: Never 20 | SpaceBeforeAssignmentOperators: true 21 | SpaceAfterLogicalNot: false 22 | BreakBeforeTernaryOperators: true 23 | DerivePointerAlignment: false 24 | PointerAlignment: Right 25 | AlignOperands: true 26 | Language: Cpp 27 | SortIncludes: true 28 | BreakBeforeBraces: Custom 29 | BraceWrapping: 30 | AfterEnum: true 31 | AfterControlStatement: true 32 | AfterFunction: true 33 | AfterUnion: true 34 | AfterStruct: true 35 | SplitEmptyFunction: false 36 | BeforeCatch: true 37 | BeforeElse: true 38 | 39 | ... 40 | -------------------------------------------------------------------------------- /.github/workflows/arduino-compile.yml: -------------------------------------------------------------------------------- 1 | name: Arduino Compile 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build-arduino: 11 | runs-on: ubuntu-latest 12 | 13 | strategy: 14 | matrix: 15 | fqbn: 16 | - arduino:avr:mega:cpu=atmega2560 #avr 17 | - arduino:samd:mzero_bl #arm m0 18 | 19 | steps: 20 | - uses: actions/checkout@v3 21 | - uses: arduino/compile-sketches@v1 22 | with: 23 | github-token: ${{ secrets.GITHUB_TOKEN }} 24 | fqbn: ${{ matrix.fqbn }} 25 | libraries: "- source-path: ./" 26 | sketch-paths: | 27 | - examples/basics 28 | - examples/write_config 29 | 30 | build-esp32: 31 | runs-on: ubuntu-latest 32 | 33 | strategy: 34 | matrix: 35 | fqbn: 36 | - esp32:esp32:esp32 # tensillica 37 | - esp32:esp32:esp32c3 # riscv 38 | 39 | steps: 40 | - uses: actions/checkout@v3 41 | - uses: arduino/compile-sketches@v1 42 | with: 43 | github-token: ${{ secrets.GITHUB_TOKEN }} 44 | platforms: | 45 | - name: esp32:esp32 46 | source-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json 47 | fqbn: ${{ matrix.fqbn }} 48 | libraries: "- source-path: ./" 49 | cli-compile-flags: | 50 | - --warnings 51 | - default 52 | sketch-paths: | 53 | - examples/basics 54 | - examples/write_config 55 | -------------------------------------------------------------------------------- /.github/workflows/arduino-lint.yml: -------------------------------------------------------------------------------- 1 | name: Arduino Registry Checks 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | unit-test: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: arduino/arduino-lint-action@v1 16 | with: 17 | library-manager: update 18 | compliance: strict 19 | -------------------------------------------------------------------------------- /.github/workflows/ceedling.yml: -------------------------------------------------------------------------------- 1 | name: Ceedling 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | unit-test: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - uses: actions/checkout@v3 15 | 16 | - name: Set up Ruby 17 | uses: ruby/setup-ruby@v1 18 | with: 19 | ruby-version: '3.0' 20 | 21 | # Install ceedling 22 | - name: Install Ceedling 23 | run: gem install ceedling 24 | 25 | # Runs a set of commands using the runners shell 26 | - name: Test 27 | run: ceedling test:all -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | CMakeCache.txt 3 | CMakeFiles 4 | CMakeScripts 5 | Testing 6 | Makefile 7 | cmake_install.cmake 8 | install_manifest.txt 9 | compile_commands.json 10 | CTestTestfile.cmake 11 | _deps 12 | 13 | build/ 14 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.6) 2 | 3 | SET( PROJECT_VERSION 0.0.8 ) 4 | 5 | project(xsens-mti) 6 | 7 | add_library(xsens-mti STATIC src/xsens_mti.c src/xsens_utility.c src/xsens_mdata2.c) 8 | 9 | target_include_directories(xsens-mti PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) 10 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # xsens MTi Serial C Library 2 | 3 | This C library is intended for use on microcontrollers (though can be used on typical computers fairly easily) for high performance robotics applications. 4 | 5 | The parser is hardware agnostic, with a design philosophy around structured data and callback functions when valid packets are decoded. 6 | 7 | > Happy to discuss issues & merge reasonable PRs. 8 | 9 | ## Why? 10 | 11 | With the exception of one Arduino library (which didn't work for me), and examples from xsens which are SPI/I2C & specific to MTi-1/3/7 modules, there wasn't anything available (other than ARM SBC options) which would let me interface an external AHRS unit to my embedded platform. 12 | 13 | At time of writing, I couldn't find any suitable C libraries/examples for communicating over UART/RS232. 14 | 15 | The packet layout and packet-in-packet (MData2) format variations mean writing a general handler and decoder is a bit terse, and the xsens C++ example shifts this responsibility to the developer. 16 | 17 | This library _aims_ to provide a more sane approach, with application code not requiring any buffer handling or IMU specific logic. 18 | 19 | ## Does it work? 20 | 21 | [Yes, example firmware and GUI here](https://github.com/Scottapotamas/xsens-stm32-eui). 22 | 23 | ![basic-charts](docs/basic-charts.png) 24 | 25 | MTManager was used with various output configurations to capture real packets alongside decoded reference outputs (see `/docs/mtmanager_packets.md`). These packets back the test-driven development approach of the library, while the [MT Low Level Documentation PDF](https://www.xsens.com/hubfs/Downloads/Manuals/MT_Low-Level_Documentation.pdf) was used as reference material. 26 | 27 | I've tested the library against a MTi-300 connected to a STM32F4 discovery board via RS232 transceiver, and a custom MTi-2 and STM32F4 board. 28 | 29 | I use this library in production with several MTi-300 units connected to a custom STM32F429 controller, also through a RS232 transceiver. 30 | 31 | Another user has reported success with MTi-7. 32 | 33 | ## What's the catch? 34 | 35 | Most of the xsens IMU hardware is pretty expensive. So it's unlikely many people will read this... 36 | 37 | - No specific effort has been put into ensuring portability of this library to big-endian processors. 38 | - Without other IMU hardware, I'm unable to provide functional/correctness guarantees. 39 | - Unit tests don't cover MTi-7/600/700 specific configuration or functionality (i.e. GNSS info). 40 | 41 | - While the library tries to follow message identifiers and fields outlined by xsens, some sections aren't implemented (but an escape hatch is provided). 42 | 43 | # Usage 44 | 45 | A minimal Arduino compatible implementation is described in `/example/basics` and is quickly explained in the following sections. 46 | 47 | > Examples aren't exhaustive across all messages/payloads, though enums describing possible inbound/outbound message identifiers, and the list of callback event flags [is described in the `xsens_constants` header](src/xsens_constants.h). 48 | 49 | ## Basic Setup 50 | 51 | - Include `xsens-mti.h` in your application code. 52 | 53 | - Create a callback function which will receive motion data after the library has parsed and decoded packets: 54 | 55 | ```c 56 | void imu_event_cb( XsensEventFlag_t event, XsensEventData_t *mtdata ); 57 | ``` 58 | 59 | - Optionally create another callback to allow the library to write binary data to the IMU: 60 | 61 | ```c 62 | void imu_write_cb( uint8_t *buffer, uint16_t len ); 63 | ``` 64 | 65 | - Declare a `xsens_interface_t`, and pass it pointers to the callback functions: 66 | 67 | - At compile time: 68 | ```c 69 | xsens_interface_t imu_interface = XSENS_INTERFACE_RX( &imu_event_cb ); 70 | xsens_interface_t imu_interface = XSENS_INTERFACE_RX_TX( &imu_event_cb, &imu_write_cb ); 71 | ``` 72 | ```c 73 | // Longform setup 74 | xsens_interface_t imu_interface = { .event_cb = &imu_event_cb, .output_cb = &imu_write_cb }; 75 | ``` 76 | 77 | - At run-time: 78 | 79 | ```c 80 | xsens_interface_t imu_interface; 81 | imu_interface.event_cb = &imu_event_cb; 82 | imu_interface.output_cb = &imu_write_cb; 83 | ``` 84 | 85 | - Pass inbound serial data to the library as it arrives. The parser uses a state-machine design and is happy accepting a single byte, or many, with no timing requirements. 86 | 87 | - Individual bytes: 88 | 89 | ```c 90 | xsens_mti_parse( &imu_interface, byte ); 91 | ``` 92 | 93 | - Buffer of data: 94 | 95 | ```c 96 | xsens_mti_parse_buffer( &imu_interface, &rx_buff, rx_len ); 97 | ``` 98 | 99 | That's it. The `/examples` folder has Arduino compatible implementations for reference. 100 | 101 | ### Low RAM targets 102 | 103 | The xsens packet specification allows for 'extended length' payloads, up to `2048B`. 104 | 105 | Current implementation in this library allows for these with a sufficiently large buffer. This means the default library implementation will exhaust RAM on particularly constrained targets like Atmel 328p (Arduino Uno). 106 | 107 | If you find yourself in this unique situation, either use your build system to define `XSENS_PAYLOAD_BUFFER_SIZE` during build, or 108 | 109 | 1. Open `/src/xsens_constants.h` in your editor 110 | 2. Manually define a new `XSENS_PAYLOAD_BUFFER_SIZE` at the top of the file, with a value larger than `255`. 111 | 3. Try building and hope it fits on your micro. 112 | 4. Reconfigure your IMU with MTManager to disable GNSS `PVT` and/or `SatInfo` packets, as these are most likely to need large packets. 113 | 4. Re-think your system design, as your IMU is massively overkill for your requirements or your micro is inadequate for your task! 114 | 115 | ## Using motion data 116 | 117 | In the application code event callback `imu_event_cb()` or similar, catch relevant events generated by the library when one of the `MData2` packets is received. 118 | 119 | - `XsensEventFlag_t` signifies what kind of data has been processed. 120 | - `XsensEventData_t` is a structure containing: 121 | - a type flag (enum) describing what data format is used in the 122 | - union of possible payload types ranging from `uint16_t` to a `float[9]` 123 | 124 | This method allows the application code to receive ready-to-use values immediately after they are decoded. 125 | 126 | > Note that `*mtdata` points to data on the stack and is not long-lived. 127 | 128 | ```c 129 | void imu_event_cb( XsensEventFlag_t event, XsensEventData_t *mtdata ) 130 | { 131 | switch( event ) 132 | { 133 | case XSENS_EVT_PACKET_COUNT: 134 | // ... 135 | break; 136 | 137 | case XSENS_EVT_ACCELERATION: 138 | if( mtdata->type == XSENS_EVT_TYPE_FLOAT3 ) 139 | { 140 | // Update the PID controller with x, y, z accelerometer 141 | // These are 4-byte floats 142 | controller_update( mtdata->data.f4x3[0], 143 | mtdata->data.f4x3[1], 144 | mtdata->data.f4x3[2] ); 145 | } 146 | break; 147 | 148 | case XSENS_EVT_PRESSURE: 149 | // ... 150 | break; 151 | } 152 | } 153 | ``` 154 | 155 | 156 | 157 | ### XsensEventFlag list 158 | 159 | List of possible user-facing events is listed below. I'd recommend just looking at the `mdata2_decode_rules` table [in `xsens_mdata2.c`](src/xsens_mdata2.c) as it also lists the corresponding event type. 160 | 161 | ``` 162 | XSENS_EVT_TEMPERATURE 163 | XSENS_EVT_UTC_TIME 164 | XSENS_EVT_PACKET_COUNT 165 | XSENS_EVT_TIME_FINE 166 | XSENS_EVT_TIME_COARSE 167 | XSENS_EVT_QUATERNION 168 | XSENS_EVT_EULER 169 | XSENS_EVT_ROT_MATRIX 170 | XSENS_EVT_PRESSURE 171 | XSENS_EVT_DELTA_V 172 | XSENS_EVT_DELTA_Q 173 | XSENS_EVT_ACCELERATION 174 | XSENS_EVT_FREE_ACCELERATION 175 | XSENS_EVT_ACCELERATION_HR 176 | XSENS_EVT_RATE_OF_TURN 177 | XSENS_EVT_RATE_OF_TURN_HR 178 | XSENS_EVT_GNSS_PVT_PULSE 179 | XSENS_EVT_RAW_ACC_GYRO_MAG_TEMP 180 | XSENS_EVT_RAW_GYRO_TEMP 181 | XSENS_EVT_MAGNETIC 182 | XSENS_EVT_STATUS_BYTE 183 | XSENS_EVT_STATUS_WORD 184 | XSENS_EVT_DEVICE_ID 185 | XSENS_EVT_LOCATION_ID 186 | XSENS_EVT_POSITION_ECEF 187 | XSENS_EVT_LAT_LON 188 | XSENS_EVT_ALTITUDE_ELLIPSOID 189 | XSENS_EVT_VELOCITY_XYZ 190 | ``` 191 | 192 | ## Overriding parser functionality 193 | 194 | In situations where you want to handle a packet directly, declare your own handler function to process the payload. 195 | 196 | This is the previously mentioned 'escape-hatch' you should use if trying to handle unsupported packets. 197 | 198 | ```c 199 | void imu_icc_command_ack( xsens_packet_buffer_t *packet ) 200 | { 201 | // Use the packet fields to post-process as needed: 202 | // packet->message_id 203 | // packet->length 204 | // packet->payload 205 | } 206 | ``` 207 | 208 | During your setup, provide the callback pointer and `MID` to filter for. Use the enum value in `xsens_constants.h` or the raw value: 209 | 210 | ```c 211 | bool ok = xsens_mti_override_id_handler( 0x75, &imu_icc_command_ack ); 212 | bool ok = xsens_mti_override_id_handler( MT_ICCCOMMANDACK, &imu_icc_command_ack ); 213 | ``` 214 | 215 | When the parser has a valid packet (passed CRC check), the callback is fired. 216 | 217 | > Note that the `packet_buffer_t` structure being pointed to is contained in the user-space `xsens_interface_t` state variable (typically heap allocated). 218 | > 219 | > This data will persist after the callback, but **will be wiped** when another packet is received. 220 | 221 | This payload data is directly from the IMU (unprocessed), and you'll likely need to convert fields from their big-endian format to little-endian. `xsens_utility.h` has some helper functions to assist: 222 | 223 | ```c 224 | uint16_t xsens_coalesce_16BE_16LE( uint8_t *source ); 225 | uint32_t xsens_coalesce_32BE_32LE( uint8_t *source ); 226 | float xsens_coalesce_32BE_F32LE( uint8_t *source ); 227 | ``` 228 | 229 | The possible message identifiers (and the id's with integrated handling) are paired in the [partially populated 'jump table'](src/xsens_mti.c). 230 | 231 | ## Sending packets to the IMU 232 | 233 | For simple queries against the hardware, use the request function with the message ID. When the device responds, the library (or user-overridden callback) will receive a callback. 234 | 235 | ```c 236 | xsens_mti_request( &imu_interface, MT_REQFWREV ); 237 | ``` 238 | 239 | Some helper functions are provided for simple commands, but may require putting the IMU into configuration mode first. Refer to `/examples/write_config` for a reference implementation of changing mode and configuring output data/rates. 240 | 241 | 242 | ```c 243 | xsens_mti_set_baudrate( &imu_interface, XSENS_BAUD_460800 ); 244 | xsens_mti_reset_orientation( &imu_interface, XSENS_ORIENTATION_ALIGNMENT_RESET ); 245 | ``` 246 | 247 | More complex configuration functions expect the end-user to provide valid settings: 248 | 249 | ```c 250 | XsensFrequencyConfig_t settings[] = { 251 | { .id = XDI_PACKET_COUNTER, .frequency = 0xFFFF }, // 0xFFFF -> included in every output packet 252 | { .id = XDI_STATUS_WORD, .frequency = 0xFFFF }, 253 | { .id = XDI_QUATERNION, .frequency = 100 }, // Hz 254 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_LAT_LON, XSENS_FLOAT_FIXED1632, XSENS_COORD_ENU), .frequency = 4 }, 255 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_VELOCITY_XYZ, XSENS_FLOAT_SINGLE, XSENS_COORD_NED), .frequency = 100 }, 256 | }; 257 | 258 | xsens_mti_set_configuration( &imu_interface, settings, XSENS_ARR_ELEM(settings) ); 259 | 260 | ``` 261 | 262 | 263 | Beyond this, create a `xsens_packet_buffer_t` object manually and send it to hardware: 264 | 265 | ```c 266 | packet.message_id = MT_SETBAUDRATE; 267 | packet.length = 1; 268 | packet.payload[0] = 0x80; 269 | 270 | xsens_mti_send( interface, &packet ); 271 | ``` 272 | 273 | 274 | # Running tests 275 | 276 | [![Ceedling](https://github.com/Scottapotamas/xsens-mti/actions/workflows/ceedling.yml/badge.svg)](https://github.com/Scottapotamas/xsens-mti/actions/workflows/ceedling.yml) 277 | 278 | Testing uses the [Ceedling](http://www.throwtheswitch.org/ceedling/) (Ruby/rake) based testing framework with `Unity` and `CMock`. 279 | 280 | 1. *If* you don't have Ceedling installed: 281 | 282 | - Either install it with your OS's package manager, 283 | - Manual install `ceedling` with `gem install --user ceedling`. 284 | 285 | 2. Once setup, run `ceedling` or `ceedling test:all`. 286 | 287 | GitHub Actions compile the Arduino examples and run unit-tests when pull-requests are made. 288 | 289 | ## Coverage Analysis 290 | 291 | - Run `ceedling gcov:all` to generate the coverage reports. 292 | - Use `ceedling utils:gcov` to generate a pretty HTML report. 293 | - HTML output is located in the `/build/artifacts/gcov` folder. 294 | 295 | You need `gcovr` installed, and on some Linux distros, may also need a `gcovr` run-time dependency `jinja2`. 296 | 297 | # References & Acknowledgement 298 | 299 | - xsens have documented their protocol and the behaviour in the [MT Low Level Documentation PDF](https://www.xsens.com/hubfs/Downloads/Manuals/MT_Low-Level_Documentation.pdf). 300 | - xsens MTManager software provides a packet viewer was used to capture and decode several 'golden' packets used in unit tests. 301 | 302 | This library is [Apache2 licenced](LICENSE). 303 | -------------------------------------------------------------------------------- /docs/basic-charts.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Scottapotamas/xsens-mti/2d29bb8dfe0b5462951db18fecc4a303f5883c75/docs/basic-charts.png -------------------------------------------------------------------------------- /docs/command_table.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Scottapotamas/xsens-mti/2d29bb8dfe0b5462951db18fecc4a303f5883c75/docs/command_table.ods -------------------------------------------------------------------------------- /docs/mtmanager_packets.md: -------------------------------------------------------------------------------- 1 | # XSens Golden Packets 2 | 3 | Recorded with xsens MT manager and decoder. 4 | Documented as reference material. 5 | Used in unit tests. 6 | 7 | Most of these examples describe the actual payloads. 8 | 9 | Typical Preamble and device ID is 0xFA 0xFF 10 | Followed by message ID. MTData2 is 0x36 11 | Then payload. 12 | Then CRC 13 | 14 | ## General Info 15 | 16 | Product Code: MTi-300-2A5G4 17 | Firmware: 1.8.2 build 37 rev 70964 18 | Hardware: 2.0.0 19 | Location ID: 0 20 | Filter Setting: General 21 | HardwareID: 15004782 22 | Device ID: 037003F8 23 | Self-test 000001FF 24 | Test-Calibration Date: 21/05/2015 25 | 26 | # Packet Breakdown 27 | 28 | This is a packet which has most fields reported. 29 | 30 | Size: 139 - 0x8B 31 | Message ID (MID): 36 32 | Items: 11 33 | 34 | ## Raw Data 35 | 36 | ``` 37 | 10 20 02 A6 55 10 60 04 00 57 47 2E 20 10 10 3F 7F 7D C4 BC 10 10 7D 3B A1 57 70 BD 7E C6 56 40 20 0C BD A2 1A F8 BE 2A 8E 96 41 1D 27 A2 40 10 0C B9 4F C8 00 B9 DA 24 00 3C C9 28 80 40 30 0C 3C 02 C8 9C 3C 35 F6 C8 3C DB 0C 00 80 20 0C BB B1 7D 81 BB 96 32 00 3C 01 E8 41 80 30 10 3F 80 00 00 B6 E3 30 01 B6 C0 40 00 37 26 48 00 C0 20 0C BE 99 9C 24 3F B6 1B 56 3F 16 6A EB 30 10 04 00 01 86 DE E0 20 04 00 40 00 03 38 | ``` 39 | 40 | ## Decoded Info 41 | 42 | ``` 43 | {(PacketCounter, 2 bytes, 42581), (SampleTimeFine, 4 bytes, 5719854), (Quaternion|Float|ENU, 16 bytes, (q0: 0.99801278, q1: -0.00879299, q2: 0.00492375, q3: -0.06220087)), (Acceleration|Float, 12 bytes, (accX: -0.07915300, accY: -0.16655955, accZ: 9.82217598)), (DeltaV|Float, 12 bytes, (x: -0.00019816, y: -0.00041607, z: 0.02455544)), (FreeAcceleration|Float, 12 bytes, (freeAccX: 0.00798240, freeAccY: 0.01110620, freeAccZ: 0.02673912)), (RateOfTurn|Float, 12 bytes, (gyrX: -0.00541657, gyrY: -0.00458360, gyrZ: 0.00792891)), (DeltaQ|Float, 16 bytes, (q0: 1.00000000, q1: -0.00000677, q2: -0.00000573, q3: 0.00000991)), (MagneticField|Float, 12 bytes, (magX: -0.30001938, magY: 1.42270923, magZ: 0.58756894)), (BaroPressure, 4 bytes, Pressure: 100062), (StatusWord, 4 bytes, 00000000010000000000000000000011)} 44 | ``` 45 | 46 | ### Item 0: Packet Counter - 10 20 47 | 48 | Uint16 - 0x02 49 | 42581 - A6 55 50 | 51 | ### Item 1: SampleTimeFine - 10 60 52 | 53 | Uint32 - 0x04 54 | 5719854 - 00 57 47 2E 55 | 56 | ### Item 2: Quaternion - 20 10 57 | 58 | Float32 x4 - 0x10 59 | q0 - 3F 7F 7D C4 - 0.9980128 60 | q1 - BC 10 10 7D - -0.0087930 61 | q2 - 3B A1 57 D4 - 0.0049238 62 | q3 - 41 1D 27 A2 - 9.8221760 63 | 64 | ### Item 3: Acceleration - 40 20 65 | 66 | Float32 x 3 - 0x0C 67 | accX - -0.0791530 - BD A2 1A F8 68 | accY - -0.1665595 - BE 2A 8E 93 69 | accZ - 9.8221760 - 41 1D 27 A2 70 | 71 | ### Item 4: DeltaV - 40 10 72 | 73 | Float32 x 3 - 0x0C 74 | x - -0.0001982 - B9 4F D3 E8 75 | y - -0.0004161 - B9 DA 27 FF 76 | z - 0.0245554 - 3C C9 28 68 77 | 78 | ### Item 5: FreeAcceleration - 40 30 79 | 80 | Float32 x 3 - 0x0C 81 | freeAccX - 0.0079824 - 3C 02 C8 9D 82 | freeAccY - 0.0111062 - 3C 35 F6 C7 83 | freeAccZ - 0.0267391 - 3C DB 0B F5 84 | 85 | ### Item 6: RateOfTurn - 80 20 86 | 87 | Float32 x 3 - 0x0C 88 | gyrX - -0.0054166 - BB B1 7D BC 89 | gyrY - -0.0045836 - BB 96 32 06 90 | gyrZ - 0.0079289 - 3C 01 E8 38 91 | 92 | ### Item 7: DeltaQ - 80 30 93 | 94 | Float32 x 4 - 0x10 95 | q0 - 1.0000000 - 3F 80 00 00 96 | q1 - -0.0000068 - B6 E4 2B 8E 97 | q2 - -0.0000057 - B6 BF 42 A1 98 | q3 - 0.0000099 - 37 26 18 2D 99 | 100 | ### Item 8: MagneticField - C0 20 101 | 102 | Float32 x 3 - 0x0C 103 | magX - -0.3000194 - BE 99 9C 25 104 | magY - 1.4227092 - 3F B6 1B 56 105 | magZ - 0.5875689 - 3F 16 6A EA 106 | 107 | ### Item 9: Barometer - 30 10 108 | 109 | Uint32 - 0x04 110 | 100062 - 00 01 86 DE 111 | 112 | ### Item 10: StatusWord - E0 20 113 | 114 | BitField32 - 0x04 115 | no decoded content - 00 40 00 03 116 | 117 | ## Temp Field - 08 10 118 | 119 | Wasn't in this longform packet. 120 | Float32 - 0x04 121 | 37.6250000 - 42 16 80 00 122 | 123 | # Packet Example 2 124 | 125 | Has Mag, no temp 126 | 127 | Size: 132 bytes - 0x84 128 | 10 items 129 | Message ID (MID): 36 130 | 131 | ## Raw Data 132 | 133 | ``` 134 | 10 20 02 A6 51 10 60 04 00 57 46 CA 20 10 10 3F 7F 7D AF BC 10 17 8D 3B A1 5D 51 BD 7E DC 78 40 20 0C BD 9A 97 A6 BE 26 F9 BF 41 1C B2 E4 40 10 0C B9 46 44 00 B9 D5 9C 00 3C C8 93 10 40 30 0C 3C 3F EE 14 3C 5F 69 B0 BA F2 80 00 80 20 0C BB 70 6E 03 BB C2 3D 00 BB D4 99 03 80 30 10 3F 80 00 00 B6 99 E0 01 B6 F8 A0 00 B7 08 10 00 C0 20 0C BE 91 DC FF 3F B6 6C 36 3F 18 71 68 E0 20 04 00 40 00 03 135 | ``` 136 | 137 | ## Decoded Info 138 | 139 | ``` 140 | {(PacketCounter, 2 bytes, 42577), (SampleTimeFine, 4 bytes, 5719754), (Quaternion|Float|ENU, 16 bytes, (q0: 0.99801153, q1: -0.00879468, q2: 0.00492445, q3: -0.06222197)), (Acceleration|Float, 12 bytes, (accX: -0.07548456, accY: -0.16306208, accZ: 9.79367447)), (DeltaV|Float, 12 bytes, (x: -0.00018908, y: -0.00040743, z: 0.02448419)), (FreeAcceleration|Float, 12 bytes, (freeAccX: 0.01171448, freeAccY: 0.01363604, freeAccZ: -0.00185013)), (RateOfTurn|Float, 12 bytes, (gyrX: -0.00366867, gyrY: -0.00592768, gyrZ: -0.00648797)), (DeltaQ|Float, 16 bytes, (q0: 1.00000000, q1: -0.00000459, q2: -0.00000741, q3: -0.00000811)), (MagneticField|Float, 12 bytes, (magX: -0.28488919, magY: 1.42517734, magZ: 0.59548044)), (StatusWord, 4 bytes, 00000000010000000000000000000011)} 141 | ``` 142 | 143 | # Packet Example 3 144 | 145 | No mag, no temp 146 | 147 | Size 117 bytes 148 | 9 Items 149 | Message ID (MID): 36 150 | 151 | ## Raw Data 152 | 153 | ``` 154 | 10 20 02 8D 90 10 60 04 00 54 DB F1 20 10 10 3F 7F 89 11 BC 11 1D FA 3B A0 CE F0 BD 73 25 73 40 20 0C BD DC F9 D0 BE 3C 86 19 41 1D 0B 47 40 10 0C B9 8D B0 00 B9 F1 48 00 3C C9 04 30 40 30 0C BC B9 89 2E BB 09 8C 00 3C A7 08 00 80 20 0C BA 63 BC 01 BC 04 D6 3F BB 6D E3 FE 80 30 10 3F 80 00 01 B5 91 C0 00 B7 2A 08 00 B6 98 40 00 E0 20 04 00 40 00 03 155 | ``` 156 | 157 | ## Decoded Info 158 | 159 | ``` 160 | {(PacketCounter, 2 bytes, 36240), (SampleTimeFine, 4 bytes, 5561329), (Quaternion|Float|ENU, 16 bytes, (q0: 0.99818522, q1: -0.00885724, q2: 0.00490748, q3: -0.05936189)), (Acceleration|Float, 12 bytes, (accX: -0.10789835, accY: -0.18410529, accZ: 9.81525326)), (DeltaV|Float, 12 bytes, (x: -0.00027025, y: -0.00046021, z: 0.02453813)), (FreeAcceleration|Float, 12 bytes, (freeAccX: -0.02264842, freeAccY: -0.00209880, freeAccZ: 0.02038956)), (RateOfTurn|Float, 12 bytes, (gyrX: -0.00086874, gyrY: -0.00810772, gyrZ: -0.00362992)), (DeltaQ|Float, 16 bytes, (q0: 1.00000012, q1: -0.00000109, q2: -0.00001013, q3: -0.00000454)), (StatusWord, 4 bytes, 00000000010000000000000000000011)} 161 | ``` 162 | 163 | # Packet Example 4 164 | 165 | Temp, Mag, and all other IMU data 166 | 167 | Size 147 - 0x92 168 | 12 Items 169 | Message ID (MID): 36 170 | 171 | ## Raw Data 172 | 173 | ``` 174 | 10 20 02 91 8D 10 60 04 01 36 3F A6 20 10 10 3F 35 E0 42 3F 31 CD 15 BD 9F 49 DB BD A9 38 CF 40 20 0C BD 63 5A 90 41 1D 08 D4 3E 5F AA 50 40 10 0C B9 11 68 00 3C C9 01 00 3A 0F 7D 00 40 30 0C BC 3B 29 80 3C 35 FC 00 3C A4 6E 00 80 20 0C 3C AE A2 41 BB 56 D8 00 BA D5 AC 01 80 30 10 3F 80 00 00 37 DF 88 01 B6 89 80 00 B6 08 C0 00 C0 20 0C BE FB FB F2 3F 33 C4 85 BF A0 A2 C1 08 10 04 42 16 80 00 30 10 04 00 01 86 E1 E0 20 04 00 40 00 03 175 | ``` 176 | 177 | ## Decoded Info 178 | 179 | ``` 180 | {(PacketCounter, 2 bytes, 37261), (SampleTimeFine, 4 bytes, 20332454), (Quaternion|Float|ENU, 16 bytes, (q0: 0.71045315, q1: 0.69453555, q2: -0.07777759, q3: -0.08262789)), (Acceleration|Float, 12 bytes, (accX: -0.05550629, accY: 9.81465530, accZ: 0.21842313)), (DeltaV|Float, 12 bytes, (x: -0.00013867, y: 0.02453661, z: 0.00054736)), (FreeAcceleration|Float, 12 bytes, (freeAccX: -0.01142347, freeAccY: 0.01110744, freeAccZ: 0.02007198)), (RateOfTurn|Float, 12 bytes, (gyrX: 0.02131760, gyrY: -0.00327826, gyrZ: -0.00163019)), (DeltaQ|Float, 16 bytes, (q0: 1.00000000, q1: 0.00002665, q2: -0.00000410, q3: -0.00000204)), (MagneticField|Float, 12 bytes, (magX: -0.49215657, magY: 0.70221740, magZ: -1.25496686)), (Temperature|Float, 4 bytes, Temp: 37.62500000), (BaroPressure, 4 bytes, Pressure: 100065), (StatusWord, 4 bytes, 00000000010000000000000000000011)} 181 | ``` 182 | 183 | # Packet Example 5 184 | 185 | Measured during high-intensity shake 186 | Expect that `FilterValid` isn't high 187 | Expect that various sensors are reporting saturation in the status bitfield. 188 | Message ID (MID): 36 189 | 190 | ## Raw Data 191 | 192 | ``` 193 | 10 20 02 FB 85 10 60 04 01 A4 98 DE 20 10 10 3F 2A 14 63 BE D7 EF A7 3C DE E5 08 3F 1D CE C9 40 20 0C C1 F2 46 C3 C1 EC E0 76 C2 8F 85 3F 40 10 0C BD 93 2C CC BD 92 0A 16 BE 3A 6E EC 40 30 0C 42 51 94 64 C2 7B 5A 5A C1 CC C0 AE 80 20 0C 40 85 4D 6D C1 25 55 9E C0 90 8E 1F 80 30 10 3F 7F F8 98 3B AA 9E E5 BC 53 9E C0 BB B9 06 0C C0 20 0C 3E DC 74 39 BE 75 2B 48 3F AF 9A 3F 30 10 04 00 01 86 DE E0 20 04 00 48 14 01 194 | ``` 195 | 196 | ## Decoded Info 197 | 198 | ``` 199 | {(PacketCounter, 2 bytes, 64389), (SampleTimeFine, 4 bytes, 27564254), (Quaternion|Float|ENU, 16 bytes, (q0: 0.66437358, q1: -0.42175028, q2: 0.02720882, q3: 0.61643654)), (Acceleration|Float, 12 bytes, (accX: -30.28455162, accY: -29.60960007, accZ: -71.76024628)), (DeltaV|Float, 12 bytes, (x: -0.07186279, y: -0.07130830, z: -0.18206376)), (FreeAcceleration|Float, 12 bytes, (freeAccX: 52.39491272, freeAccY: -62.83823395, freeAccZ: -25.59408188)), (RateOfTurn|Float, 12 bytes, (gyrX: 4.16570139, gyrY: -10.33340263, gyrZ: -4.51734877)), (DeltaQ|Float, 16 bytes, (q0: 0.99988699, q1: 0.00520693, q2: -0.01291627, q3: -0.00564647)), (MagneticField|Float, 12 bytes, (magX: 0.43057421, magY: -0.23942292, magZ: 1.37189472)), (BaroPressure, 4 bytes, Pressure: 100062), (StatusWord, 4 bytes, 00000000010010000001010000000001)} 200 | ``` 201 | 202 | 203 | 204 | # Packet Example 6 205 | 206 | Only SampleTime, PacketCounter, Orientation (quaternion) and StatusWord are enabled. 207 | 208 | Length 38 - 0x24 209 | 4 Items 210 | Message ID (MID): 36 211 | 212 | ## Raw Data 213 | 214 | ``` 215 | 10 20 02 46 82 10 60 04 01 C4 FC 3E 20 10 10 3F 71 CE 6C BE A5 6B CF 3C 61 3B D8 BD 69 1D 25 E0 20 04 00 40 00 03 216 | ``` 217 | 218 | ## Decoded Info 219 | 220 | ``` 221 | {(PacketCounter, 2 bytes, 18050), (SampleTimeFine, 4 bytes, 29686846), (Quaternion|Float|ENU, 16 bytes, (q0: 0.94455600, q1: -0.32308814, q2: 0.01374718, q3: -0.05691256)), (StatusWord, 4 bytes, 00000000010000000000000000000011)} 222 | ``` 223 | 224 | # Configuration Example 225 | 226 | Bytes _written_ to IMU to configure behaviours 227 | 228 | ``` 229 | FA FF 230 | MID 231 | SIZE 232 | PAYLOAD 233 | CRC 234 | ``` 235 | 236 | ## GoToConfig - 0x30 237 | 238 | 00 239 | D1 240 | 241 | ## SetStringOutputType - 0x8E 242 | 243 | 02 244 | 00 00 245 | 71 246 | 247 | ## SetOutputConfiguration - 0xC0 248 | 249 | 30 250 | 10 20 FF FF 10 60 FF FF 20 10 01 90 40 20 01 90 40 10 01 90 40 30 01 90 80 20 01 90 80 30 01 90 C0 20 00 64 08 10 00 0A 30 10 00 32 E0 20 FF FF 251 | 99 252 | 253 | ``` 254 | {(PacketCounter, Freq: 65535), (SampleTimeFine, Freq: 65535), (Quaternion|Float|ENU, Freq: 400), (Acceleration|Float, Freq: 400), (DeltaV|Float, Freq: 400), (FreeAcceleration|Float, Freq: 400), (RateOfTurn|Float, Freq: 400), (DeltaQ|Float, Freq: 400), (MagneticField|Float, Freq: 100), (Temperature|Float, Freq: 10), (BaroPressure, Freq: 50), (StatusWord, Freq: 65535)} 255 | ``` 256 | 257 | ## InitMT - 0x02 258 | 259 | 00 260 | FF 261 | 262 | ## RegConfiguration - 0x0C 263 | 264 | 00 265 | F5 266 | 267 | ## ReqFWRev - 0x12 268 | 269 | 00 270 | EF 271 | 272 | ## ReqAvailableFilterProfiles - 0x62 273 | 274 | 00 275 | 9F 276 | 277 | ## RegEMTS - 0x90 278 | 279 | 02 280 | 00 FF 281 | 70 282 | 283 | Unknown data format 284 | 285 | ## RegAvailableFilterProfiles - 0x62 286 | 287 | 00 288 | 9F 289 | 290 | ## GoToMeasurement - 0x10 291 | 292 | 00 293 | 294 | F1 295 | 296 | ## GoToConfig - 0x30 297 | 298 | 00 299 | D1 300 | 301 | ## InitMT - 0x02 302 | 303 | 00 304 | FF 305 | 306 | ## RegConfiguration - 0x0C 307 | 308 | 00 309 | F5 310 | 311 | ## ReqFWRef - 0x12 312 | 313 | 00 314 | EF 315 | 316 | ## ReqAvailableFilterProfiles - 0x62 317 | 318 | 00 319 | 9F 320 | 321 | ## RegEMTS - 0x90 322 | 323 | 02 324 | 00 FF 325 | 70 326 | 327 | ## RegAvailableFilterProfiles - 0x62 328 | 329 | 00 330 | 9F 331 | 332 | ## GoToMeasurement - 0x10 333 | 334 | 00 335 | F1 336 | 337 | # Configuration Response 338 | 339 | Bytes recieved during the config write process 340 | 341 | ## GoToConfigAck - 0x31 342 | 343 | 00 344 | D0 345 | 346 | ## StringOutputType - 0x8F 347 | 348 | 00 349 | 72 350 | 351 | ``` 352 | 353 | ``` 354 | 355 | ## OutputConfiguration - 0xC1 356 | 357 | 08 358 | 10 20 FF FF 10 60 FF FF 359 | 9C 360 | 361 | ``` 362 | {(PacketCounter, Freq: 65535), (SampleTimeFine, Freq: 65535)} 363 | ``` 364 | 365 | ## InitMTResults - 0x03 366 | 367 | 04 368 | 03 70 03 F8 369 | 8C 370 | 371 | ``` 372 | DeviceID: 037003F8 373 | ``` 374 | 375 | ## Configuration - 0x0D 376 | 377 | 76 378 | 03 70 03 F8 04 80 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 03 70 03 F8 00 00 00 00 00 00 00 01 00 27 01 08 02 49 05 01 379 | 9B 380 | 381 | ``` 382 | (DeviceID: 037003F8, Sample Period: 1152, OutputSkipFactor: 0, SyncInMode: 0, SyncInSkipFactor: 0, SyncInOffset: 0, (Year: 0, Month: 0, Day: 0), (Hour: 0, Min: 0, Sec: 0, Ms: 0), reserved: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00, reserved: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00, numDevices: 1, DeviceID: 037003F8, MTDataLength: 0, OutputMode: 0, OutputSettings: 1, reserved: 00 27 01 08 02 49 05 01) 383 | ``` 384 | 385 | ## FirmwareRev - 0x13 386 | 387 | 0B 388 | 01 08 02 00 00 00 25 00 01 15 34 389 | 69 390 | 391 | ``` 392 | (major: 1, minor: 8, revision: 2, buildnr: 37, svnrev: 70964) 393 | ``` 394 | 395 | ## AvailableFilterProfiles - 0x63 396 | 397 | 6E 398 | 27 0F 67 65 6E 65 72 61 6C 20 20 20 20 20 20 20 20 20 20 20 20 20 28 0F 68 69 67 68 5F 6D 61 67 5F 64 65 70 20 20 20 20 20 20 20 20 29 0F 64 79 6E 61 6D 69 63 20 20 20 20 20 20 20 20 20 20 20 20 20 2A 0F 6C 6F 77 5F 6D 61 67 5F 64 65 70 20 20 20 20 20 20 20 20 20 2B 0F 76 72 75 5F 67 65 6E 65 72 61 6C 20 20 20 20 20 20 20 20 20 399 | F1 400 | 401 | ``` 402 | {(FilterProfileType: 39, Version: 15, Label: general), (FilterProfileType: 40, Version: 15, Label: high_mag_dep), (FilterProfileType: 41, Version: 15, Label: dynamic), (FilterProfileType: 42, Version: 15, Label: low_mag_dep), (FilterProfileType: 43, Version: 15, Label: vru_general)} 403 | ``` 404 | 405 | ## EMTSData 0x91 406 | 407 | 05 28 408 | 96 50 5E 55 1E 23 37 7F 4F 5A 71 4D 11 1B 13 7D 1E 6C 76 5C 0F 09 5C 65 3C 4D 68 6D 5A 34 A5 60 76 25 6A 3B 84 31 32 58 38 4C 31 76 7A 5F 3A 39 71 33 3C 67 69 55 51 36 2D 49 2E 43 7E 3F 4D 50 E5 61 76 3E 36 7E 62 31 0B 68 D3 BB 51 52 8D A6 2E 54 44 28 34 2D 31 5D 48 25 5E 3E 41 7A 44 55 7A 50 5B 52 79 52 62 27 6B 21 56 6B 6E 7C 68 64 42 2A 77 4A 52 69 3C 35 35 3E 7E 56 29 4E 6D 65 6C 54 2F 44 73 55 7A 6E 29 35 6A 44 39 58 3D 33 5B 48 5B 61 40 2A 29 63 58 72 2E 4B 55 35 5A 61 2A 23 29 31 3E 4A 78 2F 59 48 5D 46 29 27 41 24 63 6F 3E 66 70 5D 67 60 3A 2A 3C 3A 2B 45 52 28 55 4D 51 61 4E 55 52 77 5B 67 38 6B 54 8D 7E 6A 4B 7E CA 56 3F 6E 53 2E 56 53 77 5E 52 7C 6A 41 60 23 5A 26 3A 2A 4C 56 7C 2F 29 7C 45 3C 4D 68 6F 5B 55 27 00 76 30 6F 39 7B 31 32 58 38 4C 31 76 7A 5F 3A 39 71 33 3C 67 68 55 51 36 2D 49 2F 43 7E 27 4D 50 65 65 76 C1 37 7E 62 31 2B 78 2C E5 95 51 72 59 2E 54 44 28 34 2D 31 5D 48 25 5E 3E 41 7A 44 56 78 51 5B 52 79 52 62 2D 6B 21 56 6F 6A 7D 68 64 42 2A 77 40 52 69 3C 35 35 3E 7E 56 29 4E 6D 65 6C 54 2F 44 73 55 7A 6E 29 35 6A 44 39 58 3D 33 5B 48 5B 61 40 2A 29 63 58 72 2E 4B 55 35 5A 61 2A 23 29 31 3E 4A 78 2F 59 48 5D 46 29 27 41 24 63 6F 3E 66 70 5D 67 60 3A 2A 3C 3A 2B 45 52 28 55 4D 51 61 4E 55 52 77 5B 67 38 6B 32 6B 7E 6A 49 7E 5A 57 3F 6E 53 2E 56 53 77 5E 52 7C 6A 41 60 23 5A 26 3A 2A 4C 56 7C 2F 29 7C 45 3C 4D 68 6F 5B 31 27 27 76 30 6F 39 7B 31 32 58 38 4C 31 76 7A 5F 3A 39 71 33 3C 67 68 55 51 36 2D 49 2F 43 7E 3F 4D 50 65 65 76 3E 36 7E 62 31 2B 78 2C 44 31 42 72 59 2E 54 44 28 34 2D 31 5D 48 25 5E 3E 41 7A 44 55 7A 50 5B 52 79 52 62 27 6B 21 56 6B 6E 7C 68 64 42 2A 77 4A 52 69 3C 35 35 3E 7E 56 29 4E 6D 65 6C 54 2F 44 73 55 7A 6E 29 35 6A 44 39 58 3D 33 5B 48 5B 61 40 2A 29 63 58 72 2E 4B 55 35 5A 61 B0 8F 3F 01 BF 11 0F 58 18 08 3C 85 FA 0C 24 77 02 2F 3E 66 70 5D 67 60 7E 6A 3C 3A AB 7A 52 28 55 4D 51 61 4E 55 52 77 5B 67 38 6B B2 54 7E 6A 49 7E 5A 57 3F 6E 53 2E 56 53 77 5E 52 7C 6A 41 60 23 5A 26 3A 2A 4C 56 FC 10 29 7C 45 3C 4D 68 6F 5B 31 27 27 76 30 6F 39 7B 60 9A A7 7E 31 1A 76 3D 72 2C 39 36 71 13 99 2E F0 4C C9 6B 9A 1F 43 39 BC 4D D0 5A 6F 83 3C 0A 94 D3 2C 17 FF 6E 95 8A 0B 70 D9 11 79 6D 1C 0F AA 5A 9A 72 B4 63 AA 7D 23 BF 2A 45 14 A4 2D 46 FA 92 11 50 F9 C5 26 52 82 0F D6 79 B7 88 35 6D B2 1F 02 89 B0 21 91 92 EA 31 BF D7 56 2E C4 4C 4B 0A A1 6A F2 31 94 7A 3A 28 E0 18 BB D0 5E 05 DA B0 22 1D FA 9A 75 10 D9 96 FA 94 0D DA D0 80 EF 60 0A 19 3C A2 E4 96 7A 03 7B A3 47 3B FB 4F F3 13 9D 01 75 B3 CD 10 C9 13 CA 6E DF 5E 31 70 59 ED FC E4 FA 24 1D 0C DA C7 23 72 F1 70 AB 04 8F 90 56 ED 92 BF FA 69 23 23 40 5C CB 6B CE 81 2A 4C 56 7C 2F 29 7C 45 3C 4D 68 6F 5B 31 27 27 76 30 6F 39 7B 31 32 58 F4 12 1D 41 9A 98 E0 0E 8E 9D CD DF F4 4F B3 0E 11 4E D3 F4 03 F9 8F E7 10 59 64 86 A1 19 BA 06 37 BB DD 72 43 F6 C4 67 40 C1 FD 16 8C 44 0C 62 44 EE 5B 01 C2 7D 45 6A ED 82 58 6D 79 52 62 27 6B 21 56 6B 6E 7C 68 64 42 2A 77 4A 52 69 A0 77 35 7E 62 12 46 5C 6E 5E 03 46 2C 7F 1C 47 79 55 23 E2 49 78 33 8F 1E 0F 51 9F 78 5D 40 2A 29 63 58 72 2E 4B 55 35 5A 61 2A 23 61 72 3E 4A 30 6C 59 48 15 05 29 27 41 24 63 6F 3E 66 70 5D 67 60 0E AA 8B 01 1F C5 E5 13 61 CD E6 5A 4E 55 52 77 5B 67 38 6B 32 6B 7E 6A 49 7E 5A 57 7F 68 05 3D 30 5B 33 48 50 26 A0 15 60 23 5A 26 3A 2A 4C 56 7C 2F 29 7C 45 3C 4D 68 BC D4 CE 61 D3 8B CF 29 F6 7A CE 74 80 70 33 0E 51 90 09 87 8D 27 57 01 9A 5A AD 6D 1A BB 36 10 B1 2C 03 F0 00 B8 FE CB CF 80 CD D9 B0 10 07 13 9F F4 80 31 60 71 97 07 F3 26 F2 72 06 38 91 60 82 84 54 F9 B3 00 8D 65 C7 76 CD DD F4 AF 84 E9 62 BA B3 D7 2F A2 C2 CD 6D 64 AD 85 DE 39 A1 C4 5C 18 11 54 74 41 7E 15 A3 3B 4E 41 84 A4 5D 51 E2 32 38 06 05 E7 00 60 65 40 2A 29 63 58 72 2E 4B 55 FB 5F 0D 22 23 21 5E 2C C9 43 40 4B CB 66 29 3B A4 7A 24 63 6F 3E 66 70 5D 67 60 3A 2A 3C 3A 2B 45 52 28 55 4D 51 61 4E 55 52 77 5B 67 38 6B 32 6B 7E 6A 49 7E 1A 57 3F 6E 53 2E 56 53 77 5E 52 7C 6A 41 60 23 5A 26 3A 2A 4C 56 7C 2F 29 7C 45 3C 4D 68 6F 5B 31 27 27 76 30 6F 39 7B 31 32 58 38 4D 409 | F0 410 | 411 | ``` 412 | 413 | ``` 414 | 415 | ## AvailableFilterProfiles - 0x63 416 | 417 | 6E 418 | 27 0F 67 65 6E 65 72 61 6C 20 20 20 20 20 20 20 20 20 20 20 20 20 28 0F 68 69 67 68 5F 6D 61 67 5F 64 65 70 20 20 20 20 20 20 20 20 29 0F 64 79 6E 61 6D 69 63 20 20 20 20 20 20 20 20 20 20 20 20 20 2A 0F 6C 6F 77 5F 6D 61 67 5F 64 65 70 20 20 20 20 20 20 20 20 20 2B 0F 76 72 75 5F 67 65 6E 65 72 61 6C 20 20 20 20 20 20 20 20 20 419 | D0 420 | 421 | ``` 422 | {(FilterProfileType: 39, Version: 15, Label: general), (FilterProfileType: 40, Version: 15, Label: high_mag_dep), (FilterProfileType: 41, Version: 15, Label: dynamic), (FilterProfileType: 42, Version: 15, Label: low_mag_dep), (FilterProfileType: 43, Version: 15, Label: vru_general)} 423 | ``` 424 | 425 | ## GotoMeasurementAck - 0x11 426 | 427 | 00 428 | 8C 429 | 430 | ## GotoConfigAck 431 | 432 | This is because the configuration seems to occur twice? 433 | -------------------------------------------------------------------------------- /examples/basics/basics.ino: -------------------------------------------------------------------------------- 1 | // This basic example uses 'Serial' which maps to hardware serial on an Uno. 2 | // For other hardware targets, modify the Serial usage as needed. 3 | 4 | // Use MTManager to configure outputs: Quaternion, Acceleration, pressure, temperature. 5 | // Modify the baud-rate in this sketch to match the configured rate in MTManager. 6 | 7 | #include "xsens_mti.h" // Main library 8 | #include "xsens_utility.h" // Needed for quaternion conversion function 9 | 10 | // Cache a copy of IMU data 11 | float temperature = 0; // in degress celcius 12 | uint32_t pressure = 0; // in pascals 13 | float euler_pry[3] = { 0 }; // -180 to +180 degress 14 | float acceleration[3] = { 0 }; // in m/s^2 15 | 16 | // Callback function used by the library 17 | void imu_callback( XsensEventFlag_t event, XsensEventData_t *mtdata ); 18 | 19 | // The library holds state and pointers to callbacks in this structure 20 | // - macro used to simplfiy instantiation, read write_config example for more 21 | xsens_interface_t imu_interface = XSENS_INTERFACE_RX( &imu_callback ); 22 | 23 | // Provide fallback LED pin if the selected board doesn't have one 24 | #ifndef LED_BUILTIN 25 | #define LED_BUILTIN 13 26 | #endif 27 | 28 | // Normal arduino setup/loop functions 29 | void setup( void ) 30 | { 31 | Serial.begin( 115200 ); 32 | pinMode( LED_BUILTIN, OUTPUT ); 33 | } 34 | 35 | void loop( void ) 36 | { 37 | // Inbound serial data needs to be parsed 38 | // When a valid packet is decoded, imu_callback() will fire 39 | while( Serial.available() > 0 ) 40 | { 41 | xsens_mti_parse( &imu_interface, Serial.read() ); 42 | } 43 | 44 | // Light goes high if the IMU pitch exceeds 10 degrees 45 | digitalWrite( LED_BUILTIN, (euler_pry[0] > 10.0f) ); 46 | } 47 | 48 | 49 | // Called when the library decoded an inbound packet 50 | // - If the packet was an MData2 frame (which contains packed motion data) 51 | // - the callback is called once for each sub-field 52 | void imu_callback( XsensEventFlag_t event, XsensEventData_t *mtdata ) 53 | { 54 | // The library provides a pointer to the a union containing decoded data 55 | // Use XsensEventFlag_t to determine what kind of packet arrived, 56 | // then copy data from the union as needed. 57 | 58 | // union 59 | // { 60 | // uint8_t u1; 61 | // uint16_t u2; 62 | // uint32_t u4; 63 | // float f4; 64 | // float f4x2[2]; 65 | // float f4x3[3]; 66 | // float f4x4[4]; 67 | // float f4x9[9]; 68 | // } data; 69 | 70 | switch( event ) 71 | { 72 | case XSENS_EVT_QUATERNION: 73 | // We use the type field of XsensEventData_t as a doublecheck before 74 | // blindly copying bytes out of the union 75 | 76 | if( mtdata->type == XSENS_EVT_TYPE_FLOAT4 ) 77 | { 78 | // Convert the quaternion to euler angles 79 | xsens_quaternion_to_euler( mtdata->data.f4x4, euler_pry ); 80 | 81 | // Convert from radians to degrees 82 | euler_pry[0] *= (180.0 / PI); 83 | euler_pry[1] *= (180.0 / PI); 84 | euler_pry[2] *= (180.0 / PI); 85 | } 86 | break; 87 | 88 | case XSENS_EVT_ACCELERATION: 89 | if( mtdata->type == XSENS_EVT_TYPE_FLOAT3 ) 90 | { 91 | acceleration[0] = mtdata->data.f4x3[0]; 92 | acceleration[1] = mtdata->data.f4x3[1]; 93 | acceleration[2] = mtdata->data.f4x3[2]; 94 | } 95 | break; 96 | 97 | case XSENS_EVT_PRESSURE: 98 | if( mtdata->type == XSENS_EVT_TYPE_U32 ) 99 | { 100 | pressure = mtdata->data.u4; 101 | } 102 | break; 103 | 104 | case XSENS_EVT_TEMPERATURE: 105 | if( mtdata->type == XSENS_EVT_TYPE_FLOAT ) 106 | { 107 | temperature = mtdata->data.f4; 108 | } 109 | break; 110 | } 111 | } -------------------------------------------------------------------------------- /examples/write_config/write_config.ino: -------------------------------------------------------------------------------- 1 | // Configure the library to support writing to the IMU, 2 | // used for configuration of baud-rate, output values, options, etc without MTManager 3 | 4 | #include "xsens_mti.h" // main library 5 | #include "xsens_utility.h" 6 | 7 | // Cache a copy of IMU data 8 | float acceleration[3] = { 0 }; // in m/s^2 9 | uint32_t last_measurement_ms = 0; 10 | uint32_t measurement_timer = 0; 11 | 12 | // Callback functions we'll use to catch IMU ack packets 13 | void handle_ack_gotoconfig( xsens_packet_buffer_t *packet ); 14 | void handle_ack_gotomeasurement( xsens_packet_buffer_t *packet ); 15 | void handle_ack_outputconfiguration( xsens_packet_buffer_t *packet ); 16 | void handle_ack_setoptionflags( xsens_packet_buffer_t *packet ); 17 | 18 | typedef enum { 19 | ACK_NONE = 0, 20 | ACK_CONFIG, 21 | ACK_MEASUREMENT, 22 | ACK_CONFIGURED, 23 | ACK_OPTIONFLAGS, 24 | } ACKFlags_t; 25 | 26 | ACKFlags_t ack_flag = ACK_NONE; 27 | 28 | 29 | void imu_callback( XsensEventFlag_t event, XsensEventData_t *mtdata ); 30 | void imu_send_data( uint8_t *data, uint16_t length ); 31 | 32 | // The library holds state and pointers to callbacks in this structure 33 | // - adding the imu_send_data ptr lets the library talk to the IMU 34 | xsens_interface_t imu_interface = XSENS_INTERFACE_RX_TX( &imu_callback, &imu_send_data ); 35 | 36 | 37 | // Simple state handling for configuration flow state-machine 38 | typedef enum { 39 | STATE_STARTUP = 0, 40 | STATE_REQUEST_CONFIG_MODE, 41 | STATE_ACK_CONFIG_MODE, 42 | STATE_SET_OUTPUT_CONFIG, 43 | STATE_ACK_OUTPUT_CONFIG, 44 | STATE_SET_OPTION_FLAGS, 45 | STATE_ACK_OPTION_FLAGS, 46 | STATE_REQUEST_MEASUREMENT_MODE, 47 | STATE_ACK_MEASUREMENT_MODE, 48 | } DemoStates_t; 49 | 50 | DemoStates_t demo_state = STATE_STARTUP; 51 | uint32_t demo_timer = 0; 52 | bool configure_fast = false; 53 | 54 | 55 | void setup( void ) 56 | { 57 | Serial.begin( 115200 ); // serial debug text 58 | Serial1.begin( 115200 ); // connected to IMU 59 | 60 | // Setup custom handler callbacks to catch acknowledgements from IMU 61 | xsens_mti_override_id_handler( MT_ACK_GOTOCONFIG, &handle_ack_gotoconfig ); 62 | xsens_mti_override_id_handler( MT_ACK_GOTOMEASUREMENT, &handle_ack_gotomeasurement ); 63 | xsens_mti_override_id_handler( MT_ACK_OUTPUTCONFIGURATION, &handle_ack_outputconfiguration ); 64 | xsens_mti_override_id_handler( MT_ACK_OPTIONFLAGS, &handle_ack_setoptionflags ); 65 | 66 | Serial.println("Configuration demo starting..."); 67 | 68 | demo_timer = millis(); 69 | measurement_timer = millis(); 70 | } 71 | 72 | void loop( void ) 73 | { 74 | while( Serial1.available() > 0 ) 75 | { 76 | xsens_mti_parse( &imu_interface, Serial1.read() ); 77 | } 78 | 79 | switch( demo_state ) 80 | { 81 | case STATE_STARTUP: 82 | // Wait 10 seconds before starting the configuration process 83 | if( millis() - demo_timer >= 10000 ) 84 | { 85 | demo_timer = millis(); 86 | demo_state = STATE_REQUEST_CONFIG_MODE; 87 | } 88 | else 89 | { 90 | if( millis() - measurement_timer >= 1000) 91 | { 92 | // Been too long since the last inbound packet? 93 | Serial.println("\nError: accel overdue?\n"); 94 | } 95 | else 96 | { 97 | // Acceleration packets have arrived recently, print the rate 98 | Serial.print("Accel rate: "); 99 | Serial.print( 1000 / last_measurement_ms ); 100 | Serial.println("Hz"); 101 | 102 | // TODO: non-blocking ratelimit of accel rate printout 103 | delay(5); 104 | } 105 | } 106 | break; 107 | 108 | case STATE_REQUEST_CONFIG_MODE: 109 | Serial.println("Requesting config mode..."); 110 | xsens_mti_request( &imu_interface, MT_GOTOCONFIG ); 111 | demo_state = STATE_ACK_CONFIG_MODE; 112 | break; 113 | 114 | case STATE_ACK_CONFIG_MODE: 115 | // Wait for GoToConfigAck to return 116 | // handle_gotoconfigack will fire when it does, and set our flag 117 | if( ack_flag == ACK_CONFIG) 118 | { 119 | Serial.println("IMU in config mode"); 120 | demo_state = STATE_SET_OUTPUT_CONFIG; 121 | ack_flag = ACK_NONE; 122 | } 123 | break; 124 | 125 | case STATE_SET_OUTPUT_CONFIG: 126 | { 127 | uint8_t output_rate = 10; // Hz acceleration message rate 128 | 129 | if( configure_fast ) 130 | { 131 | output_rate = 100; 132 | } 133 | 134 | configure_fast != configure_fast; 135 | 136 | 137 | Serial.println("Setting acceleration output to "); 138 | Serial.print(output_rate); 139 | Serial.println("Hz"); 140 | 141 | XsensFrequencyConfig_t settings[] = { 142 | { .id = XDI_PACKET_COUNTER, .frequency = 0xFFFF }, 143 | { .id = XDI_SAMPLE_TIME_FINE, .frequency = 0xFFFF }, 144 | { .id = XDI_ACCELERATION, .frequency = output_rate }, 145 | // { .id = XSENS_IDENTIFIER_FORMAT(XDI_QUATERNION, XSENS_FLOAT_FIXED1220, XSENS_COORD_ENU), .frequency = 100 }, 146 | }; 147 | 148 | xsens_mti_set_configuration( &imu_interface, settings, XSENS_ARR_ELEM(settings) ); 149 | demo_state = STATE_ACK_OUTPUT_CONFIG; 150 | } 151 | break; 152 | 153 | case STATE_ACK_OUTPUT_CONFIG: 154 | // Read configuration packet response 155 | // TODO: MDATA2 odd input handling? 156 | 157 | if( ack_flag == ACK_CONFIGURED) 158 | { 159 | Serial.println("IMU confirmed config..."); 160 | demo_state = STATE_SET_OPTION_FLAGS; 161 | ack_flag = ACK_NONE; 162 | } 163 | break; 164 | 165 | case STATE_SET_OPTION_FLAGS: 166 | { 167 | Serial.println("Setting option flags..."); 168 | 169 | // To enable a feature we send the corresponding flag bit in the SetFlags word 170 | // To disable the feature, we set the relevant bit in the ClearFlags word 171 | uint32_t set_flags = 0; 172 | uint32_t clear_flags = 0; 173 | 174 | // Multiple bits can be set at once 175 | XSENS_OPTION_FLAG_SET(set_flags, XSENS_OPTFLAG_DISABLE_AUTOSTORE); 176 | XSENS_OPTION_FLAG_SET(set_flags, XSENS_OPTFLAG_ENABLE_AHS); 177 | 178 | // We aren't clearing any flag right now, but the same approach is used 179 | // XSENS_OPTION_FLAG_SET(reset_flags, XSENS_OPTFLAG_DISABLE_AUTOSTORE); 180 | 181 | // Send them to the IMU 182 | xsens_mti_set_option_flags( &imu_interface, set_flags, clear_flags ); 183 | 184 | demo_state = STATE_ACK_OPTION_FLAGS; 185 | } 186 | break; 187 | 188 | case STATE_ACK_OPTION_FLAGS: 189 | // TODO: Optionally read response payload to double-check settings 190 | 191 | if( ack_flag == ACK_OPTIONFLAGS) 192 | { 193 | // We're done with configuration, we want to sample some data now 194 | Serial.println("IMU confirmed option flags..."); 195 | demo_state = STATE_REQUEST_MEASUREMENT_MODE; 196 | ack_flag = ACK_NONE; 197 | } 198 | break; 199 | 200 | case STATE_REQUEST_MEASUREMENT_MODE: 201 | Serial.println("Requesting measurement mode..."); 202 | xsens_mti_request( &imu_interface, MT_GOTOMEASUREMENT ); 203 | demo_state = STATE_ACK_MEASUREMENT_MODE; 204 | break; 205 | 206 | case STATE_ACK_MEASUREMENT_MODE: 207 | // Wait x in this mode before attempting to configure different settings 208 | 209 | if( ack_flag == ACK_MEASUREMENT) 210 | { 211 | uint32_t duration_ms = millis() - demo_timer; 212 | 213 | Serial.println("IMU back in measurement mode"); 214 | 215 | Serial.print(" - took "); 216 | Serial.print(duration_ms); 217 | Serial.println("ms\n"); 218 | 219 | // Go back to the start of the settings flow 220 | demo_state = STATE_STARTUP; 221 | ack_flag = ACK_NONE; 222 | demo_timer = millis(); 223 | } 224 | break; 225 | 226 | default: 227 | // Oops! 228 | demo_state = STATE_STARTUP; 229 | break; 230 | } 231 | 232 | } 233 | 234 | // Command ACK callback functions 235 | void handle_ack_gotoconfig( xsens_packet_buffer_t *packet ) 236 | { 237 | ack_flag = ACK_CONFIG; 238 | } 239 | 240 | void handle_ack_gotomeasurement( xsens_packet_buffer_t *packet ) 241 | { 242 | ack_flag = ACK_MEASUREMENT; 243 | } 244 | 245 | void handle_ack_outputconfiguration( xsens_packet_buffer_t *packet ) 246 | { 247 | ack_flag = ACK_CONFIGURED; 248 | } 249 | 250 | void handle_ack_setoptionflags( xsens_packet_buffer_t *packet ) 251 | { 252 | ack_flag = ACK_OPTIONFLAGS; 253 | } 254 | 255 | // MData2 callback for IMU events 256 | void imu_callback( XsensEventFlag_t event, XsensEventData_t *mtdata ) 257 | { 258 | switch( event ) 259 | { 260 | case XSENS_EVT_ACCELERATION: 261 | if( mtdata->type == XSENS_EVT_TYPE_FLOAT3 ) 262 | { 263 | acceleration[0] = mtdata->data.f4x3[0]; 264 | acceleration[1] = mtdata->data.f4x3[1]; 265 | acceleration[2] = mtdata->data.f4x3[2]; 266 | } 267 | 268 | last_measurement_ms = millis() - measurement_timer; 269 | measurement_timer = millis(); 270 | break; 271 | } 272 | } 273 | 274 | // The library calls this function to send packets to the IMU 275 | void imu_send_data( uint8_t *data, uint16_t len ) 276 | { 277 | Serial1.write( data, len ); 278 | } -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | # xsens_mti ArduinoIDE Colour Map 2 | 3 | # Datatypes (KEYWORD1) 4 | 5 | xsens_packet_buffer_t KEYWORD1 6 | xsens_parser_state_t KEYWORD1 7 | xsens_interface_t KEYWORD1 8 | message_handler_ref_t KEYWORD1 9 | XsensEventFlag_t KEYWORD1 10 | XsensEventDataType_t KEYWORD1 11 | XsensEventData_t KEYWORD1 12 | 13 | XSENS_INTERFACE_RX KEYWORD1 14 | XSENS_INTERFACE_RX_TX KEYWORD1 15 | 16 | # Methods and Functions (KEYWORD2) 17 | 18 | xsens_mti_parse_buffer KEYWORD2 19 | xsens_mti_parse KEYWORD2 20 | xsens_mti_reset_parser KEYWORD2 21 | xsens_mti_override_id_handler KEYWORD2 22 | xsens_mti_send KEYWORD2 23 | xsens_mti_request KEYWORD2 24 | xsens_mti_set_baudrate KEYWORD2 25 | xsens_mti_reset_orientation KEYWORD2 26 | 27 | xsens_coalesce_16BE_16LE KEYWORD2 28 | xsens_coalesce_32BE_32LE KEYWORD2 29 | xsens_coalesce_32BE_F32LE KEYWORD2 30 | xsens_quaternion_to_euler KEYWORD2 31 | 32 | # Constants (LITERAL1) 33 | 34 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=xsens_mti 2 | version=0.1.1 3 | author=Scott Rapson 4 | url=https://github.com/Scottapotamas/xsens-mti 5 | license=Apache2 6 | maintainer=Scott Rapson 7 | category=Communication 8 | sentence=Portable, unit-tested, from-scratch implementation for serial comms with xsens MTi AHRS modules. 9 | paragraph=The parser is hardware agnostic, with a design philosophy around structured data and callback functions when valid packets are decoded. 10 | architectures=* 11 | includes=xsens_mti.h -------------------------------------------------------------------------------- /project.yml: -------------------------------------------------------------------------------- 1 | --- 2 | # Notes: 3 | # Sample project C code is not presently written to produce a release artifact. 4 | # As such, release build options are disabled. 5 | # This sample, therefore, only demonstrates running a collection of unit tests. 6 | 7 | :project: 8 | :use_exceptions: FALSE 9 | :use_test_preprocessor: TRUE 10 | :use_auxiliary_dependencies: TRUE 11 | :build_root: build 12 | # :release_build: TRUE 13 | :test_file_prefix: test_ 14 | :which_ceedling: gem 15 | :ceedling_version: 0.32.0 16 | :default_tasks: 17 | - test:all 18 | 19 | #:test_build: 20 | # :use_assembly: TRUE 21 | 22 | #:release_build: 23 | # :output: MyApp.out 24 | # :use_assembly: FALSE 25 | 26 | :environment: 27 | 28 | :extension: 29 | :executable: .out 30 | 31 | :paths: 32 | :test: 33 | - +:test/** 34 | :include: 35 | - src/** 36 | :source: 37 | - src/** 38 | :libraries: [] 39 | 40 | :defines: 41 | # in order to add common defines: 42 | # 1) remove the trailing [] from the :common: section 43 | # 2) add entries to the :common: section (e.g. :test: has TEST defined) 44 | :common: &common_defines 45 | - UNITY_INCLUDE_DOUBLE 46 | :test: 47 | - *common_defines 48 | - TEST 49 | :test_preprocess: 50 | - *common_defines 51 | - TEST 52 | 53 | :cmock: 54 | :mock_prefix: mock_ 55 | :when_no_prototypes: :warn 56 | :enforce_strict_ordering: TRUE 57 | :plugins: 58 | - :ignore 59 | - :callback 60 | :treat_as: 61 | uint8: HEX8 62 | uint16: HEX16 63 | uint32: UINT32 64 | int8: INT8 65 | bool: UINT8 66 | 67 | # Add -gcov to the plugins list to make sure of the gcov plugin 68 | # You will need to have gcov and gcovr both installed to make it work. 69 | # For more information on these options, see docs in plugins/gcov 70 | :gcov: 71 | :reports: 72 | - HtmlDetailed 73 | :gcovr: 74 | :html_medium_threshold: 75 75 | :html_high_threshold: 90 76 | 77 | #:tools: 78 | # Ceedling defaults to using gcc for compiling, linking, etc. 79 | # As [:tools] is blank, gcc will be used (so long as it's in your system path) 80 | # See documentation to configure a given toolchain for use 81 | 82 | # LIBRARIES 83 | # These libraries are automatically injected into the build process. Those specified as 84 | # common will be used in all types of builds. Otherwise, libraries can be injected in just 85 | # tests or releases. These options are MERGED with the options in supplemental yaml files. 86 | :libraries: 87 | :placement: :end 88 | :flag: "-l${1}" 89 | :path_flag: "-L ${1}" 90 | :system: [m] # for example, you might list 'm' to grab the math library 91 | :test: [] 92 | :release: [] 93 | 94 | :plugins: 95 | :load_paths: 96 | - "#{Ceedling.load_path}" 97 | :enabled: 98 | - stdout_pretty_tests_report 99 | - module_generator 100 | - raw_output_report 101 | - gcov 102 | -------------------------------------------------------------------------------- /src/xsens_constants.h: -------------------------------------------------------------------------------- 1 | #ifndef XSENS_CONSTANTS_H 2 | #define XSENS_CONSTANTS_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "stdbool.h" 9 | #include "stdint.h" 10 | 11 | // Library Related Abstractions 12 | 13 | #ifndef XSENS_PAYLOAD_BUFFER_SIZE 14 | // Library users can define their own payload size 15 | // Default to the largest possible packet size 16 | #define XSENS_PAYLOAD_BUFFER_SIZE 2048 17 | #endif 18 | 19 | 20 | // The basic concept of a valid decoded packet 21 | typedef struct 22 | { 23 | uint8_t message_id; 24 | uint16_t length; 25 | uint8_t payload[XSENS_PAYLOAD_BUFFER_SIZE]; // xsens 'extended packets' can be up to 2kB 26 | } xsens_packet_buffer_t; 27 | 28 | #define XSENS_PACKET_BUF_EMPTY { .message_id = 0, .length = 0, .payload = {0} } 29 | 30 | // Event flags sent to application-level code 31 | typedef enum 32 | { 33 | XSENS_EVT_TEMPERATURE = 0, 34 | XSENS_EVT_UTC_TIME, 35 | XSENS_EVT_PACKET_COUNT, 36 | XSENS_EVT_TIME_FINE, 37 | XSENS_EVT_TIME_COARSE, 38 | XSENS_EVT_QUATERNION, 39 | XSENS_EVT_EULER, 40 | XSENS_EVT_ROT_MATRIX, 41 | XSENS_EVT_PRESSURE, 42 | XSENS_EVT_DELTA_V, 43 | XSENS_EVT_DELTA_Q, 44 | XSENS_EVT_ACCELERATION, 45 | XSENS_EVT_FREE_ACCELERATION, 46 | XSENS_EVT_ACCELERATION_HR, 47 | XSENS_EVT_RATE_OF_TURN, 48 | XSENS_EVT_RATE_OF_TURN_HR, 49 | XSENS_EVT_GNSS_PVT_PULSE, 50 | XSENS_EVT_RAW_ACC_GYRO_MAG_TEMP, 51 | XSENS_EVT_RAW_GYRO_TEMP, 52 | XSENS_EVT_MAGNETIC, 53 | XSENS_EVT_STATUS_BYTE, 54 | XSENS_EVT_STATUS_WORD, 55 | XSENS_EVT_DEVICE_ID, 56 | XSENS_EVT_LOCATION_ID, 57 | XSENS_EVT_POSITION_ECEF, 58 | XSENS_EVT_LAT_LON, 59 | XSENS_EVT_ALTITUDE_ELLIPSOID, 60 | XSENS_EVT_VELOCITY_XYZ, 61 | } XsensEventFlag_t; 62 | 63 | // Enum describes possible types sent to user in the event data union 64 | typedef enum 65 | { 66 | XSENS_EVT_TYPE_NONE = 0, 67 | XSENS_EVT_TYPE_U8, 68 | XSENS_EVT_TYPE_U16, 69 | XSENS_EVT_TYPE_U32, 70 | XSENS_EVT_TYPE_FLOAT = 10, 71 | XSENS_EVT_TYPE_FLOAT2, 72 | XSENS_EVT_TYPE_FLOAT3, 73 | XSENS_EVT_TYPE_FLOAT4, 74 | XSENS_EVT_TYPE_FLOAT9, 75 | XSENS_EVT_TYPE_1220FP = 20, 76 | XSENS_EVT_TYPE_1220FP2, 77 | XSENS_EVT_TYPE_1220FP3, 78 | XSENS_EVT_TYPE_1220FP4, 79 | XSENS_EVT_TYPE_1220FP9, 80 | XSENS_EVT_TYPE_1632FP = 30, 81 | XSENS_EVT_TYPE_1632FP2, 82 | XSENS_EVT_TYPE_1632FP3, 83 | XSENS_EVT_TYPE_1632FP4, 84 | XSENS_EVT_TYPE_1632FP9, 85 | XSENS_EVT_TYPE_DOUBLE = 40, 86 | XSENS_EVT_TYPE_DOUBLE2, 87 | XSENS_EVT_TYPE_DOUBLE3, 88 | XSENS_EVT_TYPE_DOUBLE4, 89 | XSENS_EVT_TYPE_DOUBLE9, 90 | } XsensEventDataType_t; 91 | 92 | // Enum describes possible types sent to user in the event data union 93 | typedef enum 94 | { 95 | XSENS_EVT_ENU = 0x0, 96 | XSENS_EVT_NED = 0x4, 97 | XSENS_EVT_NWU = 0x8, 98 | } XsensEventTangentPlaneFormat_t; 99 | 100 | // Unionised data sent to user callback 101 | typedef struct 102 | { 103 | XsensEventDataType_t type; 104 | XsensEventTangentPlaneFormat_t coord_ref; 105 | union 106 | { 107 | uint8_t u1; 108 | uint16_t u2; 109 | uint32_t u4; 110 | 111 | // Single precision 112 | float f4; 113 | float f4x2[2]; 114 | float f4x3[3]; 115 | float f4x4[4]; 116 | float f4x9[9]; 117 | 118 | // Fixed-point 12.20 119 | int32_t fp1220; 120 | int32_t fp1220x2[2]; 121 | int32_t fp1220x3[3]; 122 | int32_t fp1220x4[4]; 123 | int32_t fp1220x9[9]; 124 | 125 | // Fixed-point 16.32 126 | int64_t fp1632; 127 | int64_t fp1632x2[2]; 128 | int64_t fp1632x3[3]; 129 | int64_t fp1632x4[4]; 130 | int64_t fp1632x9[9]; 131 | 132 | // Double precision 133 | double f8; 134 | double f8x2[2]; 135 | double f8x3[3]; 136 | double f8x4[4]; 137 | double f8x9[9]; 138 | 139 | } data; 140 | } XsensEventData_t; 141 | 142 | // Userspace callback to notify application layer code of a relevant event 143 | // These events are accompanied by output-ready decoded data 144 | typedef void ( *callback_event_t )( XsensEventFlag_t, XsensEventData_t * ); 145 | 146 | // Callback to userspace serial write function 147 | // Uses args for a uint8_t buffer of bytes, with a uint16_t size value 148 | typedef void ( *callback_data_out_t )( uint8_t *, uint16_t ); 149 | 150 | // Specific Values and Structures for xsens handling 151 | 152 | #define PREAMBLE_BYTE 0xFA 153 | #define ADDRESS_BYTE 0xFF 154 | 155 | #define LENGTH_EXTENDED_MODE 0xFF 156 | #define LENGTH_NONE 0x00 157 | 158 | enum XSENS_ERRORCODE 159 | { 160 | ERROR_PERIOD_INVALID = 0x03, 161 | ERROR_MESSAGE_INVALID = 0x04, 162 | ERROR_TIMER_OVERFLOW = 0x30, 163 | ERROR_BAUDRATE = 0x20, 164 | ERROR_PARAMETER_INVALID = 0x21, 165 | ERROR_DEVICE = 0x28, 166 | }; 167 | 168 | // Message Identifiers for MTi->HOST 169 | enum XSENS_MESSAGE_ID_INBOUND 170 | { 171 | // Wakeup and State 172 | MT_WAKEUP = 0x3E, 173 | MT_ACK_GOTOCONFIG = 0x31, 174 | MT_ACK_GOTOMEASUREMENT = 0x11, 175 | MT_ACK_RESET = 0x41, 176 | 177 | // Informational Messages 178 | MT_DEVICEID = 0x01, 179 | MT_PRODUCTCODE = 0x1D, 180 | MT_HARDWAREVERSION = 0x1F, 181 | MT_FIRMWAREREV = 0x13, 182 | MT_ERROR = 0x42, 183 | 184 | // Device Specific Messages 185 | MT_ACK_BAUDRATE = 0x19, 186 | MT_ACK_SELFTEST = 0x25, 187 | MT_ACK_GNSSPLATFORM = 0x77, 188 | MT_ACK_ERRORMODE = 0xDB, // DEPRECATED by xsens 2020.A 189 | MT_ACK_TRANSMITDELAY = 0xDD, 190 | MT_ACK_OPTIONFLAGS = 0x49, 191 | MT_ACK_LOCATIONID = 0x85, 192 | 193 | // Synchronisation Messages 194 | MT_ACK_SYNCSETTINGS = 0x2D, 195 | 196 | // Configuration Messages 197 | MT_CONFIGURATION = 0x0D, 198 | MT_ACK_PERIOD = 0x05, 199 | MT_ACK_EXTOUTPUTMODE = 0x87, 200 | MT_ACK_OUTPUTCONFIGURATION = 0xC1, 201 | MT_ACK_STRINGOUTPUTTYPES = 0x8F, 202 | MT_ACK_OUTPUTMODE = 0xD1, // DEPRECATED by xsens rev w 203 | MT_ACK_OUTPUTSETTINGS = 0xD3, // DEPRECATED by xsens rev w 204 | 205 | // Data Messages 206 | MT_MTDATA = 0x32, 207 | MT_MTDATA2 = 0x36, 208 | MT_ACK_RESETORIENTATION = 0xA5, 209 | MT_UTCTIME = 0x61, 210 | MT_ACK_AVAILABLEFILTERPROFILES = 0x63, 211 | MT_ACK_FILTERPROFILE = 0x65, 212 | MT_ACK_GNSSLEVERARM = 0x69, 213 | MT_ACK_LATLONALT = 0x6F, 214 | MT_ACK_NOROTATION = 0x23, 215 | MT_ACK_ICCCOMMAND = 0x75, 216 | 217 | // Not in xsens manual summary tables 218 | MT_WARNING = 0x43, 219 | MT_ACK_INITIALHEADING = 0xD7, 220 | MT_ACK_FORWARDGNSSDATA = 0xE3, 221 | }; 222 | 223 | // Message Identifiers for Host->MTi 224 | enum XSENS_MESSAGE_ID_OUTBOUND 225 | { 226 | // Wakeup and State 227 | MT_ACK_WAKEUP = 0x3F, 228 | MT_GOTOCONFIG = 0x30, 229 | MT_GOTOMEASUREMENT = 0x10, 230 | MT_RESET = 0x40, 231 | 232 | // Informational Messages 233 | MT_REQDID = 0x00, 234 | MT_REQPRODUCTCODE = 0x1C, 235 | MT_REQHARDWAREVERSION = 0x1E, 236 | MT_REQFWREV = 0x12, 237 | 238 | // Device Specific Messages 239 | MT_RESTOREFACTORYDEF = 0x0E, 240 | MT_REQBAUDRATE = 0x18, 241 | MT_SETBAUDRATE = 0x18, 242 | MT_RUNSELFTEST = 0x24, 243 | MT_REQGNSSPLATFORM = 0x76, 244 | MT_SETGNSSPLATFORM = 0x76, 245 | MT_REQERRORMODE = 0xDA, 246 | MT_SETERRORMODE = 0xDA, 247 | MT_REQTRANSMITDELAY = 0xDC, 248 | MT_SETTRANSMITDELAY = 0xDC, 249 | MT_REQOPTIONFLAGS = 0x48, 250 | MT_SETOPTIONFLAGS = 0x48, 251 | MT_REQLOCATIONID = 0x84, 252 | MT_SETLOCATIONID = 0x84, 253 | 254 | // Synchronisation Messages 255 | MT_REQSYNCSETTINGS = 0x2C, 256 | MT_SETSYNCSETTINGS = 0x2C, 257 | 258 | // Configuration Messages 259 | MT_REQCONFIGURATION = 0x0C, 260 | MT_REQPERIOD = 0x04, 261 | MT_SETPERIOD = 0x04, 262 | MT_REQEXTOUTPUTMODE = 0x86, 263 | MT_SETEXTOUTPUTMODE = 0x86, 264 | MT_REQOUTPUTCONFIGURATION = 0xC0, 265 | MT_SETOUTPUTCONFIGURATION = 0xC0, 266 | MT_REQSTRINGOUTPUTTYPES = 0x8E, 267 | MT_SETSTRINGOUTPUTTYPES = 0x8E, 268 | MT_REQALIGNMENTROTATION = 0xEC, 269 | MT_SETALIGNMENTROTATION = 0xEC, 270 | MT_REQOUTPUTMODE = 0xD0, // DEPRECATED by xsens rev w 271 | MT_SETOUTPUTMODE = 0xD0, // DEPRECATED by xsens rev w 272 | MT_REQOUTPUTSETTINGS = 0xD2, // DEPRECATED by xsens rev w 273 | MT_SETOUTPUTSETTINGS = 0xD2, // DEPRECATED by xsens rev w 274 | 275 | // Data Messages 276 | MT_REQDATA = 0x34, 277 | MT_RESETORIENTATION = 0xA4, 278 | MT_REQUTCTIME = 0x60, 279 | MT_SETUTCTIME = 0x60, 280 | MT_ADJUSTUTCTIME = 0xA8, 281 | MT_REQAVAILABLEFILTERPROFILES = 0x62, 282 | MT_REQFILTERPROFILE = 0x64, 283 | MT_SETFILTERPROFILE = 0x64, 284 | MT_REQGNSSLEVERARM = 0x68, 285 | MT_SETGNSSLEVERARM = 0x68, 286 | MT_REQLATLONALT = 0x6E, 287 | MT_SETLATLONALT = 0x6E, 288 | MT_SETNOROTATION = 0x22, 289 | MT_ICCCOMMAND = 0x74, 290 | 291 | // Not in xsens manual summary tables 292 | MT_REQOUTPUTSKIPFACTOR = 0xD4, 293 | MT_SETOUTPUTSKIPFACTOR = 0xD4, 294 | MT_REQSETPORTCONFIG = 0x8C, 295 | MT_REQSETCANOUTPUTCONFIG = 0xE8, 296 | MT_REQSETCANCONFIG = 0xE6, 297 | MT_SETINITIALHEADING = 0xD6, 298 | MT_FORWARDGNSSDATA = 0xE2, 299 | }; 300 | 301 | enum XSENS_DEVICE_MODE 302 | { 303 | MODE_WAKEUP = 0, 304 | MODE_CONFIG, 305 | MODE_MEASUREMENT, 306 | }; 307 | 308 | typedef enum 309 | { 310 | XSENS_BAUD_4800 = 0x0B, 311 | XSENS_BAUD_9600 = 0x09, 312 | XSENS_BAUD_11400 = 0x08, 313 | XSENS_BAUD_19200 = 0x07, 314 | XSENS_BAUD_28800 = 0x06, 315 | XSENS_BAUD_38400 = 0x05, 316 | XSENS_BAUD_57600 = 0x04, 317 | XSENS_BAUD_76600 = 0x03, 318 | XSENS_BAUD_115200 = 0x02, 319 | XSENS_BAUD_230400 = 0x01, 320 | XSENS_BAUD_460800 = 0x00, 321 | XSENS_BAUD_921600 = 0x80, // two valid bytes exist for this setting 322 | // XSENS_BAUD_921600 = 0xA0, 323 | XSENS_BAUD_2000000 = 0x0C, 324 | XSENS_BAUD_3686400 = 0x0E, 325 | XSENS_BAUD_4000000 = 0x0D, 326 | } XsensBaudSetting_t; 327 | 328 | typedef enum 329 | { 330 | XSENS_RESET_ORIENTATION_RESERVED = 0x02, 331 | 332 | XSENS_ORIENTATION_STORE = 0x00, 333 | XSENS_ORIENTATION_HEADING_RESET = 0x01, 334 | XSENS_ORIENTATION_INCLINATION_RESET = 0x03, 335 | XSENS_ORIENTATION_ALIGNMENT_RESET = 0x04, 336 | XSENS_ORIENTATION_HEADING_DEFAULT = 0x05, 337 | XSENS_ORIENTATION_INCLINATION_DEFAULT = 0x06, 338 | XSENS_ORIENTATION_ALIGNMENT_DEFAULT = 0x07, 339 | } XsensOrientationSetting_t; 340 | 341 | enum XSENS_OPTION_FLAGS 342 | { 343 | XSENS_OPTFLAG_DISABLE_AUTOSTORE = 0x01, 344 | XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT = 0x02, 345 | XSENS_OPTFLAG_ENABLE_BEIDOU = 0x04, 346 | XSENS_OPTFLAG_RESERVED = 0x08, 347 | XSENS_OPTFLAG_ENABLE_AHS = 0x10, 348 | XSENS_OPTFLAG_ENABLE_ORIENTATION_SMOOTHER = 0x20, 349 | XSENS_OPTFLAG_ENABLE_CONFIG_BUS_ID = 0x40, 350 | XSENS_OPTFLAG_ENABLE_IN_RUN_COMPASS_CAL = 0x80, 351 | XSENS_OPTFLAG_ENABLE_STARTUP_CONFIG_MSG = 0x0200, 352 | XSENS_OPTFLAG_ENABLE_COLD_FILTER_RESET = 0x0400, 353 | XSENS_OPTFLAG_ENABLE_POSITION_VEL_SMOOTHER = 0x0800, 354 | XSENS_OPTFLAG_ENABLE_CONTINUOUS_ZRU = 0x1000, 355 | }; 356 | 357 | #define XSENS_OPTION_FLAG_IS_SET( OPTIONS, FLAG ) (((OPTIONS) & (FLAG)) == (FLAG)) 358 | #define XSENS_OPTION_FLAG_IS_ANY_SET(OPTIONS) ((OPTIONS) != 0) 359 | #define XSENS_OPTION_FLAG_SET(OPTIONS, FLAG) ((OPTIONS) |= (FLAG)) 360 | 361 | enum XSENS_COORDINATE_SYSTEM 362 | { 363 | XSENS_COORD_ENU = 0x0, 364 | XSENS_COORD_NED = 0x4, 365 | XSENS_COORD_NWU = 0x8, 366 | }; 367 | 368 | enum XSENS_FLOAT_TYPE 369 | { 370 | XSENS_FLOAT_SINGLE = 0x0, 371 | XSENS_FLOAT_FIXED1220 = 0x1, 372 | XSENS_FLOAT_FIXED1632 = 0x2, 373 | XSENS_FLOAT_DOUBLE = 0x3, 374 | }; 375 | 376 | // MData2 Field Identifiers 377 | enum XDA_TYPE_IDENTIFIER 378 | { 379 | XDI_UTC_TIME = 0x1010, 380 | XDI_PACKET_COUNTER = 0x1020, 381 | XDI_SAMPLE_TIME_FINE = 0x1060, 382 | XDI_SAMPLE_TIME_COARSE = 0x1070, 383 | 384 | XDI_TEMPERATURE = 0x0810, 385 | 386 | XDI_QUATERNION = 0x2010, 387 | XDI_ROTATION_MATRIX = 0x2020, 388 | XDI_EULER_ANGLES = 0x2030, 389 | 390 | XDI_BARO_PRESSURE = 0x3010, 391 | 392 | XDI_DELTA_V = 0x4010, 393 | XDI_ACCELERATION = 0x4020, 394 | XDI_FREE_ACCELERATION = 0x4030, 395 | XDI_ACCELERATION_HIGH_RATE = 0x4040, 396 | 397 | XDI_ALTITUDE_ELLIPSOID = 0x5020, 398 | XDI_POSITION_ECEF = 0x5030, 399 | XDI_LAT_LON = 0x5040, 400 | XDI_GNSS_PVT_DATA = 0x7010, 401 | XDI_GNSS_SAT_INFO = 0x7020, 402 | XDI_GNSS_PVT_PULSE = 0x7030, 403 | 404 | XDI_RATE_OF_TURN = 0x8020, 405 | XDI_DELTA_Q = 0x8030, 406 | XDI_RATE_OF_TURN_HIGH_RATE = 0x8040, 407 | 408 | XDI_RAW_ACC_GYRO_MAG_TEMP = 0xA010, 409 | XDI_RAW_GYRO_TEMP = 0xA020, 410 | 411 | XDI_MAGNETIC_FIELD = 0xC020, 412 | 413 | XDI_VELOCITY_XYZ = 0xD010, 414 | 415 | XDI_STATUS_BYTE = 0xE010, 416 | XDI_STATUS_WORD = 0xE020, 417 | XDI_DEVICE_ID = 0xE080, 418 | XDI_LOCATION_ID = 0xE090, 419 | 420 | }; 421 | 422 | // Helper to modify identifier for precision and coordinate frame settings 423 | #define XSENS_IDENTIFIER_FORMAT( ID, PRECISION, COORDINATE_SYSTEM ) ( (ID) | ( (PRECISION) | (COORDINATE_SYSTEM) ) ) 424 | #define XSENS_IDENTIFIER_FORMAT_SIMPLIFY( ID ) ( (ID) & 0xFFF0 ) 425 | #define XSENS_IDENTIFIER_FORMAT_GET_PRECISION( ID ) ( (ID) & 0x0003 ) 426 | #define XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( ID ) ( (ID) & 0x000C ) 427 | 428 | typedef struct 429 | { 430 | uint16_t id; 431 | uint16_t frequency; 432 | } XsensFrequencyConfig_t; 433 | 434 | // 32-bit status structure 435 | // Casting to bitfield is end-users responsibility. 436 | // union XDI_STATUS32_UNION status; 437 | // status.word = xsens_coalesce_32BE_32LE(&output->payload[0]); 438 | // printf("filterOK: %d\n", status.bitfield.filter_valid); 439 | typedef struct 440 | { 441 | unsigned int self_test : 1; 442 | unsigned int filter_valid : 1; 443 | unsigned int gnss_fix : 1; 444 | unsigned int no_rotation_status : 2; 445 | unsigned int representative_motion : 1; 446 | unsigned int clock_bias_estimation : 1; 447 | unsigned int reserved_0 : 1; 448 | unsigned int clip_acc_x : 1; 449 | unsigned int clip_acc_y : 1; 450 | unsigned int clip_acc_z : 1; 451 | unsigned int clip_gyro_x : 1; 452 | unsigned int clip_gyro_y : 1; 453 | unsigned int clip_gyro_z : 1; 454 | unsigned int clip_mag_x : 1; 455 | unsigned int clip_mag_y : 1; 456 | unsigned int clip_mag_z : 1; 457 | unsigned int reserved_1 : 2; 458 | unsigned int clipping : 1; 459 | unsigned int reserved_2 : 1; 460 | unsigned int sync_in_mark : 1; 461 | unsigned int sync_out_mark : 1; 462 | unsigned int filter_mode : 3; 463 | unsigned int has_gnss_pulse : 1; 464 | unsigned int rtk_status : 2; 465 | unsigned int reserved_3 : 3; 466 | } xdi_status32_t; 467 | 468 | union XDI_STATUS32_UNION 469 | { 470 | uint32_t word; 471 | xdi_status32_t bitfield; 472 | }; 473 | 474 | #ifdef __cplusplus 475 | } 476 | #endif 477 | 478 | #endif 479 | // end XSENS_CONSTANTS_H -------------------------------------------------------------------------------- /src/xsens_mdata2.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "xsens_mdata2.h" 4 | #include "xsens_utility.h" 5 | 6 | // MData2 packets have a series of smaller structures of data 7 | // This handler walks through the buffer, identifies the XDA type from two bytes 8 | // Then applies relevant conversions back into native types/structures as necessary 9 | // Packets don't have a fixed number of child elements 10 | void xsens_mdata2_process( xsens_packet_buffer_t *packet, callback_event_t evt_cb ) 11 | { 12 | mdata2_parser_state_t md2_state = XDI_PARSE_ID_B1; 13 | mdata2_packet_t output = { 0 }; 14 | uint8_t bytes_consumed = 0; 15 | 16 | // Walk through the packet and run a tiny statemachine 17 | // to parse the sub-fields 18 | for( uint16_t i = 0; i < packet->length; i++ ) 19 | { 20 | switch( md2_state ) 21 | { 22 | case XDI_PARSE_ID_B1: 23 | // High byte 24 | output.id = ( uint16_t )( (uint16_t)packet->payload[i] << 8u ); 25 | md2_state = XDI_PARSE_ID_B2; 26 | break; 27 | 28 | case XDI_PARSE_ID_B2: 29 | // Low byte 30 | output.id |= ( uint16_t )( (uint16_t)packet->payload[i] ); 31 | md2_state = XDI_PARSE_LENGTH; 32 | break; 33 | 34 | case XDI_PARSE_LENGTH: 35 | // Length is one byte 36 | output.length = packet->payload[i]; 37 | md2_state = XDI_PARSE_DATA; 38 | break; 39 | 40 | case XDI_PARSE_DATA: 41 | // Copy data across 42 | output.payload[bytes_consumed] = packet->payload[i]; 43 | bytes_consumed++; 44 | 45 | // Once the field's data been copied to our sub-buffer, 46 | // handle it 47 | if( bytes_consumed >= output.length ) 48 | { 49 | // Using the isolated field, search for matching XID 50 | // and then convert payloads to LE & structured data 51 | xsens_mdata2_decode_field( &output, evt_cb ); 52 | 53 | // Cleanup our state before parsing remaining fields 54 | md2_state = XDI_PARSE_ID_B1; 55 | bytes_consumed = 0; 56 | memset( &output, 0, sizeof( mdata2_packet_t ) ); 57 | } 58 | break; 59 | 60 | default: 61 | // Case switch should be an exhaustive match? 62 | // Reset state? 63 | md2_state = XDI_PARSE_ID_B1; 64 | bytes_consumed = 0; 65 | memset( &output, 0, sizeof( mdata2_packet_t ) ); 66 | break; 67 | } 68 | } 69 | 70 | // Finished MData2 parsing in payload 71 | } 72 | 73 | typedef struct 74 | { 75 | uint16_t xid; 76 | uint8_t event; 77 | uint8_t type; 78 | } mdata2_decode_rules_t; 79 | 80 | // Entries with XSENS_EVT_TYPE_NONE aren't supported directly by the library 81 | // User is just given the payload and they can opt to handle it themselves... 82 | // 83 | // XDI_GNSS_PVT_DATA - not supported, even passing directly to dev 84 | // XDI_GNSS_SAT_INFO - ^ 85 | static const mdata2_decode_rules_t xid_decode_table[] = { 86 | { XDI_TEMPERATURE, XSENS_EVT_TEMPERATURE, XSENS_EVT_TYPE_FLOAT }, 87 | { XDI_UTC_TIME, XSENS_EVT_UTC_TIME, XSENS_EVT_TYPE_NONE }, 88 | { XDI_PACKET_COUNTER, XSENS_EVT_PACKET_COUNT, XSENS_EVT_TYPE_U16 }, 89 | { XDI_SAMPLE_TIME_FINE, XSENS_EVT_TIME_FINE, XSENS_EVT_TYPE_U32 }, 90 | { XDI_SAMPLE_TIME_COARSE, XSENS_EVT_TIME_COARSE, XSENS_EVT_TYPE_U32 }, 91 | { XDI_QUATERNION, XSENS_EVT_QUATERNION, XSENS_EVT_TYPE_FLOAT4 }, 92 | { XDI_EULER_ANGLES, XSENS_EVT_EULER, XSENS_EVT_TYPE_FLOAT3 }, 93 | { XDI_ROTATION_MATRIX, XSENS_EVT_ROT_MATRIX, XSENS_EVT_TYPE_FLOAT9 }, 94 | { XDI_BARO_PRESSURE, XSENS_EVT_PRESSURE, XSENS_EVT_TYPE_U32 }, 95 | { XDI_DELTA_V, XSENS_EVT_DELTA_V, XSENS_EVT_TYPE_FLOAT3 }, 96 | { XDI_DELTA_Q, XSENS_EVT_DELTA_Q, XSENS_EVT_TYPE_FLOAT4 }, 97 | { XDI_ACCELERATION, XSENS_EVT_ACCELERATION, XSENS_EVT_TYPE_FLOAT3 }, 98 | { XDI_FREE_ACCELERATION, XSENS_EVT_FREE_ACCELERATION, XSENS_EVT_TYPE_FLOAT3 }, 99 | { XDI_ACCELERATION_HIGH_RATE, XSENS_EVT_ACCELERATION_HR, XSENS_EVT_TYPE_FLOAT3 }, 100 | { XDI_RATE_OF_TURN, XSENS_EVT_RATE_OF_TURN, XSENS_EVT_TYPE_FLOAT3 }, 101 | { XDI_RATE_OF_TURN_HIGH_RATE, XSENS_EVT_RATE_OF_TURN_HR, XSENS_EVT_TYPE_FLOAT3 }, 102 | { XDI_GNSS_PVT_PULSE, XSENS_EVT_GNSS_PVT_PULSE, XSENS_EVT_TYPE_U32 }, 103 | { XDI_RAW_ACC_GYRO_MAG_TEMP, XSENS_EVT_RAW_ACC_GYRO_MAG_TEMP, XSENS_EVT_TYPE_NONE }, 104 | { XDI_RAW_GYRO_TEMP, XSENS_EVT_RAW_GYRO_TEMP, XSENS_EVT_TYPE_NONE }, 105 | { XDI_MAGNETIC_FIELD, XSENS_EVT_MAGNETIC, XSENS_EVT_TYPE_FLOAT3 }, 106 | { XDI_STATUS_BYTE, XSENS_EVT_STATUS_BYTE, XSENS_EVT_TYPE_U8 }, 107 | { XDI_STATUS_WORD, XSENS_EVT_STATUS_WORD, XSENS_EVT_TYPE_U32 }, 108 | { XDI_DEVICE_ID, XSENS_EVT_DEVICE_ID, XSENS_EVT_TYPE_U32 }, 109 | { XDI_LOCATION_ID, XSENS_EVT_LOCATION_ID, XSENS_EVT_TYPE_U16 }, 110 | { XDI_POSITION_ECEF, XSENS_EVT_POSITION_ECEF, XSENS_EVT_TYPE_FLOAT3 }, 111 | { XDI_LAT_LON, XSENS_EVT_LAT_LON, XSENS_EVT_TYPE_FLOAT2 }, 112 | { XDI_ALTITUDE_ELLIPSOID, XSENS_EVT_ALTITUDE_ELLIPSOID, XSENS_EVT_TYPE_FLOAT }, 113 | { XDI_VELOCITY_XYZ, XSENS_EVT_VELOCITY_XYZ, XSENS_EVT_TYPE_FLOAT3 }, 114 | }; 115 | 116 | // With the 'isolated' field from the rest of the payload, 117 | // convert to LE and pass to user cb in a union 118 | void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb ) 119 | { 120 | XsensEventData_t value = { 0 }; 121 | const mdata2_decode_rules_t *decode_rule = 0; 122 | 123 | enum XSENS_FLOAT_TYPE number_precision = XSENS_IDENTIFIER_FORMAT_GET_PRECISION( output->id ); 124 | enum XSENS_COORDINATE_SYSTEM coordinate_system = XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( output->id ); 125 | uint16_t id_simplifed = XSENS_IDENTIFIER_FORMAT_SIMPLIFY( output->id ); 126 | 127 | // Find the matching XID in the table 128 | uint8_t table_length = sizeof( xid_decode_table ) / sizeof( mdata2_decode_rules_t ); 129 | for( uint8_t i = 0; i < table_length; i++ ) 130 | { 131 | if( xid_decode_table[i].xid == id_simplifed ) 132 | { 133 | decode_rule = &xid_decode_table[i]; 134 | break; 135 | } 136 | } 137 | 138 | // Apply post-processing (BE->LE) strategy specific to the packet type 139 | if( decode_rule ) 140 | { 141 | // The structure describes the typical type 142 | value.type = decode_rule->type; 143 | 144 | // For situations where non-single precision is used, apply an offset to the enum 145 | // to correctly describe the type as fixed-precision or double, etc 146 | if( number_precision ) 147 | { 148 | if( decode_rule->type < XSENS_EVT_TYPE_FLOAT ) 149 | { 150 | // Enum offset doesn't apply cleanly for non-float default values 151 | // So we 'zero' out the table's value, and use the precision offset to get 152 | // the single-value type field of that type 153 | value.type = XSENS_EVT_TYPE_FLOAT + ( number_precision * 10 ); 154 | } 155 | else 156 | { 157 | value.type = decode_rule->type + ( number_precision * 10 ); 158 | } 159 | } 160 | 161 | // Provide the local tangent plane coordinate scheme in the callback 162 | value.coord_ref = coordinate_system; 163 | 164 | // Convert BE data to LE, put it in the right union field 165 | switch( value.type ) 166 | { 167 | case XSENS_EVT_TYPE_U8: 168 | value.data.u1 = output->payload[0]; 169 | break; 170 | 171 | case XSENS_EVT_TYPE_U16: 172 | value.data.u2 = xsens_coalesce_16BE_16LE( &output->payload[0] ); 173 | break; 174 | 175 | case XSENS_EVT_TYPE_U32: 176 | value.data.u4 = xsens_coalesce_32BE_32LE( &output->payload[0] ); 177 | break; 178 | 179 | case XSENS_EVT_TYPE_FLOAT: 180 | case XSENS_EVT_TYPE_1220FP: 181 | value.data.f4 = xsens_coalesce_32BE_F32LE( &output->payload[0] ); 182 | break; 183 | 184 | case XSENS_EVT_TYPE_FLOAT2: 185 | case XSENS_EVT_TYPE_1220FP2: 186 | value.data.f4x2[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] ); 187 | value.data.f4x2[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] ); 188 | break; 189 | 190 | case XSENS_EVT_TYPE_FLOAT3: 191 | case XSENS_EVT_TYPE_1220FP3: 192 | value.data.f4x3[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] ); 193 | value.data.f4x3[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] ); 194 | value.data.f4x3[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] ); 195 | break; 196 | 197 | case XSENS_EVT_TYPE_FLOAT4: 198 | case XSENS_EVT_TYPE_1220FP4: 199 | value.data.f4x4[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] ); 200 | value.data.f4x4[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] ); 201 | value.data.f4x4[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] ); 202 | value.data.f4x4[3] = xsens_coalesce_32BE_F32LE( &output->payload[12] ); 203 | break; 204 | 205 | case XSENS_EVT_TYPE_FLOAT9: 206 | case XSENS_EVT_TYPE_1220FP9: 207 | value.data.f4x9[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] ); 208 | value.data.f4x9[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] ); 209 | value.data.f4x9[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] ); 210 | value.data.f4x9[3] = xsens_coalesce_32BE_F32LE( &output->payload[12] ); 211 | value.data.f4x9[4] = xsens_coalesce_32BE_F32LE( &output->payload[16] ); 212 | value.data.f4x9[5] = xsens_coalesce_32BE_F32LE( &output->payload[20] ); 213 | value.data.f4x9[6] = xsens_coalesce_32BE_F32LE( &output->payload[24] ); 214 | value.data.f4x9[7] = xsens_coalesce_32BE_F32LE( &output->payload[28] ); 215 | value.data.f4x9[8] = xsens_coalesce_32BE_F32LE( &output->payload[32] ); 216 | break; 217 | 218 | case XSENS_EVT_TYPE_1632FP: 219 | xsens_coalesce_48BE_48LE( &value.data.fp1632, &output->payload[0] ); 220 | break; 221 | 222 | case XSENS_EVT_TYPE_1632FP2: 223 | xsens_coalesce_48BE_48LE( &value.data.fp1632x2[0], &output->payload[0] ); 224 | xsens_coalesce_48BE_48LE( &value.data.fp1632x2[1], &output->payload[6] ); 225 | break; 226 | 227 | case XSENS_EVT_TYPE_1632FP3: 228 | xsens_coalesce_48BE_48LE( &value.data.fp1632x3[0], &output->payload[0] ); 229 | xsens_coalesce_48BE_48LE( &value.data.fp1632x3[1], &output->payload[6] ); 230 | xsens_coalesce_48BE_48LE( &value.data.fp1632x3[2], &output->payload[12] ); 231 | break; 232 | 233 | case XSENS_EVT_TYPE_1632FP4: 234 | xsens_coalesce_48BE_48LE( &value.data.fp1632x4[0], &output->payload[0] ); 235 | xsens_coalesce_48BE_48LE( &value.data.fp1632x4[1], &output->payload[6] ); 236 | xsens_coalesce_48BE_48LE( &value.data.fp1632x4[2], &output->payload[12] ); 237 | xsens_coalesce_48BE_48LE( &value.data.fp1632x4[3], &output->payload[18] ); 238 | break; 239 | 240 | case XSENS_EVT_TYPE_1632FP9: 241 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[0], &output->payload[0] ); 242 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[1], &output->payload[6] ); 243 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[2], &output->payload[12] ); 244 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[3], &output->payload[18] ); 245 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[4], &output->payload[24] ); 246 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[5], &output->payload[30] ); 247 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[6], &output->payload[36] ); 248 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[7], &output->payload[42] ); 249 | xsens_coalesce_48BE_48LE( &value.data.fp1632x9[8], &output->payload[48] ); 250 | break; 251 | 252 | case XSENS_EVT_TYPE_DOUBLE: 253 | xsens_swap_endian_u64( &value.data.f8, &output->payload[0] ); 254 | break; 255 | 256 | case XSENS_EVT_TYPE_DOUBLE2: 257 | xsens_swap_endian_u64( &value.data.f8x2[0], &output->payload[0] ); 258 | xsens_swap_endian_u64( &value.data.f8x2[1], &output->payload[8] ); 259 | break; 260 | 261 | case XSENS_EVT_TYPE_DOUBLE3: 262 | xsens_swap_endian_u64( &value.data.f8x3[0], &output->payload[0] ); 263 | xsens_swap_endian_u64( &value.data.f8x3[1], &output->payload[8] ); 264 | xsens_swap_endian_u64( &value.data.f8x3[2], &output->payload[16] ); 265 | break; 266 | 267 | case XSENS_EVT_TYPE_DOUBLE4: 268 | xsens_swap_endian_u64( &value.data.f8x4[0], &output->payload[0] ); 269 | xsens_swap_endian_u64( &value.data.f8x4[1], &output->payload[8] ); 270 | xsens_swap_endian_u64( &value.data.f8x4[2], &output->payload[16] ); 271 | xsens_swap_endian_u64( &value.data.f8x4[3], &output->payload[24] ); 272 | break; 273 | 274 | case XSENS_EVT_TYPE_DOUBLE9: 275 | xsens_swap_endian_u64( &value.data.f8x9[0], &output->payload[0] ); 276 | xsens_swap_endian_u64( &value.data.f8x9[1], &output->payload[8] ); 277 | xsens_swap_endian_u64( &value.data.f8x9[2], &output->payload[16] ); 278 | xsens_swap_endian_u64( &value.data.f8x9[3], &output->payload[24] ); 279 | xsens_swap_endian_u64( &value.data.f8x9[4], &output->payload[32] ); 280 | xsens_swap_endian_u64( &value.data.f8x9[5], &output->payload[40] ); 281 | xsens_swap_endian_u64( &value.data.f8x9[6], &output->payload[48] ); 282 | xsens_swap_endian_u64( &value.data.f8x9[7], &output->payload[56] ); 283 | xsens_swap_endian_u64( &value.data.f8x9[8], &output->payload[64] ); 284 | break; 285 | 286 | default: 287 | // There's an error or not supported, return a 'null' type? 288 | value.type = XSENS_EVT_TYPE_NONE; 289 | break; 290 | } 291 | 292 | // Call the user-callback with the transformed data 293 | if( evt_cb ) 294 | { 295 | evt_cb( decode_rule->event, &value ); 296 | } 297 | } 298 | } -------------------------------------------------------------------------------- /src/xsens_mdata2.h: -------------------------------------------------------------------------------- 1 | #ifndef XSENS_MDATA2_H 2 | #define XSENS_MDATA2_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "xsens_constants.h" 9 | 10 | typedef enum 11 | { 12 | XDI_PARSE_ID_B1, 13 | XDI_PARSE_ID_B2, 14 | XDI_PARSE_LENGTH, 15 | XDI_PARSE_DATA 16 | } mdata2_parser_state_t; 17 | 18 | typedef struct 19 | { 20 | uint16_t id; 21 | uint8_t length; 22 | uint8_t payload[255]; // TODO: work out what size is actually needed 23 | } mdata2_packet_t; 24 | 25 | void xsens_mdata2_process( xsens_packet_buffer_t *packet, callback_event_t evt_cb ); 26 | 27 | void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb ); 28 | 29 | #ifdef __cplusplus 30 | } 31 | #endif 32 | 33 | #endif //end XSENS_MDATA2_H -------------------------------------------------------------------------------- /src/xsens_mti.c: -------------------------------------------------------------------------------- 1 | #include "string.h" 2 | 3 | #include "xsens_mdata2.h" 4 | #include "xsens_mti.h" 5 | #include "xsens_mti_private.h" 6 | #include "xsens_utility.h" 7 | 8 | xsens_interface_t *most_recent_interface; 9 | 10 | message_handler_ref_t inbound_handler_table[] = { 11 | { .id = MT_WAKEUP, .handler_fn = NULL }, 12 | { .id = MT_ACK_GOTOCONFIG, .handler_fn = NULL }, 13 | { .id = MT_ACK_GOTOMEASUREMENT, .handler_fn = NULL }, 14 | { .id = MT_ACK_RESET, .handler_fn = NULL }, 15 | { .id = MT_DEVICEID, .handler_fn = &xsens_internal_handle_device_id }, 16 | { .id = MT_PRODUCTCODE, .handler_fn = &xsens_internal_handle_product_code }, 17 | { .id = MT_HARDWAREVERSION, .handler_fn = &xsens_internal_handle_hardware_version }, 18 | { .id = MT_FIRMWAREREV, .handler_fn = &xsens_internal_handle_firmware_version }, 19 | { .id = MT_ACK_SELFTEST, .handler_fn = &xsens_internal_handle_selftest_results }, 20 | { .id = MT_ERROR, .handler_fn = &xsens_internal_handle_error }, 21 | { .id = MT_WARNING, .handler_fn = NULL }, 22 | { .id = MT_CONFIGURATION, .handler_fn = NULL }, 23 | { .id = MT_MTDATA, .handler_fn = NULL }, 24 | { .id = MT_MTDATA2, .handler_fn = &xsens_internal_handle_mdata2 }, 25 | { .id = MT_ACK_LATLONALT, .handler_fn = NULL }, 26 | { .id = MT_ACK_FILTERPROFILE, .handler_fn = NULL }, 27 | { .id = MT_ACK_GNSSPLATFORM, .handler_fn = NULL }, 28 | { .id = MT_ACK_GNSSLEVERARM, .handler_fn = NULL }, 29 | { .id = MT_UTCTIME, .handler_fn = NULL }, 30 | { .id = MT_ACK_ICCCOMMAND, .handler_fn = NULL }, 31 | { .id = MT_ACK_INITIALHEADING, .handler_fn = NULL }, 32 | { .id = MT_ACK_FORWARDGNSSDATA, .handler_fn = NULL }, 33 | { .id = MT_ACK_BAUDRATE, .handler_fn = NULL }, 34 | { .id = MT_ACK_TRANSMITDELAY, .handler_fn = NULL }, 35 | { .id = MT_ACK_OPTIONFLAGS, .handler_fn = NULL }, 36 | { .id = MT_ACK_LOCATIONID, .handler_fn = NULL }, 37 | { .id = MT_ACK_SYNCSETTINGS, .handler_fn = NULL }, 38 | { .id = MT_ACK_PERIOD, .handler_fn = NULL }, 39 | { .id = MT_ACK_EXTOUTPUTMODE, .handler_fn = NULL }, 40 | { .id = MT_ACK_OUTPUTCONFIGURATION, .handler_fn = NULL }, 41 | { .id = MT_ACK_STRINGOUTPUTTYPES, .handler_fn = NULL }, 42 | { .id = MT_ACK_RESETORIENTATION, .handler_fn = NULL }, 43 | { .id = MT_ACK_AVAILABLEFILTERPROFILES, .handler_fn = NULL }, 44 | { .id = MT_ACK_NOROTATION, .handler_fn = NULL }, 45 | }; 46 | 47 | // Helper to parse inbound data with one call 48 | void xsens_mti_parse_buffer( xsens_interface_t *interface, uint8_t *buffer, uint16_t size ) 49 | { 50 | for( uint16_t i = 0; i < size; i++ ) 51 | { 52 | xsens_mti_parse( interface, buffer[i] ); 53 | } 54 | } 55 | 56 | // Run each byte through the packet-level statemachine 57 | void xsens_mti_parse( xsens_interface_t *interface, uint8_t byte ) 58 | { 59 | // CRC is the sum of bytes including the CRC byte (ex PREAMBLE) 60 | if( interface->state != PARSER_PREAMBLE ) 61 | { 62 | interface->crc += ( byte & 0xFF ); 63 | } 64 | 65 | switch( interface->state ) 66 | { 67 | case PARSER_PREAMBLE: 68 | if( byte == PREAMBLE_BYTE ) 69 | { 70 | xsens_mti_reset_parser( interface ); 71 | interface->state = PARSER_ADDRESS; 72 | } 73 | break; 74 | 75 | case PARSER_ADDRESS: 76 | if( byte == ADDRESS_BYTE ) 77 | { 78 | interface->state = PARSER_MESSAGE_ID; 79 | } 80 | break; 81 | 82 | case PARSER_MESSAGE_ID: 83 | interface->packet.message_id = byte; 84 | interface->state = PARSER_LENGTH; 85 | break; 86 | 87 | case PARSER_LENGTH: 88 | if( byte == LENGTH_EXTENDED_MODE ) 89 | { 90 | interface->state = PARSER_LENGTH_EXTENDED_B1; 91 | } 92 | if( byte == LENGTH_NONE ) 93 | { 94 | interface->packet.length = 0; 95 | interface->state = PARSER_CRC; 96 | } 97 | else 98 | { 99 | interface->packet.length = byte; 100 | interface->state = PARSER_PAYLOAD; 101 | } 102 | break; 103 | 104 | case PARSER_LENGTH_EXTENDED_B1: 105 | interface->packet.length = byte; 106 | // TODO decode long length packets 107 | interface->state = PARSER_LENGTH_EXTENDED_B2; 108 | break; 109 | 110 | case PARSER_LENGTH_EXTENDED_B2: 111 | // TODO decode long length packets 112 | interface->packet.length &= byte << 8; 113 | interface->state = PARSER_PAYLOAD; 114 | break; 115 | 116 | case PARSER_PAYLOAD: 117 | interface->packet.payload[interface->payload_pos] = byte; 118 | interface->payload_pos++; 119 | 120 | // Once we've buffered the whole payload, 121 | // prepare to read the CRC 122 | if( interface->payload_pos >= interface->packet.length ) 123 | { 124 | interface->state = PARSER_CRC; 125 | } 126 | break; 127 | 128 | case PARSER_CRC: 129 | // Check if CRC is valid 130 | if( interface->crc == 0x00 ) 131 | { 132 | // Packet was successfully recieved 133 | // Run the payload handling function 134 | xsens_mti_handle_payload( interface ); 135 | } 136 | else 137 | { 138 | // TODO send CRC failed event to user? 139 | } 140 | 141 | interface->state = PARSER_PREAMBLE; 142 | break; 143 | } 144 | } 145 | 146 | void xsens_mti_reset_parser( xsens_interface_t *interface ) 147 | { 148 | // Clear the parser state and buffers 149 | memset( &( interface->packet ), 0, sizeof( xsens_packet_buffer_t ) ); 150 | interface->payload_pos = 0; 151 | interface->crc = 0; 152 | } 153 | 154 | // With a valid packet, process the payload 155 | void xsens_mti_handle_payload( xsens_interface_t *interface ) 156 | { 157 | xsens_packet_buffer_t *packet = &( interface->packet ); 158 | 159 | // Search the inbound handler table for a match 160 | message_handler_ref_t *handler = xsens_mti_find_inbound_handler_entry( packet->message_id ); 161 | 162 | // If the ID is recognised, call the handler function (if it exists) 163 | if( handler ) 164 | { 165 | callback_payload_t payload_handler_fn = (callback_payload_t)handler->handler_fn; 166 | if( payload_handler_fn ) 167 | { 168 | most_recent_interface = interface; // internally cache the interface for cb access 169 | payload_handler_fn( packet ); 170 | } 171 | } 172 | } 173 | 174 | bool xsens_mti_override_id_handler( uint8_t id, callback_payload_t user_fn ) 175 | { 176 | if( user_fn ) 177 | { 178 | // Find the ID in the inbound handler 'jump table' 179 | message_handler_ref_t *handler = xsens_mti_find_inbound_handler_entry( id ); 180 | 181 | if( handler ) 182 | { 183 | handler->handler_fn = user_fn; 184 | return true; 185 | } 186 | } 187 | 188 | return false; 189 | } 190 | 191 | message_handler_ref_t *xsens_mti_find_inbound_handler_entry( uint8_t find_id ) 192 | { 193 | uint8_t table_length = sizeof( inbound_handler_table ) / sizeof( message_handler_ref_t ); 194 | 195 | return xsens_mti_find_handler_entry(find_id, inbound_handler_table, table_length ); 196 | } 197 | 198 | message_handler_ref_t *xsens_mti_find_handler_entry( uint8_t find_id, message_handler_ref_t *entry_table, uint8_t entry_count ) 199 | { 200 | for( uint8_t i = 0; i < entry_count; i++ ) 201 | { 202 | if( entry_table[i].id == find_id ) 203 | { 204 | return &entry_table[i]; 205 | } 206 | } 207 | 208 | return (message_handler_ref_t *)NULL; 209 | } 210 | 211 | uint8_t xsens_mti_buffer_crc( uint8_t *buffer, uint16_t size ) 212 | { 213 | uint8_t crc = 0; 214 | 215 | for( uint16_t i = 0; i < size; i++ ) 216 | { 217 | crc -= buffer[i]; 218 | } 219 | 220 | return crc; 221 | } 222 | 223 | void xsens_mti_send( xsens_interface_t *interface, xsens_packet_buffer_t *packet ) 224 | { 225 | if( interface && packet ) 226 | { 227 | uint8_t buffer[2048] = { 0 }; 228 | uint16_t buffer_pos = 0; 229 | uint8_t crc = 0; 230 | 231 | // Preamble 232 | buffer[buffer_pos++] = PREAMBLE_BYTE; 233 | 234 | // Device Address 235 | buffer[buffer_pos++] = ADDRESS_BYTE; 236 | 237 | // Message ID 238 | buffer[buffer_pos++] = packet->message_id; 239 | 240 | // Payload Length 241 | if( packet->length < 0xFF ) 242 | { 243 | buffer[buffer_pos++] = packet->length; 244 | } 245 | else 246 | { 247 | // Extended packet handling sets the normal length byte to 255, 248 | // followed by two bytes of payload data 249 | buffer[buffer_pos++] = 0xFF; 250 | 251 | memcpy( &buffer[buffer_pos], &packet->length, 2 ); 252 | buffer_pos += 2; 253 | } 254 | 255 | // Payload Data 256 | if( packet->length != 0 ) 257 | { 258 | memcpy( &buffer[buffer_pos], (uint8_t *)packet->payload, packet->length ); 259 | buffer_pos += packet->length; 260 | } 261 | 262 | // Calculate the CRC of the packet, exluding the preamble 263 | buffer[buffer_pos] = xsens_mti_buffer_crc( &buffer[1], buffer_pos-1 ); 264 | buffer_pos += 1; 265 | 266 | // Pass the buffer to the user so they can send to hardware 267 | if( interface->output_cb ) 268 | { 269 | interface->output_cb( buffer, buffer_pos ); 270 | } 271 | } 272 | } 273 | 274 | void xsens_mti_request( xsens_interface_t *interface, uint8_t id ) 275 | { 276 | xsens_packet_buffer_t packet = { 0 }; 277 | 278 | packet.message_id = id; 279 | packet.length = 0; 280 | packet.payload[0] = 0; 281 | 282 | xsens_mti_send( interface, &packet ); 283 | } 284 | 285 | 286 | 287 | void xsens_mti_set_baudrate( xsens_interface_t *interface, XsensBaudSetting_t baudrate ) 288 | { 289 | xsens_packet_buffer_t packet = { 0 }; 290 | 291 | packet.message_id = MT_SETBAUDRATE; 292 | packet.length = 1; 293 | packet.payload[0] = baudrate; 294 | 295 | xsens_mti_send( interface, &packet ); 296 | } 297 | 298 | 299 | void xsens_mti_reset_orientation( xsens_interface_t *interface, XsensOrientationSetting_t code ) 300 | { 301 | xsens_packet_buffer_t packet = { 0 }; 302 | 303 | packet.message_id = MT_RESETORIENTATION; 304 | packet.length = 2; 305 | packet.payload[0] = 0x00; 306 | packet.payload[1] = code; 307 | 308 | xsens_mti_send( interface, &packet ); 309 | } 310 | 311 | void xsens_mti_set_configuration( xsens_interface_t *interface, XsensFrequencyConfig_t config[], uint8_t num_config ) 312 | { 313 | // No more than 32 values can be configured 314 | if( interface && config && num_config && num_config <= 32 ) 315 | { 316 | xsens_packet_buffer_t packet = { 0 }; 317 | packet.message_id = MT_SETOUTPUTCONFIGURATION; 318 | 319 | // Form a big-endian MData2 style 'packet' for each field 320 | // 2 bytes for ID 321 | // 2 bytes for frequency 322 | for( uint8_t i = 0; i <= num_config; i++ ) 323 | { 324 | uint8_t buff_pos = i * 4; 325 | 326 | // LE to BE conversion directly into the output buffer... 327 | xsens_swap_endian_u16( &packet.payload[buff_pos], (uint8_t*)&config[i].id ); 328 | xsens_swap_endian_u16( &packet.payload[buff_pos+2], (uint8_t*)&config[i].frequency ); 329 | packet.length = buff_pos; 330 | } 331 | 332 | // TODO: refactor as a MDATA2 output problem once generation fns are implemented? 333 | xsens_mti_send( interface, &packet ); 334 | } 335 | } 336 | 337 | // User needs to bitwise or a series of desired flags using XSENS_OPTION_FLAGS enum values 338 | // The IMU takes two fields, one for bits to set, another for bits to clear 339 | // Read MT LowLevel documentation for precedence edgecases 340 | void xsens_mti_set_option_flags( xsens_interface_t *interface, uint32_t set_flags, uint32_t clear_flags ) 341 | { 342 | if( interface ) 343 | { 344 | xsens_packet_buffer_t packet = { 0 }; 345 | packet.message_id = MT_SETOPTIONFLAGS; 346 | 347 | // LE to BE conversion directly into the output buffer... 348 | xsens_swap_endian_u32( &packet.payload[0], (uint8_t*)&set_flags ); 349 | xsens_swap_endian_u32( &packet.payload[4], (uint8_t*)&clear_flags ); 350 | packet.length = 8; 351 | 352 | xsens_mti_send( interface, &packet ); 353 | } 354 | } 355 | 356 | void xsens_internal_handle_device_id( xsens_packet_buffer_t *packet ) 357 | { 358 | XsensEventData_t value = { 0 }; 359 | 360 | if( packet->length == 4 ) // MTi 1, 10, 100 361 | { 362 | value.type = XSENS_EVT_TYPE_U32; 363 | value.data.u4 = xsens_coalesce_32BE_32LE( &packet->payload[0] ); 364 | } 365 | else if( packet->length == 8 ) // MTi-600 366 | { 367 | // TODO: untested 8-byte device ID 368 | value.type = XSENS_EVT_TYPE_U32; 369 | value.data.u4 = xsens_coalesce_32BE_32LE( &packet->payload[4] ); 370 | } 371 | 372 | if( most_recent_interface->event_cb ) 373 | { 374 | most_recent_interface->event_cb( XSENS_EVT_DEVICE_ID, &value ); 375 | } 376 | } 377 | 378 | void xsens_internal_handle_product_code( xsens_packet_buffer_t *packet ) 379 | { 380 | // ASCII formatted code max 20 bytes 381 | // TODO: handle product code 382 | } 383 | 384 | void xsens_internal_handle_hardware_version( xsens_packet_buffer_t *packet ) 385 | { 386 | // TODO: handle product code 387 | 388 | // uint8_t hw_version[2]; 389 | // uint16_t *hw_ptr = (uint16_t *)&hw_version; 390 | // hw_ptr = xsens_coalesce_16BE_16LE( &packet->payload[0] ); 391 | } 392 | 393 | void xsens_internal_handle_firmware_version( xsens_packet_buffer_t *packet ) 394 | { 395 | // TODO: handle firmware version 396 | 397 | uint8_t major = packet->payload[0]; 398 | uint8_t minor = packet->payload[1]; 399 | uint8_t revision = packet->payload[2]; 400 | uint32_t build = xsens_coalesce_32BE_32LE( &packet->payload[3] ); 401 | uint32_t scm = xsens_coalesce_32BE_32LE( &packet->payload[7] ); 402 | } 403 | 404 | void xsens_internal_handle_selftest_results( xsens_packet_buffer_t *packet ) 405 | { 406 | // TODO: handle selftest results 407 | } 408 | 409 | void xsens_internal_handle_error( xsens_packet_buffer_t *packet ) 410 | { 411 | uint8_t error_code = packet->payload[0]; 412 | 413 | /* 414 | switch( error_code ) 415 | { 416 | case ERROR_PERIOD_INVALID: 417 | break; 418 | 419 | case ERROR_MESSAGE_INVALID: 420 | break; 421 | 422 | case ERROR_TIMER_OVERFLOW: 423 | break; 424 | 425 | case ERROR_BAUDRATE: 426 | break; 427 | 428 | case ERROR_PARAMETER_INVALID: 429 | break; 430 | 431 | case ERROR_DEVICE: 432 | break; 433 | 434 | default: 435 | break; 436 | } 437 | */ 438 | } 439 | 440 | void xsens_internal_handle_mdata2( xsens_packet_buffer_t *packet ) 441 | { 442 | // MData2 packets contain 1 to n smaller packets 443 | // with variable length fields, see xsens_mdata2.c/.h 444 | xsens_mdata2_process( packet, most_recent_interface->event_cb ); 445 | } 446 | -------------------------------------------------------------------------------- /src/xsens_mti.h: -------------------------------------------------------------------------------- 1 | #ifndef XSENS_MTI_H 2 | #define XSENS_MTI_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "xsens_constants.h" 9 | 10 | // Packet state machine values 11 | typedef enum 12 | { 13 | PARSER_PREAMBLE = 0, 14 | PARSER_ADDRESS, 15 | PARSER_MESSAGE_ID, 16 | PARSER_LENGTH, 17 | PARSER_LENGTH_EXTENDED_B1, 18 | PARSER_LENGTH_EXTENDED_B2, 19 | PARSER_PAYLOAD, 20 | PARSER_CRC, 21 | } xsens_parser_state_t; 22 | 23 | // Userspace storage for library state and buffers 24 | typedef struct 25 | { 26 | xsens_parser_state_t state; 27 | xsens_packet_buffer_t packet; 28 | uint16_t payload_pos; 29 | uint8_t crc; 30 | 31 | callback_event_t event_cb; 32 | callback_data_out_t output_cb; 33 | } xsens_interface_t; 34 | 35 | // Macros to make interface object instantiation cleaner 36 | // Needed because Arduino/C++ doesn't support out-of-order/incomplete 37 | #define XSENS_INTERFACE_RX( CB_PTR ) { .state = PARSER_PREAMBLE, .packet = XSENS_PACKET_BUF_EMPTY, .payload_pos = 0, .crc = 0, .event_cb = CB_PTR, .output_cb = 0 } 38 | #define XSENS_INTERFACE_RX_TX( CB_PTR, OUTPUT_PTR ) { .state = PARSER_PREAMBLE, .packet = XSENS_PACKET_BUF_EMPTY, .payload_pos = 0, .crc = 0, .event_cb = CB_PTR, .output_cb = OUTPUT_PTR } 39 | 40 | // Callback to internal (or userspace) handler function 41 | // Function is passed a successfully receieved packet for processing 42 | typedef void ( *callback_payload_t )( xsens_packet_buffer_t * ); 43 | 44 | // Entry for a list of function pointers to handle each MID correctly 45 | typedef struct 46 | { 47 | uint8_t id; 48 | callback_payload_t handler_fn; 49 | } message_handler_ref_t; 50 | 51 | // Parse inbound bytes received from the MTi device 52 | void xsens_mti_parse_buffer( xsens_interface_t *interface, uint8_t *buffer, uint16_t size ); 53 | void xsens_mti_parse( xsens_interface_t *interface, uint8_t byte ); 54 | void xsens_mti_reset_parser( xsens_interface_t *interface ); 55 | 56 | // Add or override the internal packet handler function with a userspace function 57 | bool xsens_mti_override_id_handler( uint8_t id, callback_payload_t user_fn ); 58 | 59 | // Send a ready-made packet to hardware 60 | void xsens_mti_send( xsens_interface_t *interface, xsens_packet_buffer_t *packet ); 61 | 62 | // Send a request message (no payload) to hardware 63 | void xsens_mti_request( xsens_interface_t *interface, uint8_t id ); 64 | 65 | 66 | // Helper functions for configuring IMU settings 67 | void xsens_mti_set_baudrate( xsens_interface_t *interface, XsensBaudSetting_t baudrate ); 68 | void xsens_mti_reset_orientation( xsens_interface_t *interface, XsensOrientationSetting_t code ); 69 | void xsens_mti_set_configuration( xsens_interface_t *interface, XsensFrequencyConfig_t config[], uint8_t num_config ); 70 | void xsens_mti_set_option_flags( xsens_interface_t *interface, uint32_t set_flags, uint32_t clear_flags ); 71 | 72 | #ifdef __cplusplus 73 | } 74 | #endif 75 | 76 | #endif //end XSENS_MTI_H -------------------------------------------------------------------------------- /src/xsens_mti_private.h: -------------------------------------------------------------------------------- 1 | #ifndef XSENS_MTI_PRIVATE_H 2 | #define XSENS_MTI_PRIVATE_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "xsens_mti.h" 9 | 10 | // Library provided payload handler functions 11 | void xsens_internal_handle_device_id( xsens_packet_buffer_t *packet ); 12 | void xsens_internal_handle_product_code( xsens_packet_buffer_t *packet ); 13 | void xsens_internal_handle_hardware_version( xsens_packet_buffer_t *packet ); 14 | void xsens_internal_handle_firmware_version( xsens_packet_buffer_t *packet ); 15 | void xsens_internal_handle_selftest_results( xsens_packet_buffer_t *packet ); 16 | void xsens_internal_handle_error( xsens_packet_buffer_t *packet ); 17 | 18 | void xsens_internal_handle_mdata2( xsens_packet_buffer_t *packet ); 19 | 20 | // Responsible for calling downstream handler and post-processing functions 21 | // after a valid packet is parsed 22 | void xsens_mti_handle_payload( xsens_interface_t *interface ); 23 | 24 | // Search the hardcoded message handler table for a given ID 25 | // Returns a pointer to the entry if it exists 26 | // Returns 0 if no match is found 27 | message_handler_ref_t *xsens_mti_find_inbound_handler_entry( uint8_t find_id ); 28 | 29 | // Testable find-in-table function used by xsens_mti_find_inbound_handler_entry 30 | message_handler_ref_t *xsens_mti_find_handler_entry( uint8_t find_id, message_handler_ref_t *entry_table, uint8_t entry_num ); 31 | 32 | // Run through a buffer of bytes and return the CRC 33 | // Used when sending messages 34 | uint8_t xsens_mti_buffer_crc( uint8_t *buffer, uint16_t size ); 35 | 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #endif //end XSENS_MTI_PRIVATE_H -------------------------------------------------------------------------------- /src/xsens_utility.c: -------------------------------------------------------------------------------- 1 | #include "xsens_utility.h" 2 | #include "math.h" 3 | #include "stdlib.h" 4 | 5 | #ifndef M_PI 6 | #define M_PI 3.14159265358979323846 7 | #endif 8 | 9 | void xsens_swap_endian_u16( uint8_t *dest, uint8_t *source ) 10 | { 11 | dest[1] = source[0]; 12 | dest[0] = source[1]; 13 | } 14 | 15 | void xsens_swap_endian_u32( uint8_t *dest, uint8_t *source ) 16 | { 17 | dest[3] = source[0]; 18 | dest[2] = source[1]; 19 | dest[1] = source[2]; 20 | dest[0] = source[3]; 21 | } 22 | 23 | void xsens_swap_endian_u64( uint8_t *dest, uint8_t *source ) 24 | { 25 | dest[7] = source[0]; 26 | dest[6] = source[1]; 27 | dest[5] = source[2]; 28 | dest[4] = source[3]; 29 | dest[3] = source[4]; 30 | dest[2] = source[5]; 31 | dest[1] = source[6]; 32 | dest[0] = source[7]; 33 | } 34 | 35 | uint16_t xsens_coalesce_16BE_16LE( uint8_t *source ) 36 | { 37 | return ( source[1] << 0 | source[0] << 8 ); 38 | } 39 | 40 | uint32_t xsens_coalesce_32BE_32LE( uint8_t *source ) 41 | { 42 | return ( ( source[3] << 0 ) 43 | | ( source[2] << 8 ) 44 | | ( source[1] << 16 ) 45 | | ( source[0] << 24 ) ); 46 | } 47 | 48 | float xsens_coalesce_32BE_F32LE( uint8_t *source ) 49 | { 50 | float f; 51 | uint8_t *f_ptr = (uint8_t *)&f; 52 | 53 | f_ptr[3] = source[0]; 54 | f_ptr[2] = source[1]; 55 | f_ptr[1] = source[2]; 56 | f_ptr[0] = source[3]; 57 | 58 | return f; 59 | } 60 | 61 | // As per manual, big-endian 32-bit first, then BE 16-bit part i.e [b3, b2, b1, b0, b5, b4] 62 | void xsens_coalesce_48BE_48LE( uint8_t *dest, uint8_t *source ) 63 | { 64 | dest[0] = source[3]; 65 | dest[1] = source[2]; 66 | dest[2] = source[1]; 67 | dest[3] = source[0]; 68 | dest[4] = source[5]; 69 | dest[5] = source[4]; 70 | } 71 | 72 | void xsens_quaternion_to_euler( float *quaternion, float *euler ) 73 | { 74 | float w = quaternion[0]; 75 | float x = quaternion[1]; 76 | float y = quaternion[2]; 77 | float z = quaternion[3]; 78 | 79 | // Roll: x-axis 80 | float sinr_cosp = 2 * ( w * x + y * z ); 81 | float cosr_cosp = 1 - 2 * ( x * x + y * y ); 82 | euler[0] = (float)atan2( sinr_cosp, cosr_cosp ); 83 | 84 | // Pitch: y-axis 85 | float sinp = 2 * ( w * y - z * x ); 86 | if( fabs( sinp ) >= 1 ) 87 | { 88 | euler[1] = (float)copysign( M_PI / 2, sinp ); // use 90 degrees if out of range 89 | } 90 | else 91 | { 92 | euler[1] = (float)asin( sinp ); 93 | } 94 | 95 | // Yaw: z-axis 96 | float siny_cosp = 2 * ( w * z + x * y ); 97 | float cosy_cosp = 1 - 2 * ( y * y + z * z ); 98 | euler[2] = (float)atan2( siny_cosp, cosy_cosp ); 99 | } 100 | 101 | void xsens_euler_to_quaternion( float *euler, float *quaternion ) 102 | { 103 | float roll = euler[0]; 104 | float pitch = euler[1]; 105 | float yaw = euler[2]; 106 | 107 | float cy = cos( yaw * 0.5f ); 108 | float sy = sin( yaw * 0.5f ); 109 | float cp = cos( pitch * 0.5f ); 110 | float sp = sin( pitch * 0.5f ); 111 | float cr = cos( roll * 0.5f ); 112 | float sr = sin( roll * 0.5f ); 113 | 114 | quaternion[0] = cr * cp * cy + sr * sp * sy; 115 | quaternion[1] = sr * cp * cy - cr * sp * sy; 116 | quaternion[2] = cr * sp * cy + sr * cp * sy; 117 | quaternion[3] = cr * cp * sy - sr * sp * cy; 118 | } 119 | 120 | int32_t xsens_f32_to_fp1220( float value ) 121 | { 122 | return round(value * (1UL<<20)); 123 | } 124 | 125 | float xsens_fp1220_to_f32( int32_t value ) 126 | { 127 | return (float)value / (1UL<<20); 128 | } 129 | 130 | int64_t xsens_f64_to_fp1632( double value ) 131 | { 132 | return (int64_t)(value * (1ULL<<32))&0x0000FFFFFFFFFFFF; 133 | } 134 | 135 | double xsens_fp1632_to_f64( int64_t value ) 136 | { 137 | // Is the sign-bit set? 138 | if( value&0x0000800000000000 ) 139 | { 140 | // Set the unused bytes, the division will handle -ve 141 | value |= 0xFFFF000000000000; 142 | } 143 | 144 | return (double)(value) / (1ULL<<32); 145 | } 146 | -------------------------------------------------------------------------------- /src/xsens_utility.h: -------------------------------------------------------------------------------- 1 | #ifndef XSENS_UTILITY_H 2 | #define XSENS_UTILITY_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "stdint.h" 9 | 10 | // Helper for number of elements in array 11 | #define XSENS_ARR_ELEM(arr) (sizeof(arr) / sizeof(*arr)) // Failed to compile? Ensure the type is an array! 12 | 13 | // Endianness conversion functions 14 | void xsens_swap_endian_u16( uint8_t *dest, uint8_t *source ); 15 | 16 | void xsens_swap_endian_u32( uint8_t *dest, uint8_t *source ); 17 | 18 | void xsens_swap_endian_u64( uint8_t *dest, uint8_t *source ); 19 | 20 | // Read 2 big-endian bytes and return a little-endian uint16 21 | uint16_t xsens_coalesce_16BE_16LE( uint8_t *source ); 22 | 23 | // Read 4 big-endian bytes and return a little-endian uint32 24 | uint32_t xsens_coalesce_32BE_32LE( uint8_t *source ); 25 | 26 | // Read 4 big-endian bytes and return a little-endian float 27 | float xsens_coalesce_32BE_F32LE( uint8_t *source ); 28 | 29 | // Converts 6-byte big-endian 'fixed-point' Q16.32 packet format to little-endian 30 | void xsens_coalesce_48BE_48LE( uint8_t *dest, uint8_t *source ); 31 | 32 | // Convert quaternion to roll/pitch/yaw angles (in radians) 33 | void xsens_quaternion_to_euler( float *quaternion, float *euler ); 34 | 35 | // Convert roll/pitch/yaw euler angles (in radians) to quaternion 36 | void xsens_euler_to_quaternion( float *euler, float *quaternion ); 37 | 38 | // Convert float32 to fixed-point 12.20 39 | // 12-bit integer with 20-bit fractional part 40 | // Expresses values across range [-2048.0 to 2047.9999990] 41 | // Refer https://base.xsens.com/s/article/An-explanation-of-the-MTi-fixed-point-output-formats-1605869706093 42 | int32_t xsens_f32_to_fp1220( float value ); 43 | 44 | // Convert fixed-point 12.20 to float32 45 | float xsens_fp1220_to_f32( int32_t value ); 46 | 47 | // Convert float32 to fixed-point 16.32 48 | // 16-bit integer with 32-bit fractional part 49 | // Expresses values across range [-32768.0 to 32767.9999999998] 50 | int64_t xsens_f64_to_fp1632( double value ); 51 | 52 | // Convert fixed-point 16.32 to float64 53 | double xsens_fp1632_to_f64( int64_t value ); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | 59 | #endif //end XSENS_UTILITY_H -------------------------------------------------------------------------------- /test/test_xsens_mti.c: -------------------------------------------------------------------------------- 1 | #include "unity.h" 2 | #include 3 | 4 | // MODULE UNDER TEST 5 | #include "xsens_mti.h" 6 | #include "xsens_mti_private.h" 7 | #include "xsens_constants.h" 8 | #include "xsens_mdata2.h" 9 | #include "xsens_utility.h" 10 | 11 | 12 | // DEFINITIONS 13 | 14 | // PRIVATE TYPES 15 | 16 | // PRIVATE DATA 17 | xsens_interface_t test_imu = { 0 }; 18 | 19 | uint32_t mocked_fn_override_called = 0; 20 | bool ack_config = false; 21 | 22 | // User-space callback overrides 23 | void handle_ack_gotoconfig( xsens_packet_buffer_t *packet ); 24 | 25 | void handle_ack_gotoconfig( xsens_packet_buffer_t *packet ) 26 | { 27 | ack_config = true; 28 | } 29 | 30 | 31 | // PRIVATE FUNCTIONS 32 | 33 | void mock_output_function( uint8_t *buffer, uint16_t size ); 34 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ); 35 | void mock_override_error_handler( xsens_packet_buffer_t *packet ); 36 | 37 | void mock_output_function( uint8_t *buffer, uint16_t size ) 38 | { 39 | printf("Output of %d bytes\n", size); 40 | } 41 | 42 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ) 43 | { 44 | printf("Notified of evt: %d\n", event); 45 | } 46 | 47 | void mock_override_error_handler( xsens_packet_buffer_t *packet ) 48 | { 49 | // Stub function - expect this to be called when a MT_ERROR is handled 50 | mocked_fn_override_called += 1; 51 | } 52 | 53 | // SETUP, TEARDOWN 54 | void setUp(void) 55 | { 56 | mocked_fn_override_called = 0; 57 | memset( &test_imu, 0, sizeof(test_imu) ); 58 | test_imu.output_cb = &mock_output_function; 59 | test_imu.event_cb = &mock_event_function; 60 | 61 | ack_config = false; 62 | } 63 | 64 | void tearDown(void) 65 | { 66 | 67 | } 68 | 69 | void test_parse_basic( void ) 70 | { 71 | // Simulate the IMU sending a GoToConfigAck packet 72 | // Preamble 0xFA 73 | // Address 0xFF 74 | // MID - 0x31 75 | // Size is 0x00 76 | // CRC is 0xD0 77 | 78 | // Setup userspace callback 79 | xsens_mti_override_id_handler( MT_ACK_GOTOCONFIG, &handle_ack_gotoconfig ); 80 | 81 | uint8_t test_packet[] = { 0xFA, 82 | 0xFF, 83 | 0x31, 84 | 0x00, 85 | 0xD0 }; 86 | 87 | for( uint16_t i = 0; i < sizeof(test_packet); i++ ) 88 | { 89 | xsens_mti_parse( &test_imu, test_packet[i]); 90 | } 91 | 92 | // Expect the callback to have fired 93 | TEST_ASSERT_TRUE( ack_config ); 94 | } 95 | 96 | void test_parse_buffer( void ) 97 | { 98 | // Check that the buffer helper function behaves the same 99 | // as the basic test above 100 | // Setup userspace callback 101 | xsens_mti_override_id_handler( MT_ACK_GOTOCONFIG, &handle_ack_gotoconfig ); 102 | 103 | uint8_t test_packet[] = { 0xFA, 104 | 0xFF, 105 | 0x31, 106 | 0x00, 107 | 0xD0 }; 108 | 109 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 110 | TEST_ASSERT_TRUE( ack_config ); 111 | } 112 | 113 | void test_handler_override( void ) 114 | { 115 | // Test that the internal function is called by default 116 | uint8_t test_packet[] = { 0xFA, 117 | 0xFF, 118 | 0x42, // Error packet type 119 | 0x01, // 1 byte payload 120 | 0x21, // parameter invalid/not in range 121 | 0x9D }; 122 | 123 | // Setup the override 124 | bool status = xsens_mti_override_id_handler( MT_ERROR, &mock_override_error_handler ); 125 | TEST_ASSERT_TRUE(status); 126 | 127 | // Test that the new function is called 128 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 129 | 130 | TEST_ASSERT_EQUAL_INT( 1, mocked_fn_override_called ); 131 | } 132 | 133 | void test_handler_override_with_null( void ) 134 | { 135 | bool status = xsens_mti_override_id_handler( MT_ERROR, NULL ); 136 | TEST_ASSERT_FALSE( status ); 137 | } 138 | 139 | void test_handler_override_invalid( void ) 140 | { 141 | bool status = xsens_mti_override_id_handler( 0, &mock_override_error_handler ); 142 | TEST_ASSERT_FALSE( status ); 143 | } 144 | 145 | 146 | /* 147 | // Quick function to calculate the correct CRC for a message 148 | // As some golden sample payloads didn't have one recorded... 149 | void test_calculate_crc( void ) 150 | { 151 | uint8_t test_packet[] = { 0xFA, 152 | 0xFF, 153 | 0x31, 154 | 0x00, 155 | }; 156 | 157 | uint8_t checksum = 0; 158 | 159 | for (int i = 1; i < sizeof(test_packet); i++) 160 | { 161 | checksum -= test_packet[i]; 162 | } 163 | 164 | printf("Output CRC should be 0x%X\n", (checksum & 0xff)); 165 | } 166 | */ -------------------------------------------------------------------------------- /test/test_xsens_mti_macros.c: -------------------------------------------------------------------------------- 1 | #include "unity.h" 2 | #include 3 | 4 | // MODULE UNDER TEST 5 | #include "xsens_mti.h" 6 | #include "xsens_mti_private.h" 7 | #include "xsens_constants.h" 8 | #include "xsens_mdata2.h" 9 | #include "xsens_utility.h" 10 | 11 | 12 | // DEFINITIONS 13 | 14 | // PRIVATE TYPES 15 | 16 | // PRIVATE FUNCTIONS 17 | 18 | void mock_output_function( uint8_t *buffer, uint16_t size ); 19 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ); 20 | 21 | 22 | void mock_output_function( uint8_t *buffer, uint16_t size ) 23 | { 24 | printf("Output of %d bytes\n", size); 25 | } 26 | 27 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ) 28 | { 29 | printf("Notified of evt: %d\n", event); 30 | } 31 | 32 | // PRIVATE DATA 33 | xsens_interface_t if_ground_truth = { 0 }; 34 | 35 | xsens_interface_t if_manual = { 36 | .state = 0, 37 | .packet = {0}, 38 | .payload_pos = 0, 39 | .crc = 0, 40 | .event_cb = &mock_event_function, 41 | .output_cb = &mock_output_function, 42 | }; 43 | 44 | xsens_interface_t if_rx_macro = XSENS_INTERFACE_RX( &mock_event_function ); 45 | xsens_interface_t if_rx_tx_macro = XSENS_INTERFACE_RX_TX( &mock_event_function, &mock_output_function ); 46 | 47 | // SETUP, TEARDOWN 48 | void setUp(void) 49 | { 50 | memset( &if_ground_truth, 0, sizeof(if_ground_truth) ); 51 | } 52 | 53 | void tearDown(void) 54 | { 55 | 56 | } 57 | 58 | // TESTS 59 | 60 | // Check a compile-time and run-time structure setup match 61 | void test_manual_macro( void ) 62 | { 63 | if_ground_truth.event_cb = &mock_event_function; 64 | if_ground_truth.output_cb = &mock_output_function; 65 | 66 | TEST_ASSERT_EQUAL_INT8_ARRAY( &if_manual, &if_ground_truth, sizeof(xsens_interface_t)); 67 | } 68 | 69 | // Check read-only macro matches 70 | void test_rx_macro( void ) 71 | { 72 | if_ground_truth.event_cb = &mock_event_function; 73 | 74 | TEST_ASSERT_EQUAL_INT8_ARRAY( &if_rx_macro, &if_ground_truth, sizeof(xsens_interface_t)); 75 | } 76 | 77 | // Check read-write macro matches 78 | void test_rx_tx_macro( void ) 79 | { 80 | if_ground_truth.event_cb = &mock_event_function; 81 | if_ground_truth.output_cb = &mock_output_function; 82 | 83 | TEST_ASSERT_EQUAL_INT8_ARRAY( &if_rx_tx_macro, &if_ground_truth, sizeof(xsens_interface_t)); 84 | } 85 | 86 | // From MT manual example, page 34 87 | void test_identifier_format_raw( void ) 88 | { 89 | // Quaternion ID 0x2010 90 | // Fp16.32 = 0x2 91 | // NED = 0x4 92 | // Fp16.32 and NED = 0x6 93 | // Results in 0x2016 94 | 95 | uint16_t result = XSENS_IDENTIFIER_FORMAT( 0x2010, 0x2, 0x4 ); 96 | TEST_ASSERT_EQUAL_HEX16( 0x2016, result ); 97 | } 98 | 99 | void test_identifier_format_pretty( void ) 100 | { 101 | // Same as test_identifier_format_raw(), but uses enums 102 | uint16_t result = XSENS_IDENTIFIER_FORMAT( XDI_QUATERNION, XSENS_FLOAT_FIXED1632, XSENS_COORD_NED ); 103 | TEST_ASSERT_EQUAL_HEX16( 0x2016, result ); 104 | } 105 | 106 | void test_identifier_format_get_precision( void ) 107 | { 108 | // (Quaternion,Float,ENU) 109 | TEST_ASSERT_EQUAL_HEX8( XSENS_FLOAT_SINGLE, XSENS_IDENTIFIER_FORMAT_GET_PRECISION( 0x2010 ) ); 110 | 111 | // (Magnetic Field,Fp1220,ENU) 112 | TEST_ASSERT_EQUAL_HEX8( XSENS_FLOAT_FIXED1220, XSENS_IDENTIFIER_FORMAT_GET_PRECISION( 0xC021 ) ); 113 | 114 | // Based on test_identifier_format_raw() value (Quaternion,Fp1632,NED) 115 | TEST_ASSERT_EQUAL_HEX8( XSENS_FLOAT_FIXED1632, XSENS_IDENTIFIER_FORMAT_GET_PRECISION( 0x2016 ) ); 116 | 117 | // (Temperature,Fp1632) 118 | TEST_ASSERT_EQUAL_HEX8( XSENS_FLOAT_FIXED1632, XSENS_IDENTIFIER_FORMAT_GET_PRECISION( 0x0812 ) ); 119 | 120 | // (Barometric Pressure,Double) 121 | TEST_ASSERT_EQUAL_HEX8( XSENS_FLOAT_DOUBLE, XSENS_IDENTIFIER_FORMAT_GET_PRECISION( 0x3013 ) ); 122 | } 123 | 124 | void test_identifier_format_get_coordinate_system( void ) 125 | { 126 | // (Quaternion,Float,ENU) 127 | TEST_ASSERT_EQUAL_HEX8( XSENS_COORD_ENU, XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( 0x2010 ) ); 128 | 129 | // Based on test_identifier_format_raw() value (Quaternion,Fp1632,NED) 130 | TEST_ASSERT_EQUAL_HEX8( XSENS_COORD_NED, XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( 0x2016 ) ); 131 | 132 | // (Quaternion,Float,NWU) 133 | TEST_ASSERT_EQUAL_HEX8( XSENS_COORD_NWU, XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( 0x201A ) ); 134 | } 135 | 136 | void test_identifier_format_simplify( void ) 137 | { 138 | // Macro returns the value with a reset final nibble (no number precision or coordinate ref) 139 | TEST_ASSERT_EQUAL_HEX16( 0x2010, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0x2010 ) ); 140 | TEST_ASSERT_EQUAL_HEX16( 0xC020, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0xC021 ) ); 141 | TEST_ASSERT_EQUAL_HEX16( 0x2010, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0x2016 ) ); 142 | TEST_ASSERT_EQUAL_HEX16( 0x0810, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0x0812 ) ); 143 | TEST_ASSERT_EQUAL_HEX16( 0x3010, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0x3013 ) ); 144 | TEST_ASSERT_EQUAL_HEX16( 0x2010, XSENS_IDENTIFIER_FORMAT_SIMPLIFY( 0x201A ) ); 145 | } 146 | 147 | void test_optionflag_set( void ) 148 | { 149 | uint32_t options = 0; 150 | 151 | // Set DisableAutoStore 152 | TEST_ASSERT_EQUAL( 0x00000001, XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_DISABLE_AUTOSTORE) ); 153 | options = 0; 154 | 155 | // Set DisableAutoMeasurement 156 | TEST_ASSERT_EQUAL( 0x00000002, XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT) ); 157 | options = 0; 158 | 159 | // Set AHS 160 | TEST_ASSERT_EQUAL( 0x00000010, XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_ENABLE_AHS) ); 161 | options = 0; 162 | 163 | // Set all three of the above at once 164 | XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_DISABLE_AUTOSTORE); 165 | XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT); 166 | XSENS_OPTION_FLAG_SET(options, XSENS_OPTFLAG_ENABLE_AHS); 167 | TEST_ASSERT_EQUAL( 0x00000013, options ); 168 | } 169 | 170 | void test_optionflag_is_set( void ) 171 | { 172 | uint32_t options = 0; 173 | 174 | TEST_ASSERT_FALSE( XSENS_OPTION_FLAG_IS_ANY_SET(options) ); 175 | 176 | // Set DisableAutoStore 177 | options = 0x00000001; 178 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_SET(options, XSENS_OPTFLAG_DISABLE_AUTOSTORE) ); 179 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_ANY_SET(options) ); 180 | 181 | // Set DisableAutoMeasurement 182 | options = 0x00000002; 183 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_SET(options, XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT) ); 184 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_ANY_SET(options) ); 185 | TEST_ASSERT_FALSE( XSENS_OPTION_FLAG_IS_SET(options, XSENS_OPTFLAG_DISABLE_AUTOSTORE) ); 186 | 187 | // Set AHS 188 | options = 0x00000010; 189 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_SET(options, XSENS_OPTFLAG_ENABLE_AHS) ); 190 | TEST_ASSERT_TRUE( XSENS_OPTION_FLAG_IS_ANY_SET(options) ); 191 | TEST_ASSERT_FALSE( XSENS_OPTION_FLAG_IS_SET(options, XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT) ); 192 | } -------------------------------------------------------------------------------- /test/test_xsens_mti_parse_messages.c: -------------------------------------------------------------------------------- 1 | // These tests cover inbound message IDs 2 | // Tests written from real captured packets from MTi-300 3 | 4 | #include "unity.h" 5 | #include 6 | 7 | // MODULE UNDER TEST 8 | #include "xsens_mti.h" 9 | #include "xsens_mti_private.h" 10 | #include "xsens_constants.h" 11 | #include "xsens_mdata2.h" 12 | #include "xsens_utility.h" 13 | 14 | // DEFINITIONS 15 | 16 | // PRIVATE TYPES 17 | 18 | // PRIVATE DATA 19 | xsens_interface_t test_imu = { 0 }; 20 | 21 | // PRIVATE FUNCTIONS 22 | 23 | void mock_output_function( uint8_t *buffer, uint16_t size ); 24 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ); 25 | 26 | void mock_output_function( uint8_t *buffer, uint16_t size ) 27 | { 28 | printf("Output of %d bytes\n", size); 29 | } 30 | 31 | // Because the library callbacks are fired once per output type, 32 | // its unclear how to handle that at a test level 33 | // so just cache all the callbacks and check against those? 34 | #define CACHE_DEPTH 20 35 | 36 | XsensEventFlag_t cb_evt_flag_cache[CACHE_DEPTH]; 37 | XsensEventData_t cb_evt_data_cache[CACHE_DEPTH]; 38 | uint8_t cache_usage = 0; 39 | 40 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ) 41 | { 42 | cb_evt_flag_cache[cache_usage] = event; 43 | memcpy( &cb_evt_data_cache[cache_usage], data, sizeof(XsensEventData_t)); 44 | cache_usage++; 45 | 46 | if( cache_usage > CACHE_DEPTH ) 47 | { 48 | TEST_FAIL_MESSAGE("Test harness cache ran out of room"); 49 | } 50 | } 51 | 52 | // SETUP, TEARDOWN 53 | void setUp(void) 54 | { 55 | memset( &cb_evt_flag_cache, 0, sizeof(cb_evt_flag_cache) ); 56 | memset( &cb_evt_data_cache, 0, sizeof(cb_evt_data_cache) ); 57 | cache_usage = 0; 58 | 59 | memset( &test_imu, 0, sizeof(test_imu) ); 60 | test_imu.output_cb = &mock_output_function; 61 | test_imu.event_cb = &mock_event_function; 62 | } 63 | 64 | void tearDown(void) 65 | { 66 | 67 | } 68 | 69 | // TESTS 70 | void test_parse_GoToConfigAck( void ) 71 | { 72 | uint8_t test_packet[] = { 0xFA, 73 | 0xFF, 74 | 0x31, 75 | 0x00, 76 | 0xD0 }; 77 | 78 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 79 | 80 | TEST_IGNORE(); 81 | } 82 | 83 | void test_parse_StringOutputType( void ) 84 | { 85 | // 86 | uint8_t test_packet[] = { 0xFA, 87 | 0xFF, 88 | 0x8F, 89 | 0x00, 90 | 0x72 }; 91 | 92 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 93 | 94 | TEST_IGNORE(); 95 | } 96 | 97 | void test_parse_OutputConfiguration( void ) 98 | { 99 | //{(PacketCounter, Freq: 65535), (SampleTimeFine, Freq: 65535)} 100 | uint8_t test_packet[] = { 0xFA, 101 | 0xFF, 102 | 0xC1, 103 | 0x08, 104 | 0x10, 0x20, 0xFF, 0xFF, 0x10, 0x60, 0xFF, 0xFF, 105 | 0x9C }; 106 | 107 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 108 | 109 | TEST_IGNORE(); 110 | 111 | } 112 | 113 | void test_parse_InitMTResults( void ) 114 | { 115 | //DeviceID: 037003F8 116 | uint8_t test_packet[] = { 0xFA, 117 | 0xFF, 118 | 0x03, 119 | 0x04, 120 | 0x03, 0x70, 0x03, 0xF8, 121 | 0x8C }; 122 | 123 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 124 | 125 | TEST_IGNORE(); 126 | } 127 | 128 | void test_parse_Configuration( void ) 129 | { 130 | // DeviceID: 037003F8, 131 | // Sample Period: 1152, 132 | // OutputSkipFactor: 0, 133 | // SyncInMode: 0, 134 | // SyncInSkipFactor: 0, 135 | // SyncInOffset: 0, (Year: 0, Month: 0, Day: 0), (Hour: 0, Min: 0, Sec: 0, Ms: 0), 136 | // reserved: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00, 137 | // reserved: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00, 138 | // numDevices: 1, 139 | // DeviceID: 037003F8, 140 | // MTDataLength: 0, 141 | // OutputMode: 0, 142 | // OutputSettings: 1, 143 | // reserved: 00 27 01 08 02 49 05 01 144 | uint8_t test_packet[] = { 0xFA, 145 | 0xFF, 146 | 0x0D, 147 | 0x76, 148 | 0x03, 0x70, 0x03, 0xF8, 0x04, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x70, 0x03, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x27, 0x01, 0x08, 0x02, 0x49, 0x05, 0x01, 149 | 0x9B }; 150 | 151 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 152 | 153 | 154 | 155 | TEST_IGNORE(); 156 | 157 | } 158 | 159 | void test_parse_FirmwareRev( void ) 160 | { 161 | //(major: 1, minor: 8, revision: 2, buildnr: 37, svnrev: 70964) 162 | uint8_t test_packet[] = { 0xFA, 163 | 0xFF, 164 | 0x13, 165 | 0x0B, 166 | 0x01, 0x08, 0x02, 0x00, 0x00, 0x00, 0x25, 0x00, 0x01, 0x15, 0x34, 167 | 0x69 }; 168 | 169 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 170 | 171 | TEST_IGNORE(); 172 | } 173 | 174 | void test_parse_GotoMeasurementAck( void ) 175 | { 176 | uint8_t test_packet[] = { 0xFA, 177 | 0xFF, 178 | 0x11, 179 | 0x00, 180 | 0xF0 }; 181 | 182 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 183 | 184 | TEST_IGNORE(); 185 | } 186 | 187 | 188 | 189 | 190 | void test_parse_mdata2_quat_minimal( void ) 191 | { 192 | // Basic quaternion 'only' packet 193 | 194 | // (PacketCounter, 2 bytes, 18050), 195 | // (SampleTimeFine, 4 bytes, 29686846), 196 | // (Quaternion|Float|ENU, 16 bytes, (q0: 0.94455600, q1: -0.32308814, q2: 0.01374718, q3: -0.05691256)), 197 | // (StatusWord, 4 bytes, 00000000010000000000000000000011) 198 | 199 | uint8_t test_packet[] = { 0xFA, 200 | 0xFF, 201 | 0x36, 202 | 0x26, 203 | 0x10, 0x20, 0x02, 0x46, 0x82, 0x10, 0x60, 0x04, 0x01, 0xC4, 0xFC, 0x3E, 0x20, 0x10, 0x10, 0x3F, 0x71, 0xCE, 0x6C, 0xBE, 0xA5, 0x6B, 0xCF, 0x3C, 0x61, 0x3B, 0xD8, 0xBD, 0x69, 0x1D, 0x25, 0xE0, 0x20, 0x04, 0x00, 0x40, 0x00, 0x03, 204 | 0x12 }; 205 | 206 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 207 | 208 | // PacketCounter 209 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] ); 210 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type ); 211 | TEST_ASSERT_EQUAL_INT( 18050, cb_evt_data_cache[0].data.u2); 212 | 213 | // SampleTimeFine 214 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TIME_FINE, cb_evt_flag_cache[1] ); 215 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[1].type ); 216 | TEST_ASSERT_EQUAL_INT( 29686846, cb_evt_data_cache[1].data.u4); 217 | 218 | // Quaternion 219 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] ); 220 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type ); 221 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref ); 222 | float golden_quat[4] = { 0.94455600, -0.32308814, 0.01374718, -0.05691256 }; 223 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4); 224 | 225 | // StatusWord 226 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_STATUS_WORD, cb_evt_flag_cache[3] ); 227 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[3].type ); 228 | union XDI_STATUS32_UNION status; 229 | status.word = cb_evt_data_cache[3].data.u4; 230 | TEST_ASSERT_TRUE( status.bitfield.self_test ); 231 | TEST_ASSERT_TRUE( status.bitfield.filter_valid ); 232 | TEST_ASSERT_FALSE( status.bitfield.clipping ); 233 | TEST_ASSERT_TRUE( status.bitfield.sync_out_mark ); 234 | // not an exhaustive status check, but hits critical flags 235 | } 236 | 237 | void test_parse_mdata2_shake_clipping( void ) 238 | { 239 | // Packet when the IMU was being shaked intensely 240 | // GyroY, AccZ should be clipping 241 | // FilterValid flag should be false 242 | 243 | // (PacketCounter, 2 bytes, 64389), 244 | // (SampleTimeFine, 4 bytes, 27564254), 245 | // (Quaternion|Float|ENU, 16 bytes, 246 | // (q0: 0.66437358, q1: -0.42175028, q2: 0.02720882, q3: 0.61643654)), 247 | // (Acceleration|Float, 12 bytes, 248 | // (accX: -30.28455162, accY: -29.60960007, accZ: -71.76024628)), 249 | // (DeltaV|Float, 12 bytes, 250 | // (x: -0.07186279, y: -0.07130830, z: -0.18206376)), 251 | // (FreeAcceleration|Float, 12 bytes, (freeAccX: 52.39491272, freeAccY: -62.83823395, freeAccZ: -25.59408188)), 252 | // (RateOfTurn|Float, 12 bytes, (gyrX: 4.16570139, gyrY: -10.33340263, gyrZ: -4.51734877)), 253 | // (DeltaQ|Float, 16 bytes, (q0: 0.99988699, q1: 0.00520693, q2: -0.01291627, q3: -0.00564647)), 254 | // (MagneticField|Float, 12 bytes, (magX: 0.43057421, magY: -0.23942292, magZ: 1.37189472)), 255 | // (BaroPressure, 4 bytes, Pressure: 100062), 256 | // (StatusWord, 4 bytes, 00000000010010000001010000000001) 257 | 258 | uint8_t test_packet[] = { 0xFA, 259 | 0xFF, 260 | 0x36, 261 | 0x8B, 262 | 0x10, 0x20, 0x02, 0xFB, 0x85, 0x10, 0x60, 0x04, 0x01, 0xA4, 0x98, 0xDE, 0x20, 0x10, 0x10, 0x3F, 0x2A, 0x14, 0x63, 0xBE, 0xD7, 0xEF, 0xA7, 0x3C, 0xDE, 0xE5, 0x08, 0x3F, 0x1D, 0xCE, 0xC9, 0x40, 0x20, 0x0C, 0xC1, 0xF2, 0x46, 0xC3, 0xC1, 0xEC, 0xE0, 0x76, 0xC2, 0x8F, 0x85, 0x3F, 0x40, 0x10, 0x0C, 0xBD, 0x93, 0x2C, 0xCC, 0xBD, 0x92, 0x0A, 0x16, 0xBE, 0x3A, 0x6E, 0xEC, 0x40, 0x30, 0x0C, 0x42, 0x51, 0x94, 0x64, 0xC2, 0x7B, 0x5A, 0x5A, 0xC1, 0xCC, 0xC0, 0xAE, 0x80, 0x20, 0x0C, 0x40, 0x85, 0x4D, 0x6D, 0xC1, 0x25, 0x55, 0x9E, 0xC0, 0x90, 0x8E, 0x1F, 0x80, 0x30, 0x10, 0x3F, 0x7F, 0xF8, 0x98, 0x3B, 0xAA, 0x9E, 0xE5, 0xBC, 0x53, 0x9E, 0xC0, 0xBB, 0xB9, 0x06, 0x0C, 0xC0, 0x20, 0x0C, 0x3E, 0xDC, 0x74, 0x39, 0xBE, 0x75, 0x2B, 0x48, 0x3F, 0xAF, 0x9A, 0x3F, 0x30, 0x10, 0x04, 0x00, 0x01, 0x86, 0xDE, 0xE0, 0x20, 0x04, 0x00, 0x48, 0x14, 0x01, 263 | 0x8E }; 264 | 265 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 266 | 267 | // PacketCounter 268 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] ); 269 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type ); 270 | TEST_ASSERT_EQUAL_INT( 64389, cb_evt_data_cache[0].data.u2); 271 | 272 | // SampleTimeFine 273 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TIME_FINE, cb_evt_flag_cache[1] ); 274 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[1].type ); 275 | TEST_ASSERT_EQUAL_INT( 27564254, cb_evt_data_cache[1].data.u4); 276 | 277 | // Quaternion 278 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] ); 279 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type ); 280 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref ); 281 | float golden_quat[4] = { 0.66437358, -0.42175028, 0.02720882, 0.61643654 }; 282 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4); 283 | 284 | // Acceleration 285 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ACCELERATION, cb_evt_flag_cache[3] ); 286 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[3].type ); 287 | float golden_acc[3] = { -30.28455162, -29.60960007, -71.76024628 }; 288 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_acc, cb_evt_data_cache[3].data.f4x3, 3); 289 | 290 | // DeltaV 291 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_V, cb_evt_flag_cache[4] ); 292 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[4].type ); 293 | float golden_dv[3] = { -0.07186279, -0.07130830, -0.18206376 }; 294 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_dv, cb_evt_data_cache[4].data.f4x3, 3); 295 | 296 | // FreeAcceleration 297 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_FREE_ACCELERATION, cb_evt_flag_cache[5] ); 298 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[5].type ); 299 | float golden_fr_acc[3] = { 52.39491272, -62.83823395, -25.59408188 }; 300 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_fr_acc, cb_evt_data_cache[5].data.f4x3, 3); 301 | 302 | // RateOfTurn 303 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_RATE_OF_TURN, cb_evt_flag_cache[6] ); 304 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[6].type ); 305 | float golden_rot[3] = { 4.16570139, -10.33340263, -4.51734877 }; 306 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_rot, cb_evt_data_cache[6].data.f4x3, 3); 307 | 308 | // DeltaQ 309 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_Q, cb_evt_flag_cache[7] ); 310 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[7].type ); 311 | float golden_dq[4] = { 0.99988699, 0.00520693, -0.01291627, -0.00564647 }; 312 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_dq, cb_evt_data_cache[7].data.f4x4, 4); 313 | 314 | // MagneticField 315 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_MAGNETIC, cb_evt_flag_cache[8] ); 316 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[8].type ); 317 | float golden_mag[3] = { 0.43057421, -0.23942292, 1.37189472 }; 318 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_mag, cb_evt_data_cache[8].data.f4x3, 3); 319 | 320 | // Pressure 321 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PRESSURE, cb_evt_flag_cache[9] ); 322 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[9].type ); 323 | TEST_ASSERT_EQUAL_INT( 100062, cb_evt_data_cache[9].data.u4); 324 | 325 | // StatusWord 326 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_STATUS_WORD, cb_evt_flag_cache[10] ); 327 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[10].type ); 328 | union XDI_STATUS32_UNION status; 329 | status.word = cb_evt_data_cache[10].data.u4; 330 | 331 | TEST_ASSERT_TRUE( status.bitfield.self_test ); 332 | TEST_ASSERT_FALSE( status.bitfield.filter_valid ); 333 | TEST_ASSERT_TRUE( status.bitfield.clipping ); 334 | TEST_ASSERT_TRUE( status.bitfield.sync_out_mark ); 335 | 336 | TEST_ASSERT_FALSE( status.bitfield.clip_acc_x ); 337 | TEST_ASSERT_FALSE( status.bitfield.clip_acc_y ); 338 | TEST_ASSERT_TRUE( status.bitfield.clip_acc_z ); 339 | 340 | TEST_ASSERT_FALSE( status.bitfield.clip_gyro_x ); 341 | TEST_ASSERT_TRUE( status.bitfield.clip_gyro_y ); 342 | TEST_ASSERT_FALSE( status.bitfield.clip_gyro_z ); 343 | 344 | TEST_ASSERT_FALSE( status.bitfield.clip_mag_x ); 345 | TEST_ASSERT_FALSE( status.bitfield.clip_mag_y ); 346 | TEST_ASSERT_FALSE( status.bitfield.clip_mag_z ); 347 | } 348 | 349 | void test_parse_mdata2_full( void ) 350 | { 351 | // Tests a mdata2 packet where most fields are in one packet 352 | 353 | // (PacketCounter, 2 bytes, 37261), 354 | // (SampleTimeFine, 4 bytes, 20332454), 355 | // (Quaternion|Float|ENU, 16 bytes, 356 | // (q0: 0.71045315, q1: 0.69453555, q2: -0.07777759, q3: -0.08262789)), 357 | // (Acceleration|Float, 12 bytes, 358 | // (accX: -0.05550629, accY: 9.81465530, accZ: 0.21842313)), 359 | // (DeltaV|Float, 12 bytes, 360 | // (x: -0.00013867, y: 0.02453661, z: 0.00054736)), 361 | // (FreeAcceleration|Float, 12 bytes, 362 | // (freeAccX: -0.01142347, freeAccY: 0.01110744, freeAccZ: 0.02007198)), 363 | // (RateOfTurn|Float, 12 bytes, 364 | // (gyrX: 0.02131760, gyrY: -0.00327826, gyrZ: -0.00163019)), 365 | // (DeltaQ|Float, 16 bytes, 366 | // (q0: 1.00000000, q1: 0.00002665, q2: -0.00000410, q3: -0.00000204)), 367 | // (MagneticField|Float, 12 bytes, 368 | // (magX: -0.49215657, magY: 0.70221740, magZ: -1.25496686)), 369 | // (Temperature|Float, 4 bytes, Temp: 37.62500000), 370 | // (BaroPressure, 4 bytes, Pressure: 100065), 371 | // (StatusWord, 4 bytes, 00000000010000000000000000000011) 372 | 373 | uint8_t test_packet[] = { 0xFA, 374 | 0xFF, 375 | 0x36, 376 | 0x92, 377 | 0x10, 0x20, 0x02, 0x91, 0x8D, 0x10, 0x60, 0x04, 0x01, 0x36, 0x3F, 0xA6, 0x20, 0x10, 0x10, 0x3F, 0x35, 0xE0, 0x42, 0x3F, 0x31, 0xCD, 0x15, 0xBD, 0x9F, 0x49, 0xDB, 0xBD, 0xA9, 0x38, 0xCF, 0x40, 0x20, 0x0C, 0xBD, 0x63, 0x5A, 0x90, 0x41, 0x1D, 0x08, 0xD4, 0x3E, 0x5F, 0xAA, 0x50, 0x40, 0x10, 0x0C, 0xB9, 0x11, 0x68, 0x00, 0x3C, 0xC9, 0x01, 0x00, 0x3A, 0x0F, 0x7D, 0x00, 0x40, 0x30, 0x0C, 0xBC, 0x3B, 0x29, 0x80, 0x3C, 0x35, 0xFC, 0x00, 0x3C, 0xA4, 0x6E, 0x00, 0x80, 0x20, 0x0C, 0x3C, 0xAE, 0xA2, 0x41, 0xBB, 0x56, 0xD8, 0x00, 0xBA, 0xD5, 0xAC, 0x01, 0x80, 0x30, 0x10, 0x3F, 0x80, 0x00, 0x00, 0x37, 0xDF, 0x88, 0x01, 0xB6, 0x89, 0x80, 0x00, 0xB6, 0x08, 0xC0, 0x00, 0xC0, 0x20, 0x0C, 0xBE, 0xFB, 0xFB, 0xF2, 0x3F, 0x33, 0xC4, 0x85, 0xBF, 0xA0, 0xA2, 0xC1, 0x08, 0x10, 0x04, 0x42, 0x16, 0x80, 0x00, 0x30, 0x10, 0x04, 0x00, 0x01, 0x86, 0xE1, 0xE0, 0x20, 0x04, 0x00, 0x40, 0x00, 0x03, 378 | 0xDD }; 379 | 380 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 381 | 382 | // PacketCounter 383 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] ); 384 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type ); 385 | TEST_ASSERT_EQUAL_INT( 37261, cb_evt_data_cache[0].data.u2); 386 | 387 | // SampleTimeFine 388 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TIME_FINE, cb_evt_flag_cache[1] ); 389 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[1].type ); 390 | TEST_ASSERT_EQUAL_INT( 20332454, cb_evt_data_cache[1].data.u4); 391 | 392 | // Quaternion 393 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] ); 394 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type ); 395 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref ); 396 | float golden_quat[4] = { 0.71045315, 0.69453555, -0.07777759, -0.08262789 }; 397 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4); 398 | 399 | // Acceleration 400 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ACCELERATION, cb_evt_flag_cache[3] ); 401 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[3].type ); 402 | float golden_acc[3] = { -0.05550629, 9.81465530, 0.21842313 }; 403 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_acc, cb_evt_data_cache[3].data.f4x3, 3); 404 | 405 | // DeltaV 406 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_V, cb_evt_flag_cache[4] ); 407 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[4].type ); 408 | float golden_dv[3] = { -0.00013867, 0.02453661, 0.00054736 }; 409 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_dv, cb_evt_data_cache[4].data.f4x3, 3); 410 | 411 | // FreeAcceleration 412 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_FREE_ACCELERATION, cb_evt_flag_cache[5] ); 413 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[5].type ); 414 | float golden_fr_acc[3] = { -0.01142347, 0.01110744, 0.02007198 }; 415 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_fr_acc, cb_evt_data_cache[5].data.f4x3, 3); 416 | 417 | // RateOfTurn 418 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_RATE_OF_TURN, cb_evt_flag_cache[6] ); 419 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[6].type ); 420 | float golden_rot[3] = { 0.02131760, -0.00327826, -0.00163019 }; 421 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_rot, cb_evt_data_cache[6].data.f4x3, 3); 422 | 423 | // DeltaQ 424 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_Q, cb_evt_flag_cache[7] ); 425 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[7].type ); 426 | // apparently floating point conversion is differing between MTManager and the decoded version 427 | // manually approved with a tolerance test instead of exact match 428 | float golden_dq[4] = { 1.00000000, 0.000026647, -0.00000410, -0.000002040 }; 429 | 430 | double flt_epsilon = 0.000000119; 431 | TEST_ASSERT_FLOAT_WITHIN( flt_epsilon, golden_dq[0], cb_evt_data_cache[7].data.f4x4[0]); 432 | TEST_ASSERT_FLOAT_WITHIN( flt_epsilon, golden_dq[1], cb_evt_data_cache[7].data.f4x4[1]); 433 | TEST_ASSERT_FLOAT_WITHIN( flt_epsilon, golden_dq[2], cb_evt_data_cache[7].data.f4x4[2]); 434 | TEST_ASSERT_FLOAT_WITHIN( flt_epsilon, golden_dq[3], cb_evt_data_cache[7].data.f4x4[3]); 435 | 436 | // MagneticField 437 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_MAGNETIC, cb_evt_flag_cache[8] ); 438 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[8].type ); 439 | float golden_mag[3] = { -0.49215657, 0.70221740, -1.25496686 }; 440 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_mag, cb_evt_data_cache[8].data.f4x3, 3); 441 | 442 | // Temperature 443 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TEMPERATURE, cb_evt_flag_cache[9] ); 444 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT, cb_evt_data_cache[9].type ); 445 | TEST_ASSERT_EQUAL_FLOAT( 37.62500000, cb_evt_data_cache[9].data.f4); 446 | 447 | // Pressure 448 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PRESSURE, cb_evt_flag_cache[10] ); 449 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[10].type ); 450 | TEST_ASSERT_EQUAL_INT( 100065, cb_evt_data_cache[10].data.u4); 451 | 452 | 453 | // StatusWord 454 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_STATUS_WORD, cb_evt_flag_cache[11] ); 455 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U32, cb_evt_data_cache[11].type ); 456 | union XDI_STATUS32_UNION status; 457 | status.word = cb_evt_data_cache[11].data.u4; 458 | 459 | TEST_ASSERT_TRUE( status.bitfield.self_test ); 460 | TEST_ASSERT_TRUE( status.bitfield.filter_valid ); 461 | TEST_ASSERT_FALSE( status.bitfield.clipping ); 462 | TEST_ASSERT_TRUE( status.bitfield.sync_out_mark ); 463 | 464 | } 465 | 466 | void test_parse_mdata2_number_formats( void ) 467 | { 468 | // Tests a packet where fixed-point 12.20, 16.32, and double formatted outputs are used 469 | 470 | // (PacketCounter, 2 bytes, 15325), 471 | // (EulerAngles|Fp1220|ENU, 12 bytes, 472 | // (Roll: -0.9826155, Pitch: -0.1385441, Yaw: 115.7006302)), 473 | // (DeltaQ|Fp1632, 24 bytes, 474 | // q0: 1.000000000233, q1: -0.000429107808, q2: -0.000302935718, q3: -0.000177090988)), 475 | // (MagneticField|Double, 24 bytes, 476 | // (magX: 0.833747744560242, magY: -0.434182971715927, magZ: 1.617681622505188)), 477 | // (Temperature|Float, 4 bytes, Temp: 27.4375000), 478 | 479 | uint8_t test_packet[] = { 0xFA, 480 | 0xFF, 481 | 0x36, 482 | 0x51, 483 | 0x10, 0x20, 0x02, 0x3B, 0xDD, 0x20, 0x31, 0x0C, 0xFF, 0xF0, 0x47, 0x35, 0xFF, 0xFD, 0xC8, 0x86, 0x07, 0x3B, 0x35, 0xC8, 0x80, 0x32, 0x18, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0xFF, 0xE3, 0xE0, 0xC4, 0xFF, 0xFF, 0xFF, 0xEC, 0x25, 0x95, 0xFF, 0xFF, 0xFF, 0xF4, 0x64, 0xE8, 0xFF, 0xFF, 0xC0, 0x23, 0x18, 0x3F, 0xEA, 0xAE, 0x0F, 0xC0, 0x00, 0x00, 0x00, 0xBF, 0xDB, 0xC9, 0xA7, 0x60, 0x00, 0x00, 0x00, 0x3F, 0xF9, 0xE2, 0x06, 0x20, 0x00, 0x00, 0x00, 0x08, 0x10, 0x04, 0x41, 0xDB, 0x80, 0x00, 484 | 0xAC }; 485 | 486 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 487 | 488 | // PacketCounter 489 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] ); 490 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type ); 491 | TEST_ASSERT_EQUAL_INT( 15325, cb_evt_data_cache[0].data.u2); 492 | 493 | // EulerAngles 494 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_EULER, cb_evt_flag_cache[1] ); 495 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[1].coord_ref ); 496 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1220FP3, cb_evt_data_cache[1].type ); 497 | float golden_pry[3] = { -0.9826155, -0.1385441, 115.7006302 }; 498 | 499 | // Convert the fixed-point 1220 roll/pitch/yaw array into floats 500 | float result_pry[3] = { 0 }; 501 | result_pry[0] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[0] ); 502 | result_pry[1] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[1] ); 503 | result_pry[2] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[2] ); 504 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_pry, result_pry, 3); 505 | 506 | // DeltaQ 507 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_Q, cb_evt_flag_cache[2] ); 508 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1632FP4, cb_evt_data_cache[2].type ); 509 | double golden_dq[4] = { 1.000000000233, -0.000429107808, -0.000302935718, -0.000177090988 }; 510 | 511 | // Convert the fixed-point 1632 delta-quaternion array[4] into doubles 512 | double result_dq[4] = { 0 }; 513 | result_dq[0] = xsens_fp1632_to_f64( cb_evt_data_cache[2].data.fp1632x4[0] ); 514 | result_dq[1] = xsens_fp1632_to_f64( cb_evt_data_cache[2].data.fp1632x4[1] ); 515 | result_dq[2] = xsens_fp1632_to_f64( cb_evt_data_cache[2].data.fp1632x4[2] ); 516 | result_dq[3] = xsens_fp1632_to_f64( cb_evt_data_cache[2].data.fp1632x4[3] ); 517 | 518 | double dbl_epsilon = 0.00000000000050000; 519 | TEST_ASSERT_DOUBLE_WITHIN( dbl_epsilon, golden_dq[0], result_dq[0] ); 520 | TEST_ASSERT_DOUBLE_WITHIN( dbl_epsilon, golden_dq[1], result_dq[1] ); 521 | TEST_ASSERT_DOUBLE_WITHIN( dbl_epsilon, golden_dq[2], result_dq[2] ); 522 | TEST_ASSERT_DOUBLE_WITHIN( dbl_epsilon, golden_dq[3], result_dq[3] ); 523 | 524 | // MagneticField 525 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_MAGNETIC, cb_evt_flag_cache[3] ); 526 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_DOUBLE3, cb_evt_data_cache[3].type ); 527 | double golden_mag[3] = { 0.833747744560242, -0.434182971715927, 1.617681622505188 }; 528 | TEST_ASSERT_EQUAL_DOUBLE_ARRAY( golden_mag, cb_evt_data_cache[3].data.f4x3, 3); 529 | 530 | // Temperature 531 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TEMPERATURE, cb_evt_flag_cache[4] ); 532 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT, cb_evt_data_cache[4].type ); 533 | TEST_ASSERT_EQUAL_FLOAT( 27.4375000, cb_evt_data_cache[4].data.f4); 534 | } 535 | 536 | void test_parse_mdata2_number_formats_2( void ) 537 | { 538 | // Tests another packet where single, fixed-point 12.20, 16.32, and double formatted outputs are used 539 | 540 | // (PacketCounter, 2 bytes, 65144), 541 | // (Quaternion|Float|ENU, 16 bytes, 542 | // q0: 0.6447144, q1: -0.00622723, q2: -0.00247884, q3: 0.7643942)), 543 | // (MagneticField|Fp1220, 12 bytes, 544 | // (magX: 0.9619465, magY: -0.2602215, magZ: 1.7812529)), 545 | // (Temperature|Fp1632, 6 bytes, Temp: 24.375000000000), 546 | // (Pressure|Double, 8 bytes, Pressure: 101669), 547 | // MT Manager configuration, packet viewer and MID show this as double, but actual packet is 4-byte uint32 548 | // TODO: report MTManager/documentation/firmware bug to xsens? 549 | 550 | uint8_t test_packet[] = { 0xFA, 551 | 0xFF, 552 | 0x36, 553 | 0x37, 554 | 0x10, 0x20, 0x02, 0xFE, 0x78, 0x20, 0x10, 0x10, 0x3F, 0x25, 0x0C, 0x01, 0xBB, 0xCC, 0x0D, 0xD7, 0xBB, 0x22, 0x74, 0x0C, 0x3F, 0x43, 0xAF, 0x57, 0xC0, 0x21, 0x0C, 0x00, 0x0F, 0x64, 0x22, 0xFF, 0xFB, 0xD6, 0x22, 0x00, 0x1C, 0x80, 0x03, 0x08, 0x12, 0x06, 0x60, 0x00, 0x00, 0x00, 0x00, 0x18, 0x30, 0x13, 0x04, 0x00, 0x01, 0x8D, 0x25, 555 | 0x46 }; 556 | 557 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 558 | 559 | // PacketCounter 560 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] ); 561 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type ); 562 | TEST_ASSERT_EQUAL_INT( 65144, cb_evt_data_cache[0].data.u2); 563 | 564 | // Quaternion 565 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[1] ); 566 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[1].type ); 567 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[1].coord_ref ); 568 | float golden_quat[4] = { 0.6447144, -0.00622723, -0.00247884, 0.7643942 }; 569 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[1].data.f4x4, 4); 570 | 571 | // MagneticField 572 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_MAGNETIC, cb_evt_flag_cache[2] ); 573 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1220FP3, cb_evt_data_cache[2].type ); 574 | float golden_mag[3] = { 0.9619465, -0.2602215, 1.7812529 }; 575 | 576 | // Convert to floats 577 | float result_mag[3] = { 0 }; 578 | result_mag[0] = xsens_fp1220_to_f32( cb_evt_data_cache[2].data.fp1220x3[0] ); 579 | result_mag[1] = xsens_fp1220_to_f32( cb_evt_data_cache[2].data.fp1220x3[1] ); 580 | result_mag[2] = xsens_fp1220_to_f32( cb_evt_data_cache[2].data.fp1220x3[2] ); 581 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_mag, result_mag, 3); 582 | 583 | // Temperature 584 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TEMPERATURE, cb_evt_flag_cache[3] ); 585 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1632FP, cb_evt_data_cache[3].type ); 586 | double result_temp = xsens_fp1632_to_f64( cb_evt_data_cache[3].data.fp1632 ); 587 | TEST_ASSERT_EQUAL_DOUBLE( 24.375000000000, result_temp); 588 | 589 | // Pressure 590 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_PRESSURE, cb_evt_flag_cache[4] ); 591 | 592 | // Probable bug in firmware/MTManager output - configuration asked for double precision and the 593 | // identifier reflects that 594 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_DOUBLE, cb_evt_data_cache[4].type ); 595 | // but the actual payload is still a uint32, so this test fails 596 | 597 | // TEST_ASSERT_EQUAL_INT( 101669, cb_evt_data_cache[4].data.u4); 598 | 599 | } 600 | 601 | void test_parse_mdata2_rotmatrix_float( void ) 602 | { 603 | // (RotationMatrix|Float|ENU, 36 bytes, 604 | // (a: -0.21940619, b: -0.97555816, c: 0.01214410, 605 | // d: 0.97537732, e: -0.21961689, f: -0.02019581, 606 | // g: 0.02236923, h: 0.00741399, i: 0.99972248)) 607 | 608 | uint8_t test_packet[] = { 0xFA, 609 | 0xFF, 610 | 0x36, 611 | 0x27, 612 | 0x20, 0x20, 0x24, 0xBE, 0x60, 0xAC, 0x04, 0xBF, 0x79, 0xBE, 0x2E, 0x3C, 0x46, 0xF8, 0x07, 0x3F, 0x79, 0xB2, 0x54, 0xBE, 0x60, 0xE3, 0x40, 0xBC, 0xA5, 0x71, 0xAF, 0x3C, 0xB7, 0x3F, 0xAE, 0x3B, 0xF2, 0xF1, 0x0C, 0x3F, 0x7F, 0xED, 0xD0, 613 | 0xC8 }; 614 | 615 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 616 | 617 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] ); 618 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref ); 619 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT9, cb_evt_data_cache[0].type ); 620 | float golden_rot[9] = { -0.21940619, -0.97555816, 0.01214410, 621 | 0.97537732, -0.21961689, -0.02019581, 622 | 0.02236923, 0.00741399, 0.99972248 }; 623 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_rot, cb_evt_data_cache[0].data.f4x9, 9); 624 | 625 | } 626 | 627 | void test_parse_mdata2_rotmatrix_fp1220( void ) 628 | { 629 | // (RotationMatrix|Fp1220|ENU, 36 bytes, 630 | // (a: -0.23230076, b: -0.97257423, c: 0.01154137, 631 | // d: 0.97240829, e: -0.23248863, f: -0.01924133, 632 | // g: 0.02139759, h: 0.00675297, i: 0.99974823)) 633 | 634 | uint8_t test_packet[] = { 0xFA, 635 | 0xFF, 636 | 0x36, 637 | 0x27, 638 | 0x20, 0x21, 0x24, 0xFF, 0xFC, 0x48, 0x7F, 0xFF, 0xF0, 0x70, 0x56, 0x00, 0x00, 0x2F, 0x46, 0x00, 0x0F, 0x8E, 0xFC, 0xFF, 0xFC, 0x47, 0xBA, 0xFF, 0xFF, 0xB1, 0x30, 0x00, 0x00, 0x57, 0xA5, 0x00, 0x00, 0x1B, 0xA9, 0x00, 0x0F, 0xFE, 0xF8, 639 | 0x1A }; 640 | 641 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 642 | 643 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] ); 644 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref ); 645 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1220FP9, cb_evt_data_cache[0].type ); 646 | float golden_rot[9] = { -0.23230076, -0.97257423, 0.01154137, 647 | 0.97240829, -0.23248863, -0.01924133, 648 | 0.02139759, 0.00675297, 0.99974823 }; 649 | float result_rot[9] = { 0 }; 650 | 651 | for( uint8_t i = 0; i < 9; i++ ) 652 | { 653 | result_rot[i] = xsens_fp1220_to_f32( cb_evt_data_cache[0].data.fp1220x9[i] ); 654 | } 655 | 656 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_rot, result_rot, 9); 657 | } 658 | 659 | void test_parse_mdata2_rotmatrix_fp1632( void ) 660 | { 661 | // Set epsilon 'high' because MTManager only prints single precision... 662 | double fp1632_epsilon = 0.0000001; 663 | 664 | // (RotationMatrix|Fp1632|ENU, 54 bytes, 665 | // (a: -0.23666316, b: -0.97153002, c: 0.01097167, 666 | // d: 0.97137016, e: -0.23683536, f: -0.01869209, 667 | // g: 0.02075840, h: 0.00623383, i: 0.99976528)) 668 | 669 | uint8_t test_packet[] = { 0xFA, 670 | 0xFF, 671 | 0x36, 672 | 0x39, 673 | 0x20, 0x22, 0x36, 0xC3, 0x6A, 0x0B, 0x00, 0xFF, 0xFF, 0x07, 0x49, 0xCF, 0x00, 0xFF, 0xFF, 0x02, 0xCF, 0x0A, 0x28, 0x00, 0x00, 0xF8, 0xAB, 0xB7, 0x00, 0x00, 0x00, 0xC3, 0x5E, 0xC2, 0x00, 0xFF, 0xFF, 0xFB, 0x36, 0xFE, 0xC0, 0xFF, 0xFF, 0x05, 0x50, 0x6C, 0x38, 0x00, 0x00, 0x01, 0x98, 0x8A, 0x4A, 0x00, 0x00, 0xFF, 0xF0, 0x9E, 0x00, 0x00, 0x00, 674 | 0xA9 }; 675 | 676 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 677 | 678 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] ); 679 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref ); 680 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1632FP9, cb_evt_data_cache[0].type ); 681 | float golden_rot[9] = { -0.23666316, -0.97153002, 0.01097167, 682 | 0.97137016, -0.23683536, -0.01869209, 683 | 0.02075840, 0.00623383, 0.99976528 }; 684 | 685 | double result_rot[9] = { 0 }; 686 | 687 | for( uint8_t i = 0; i < 9; i++ ) 688 | { 689 | result_rot[i] = xsens_fp1632_to_f64( cb_evt_data_cache[0].data.fp1632x9[i] ); 690 | TEST_ASSERT_DOUBLE_WITHIN( fp1632_epsilon,golden_rot[i], result_rot[i] ); 691 | } 692 | } 693 | 694 | void test_parse_mdata2_rotmatrix_double( void ) 695 | { 696 | // Set epsilon 'high' because MTManager only prints single precision... 697 | double dbl_epsilon = 0.00000001; 698 | 699 | // (RotationMatrix|Double|ENU, 72 bytes, 700 | // (a: -0.23455584, b: -0.97204965, c: 0.01015384, 701 | // d: 0.97191745, e: -0.23470223, f: -0.01706769, 702 | // g: 0.01897377, h: 0.00586537, i: 0.99980283)) 703 | 704 | uint8_t test_packet[] = { 0xFA, 705 | 0xFF, 706 | 0x36, 707 | 0x4B, 708 | 0x20, 0x23, 0x48, 0xBF, 0xCE, 0x05, 0xED, 0x00, 0x00, 0x00, 0x00, 0xBF, 0xEF, 0x1B, 0x07, 0xE0, 0x00, 0x00, 0x00, 0x3F, 0x84, 0xCB, 0x89, 0xA0, 0x00, 0x00, 0x00, 0x3F, 0xEF, 0x19, 0xF2, 0xA0, 0x00, 0x00, 0x00, 0xBF, 0xCE, 0x0A, 0xB9, 0x00, 0x00, 0x00, 0x00, 0xBF, 0x91, 0x7A, 0x31, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x93, 0x6D, 0xDC, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x78, 0x06, 0x49, 0x80, 0x00, 0x00, 0x00, 0x3F, 0xEF, 0xFE, 0x62, 0x80, 0x00, 0x00, 0x00, 709 | 0x3C }; 710 | 711 | xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet)); 712 | 713 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] ); 714 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref ); 715 | TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_DOUBLE9, cb_evt_data_cache[0].type ); 716 | double golden_rot[9] = { -0.23455584, -0.97204965, 0.01015384, 717 | 0.97191745, -0.23470223, -0.01706769, 718 | 0.01897377, 0.00586537, 0.99980283 }; 719 | 720 | for( uint8_t i = 0; i < 9; i++ ) 721 | { 722 | TEST_ASSERT_DOUBLE_WITHIN( dbl_epsilon, golden_rot[i], cb_evt_data_cache[0].data.f8x9[i] ); 723 | } 724 | } 725 | -------------------------------------------------------------------------------- /test/test_xsens_mti_send.c: -------------------------------------------------------------------------------- 1 | #include "unity.h" 2 | #include 3 | 4 | // MODULE UNDER TEST 5 | #include "xsens_mti.h" 6 | #include "xsens_mti_private.h" 7 | #include "xsens_constants.h" 8 | #include "xsens_mdata2.h" 9 | #include "xsens_utility.h" 10 | 11 | 12 | // DEFINITIONS 13 | 14 | // PRIVATE TYPES 15 | 16 | // PRIVATE DATA 17 | xsens_interface_t test_imu = { 0 }; 18 | 19 | 20 | // PRIVATE FUNCTIONS 21 | 22 | void mock_output_function( uint8_t *buffer, uint16_t size ); 23 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ); 24 | 25 | #define OUTBOUND_BUFFER_SIZE 2048 26 | 27 | uint8_t outbound_cache[OUTBOUND_BUFFER_SIZE] = { 0 }; 28 | uint16_t outbound_cache_pos = 0; 29 | 30 | void mock_output_function( uint8_t *buffer, uint16_t size ) 31 | { 32 | memcpy( &outbound_cache, buffer, size ); 33 | outbound_cache_pos += size; 34 | 35 | if( outbound_cache_pos > OUTBOUND_BUFFER_SIZE ) 36 | { 37 | TEST_FAIL_MESSAGE("Test harness cache ran out of room"); 38 | } 39 | } 40 | 41 | void mock_event_function( XsensEventFlag_t event, XsensEventData_t *data ) 42 | { 43 | // printf("Notified of evt: %d\n", event); 44 | } 45 | 46 | // SETUP, TEARDOWN 47 | void setUp(void) 48 | { 49 | memset( &outbound_cache, 0, sizeof(outbound_cache) ); 50 | outbound_cache_pos = 0; 51 | 52 | memset( &test_imu, 0, sizeof(test_imu) ); 53 | test_imu.output_cb = &mock_output_function; 54 | test_imu.event_cb = &mock_event_function; 55 | } 56 | 57 | void tearDown(void) 58 | { 59 | 60 | } 61 | 62 | // TESTS 63 | void test_send_configstate( void ) 64 | { 65 | 66 | uint8_t expected[] = { 0xFA, 67 | 0xFF, 68 | 0x30, 69 | 0x00, 70 | 0xD1 }; 71 | 72 | 73 | xsens_mti_request( &test_imu, MT_GOTOCONFIG ); 74 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 75 | } 76 | 77 | void test_send_measuremenetstate( void ) 78 | { 79 | 80 | uint8_t expected[] = { 0xFA, 81 | 0xFF, 82 | 0x10, 83 | 0x00, 84 | 0xF1 }; 85 | 86 | xsens_mti_request( &test_imu, MT_GOTOMEASUREMENT ); 87 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 88 | } 89 | 90 | 91 | void test_request_baud( void ) 92 | { 93 | 94 | uint8_t expected[] = { 0xFA, 95 | 0xFF, 96 | 0x18, 97 | 0x00, 98 | 0xE9 }; 99 | 100 | xsens_mti_request( &test_imu, MT_REQBAUDRATE ); 101 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 102 | } 103 | 104 | void test_send_setbaud_115k2( void ) 105 | { 106 | 107 | uint8_t expected[] = { 0xFA, 108 | 0xFF, 109 | 0x18, 110 | 0x01, 111 | 0x02, 112 | 0xE6 }; 113 | 114 | xsens_mti_set_baudrate( &test_imu, XSENS_BAUD_115200 ); 115 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 116 | } 117 | 118 | void test_send_setbaud_230k4( void ) 119 | { 120 | 121 | uint8_t expected[] = { 0xFA, 122 | 0xFF, 123 | 0x18, 124 | 0x01, 125 | 0x01, 126 | 0xE7 }; 127 | 128 | xsens_mti_set_baudrate( &test_imu, XSENS_BAUD_230400 ); 129 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 130 | 131 | } 132 | 133 | void test_send_setbaud_921k6( void ) 134 | { 135 | 136 | uint8_t expected[] = { 0xFA, 137 | 0xFF, 138 | 0x18, 139 | 0x01, 140 | 0x80, 141 | 0x68 }; 142 | 143 | xsens_mti_set_baudrate( &test_imu, XSENS_BAUD_921600 ); 144 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 145 | } 146 | 147 | void test_send_resetorientation_heading( void ) 148 | { 149 | 150 | uint8_t expected[] = { 0xFA, 151 | 0xFF, 152 | 0xA4, 153 | 0x02, 154 | 0x00, 155 | 0x01, 156 | 0x5A }; 157 | 158 | xsens_mti_reset_orientation( &test_imu, XSENS_ORIENTATION_HEADING_RESET ); 159 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 160 | } 161 | 162 | void test_send_resetorientation_align( void ) 163 | { 164 | 165 | uint8_t expected[] = { 0xFA, 166 | 0xFF, 167 | 0xA4, 168 | 0x02, 169 | 0x00, 170 | 0x04, 171 | 0x57 }; 172 | 173 | xsens_mti_reset_orientation( &test_imu, XSENS_ORIENTATION_ALIGNMENT_RESET ); 174 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 175 | } 176 | 177 | void test_request_reqdid( void ) 178 | { 179 | 180 | uint8_t expected[] = { 0xFA, 181 | 0xFF, 182 | 0x00, 183 | 0x00, 184 | 0x01 }; 185 | 186 | xsens_mti_request( &test_imu, MT_REQDID ); 187 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 188 | } 189 | 190 | void test_request_reqproductcode( void ) 191 | { 192 | 193 | uint8_t expected[] = { 0xFA, 194 | 0xFF, 195 | 0x1C, 196 | 0x00, 197 | 0xE5 }; 198 | 199 | xsens_mti_request( &test_imu, MT_REQPRODUCTCODE ); 200 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 201 | } 202 | 203 | void test_request_hardwareversion( void ) 204 | { 205 | 206 | uint8_t expected[] = { 0xFA, 207 | 0xFF, 208 | 0x1E, 209 | 0x00, 210 | 0xE3 }; 211 | 212 | xsens_mti_request( &test_imu, MT_REQHARDWAREVERSION ); 213 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 214 | } 215 | 216 | void test_request_reqfirmwareversion( void ) 217 | { 218 | 219 | uint8_t expected[] = { 0xFA, 220 | 0xFF, 221 | 0x12, 222 | 0x00, 223 | 0xEF }; 224 | 225 | xsens_mti_request( &test_imu, MT_REQFWREV ); 226 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 227 | } 228 | 229 | void test_request_runselftest( void ) 230 | { 231 | 232 | uint8_t expected[] = { 0xFA, 233 | 0xFF, 234 | 0x24, 235 | 0x00, 236 | 0xDD }; 237 | 238 | xsens_mti_request( &test_imu, MT_RUNSELFTEST ); 239 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 240 | } 241 | 242 | // From MT manual example, page 24 243 | void test_set_option_flags( void ) 244 | { 245 | // Enable AHS 246 | uint8_t expected[] = { 0xFA, 247 | 0xFF, 248 | 0x48, 249 | 0x08, 250 | 0x00, 0x00, 0x00, 0x10, // Set flags 251 | 0x00, 0x00, 0x00, 0x00, // Clear flags 252 | 0xA1 }; 253 | 254 | uint32_t set_flags = 0; 255 | uint32_t reset_flags = 0; 256 | XSENS_OPTION_FLAG_SET(set_flags, XSENS_OPTFLAG_ENABLE_AHS); 257 | xsens_mti_set_option_flags( &test_imu, set_flags, reset_flags ); 258 | 259 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 260 | } 261 | 262 | // From MT manual example, page 23 263 | void test_set_and_reset_option_flags( void ) 264 | { 265 | // Set DisableAutoStore and clear DisableAutoMeasurement 266 | uint8_t expected[] = { 0xFA, 267 | 0xFF, 268 | 0x48, 269 | 0x08, 270 | 0x00, 0x00, 0x00, 0x01, // Set flags 271 | 0x00, 0x00, 0x00, 0x02, // Clear flags 272 | 0xAE }; 273 | 274 | uint32_t set_flags = 0; 275 | uint32_t reset_flags = 0; 276 | XSENS_OPTION_FLAG_SET(set_flags, XSENS_OPTFLAG_DISABLE_AUTOSTORE); 277 | XSENS_OPTION_FLAG_SET(reset_flags, XSENS_OPTFLAG_DISABLE_AUTOMEASUREMENT); 278 | xsens_mti_set_option_flags( &test_imu, set_flags, reset_flags ); 279 | 280 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 281 | } 282 | 283 | // From MT manual example, page 66 284 | void test_set_configuration( void ) 285 | { 286 | // PacketCounter 0x1020 287 | // SampleTimeFine 0x1060 288 | // Quaternion 0x2010 @ 100 Hz (0x0064) 289 | // Acceleration 0x4020 @ 400 Hz (0x0190) 290 | // Rate of Turn 0x8020 @ 400 Hz (0x0190) 291 | // Magnetic Field 0xC020 @ 100 Hz (0x0064) 292 | // Statusword 0xE020 293 | // LatLon 0x5042 @ 100 Hz (0x0064) 294 | // Altitude 0x5022 @ 100 Hz (0x0064) 295 | // Velocity XYZ 0xD012 @ 100 Hz (0x0064) 296 | 297 | // 0xFFFF and 0x0000 are both valid representations for 'max frequency' 298 | 299 | uint8_t expected[] = { 0xFA, 300 | 0xFF, 301 | 0xC0, 302 | 0x28, 303 | 0x10, 0x20, 0xFF, 0xFF, 0x10, 0x60, 0xFF, 0xFF, 0x20, 0x10, 0x00, 0x64, 0x40, 0x20, 0x01, 0x90, 0x80, 0x20, 0x01, 0x90, 0xC0, 0x20, 0x00, 0x64, 0xE0, 0x20, 0xFF, 0xFF, 0x50, 0x42, 0x00, 0x64, 0x50, 0x22, 0x00, 0x64, 0xD0, 0x12, 0x00, 0x64, 304 | 0x73 }; 305 | 306 | XsensFrequencyConfig_t settings[] = { 307 | { .id = XDI_PACKET_COUNTER, .frequency = 0xFFFF }, 308 | { .id = XDI_SAMPLE_TIME_FINE, .frequency = 0xFFFF }, 309 | { .id = XDI_QUATERNION, .frequency = 100 }, 310 | { .id = XDI_ACCELERATION, .frequency = 400 }, 311 | { .id = XDI_RATE_OF_TURN, .frequency = 400 }, 312 | { .id = XDI_MAGNETIC_FIELD, .frequency = 100 }, 313 | { .id = XDI_STATUS_WORD, .frequency = 0xFFFF }, 314 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_LAT_LON, XSENS_FLOAT_FIXED1632, XSENS_COORD_ENU), .frequency = 100 }, 315 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_ALTITUDE_ELLIPSOID, XSENS_FLOAT_FIXED1632, XSENS_COORD_ENU), .frequency = 100 }, 316 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_VELOCITY_XYZ, XSENS_FLOAT_FIXED1632, XSENS_COORD_ENU), .frequency = 100 }, 317 | }; 318 | 319 | xsens_mti_set_configuration( &test_imu, settings, 10 ); 320 | 321 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 322 | } 323 | 324 | // From sniffed flow with MTi-300 and MTManager software 325 | void test_set_configuration_2( void ) 326 | { 327 | // PacketCounter, Freq: 65535, 328 | // SampleTimeFine, Freq: 65535, 329 | // Quaternion|Float|ENU, Freq: 400, 330 | // Acceleration|Float, Freq: 400, 331 | // DeltaV|Float, Freq: 400, 332 | // FreeAcceleration|Float, Freq: 400, 333 | // RateOfTurn|Float, Freq: 400, 334 | // DeltaQ|Float, Freq: 400, 335 | // MagneticField|Float, Freq: 100, 336 | // Temperature|Float, Freq: 10, 337 | // BaroPressure, Freq: 50, 338 | // StatusWord, Freq: 65535 339 | 340 | uint8_t expected[] = { 0xFA, 341 | 0xFF, 342 | 0xC0, 343 | 0x30, 344 | 0x10, 0x20, 0xFF, 0xFF, 0x10, 0x60, 0xFF, 0xFF, 0x20, 0x10, 0x01, 0x90, 0x40, 0x20, 0x01, 0x90, 0x40, 0x10, 0x01, 0x90, 0x40, 0x30, 0x01, 0x90, 0x80, 0x20, 0x01, 0x90, 0x80, 0x30, 0x01, 0x90, 0xC0, 0x20, 0x00, 0x64, 0x08, 0x10, 0x00, 0x0A, 0x30, 0x10, 0x00, 0x32, 0xE0, 0x20, 0xFF, 0xFF, 345 | 0x99 }; 346 | 347 | XsensFrequencyConfig_t settings[] = { 348 | { .id = XDI_PACKET_COUNTER, .frequency = 0xFFFF }, 349 | { .id = XDI_SAMPLE_TIME_FINE, .frequency = 0xFFFF }, 350 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_QUATERNION, XSENS_FLOAT_SINGLE, XSENS_COORD_ENU), .frequency = 400 }, 351 | { .id = XDI_ACCELERATION, .frequency = 400 }, 352 | { .id = XDI_DELTA_V, .frequency = 400 }, 353 | { .id = XDI_FREE_ACCELERATION, .frequency = 400 }, 354 | { .id = XDI_RATE_OF_TURN, .frequency = 400 }, 355 | { .id = XDI_DELTA_Q, .frequency = 400 }, 356 | { .id = XDI_MAGNETIC_FIELD, .frequency = 100 }, 357 | { .id = XDI_TEMPERATURE, .frequency = 10 }, 358 | { .id = XDI_BARO_PRESSURE, .frequency = 50 }, 359 | { .id = XDI_STATUS_WORD, .frequency = 0xFFFF }, 360 | }; 361 | 362 | xsens_mti_set_configuration( &test_imu, settings, XSENS_ARR_ELEM(settings) ); 363 | 364 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 365 | } 366 | 367 | // From sniffed flow with MTi-300 and MTManager software 368 | void test_set_configuration_3( void ) 369 | { 370 | 371 | // PacketCounter, Freq: 65535, 372 | // SampleTimeFine, Freq: 65535, 373 | // Quaternion|Float|ENU, Freq: 200, 374 | // RateOfTurn|1220, Freq: 100, 375 | // MagneticField|1632, Freq: 100, 376 | // Temperature|Float, Freq: 1, 377 | // BaroPressure|float, Freq: 2, 378 | 379 | uint8_t expected[] = { 0xFA, 380 | 0xFF, 381 | 0xC0, 382 | 0x1C, 383 | 0x10, 0x20, 0xFF, 0xFF, 0x20, 0x10, 0x00, 0xC8, 0x80, 0x21, 0x00, 0x64, 0xC0, 0x22, 0x00, 0x64, 0x08, 0x10, 0x00, 0x01, 0x30, 0x10, 0x00, 0x02, 0xE0, 0x20, 0xFF, 0xFF, 384 | 0x5B }; 385 | 386 | XsensFrequencyConfig_t settings[] = { 387 | { .id = XDI_PACKET_COUNTER, .frequency = 0xFFFF }, 388 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_QUATERNION, XSENS_FLOAT_SINGLE, XSENS_COORD_ENU), .frequency = 200 }, 389 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_RATE_OF_TURN, XSENS_FLOAT_FIXED1220, XSENS_COORD_ENU), .frequency = 100 }, 390 | { .id = XSENS_IDENTIFIER_FORMAT(XDI_MAGNETIC_FIELD, XSENS_FLOAT_FIXED1632, XSENS_COORD_ENU), .frequency = 100 }, 391 | { .id = XDI_TEMPERATURE, .frequency = 1 }, 392 | { .id = XDI_BARO_PRESSURE, .frequency = 2 }, 393 | { .id = XDI_STATUS_WORD, .frequency = 0xFFFF }, 394 | }; 395 | 396 | xsens_mti_set_configuration( &test_imu, settings, XSENS_ARR_ELEM(settings) ); 397 | 398 | TEST_ASSERT_EQUAL_HEX8_ARRAY( &expected, &outbound_cache, sizeof(expected) ); 399 | } 400 | 401 | // Check configuration function doesn't generate outputs for stupid arguments 402 | void test_set_configuration_empty( void ) 403 | { 404 | // Lie about count with the null-ptr 405 | xsens_mti_set_configuration( &test_imu, NULL, 1 ); 406 | 407 | // No bytes output 408 | TEST_ASSERT_EQUAL_INT( 0, outbound_cache_pos); 409 | 410 | // Test valid pointer but invalid count 411 | XsensFrequencyConfig_t settings[2] = { 0 }; 412 | xsens_mti_set_configuration( &test_imu, settings, 0 ); 413 | TEST_ASSERT_EQUAL_INT( 0, outbound_cache_pos); 414 | } 415 | 416 | void test_set_configuration_maximum( void ) 417 | { 418 | // Generates packet with the maximum number of configuration fields 419 | // these are empty/null values, intended to test argument/count handling 420 | XsensFrequencyConfig_t settings[32] = { 0 }; 421 | xsens_mti_set_configuration( &test_imu, settings, 32 ); 422 | 423 | uint8_t packet_overhead_bytes = 5; 424 | uint8_t payload_bytes = 32 * 4; // 4-bytes per configured field 425 | 426 | TEST_ASSERT_EQUAL_INT( packet_overhead_bytes+payload_bytes, outbound_cache_pos); 427 | } 428 | 429 | void test_set_configuration_too_many( void ) 430 | { 431 | XsensFrequencyConfig_t settings[35] = { 0 }; 432 | xsens_mti_set_configuration( &test_imu, settings, 34 ); 433 | 434 | TEST_ASSERT_EQUAL_INT( 0, outbound_cache_pos); 435 | } -------------------------------------------------------------------------------- /test/test_xsens_mti_table_handling.c: -------------------------------------------------------------------------------- 1 | #include "unity.h" 2 | #include 3 | 4 | // MODULE UNDER TEST 5 | #include "xsens_mti.h" 6 | #include "xsens_mti_private.h" 7 | #include "xsens_constants.h" 8 | #include "xsens_mdata2.h" 9 | #include "xsens_utility.h" 10 | 11 | 12 | // DEFINITIONS 13 | 14 | // PRIVATE TYPES 15 | 16 | // PRIVATE FUNCTIONS 17 | void mock_handler_a( xsens_packet_buffer_t *packet ) 18 | { 19 | 20 | } 21 | 22 | void mock_handler_b( xsens_packet_buffer_t *packet ) 23 | { 24 | 25 | } 26 | 27 | void mock_handler_c( xsens_packet_buffer_t *packet ) 28 | { 29 | 30 | } 31 | 32 | // PRIVATE DATA 33 | #define MOCK_TABLE_COUNT 8 34 | 35 | message_handler_ref_t mock_handler_table[ ] = { 36 | { .id = 1, .handler_fn = NULL }, 37 | { .id = 2, .handler_fn = NULL }, 38 | { .id = 3, .handler_fn = &mock_handler_a }, 39 | { .id = 4, .handler_fn = &mock_handler_b }, 40 | { .id = 5, .handler_fn = NULL }, 41 | { .id = 100, .handler_fn = &mock_handler_c }, 42 | { .id = 20, .handler_fn = NULL }, 43 | { .id = 6, .handler_fn = NULL }, 44 | }; 45 | 46 | 47 | // SETUP, TEARDOWN 48 | void setUp(void) 49 | { 50 | 51 | } 52 | 53 | void tearDown(void) 54 | { 55 | 56 | } 57 | 58 | // TESTS 59 | 60 | void test_table_length_elements( void ) 61 | { 62 | // This approach is used in xsens_mti.c when calling xsens_mti_find_handler_entry 63 | // TODO: consider making it into a macro to test what's actually run? 64 | uint8_t table_length = sizeof( mock_handler_table ) / sizeof( message_handler_ref_t ); 65 | TEST_ASSERT_EQUAL_INT( MOCK_TABLE_COUNT, table_length ); 66 | } 67 | 68 | // Attempting to find a missing ID should return null 69 | void test_find_handler_invalid_id( void ) 70 | { 71 | message_handler_ref_t* table_entry = 0; 72 | 73 | table_entry = xsens_mti_find_handler_entry( 11, mock_handler_table, MOCK_TABLE_COUNT ); 74 | 75 | TEST_ASSERT_EQUAL_PTR( NULL, table_entry ); 76 | } 77 | 78 | // Finding an ID in the table with placeholder (NULL) should still return the entry 79 | void test_find_handler_id_with_null( void ) 80 | { 81 | message_handler_ref_t* table_entry = 0; 82 | 83 | table_entry = xsens_mti_find_handler_entry( 2, mock_handler_table, MOCK_TABLE_COUNT ); 84 | 85 | TEST_ASSERT_EQUAL_PTR( &mock_handler_table[1], table_entry ); 86 | TEST_ASSERT_NULL( table_entry->handler_fn ); 87 | } 88 | 89 | // Finding an ID in the table with pointer should point to the right place 90 | void test_find_handler( void ) 91 | { 92 | message_handler_ref_t* table_entry = 0; 93 | 94 | table_entry = xsens_mti_find_handler_entry( 3, mock_handler_table, MOCK_TABLE_COUNT ); 95 | TEST_ASSERT_EQUAL_PTR( &mock_handler_table[2], table_entry ); 96 | TEST_ASSERT_EQUAL_PTR( &mock_handler_a, table_entry->handler_fn ); 97 | } 98 | -------------------------------------------------------------------------------- /test/test_xsens_utility.c: -------------------------------------------------------------------------------- 1 | #include "unity.h" 2 | #include 3 | 4 | // MODULE UNDER TEST 5 | #include "xsens_utility.h" 6 | 7 | // DEFINITIONS 8 | 9 | // PRIVATE TYPES 10 | 11 | // PRIVATE DATA 12 | 13 | // Test arrays for the array sizing macro 14 | uint8_t no_array[0] = { }; 15 | uint8_t small_array[1] = { 0 }; 16 | uint8_t standard_array[40] = { 0 }; 17 | uint8_t large_array[350] = { 0 }; 18 | 19 | // PRIVATE FUNCTIONS 20 | 21 | 22 | // SETUP, TEARDOWN 23 | void setUp(void) 24 | { 25 | } 26 | 27 | void tearDown(void) 28 | { 29 | 30 | } 31 | 32 | // TESTS 33 | 34 | void test_array_element_count( void ) 35 | { 36 | TEST_ASSERT_EQUAL_INT_MESSAGE( 0, XSENS_ARR_ELEM(no_array), "Null array elements incorrectly counted." ); 37 | TEST_ASSERT_EQUAL_INT_MESSAGE( 1, XSENS_ARR_ELEM(small_array), "Small array elements incorrectly counted." ); 38 | TEST_ASSERT_EQUAL_INT_MESSAGE( 40, XSENS_ARR_ELEM(standard_array), "Standard array elements incorrectly counted." ); 39 | TEST_ASSERT_EQUAL_INT_MESSAGE( 350, XSENS_ARR_ELEM(large_array), "Large array elements incorrectly counted." ); 40 | } 41 | 42 | void test_endian_swap_u16( void ) 43 | { 44 | uint16_t be = 0xAAFF; 45 | uint16_t le = 0xFFAA; 46 | uint16_t result = 0x0000; 47 | xsens_swap_endian_u16( (uint8_t*)&result, (uint8_t*)&le ); 48 | TEST_ASSERT_EQUAL_HEX16( be, result ); 49 | 50 | be = 0x4321; 51 | le = 0x2143; 52 | result = 0x0000; 53 | xsens_swap_endian_u16( (uint8_t*)&result, (uint8_t*)&le ); 54 | TEST_ASSERT_EQUAL_HEX16( be, result ); 55 | 56 | be = 0x0000; 57 | le = 0x0000; 58 | result = 0x0000; 59 | xsens_swap_endian_u16( (uint8_t*)&result, (uint8_t*)&le ); 60 | TEST_ASSERT_EQUAL_HEX16( be, result ); 61 | } 62 | 63 | void test_endian_swap_u32( void ) 64 | { 65 | uint32_t be = 0xAABBCCDD; 66 | uint32_t le = 0xDDCCBBAA; 67 | uint32_t result = 0x00000000; 68 | xsens_swap_endian_u32( (uint8_t*)&result, (uint8_t*)&le ); 69 | TEST_ASSERT_EQUAL_HEX32( be, result ); 70 | 71 | be = 0x00110000; 72 | le = 0x00001100; 73 | result = 0x00000000; 74 | xsens_swap_endian_u32( (uint8_t*)&result, (uint8_t*)&le ); 75 | TEST_ASSERT_EQUAL_HEX32( be, result ); 76 | } 77 | 78 | void test_quaternion_euler_basic( void ) 79 | { 80 | // Euler angles are in radians 81 | float expected_euler[3] = { 1.0f, 0.0f, 0.0f }; 82 | 83 | float quaternion[4] = { 0.8775826f, 0.4794255f, 0.0f, 0.0f}; 84 | float result_euler[3] = { 0.0f }; 85 | 86 | xsens_quaternion_to_euler( &quaternion, &result_euler ); 87 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_euler, result_euler, 3); 88 | } 89 | 90 | void test_quaternion_euler_zero( void ) 91 | { 92 | float expected_euler[3] = { 0.0f, 0.0f, 0.0f }; 93 | 94 | float quaternion[4] = { 1.0f, 0.0, 0.0f, 0.0f}; 95 | float result_euler[3] = { 1.0f }; 96 | 97 | xsens_quaternion_to_euler( &quaternion, &result_euler ); 98 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_euler, result_euler, 3); 99 | } 100 | 101 | // Roll, Pitch and Yaw all have positive values 102 | void test_quaternion_euler_1( void ) 103 | { 104 | float expected_euler[3] = { 0.5f, 1.0f, -0.25f }; 105 | 106 | float quaternion[4] = { 0.8584542f, 0.1575093f, 0.4879661f, 0.0116753f }; 107 | float result_euler[3] = { 0.0f }; 108 | 109 | xsens_quaternion_to_euler( &quaternion, &result_euler ); 110 | 111 | //TODO: Investigate quaternion conversion accuracy issues 112 | TEST_IGNORE(); 113 | 114 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_euler, result_euler, 3); 115 | } 116 | 117 | // This one has negative roll angle 118 | void test_quaternion_euler_2( void ) 119 | { 120 | float expected_euler[3] = { -1.5f, 0.0f, 0.0f }; 121 | 122 | float quaternion[4] = { 0.7316889f, -0.6816388f, 0.0f, 0.0f }; 123 | float result_euler[3] = { 0.0f }; 124 | 125 | xsens_quaternion_to_euler( &quaternion, &result_euler ); 126 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_euler, result_euler, 3); 127 | } 128 | 129 | void test_euler_quaternion_basic( void ) 130 | { 131 | float expected_quat[4] = { 0.8775826f, 0.4794255f, 0.0f, 0.0f }; 132 | 133 | float euler[3] = { 1.0f, 0.0f, 0.0f }; 134 | float result_quat[4] = { 1.0f }; 135 | 136 | xsens_euler_to_quaternion( &euler, &result_quat ); 137 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_quat, result_quat, 4); 138 | } 139 | 140 | void test_euler_quaternion_zero( void ) 141 | { 142 | float expected_quat[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; 143 | 144 | float euler[3] = { 0.0f, 0.0, 0.0f}; 145 | float result_quat[4] = { 1.0f }; 146 | 147 | xsens_euler_to_quaternion( &euler, &result_quat ); 148 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_quat, result_quat, 4); 149 | } 150 | 151 | void test_euler_quaternion_1( void ) 152 | { 153 | float expected_quat[4] = { 0.8584542f, 0.1575093f, 0.4879661f, 0.0116753f }; 154 | 155 | float euler[3] = { 0.5f, 1.0f, -0.25f }; 156 | float result_quat[4] = { 1.0f }; 157 | 158 | xsens_euler_to_quaternion( &euler, &result_quat ); 159 | 160 | //TODO: Investigate quaternion conversion accuracy issues 161 | TEST_IGNORE(); 162 | 163 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_quat, result_quat, 4); 164 | } 165 | 166 | void test_euler_quaternion_2( void ) 167 | { 168 | float expected_quat[4] = { 0.7316889f, -0.6816388f, 0.0f, 0.0f }; 169 | 170 | float euler[3] = { -1.5f, 0.0f, 0.0f }; 171 | float result_quat[4] = { 1.0f }; 172 | 173 | xsens_euler_to_quaternion( &euler, &result_quat ); 174 | TEST_ASSERT_EQUAL_FLOAT_ARRAY( expected_quat, result_quat, 4); 175 | } 176 | 177 | 178 | void test_into_fp1220( void ) 179 | { 180 | int32_t expected = 0x00324396; // 0x003 integer, 0x24396 fractional 181 | 182 | float value = 3.1415f; 183 | int32_t result = xsens_f32_to_fp1220( value ); 184 | 185 | TEST_ASSERT_EQUAL_INT32( expected, result ); 186 | } 187 | 188 | // TODO: check fp1220 transmits with byte order as mentioned in xsens doc 189 | // {0x00, 0x32, 0x43, 0x96} 190 | 191 | void test_into_fp1220_negative( void ) 192 | { 193 | int32_t expected = 0xFFCDBC6A; // Converted for negative -3 integer, 0x24396 as above 194 | 195 | float value = -3.1415f; 196 | int32_t result = xsens_f32_to_fp1220( value ); 197 | 198 | TEST_ASSERT_EQUAL_INT32( expected, result ); 199 | } 200 | 201 | void test_from_fp1220( void ) 202 | { 203 | float expected = 3.1415f; 204 | 205 | uint32_t value = 0x00324396; // 0x003 integer, 0x24396 fractional 206 | float result = xsens_fp1220_to_f32( value ); 207 | 208 | TEST_ASSERT_EQUAL_FLOAT( expected, result ); 209 | } 210 | 211 | void test_fp1220_f32_roundtrip( void ) 212 | { 213 | float seed_value = 0; 214 | int32_t fp_result = 0; 215 | float sp_result = 0; 216 | 217 | seed_value = 0.00f; 218 | fp_result = xsens_f32_to_fp1220( seed_value ); 219 | sp_result = xsens_fp1220_to_f32( fp_result ); 220 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 221 | 222 | seed_value = -0.00f; 223 | fp_result = xsens_f32_to_fp1220( seed_value ); 224 | sp_result = xsens_fp1220_to_f32( fp_result ); 225 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 226 | 227 | seed_value = 0.1234567890123456789; 228 | fp_result = xsens_f32_to_fp1220( seed_value ); 229 | sp_result = xsens_fp1220_to_f32( fp_result ); 230 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 231 | 232 | seed_value = -0.1234567890123456789; 233 | fp_result = xsens_f32_to_fp1220( seed_value ); 234 | sp_result = xsens_fp1220_to_f32( fp_result ); 235 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 236 | 237 | seed_value = 10.000f; 238 | fp_result = xsens_f32_to_fp1220( seed_value ); 239 | sp_result = xsens_fp1220_to_f32( fp_result ); 240 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 241 | 242 | seed_value = -10.000f; 243 | fp_result = xsens_f32_to_fp1220( seed_value ); 244 | sp_result = xsens_fp1220_to_f32( fp_result ); 245 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 246 | 247 | seed_value = 3.14159265358979323846; 248 | fp_result = xsens_f32_to_fp1220( seed_value ); 249 | sp_result = xsens_fp1220_to_f32( fp_result ); 250 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 251 | 252 | seed_value = -3.14159265358979323846; 253 | fp_result = xsens_f32_to_fp1220( seed_value ); 254 | sp_result = xsens_fp1220_to_f32( fp_result ); 255 | TEST_ASSERT_EQUAL_FLOAT( seed_value, sp_result ); 256 | } 257 | 258 | void test_into_fp1632( void ) 259 | { 260 | int64_t expected = 0x000324395810; // 0x0003 integer, 0x24395810 fractional 261 | 262 | double value = 3.1415; 263 | int64_t result = xsens_f64_to_fp1632( value ); 264 | 265 | TEST_ASSERT_EQUAL_INT64( expected, result ); 266 | } 267 | 268 | // TODO: check fp1632 transmits with byte order as mentioned in xsens doc 269 | // {0x24, 0x39, 0x58, 0x10, 0x00, 0x03} 270 | 271 | void test_into_fp1632_negative( void ) 272 | { 273 | // Value is one-bit higher than 0x24395810 example 274 | int64_t expected = 0xFFFCDBC6A7F0; // Negative/signed equiv from 0x00032439580F 275 | 276 | double value = -3.1415; 277 | int64_t result = xsens_f64_to_fp1632( value ); 278 | 279 | TEST_ASSERT_EQUAL_HEX64( expected, result ); 280 | } 281 | 282 | void test_from_fp1632( void ) 283 | { 284 | double expected = 3.14150000014342; 285 | 286 | int64_t value = 0x000324395811; // 0x0003 integer, 0x24395811 fractional 287 | double result = xsens_fp1632_to_f64( value ); 288 | 289 | // This particular test has an error slightly under 15x DBL_EPSILON 290 | TEST_ASSERT_DOUBLE_WITHIN( 0.0000000000000040, expected, result ); 291 | } 292 | 293 | void test_fp1632_f64_roundtrip( void ) 294 | { 295 | const double flt_epsilon = 0.0000000002; 296 | 297 | double seed_value = 0; 298 | int64_t fp_result = 0; 299 | double dp_result = 0; 300 | 301 | seed_value = 0.00; 302 | fp_result = xsens_f64_to_fp1632( seed_value ); 303 | dp_result = xsens_fp1632_to_f64( fp_result ); 304 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 305 | 306 | seed_value = -0.00; 307 | fp_result = xsens_f64_to_fp1632( seed_value ); 308 | dp_result = xsens_fp1632_to_f64( fp_result ); 309 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 310 | 311 | seed_value = 0.1234567890123456789; 312 | fp_result = xsens_f64_to_fp1632( seed_value ); 313 | dp_result = xsens_fp1632_to_f64( fp_result ); 314 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 315 | 316 | seed_value = -0.1234567890123456789; 317 | fp_result = xsens_f64_to_fp1632( seed_value ); 318 | dp_result = xsens_fp1632_to_f64( fp_result ); 319 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 320 | 321 | seed_value = 10.000; 322 | fp_result = xsens_f64_to_fp1632( seed_value ); 323 | dp_result = xsens_fp1632_to_f64( fp_result ); 324 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 325 | 326 | seed_value = -10.000; 327 | fp_result = xsens_f64_to_fp1632( seed_value ); 328 | dp_result = xsens_fp1632_to_f64( fp_result ); 329 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 330 | 331 | seed_value = 3.14159265358979323846; 332 | fp_result = xsens_f64_to_fp1632( seed_value ); 333 | dp_result = xsens_fp1632_to_f64( fp_result ); 334 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 335 | 336 | seed_value = -3.14159265358979323846; 337 | fp_result = xsens_f64_to_fp1632( seed_value ); 338 | dp_result = xsens_fp1632_to_f64( fp_result ); 339 | TEST_ASSERT_DOUBLE_WITHIN( flt_epsilon, seed_value, dp_result ); 340 | 341 | } --------------------------------------------------------------------------------