├── .clang-format ├── .clwb └── .bazelproject ├── .gitattributes ├── .github └── workflows │ ├── codeql.yml │ └── release_build.yml ├── .gitignore ├── BUILD ├── CMakeLists.txt ├── CONTRIBUTING.md ├── Doxyfile ├── LICENSE ├── README.md ├── WORKSPACE ├── docs ├── DoxygenLayout.xml ├── doxy_footer.html ├── doxy_header.html ├── doxy_style.css ├── error.html ├── favicon.ico ├── images │ ├── vehicle_frame_back.svg │ └── vehicle_frame_side.svg ├── include_header.js.template ├── index.dox ├── index.html ├── point_one_logo.png ├── update_versions.py └── versions.html.template ├── examples ├── BUILD ├── CMakeLists.txt ├── WORKSPACE ├── common │ ├── BUILD │ ├── CMakeLists.txt │ ├── print_message.cc │ └── print_message.h ├── external_cmake_project │ ├── CMakeLists.txt │ └── main.cc ├── generate_data │ ├── BUILD │ ├── CMakeLists.txt │ └── generate_data.cc ├── lband_decode │ ├── BUILD │ ├── CMakeLists.txt │ └── lband_decode.cc ├── message_decode │ ├── BUILD │ ├── CMakeLists.txt │ ├── example_data.p1log │ └── message_decode.cc ├── raw_message_decode │ ├── BUILD │ ├── CMakeLists.txt │ └── raw_message_decode.cc ├── request_version │ ├── BUILD │ ├── CMakeLists.txt │ └── request_version.cc ├── tcp_client │ ├── BUILD │ ├── CMakeLists.txt │ └── linux_tcp_client.cc └── udp_client │ ├── BUILD │ ├── CMakeLists.txt │ └── linux_udp_client.cc ├── python ├── .pep8 ├── README.md ├── examples │ ├── analyze_data.py │ ├── binary_message_decode.py │ ├── encode_data.py │ ├── encode_message.py │ ├── extract_imu_data.py │ ├── extract_position_data.py │ ├── extract_satellite_info.py │ ├── extract_vehicle_speed_data.py │ ├── manual_message_decode.py │ ├── manual_tcp_client.py │ ├── message_decode.py │ ├── send_command.py │ ├── send_vehicle_speed.py │ ├── serial_client.py │ ├── tcp_client.py │ └── udp_client.py ├── fusion_engine_client │ ├── __init__.py │ ├── analysis │ │ ├── __init__.py │ │ ├── analyzer.py │ │ ├── attitude.py │ │ └── data_loader.py │ ├── applications │ │ ├── __init__.py │ │ ├── import_utils.py │ │ ├── p1_capture.py │ │ ├── p1_display.py │ │ ├── p1_extract.py │ │ ├── p1_filter.py │ │ ├── p1_lband_extract.py │ │ └── p1_print.py │ ├── messages │ │ ├── __init__.py │ │ ├── configuration.py │ │ ├── control.py │ │ ├── core.py │ │ ├── defs.py │ │ ├── device.py │ │ ├── fault_control.py │ │ ├── gnss_corrections.py │ │ ├── measurement_details.py │ │ ├── measurements.py │ │ ├── ros.py │ │ ├── signal_defs.py │ │ ├── solution.py │ │ ├── sta5635.py │ │ └── timestamp.py │ ├── parsers │ │ ├── __init__.py │ │ ├── decoder.py │ │ ├── encoder.py │ │ ├── fast_indexer.py │ │ ├── file_index.py │ │ └── mixed_log_reader.py │ └── utils │ │ ├── __init__.py │ │ ├── argument_parser.py │ │ ├── bin_utils.py │ │ ├── construct_utils.py │ │ ├── enum_utils.py │ │ ├── log.py │ │ ├── numpy_utils.py │ │ ├── print_utils.py │ │ ├── time_range.py │ │ ├── trace.py │ │ └── transport_utils.py ├── requirements.txt ├── setup.py └── tests │ ├── test_config.py │ ├── test_construct_utils.py │ ├── test_data_loader.py │ ├── test_decoder.py │ ├── test_encoder.py │ ├── test_enum_utils.py │ ├── test_file_index.py │ ├── test_message_defs.py │ ├── test_mixed_log_reader.py │ └── test_time_range.py ├── scripts └── tag_release.sh ├── src └── point_one │ ├── fusion_engine │ ├── common │ │ ├── logging.cc │ │ ├── logging.h │ │ ├── portability.h │ │ └── version.h │ ├── messages │ │ ├── configuration.h │ │ ├── control.h │ │ ├── core.h │ │ ├── crc.cc │ │ ├── crc.h │ │ ├── data_version.cc │ │ ├── data_version.h │ │ ├── defs.h │ │ ├── device.h │ │ ├── fault_control.h │ │ ├── gnss_corrections.h │ │ ├── measurements.h │ │ ├── ros.h │ │ ├── signal_defs.h │ │ ├── solution.h │ │ └── sta5635.h │ └── parsers │ │ ├── fusion_engine_framer.cc │ │ └── fusion_engine_framer.h │ └── rtcm │ ├── rtcm_framer.cc │ └── rtcm_framer.h └── wireshark └── p1_fusion_engine_dissector.lua /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlinesLeft: true 9 | AlignOperands: true 10 | AlignTrailingComments: false # Point One 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | BeforeCatch: true # Point One 33 | BeforeElse: true # Point One 34 | IndentBraces: false 35 | BreakBeforeBinaryOperators: None 36 | BreakBeforeBraces: Attach # Point One 37 | BreakBeforeTernaryOperators: true 38 | BreakConstructorInitializersBeforeComma: false 39 | ColumnLimit: 80 40 | CommentPragmas: '^ IWYU pragma:' 41 | CompactNamespaces: false # Point One 42 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 43 | ConstructorInitializerIndentWidth: 4 44 | ContinuationIndentWidth: 4 45 | Cpp11BracedListStyle: true 46 | DerivePointerAlignment: false # Point One 47 | DisableFormat: false 48 | ExperimentalAutoDetectBinPacking: false 49 | FixNamespaceComments: true # Point One 50 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 51 | IncludeBlocks: Preserve # Point One 52 | IncludeCategories: 53 | - Regex: '^<.*\.h>' 54 | Priority: 1 55 | - Regex: '^<.*' 56 | Priority: 2 57 | - Regex: '.*' 58 | Priority: 3 59 | IndentCaseLabels: true 60 | IndentPPDirectives: AfterHash 61 | IndentWidth: 2 62 | IndentWrappedFunctionNames: false 63 | KeepEmptyLinesAtTheStartOfBlocks: false 64 | MacroBlockBegin: '' 65 | MacroBlockEnd: '' 66 | MaxEmptyLinesToKeep: 1 67 | NamespaceIndentation: None 68 | ObjCBlockIndentWidth: 2 69 | ObjCSpaceAfterProperty: false 70 | ObjCSpaceBeforeProtocolList: false 71 | PenaltyBreakBeforeFirstCallParameter: 1 72 | PenaltyBreakComment: 300 73 | PenaltyBreakFirstLessLess: 120 74 | PenaltyBreakString: 1000 75 | PenaltyExcessCharacter: 1000000 76 | PenaltyReturnTypeOnItsOwnLine: 200 77 | PointerAlignment: Left 78 | ReflowComments: false # Point One 79 | SortIncludes: true 80 | SpaceAfterCStyleCast: false 81 | SpaceBeforeAssignmentOperators: true 82 | # clang-format >=8 only 83 | #SpaceBeforeCpp11BracedList: false # Point One 84 | SpaceBeforeParens: ControlStatements 85 | # clang-format >=8 only 86 | #SpaceBeforeRangeBasedForLoopColon: true # Point One 87 | SpaceInEmptyParentheses: false 88 | SpacesBeforeTrailingComments: 1 # Point One 89 | SpacesInAngles: false 90 | SpacesInContainerLiterals: false # Point One 91 | SpacesInCStyleCastParentheses: false 92 | SpacesInParentheses: false 93 | SpacesInSquareBrackets: false 94 | Standard: Cpp11 # Point One 95 | TabWidth: 8 96 | UseTab: Never 97 | ... 98 | -------------------------------------------------------------------------------- /.clwb/.bazelproject: -------------------------------------------------------------------------------- 1 | directories: 2 | # Add the directories you want added as source here 3 | # By default, we've added your entire workspace ('.') 4 | . 5 | -build/ 6 | -examples/bazel-bin 7 | -examples/bazel-examples 8 | -examples/bazel-out 9 | -examples/bazel-testlogs 10 | -docs/html 11 | -python/.pytest_cache 12 | -python/venv* 13 | -wasm/build 14 | -wasm/emsdk 15 | 16 | 17 | # Automatically includes all relevant targets under the 'directories' above 18 | derive_targets_from_directories: true 19 | 20 | targets: 21 | # If source code isn't resolving, add additional targets that compile it here 22 | -//examples/...:all 23 | 24 | additional_languages: 25 | # Uncomment any additional languages you want supported 26 | # dart 27 | # javascript 28 | # python 29 | # typescript 30 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | * text=auto 2 | -------------------------------------------------------------------------------- /.github/workflows/codeql.yml: -------------------------------------------------------------------------------- 1 | name: "CodeQL" 2 | 3 | on: 4 | push: 5 | branches: [ "master" ] 6 | pull_request: 7 | branches: [ "master" ] 8 | schedule: 9 | - cron: "39 6 * * 0" 10 | 11 | jobs: 12 | analyze: 13 | name: Analyze 14 | runs-on: ubuntu-latest 15 | permissions: 16 | actions: read 17 | contents: read 18 | security-events: write 19 | 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | language: [ cpp ] 24 | 25 | steps: 26 | - name: Checkout 27 | uses: actions/checkout@v3 28 | 29 | - name: Initialize CodeQL 30 | uses: github/codeql-action/init@v2 31 | with: 32 | languages: ${{ matrix.language }} 33 | queries: +security-and-quality 34 | 35 | - name: Autobuild 36 | uses: github/codeql-action/autobuild@v2 37 | 38 | - name: Perform CodeQL Analysis 39 | uses: github/codeql-action/analyze@v2 40 | with: 41 | category: "/language:${{ matrix.language }}" 42 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated files. 2 | *.pyc 3 | *.html 4 | *.egg-info 5 | 6 | # Bazel output directories. 7 | bazel-* 8 | 9 | # Default cmake build directory. 10 | build/ 11 | 12 | # Python files. 13 | venv*/ 14 | *.pyc 15 | 16 | # IDE project settings. 17 | .clwb/ 18 | .idea/ 19 | .vscode 20 | *.code-workspace 21 | 22 | # P1 binary files. 23 | *.p1bin 24 | *.p1log 25 | *.p1i 26 | 27 | # Doxygen output. 28 | docs/html/ 29 | 30 | Doxyfile.version 31 | docs/include_header.js 32 | docs/versions.html 33 | 34 | # Other output files. 35 | *.csv 36 | *.kml 37 | -------------------------------------------------------------------------------- /BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | # Default target: include all messages and supporting code. 4 | cc_library( 5 | name = "fusion_engine_client", 6 | deps = [ 7 | ":core", 8 | ":messages", 9 | ":parsers", 10 | ":rtcm", 11 | ], 12 | ) 13 | 14 | # Support for building a shared library if desired. 15 | cc_binary( 16 | name = "libfusion_engine_client.so", 17 | linkshared = True, 18 | deps = [ 19 | ":fusion_engine_client", 20 | ], 21 | ) 22 | 23 | # Core navigation solution support functionality. 24 | cc_library( 25 | name = "core", 26 | deps = [ 27 | ":core_headers", 28 | ":crc", 29 | ], 30 | ) 31 | 32 | ################################################################################ 33 | # Message Definitions 34 | ################################################################################ 35 | 36 | # Message definition headers only (all message types). 37 | cc_library( 38 | name = "messages", 39 | deps = [ 40 | ":core_headers", 41 | ":ros_support", 42 | ], 43 | ) 44 | 45 | # Core navigation solution message definitions. 46 | cc_library( 47 | name = "core_headers", 48 | hdrs = [ 49 | "src/point_one/fusion_engine/messages/configuration.h", 50 | "src/point_one/fusion_engine/messages/control.h", 51 | "src/point_one/fusion_engine/messages/core.h", 52 | "src/point_one/fusion_engine/messages/defs.h", 53 | "src/point_one/fusion_engine/messages/device.h", 54 | "src/point_one/fusion_engine/messages/fault_control.h", 55 | "src/point_one/fusion_engine/messages/gnss_corrections.h", 56 | "src/point_one/fusion_engine/messages/measurements.h", 57 | "src/point_one/fusion_engine/messages/signal_defs.h", 58 | "src/point_one/fusion_engine/messages/solution.h", 59 | ], 60 | deps = [ 61 | ":common", 62 | ":data_version", 63 | ], 64 | ) 65 | 66 | # STA5635 RF front-end message definitions. 67 | cc_library( 68 | name = "sta5635", 69 | hdrs = [ 70 | "src/point_one/fusion_engine/messages/sta5635.h", 71 | ], 72 | deps = [ 73 | ":core_headers", 74 | ], 75 | ) 76 | 77 | # ROS translation message definitions. 78 | cc_library( 79 | name = "ros_support", 80 | hdrs = [ 81 | "src/point_one/fusion_engine/messages/ros.h", 82 | ], 83 | deps = [ 84 | ":core_headers", 85 | ], 86 | ) 87 | 88 | ################################################################################ 89 | # Support Functionality 90 | ################################################################################ 91 | 92 | # Common support code. 93 | cc_library( 94 | name = "common", 95 | srcs = [ 96 | "src/point_one/fusion_engine/common/logging.cc", 97 | ], 98 | hdrs = [ 99 | "src/point_one/fusion_engine/common/logging.h", 100 | "src/point_one/fusion_engine/common/portability.h", 101 | "src/point_one/fusion_engine/common/version.h", 102 | ], 103 | includes = ["src"], 104 | ) 105 | 106 | # Message encode/decode support. 107 | cc_library( 108 | name = "parsers", 109 | srcs = [ 110 | "src/point_one/fusion_engine/parsers/fusion_engine_framer.cc", 111 | ], 112 | hdrs = [ 113 | "src/point_one/fusion_engine/parsers/fusion_engine_framer.h", 114 | ], 115 | deps = [ 116 | ":core_headers", 117 | ":crc", 118 | ], 119 | ) 120 | 121 | # CRC support. 122 | cc_library( 123 | name = "crc", 124 | srcs = [ 125 | "src/point_one/fusion_engine/messages/crc.cc", 126 | ], 127 | hdrs = [ 128 | "src/point_one/fusion_engine/messages/crc.h", 129 | ], 130 | deps = [ 131 | ":core_headers", 132 | ], 133 | ) 134 | 135 | # Data versioning support. 136 | cc_library( 137 | name = "data_version", 138 | srcs = [ 139 | "src/point_one/fusion_engine/messages/data_version.cc", 140 | ], 141 | hdrs = [ 142 | "src/point_one/fusion_engine/messages/data_version.h", 143 | ], 144 | includes = ["src"], 145 | deps = [ 146 | ":common", 147 | ], 148 | ) 149 | 150 | # Message encode/decode support. 151 | cc_library( 152 | name = "rtcm", 153 | srcs = [ 154 | "src/point_one/rtcm/rtcm_framer.cc", 155 | ], 156 | hdrs = [ 157 | "src/point_one/rtcm/rtcm_framer.h", 158 | ], 159 | deps = [ 160 | ":common", 161 | ], 162 | ) 163 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) Point One Navigation - All Rights Reserved 2 | cmake_minimum_required(VERSION 3.12) 3 | 4 | # Set toolchain parameters before calling project(). 5 | set(CMAKE_CXX_STANDARD 11) 6 | set(CMAKE_CXX_STANDARD_REQUIRED True) 7 | 8 | # Define user options. 9 | option(P1_FE_BUILD_EXAMPLES "Build example applications." ON) 10 | 11 | if (NOT DEFINED BUILD_SHARED_LIBS) 12 | option(BUILD_SHARED_LIBS 13 | "Build shared libraries instead of static libraries." 14 | ON) 15 | endif() 16 | 17 | # Define the project and setup the compiler toolchain. This will establish 18 | # default compiler/linker flags. If the user specifies a cross-compilation 19 | # toolchain (-DCMAKE_TOOLCHAIN_FILE=...), it will be applied now. 20 | project(p1_fusion_engine_client VERSION 1.24.1) 21 | 22 | # Set additional compilation flags. 23 | if (MSVC) 24 | add_compile_options(/W4 /WX) 25 | else() 26 | add_compile_options(-Wall -Werror) 27 | endif() 28 | 29 | ################################################################################ 30 | # Library Definitions 31 | ################################################################################ 32 | 33 | # Define the fusion_engine_client library and supporting code. 34 | add_library(fusion_engine_client 35 | src/point_one/fusion_engine/common/logging.cc 36 | src/point_one/fusion_engine/messages/crc.cc 37 | src/point_one/fusion_engine/messages/data_version.cc 38 | src/point_one/fusion_engine/parsers/fusion_engine_framer.cc 39 | src/point_one/rtcm/rtcm_framer.cc) 40 | target_include_directories(fusion_engine_client PUBLIC ${PROJECT_SOURCE_DIR}/src) 41 | if (MSVC) 42 | target_compile_definitions(fusion_engine_client PRIVATE BUILDING_DLL) 43 | endif() 44 | 45 | set_target_properties(fusion_engine_client PROPERTIES 46 | VERSION ${PROJECT_VERSION} 47 | SOVERSION ${PROJECT_VERSION_MAJOR}) 48 | 49 | # Install targets. 50 | install(TARGETS fusion_engine_client 51 | LIBRARY DESTINATION lib) 52 | 53 | install(DIRECTORY src/point_one DESTINATION include 54 | FILES_MATCHING PATTERN "*.h") 55 | 56 | ################################################################################ 57 | # Example Applications (Optional) 58 | ################################################################################ 59 | 60 | if (P1_FE_BUILD_EXAMPLES) 61 | add_subdirectory(examples) 62 | endif() 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Point One Navigation 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /WORKSPACE: -------------------------------------------------------------------------------- 1 | workspace(name = "p1_fusion_engine_client") 2 | -------------------------------------------------------------------------------- /docs/doxy_footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | 13 | 14 |
15 | $generatedby   16 | doxygen 17 | $doxygenversion 18 |
19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /docs/doxy_header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | $projectname: $title 10 | $title 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $treeview 22 | $search 23 | $mathjax 24 | 25 | $extrastylesheet 26 | 27 | 28 |
29 | 30 | 31 |
32 | 33 |
34 | 35 | 36 | -------------------------------------------------------------------------------- /docs/error.html: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | Point One FusionEngine Client: 404 - File Not Found 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 |
20 | 21 |
22 | 23 |

Oops!

24 | 25 |

Sorry, the file you requested cannot be found.

26 |
27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /docs/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/docs/favicon.ico -------------------------------------------------------------------------------- /docs/include_header.js.template: -------------------------------------------------------------------------------- 1 | var stableVersion = "%(latest_version)s"; 2 | 3 | function addNavBar(version) { 4 | var topElem = document.getElementById("p1_nav"); 5 | 6 | var content = 7 | ''; 20 | 21 | topElem.innerHTML = content; 22 | 23 | if (version === undefined && currentVersion !== undefined) { 24 | version = currentVersion; 25 | } 26 | 27 | if (version === "versions") { 28 | var linkElem = document.getElementById("versions_link"); 29 | linkElem.className += "active"; 30 | } 31 | else if (version === "latest" || version === "master") { 32 | var linkElem = document.getElementById("latest_link"); 33 | linkElem.className += "active"; 34 | 35 | var brandElem = document.getElementById("brand_link"); 36 | brandElem.innerHTML += " (Latest)"; 37 | } 38 | else { 39 | if (version === stableVersion) { 40 | var linkElem = document.getElementById("stable_link"); 41 | linkElem.className += "active"; 42 | } 43 | 44 | if (version !== undefined) { 45 | var brandElem = document.getElementById("brand_link"); 46 | brandElem.innerHTML += " (" + version + ")"; 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /docs/index.dox: -------------------------------------------------------------------------------- 1 | /** \mainpage FusionEngine Client Documentation 2 | 3 | This library provides message definitions and support functionality for interacting with Point One FusionEngine in real 4 | time, as well as processing recorded output data. Both C++ and Python are supported. 5 | 6 | See https://github.com/PointOneNav/fusion-engine-client for requirements and installation/build instructions. 7 | 8 | \section message_format Message Format 9 | 10 | All messages begin with a @ref point_one::fusion_engine::messages::MessageHeader "MessageHeader", followed by a message 11 | payload corresponding with the @ref point_one::fusion_engine::messages::MessageType "MessageType" in the header. 12 | 13 | See @ref messages for a complete list of available messages. 14 | 15 | \subsection ros_message_support ROS Message Support 16 | 17 | For convenience, this library includes some messages that can be directly translated into ROS counterparts where ROS 18 | integration is required. See @ref ros_messages. 19 | 20 | \section body_frame Body Coordinate Frame Definition 21 | 22 |
23 |
24 | 25 |
Vehicle frame: side view"
26 |
27 |
28 | 29 |
Vehicle frame: back view"
30 |
31 |
32 | 33 | The platform body axes are defined as +x forward, +y left, and +z up. 34 | 35 | A positive yaw is a left turn, positive pitch points the nose of the vehicle down, and positive roll is a roll toward 36 | the right. 37 | 38 | Yaw is measured from east in a counter-clockwise direction. For example, north is +90 degrees (i.e., 39 | `heading = 90.0 - yaw`). 40 | 41 | */ 42 | -------------------------------------------------------------------------------- /docs/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /docs/point_one_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/docs/point_one_logo.png -------------------------------------------------------------------------------- /docs/update_versions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | from packaging import version 5 | import subprocess 6 | import sys 7 | 8 | 9 | if __name__ == "__main__": 10 | entry_template = """\ 11 | 12 | %(version)s%(current)s 13 | Documentation 14 | Release Notes 15 | 16 | """ 17 | 18 | entry_template_no_docs = """\ 19 | 20 | %(version)s%(current)s 21 | 22 | Release Notes 23 | 24 | """ 25 | 26 | # List available versions. 27 | versions = [version.parse(v.lstrip('v')) 28 | for v in subprocess.check_output(['git', 'tag']).decode('utf-8').strip().split() 29 | if v.startswith('v')] 30 | versions.sort(reverse=True) 31 | latest_version = versions[0] 32 | 33 | # Find the docs/ directory. 34 | docs_dir = os.path.dirname(os.path.abspath(__file__)) 35 | 36 | # Set version number in Doxyfile. 37 | if len(sys.argv) > 1: 38 | current_version = sys.argv[1] 39 | if not current_version.startswith('v'): 40 | current_version = f'v{current_version}' 41 | 42 | with open('%s/../Doxyfile' % docs_dir, 'r') as f: 43 | file_contents = f.read() 44 | file_contents = file_contents.format(current_version=current_version) 45 | with open('%s/../Doxyfile.version' % docs_dir, 'w') as f: 46 | f.write(file_contents) 47 | 48 | # Generate include_header.js. 49 | with open('%s/include_header.js.template' % docs_dir, 'r') as f: 50 | file_contents = f.read() 51 | with open('%s/include_header.js' % docs_dir, 'w') as f: 52 | f.write(file_contents % {'latest_version': f'v{str(latest_version)}'}) 53 | 54 | # Generate versions.html. 55 | FIRST_RELEASE_WITH_DOCS = version.Version('1.4.0') 56 | with open('%s/versions.html.template' % docs_dir, 'r') as f: 57 | file_contents = f.read() 58 | 59 | table_contents = "" 60 | for v in versions: 61 | if v >= FIRST_RELEASE_WITH_DOCS: 62 | template = entry_template 63 | else: 64 | template = entry_template_no_docs 65 | 66 | table_contents += template % {'version': f'v{str(v)}', 67 | 'current': ' (Current)' if v == latest_version else ''} 68 | 69 | with open('%s/versions.html' % docs_dir, 'w') as f: 70 | f.write(file_contents % {'content': table_contents}) 71 | -------------------------------------------------------------------------------- /docs/versions.html.template: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Point One FusionEngine Client: Versions 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 |
15 | 16 |
17 | 18 |

FusionEngine Client Versions

19 | 20 |

Stable Versions

21 | 22 | 23 | %(content)s 24 | 25 |
26 | 27 |

Latest Version

28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 |
master branchDocumentationSource Code
37 |
38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /examples/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | # All example applications. 4 | filegroup( 5 | name = "examples", 6 | srcs = [ 7 | "//generate_data", 8 | "//message_decode", 9 | "//request_version", 10 | "//tcp_client", 11 | "//udp_client", 12 | ] 13 | ) 14 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(common) 2 | 3 | add_subdirectory(generate_data) 4 | add_subdirectory(lband_decode) 5 | add_subdirectory(message_decode) 6 | add_subdirectory(raw_message_decode) 7 | add_subdirectory(request_version) 8 | add_subdirectory(tcp_client) 9 | add_subdirectory(udp_client) 10 | 11 | # Note that we do _not_ include the external_cmake_project/ subdirectory here. 12 | # That application is designed as a standalone project, not built by the 13 | # fusion-engine-client CMake project. See external_cmake_project/CMakeLists.txt 14 | # for details. 15 | -------------------------------------------------------------------------------- /examples/WORKSPACE: -------------------------------------------------------------------------------- 1 | local_repository( 2 | name = "fusion_engine_client", 3 | path = "..", 4 | ) 5 | -------------------------------------------------------------------------------- /examples/common/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_library( 4 | name = "print_message", 5 | srcs = [ 6 | "print_message.cc", 7 | ], 8 | hdrs = [ 9 | "print_message.h", 10 | ], 11 | deps = [ 12 | "@fusion_engine_client", 13 | ], 14 | ) 15 | -------------------------------------------------------------------------------- /examples/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(print_message STATIC print_message.cc) 2 | target_link_libraries(print_message PUBLIC fusion_engine_client) 3 | -------------------------------------------------------------------------------- /examples/common/print_message.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Common print function used by example applications. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #include "print_message.h" 7 | 8 | using namespace point_one::fusion_engine::messages; 9 | 10 | namespace point_one { 11 | namespace fusion_engine { 12 | namespace examples { 13 | 14 | /******************************************************************************/ 15 | void PrintMessage(const MessageHeader& header, const void* payload_in) { 16 | auto payload = static_cast(payload_in); 17 | size_t message_size = sizeof(MessageHeader) + header.payload_size_bytes; 18 | 19 | if (header.message_type == MessageType::POSE) { 20 | auto& contents = *reinterpret_cast(payload); 21 | 22 | double p1_time_sec = 23 | contents.p1_time.seconds + (contents.p1_time.fraction_ns * 1e-9); 24 | 25 | static constexpr double SEC_PER_WEEK = 7 * 24 * 3600.0; 26 | double gps_time_sec = 27 | contents.gps_time.seconds + (contents.gps_time.fraction_ns * 1e-9); 28 | int gps_week = std::lround(gps_time_sec / SEC_PER_WEEK); 29 | double gps_tow_sec = gps_time_sec - (gps_week * SEC_PER_WEEK); 30 | 31 | printf("Pose message @ P1 time %.3f seconds. [sequence=%u, size=%zu B]\n", 32 | p1_time_sec, header.sequence_number, message_size); 33 | printf(" Position (LLA): %.6f, %.6f, %.3f (deg, deg, m)\n", 34 | contents.lla_deg[0], contents.lla_deg[1], contents.lla_deg[2]); 35 | printf(" GPS Time: %d:%.3f (%.3f seconds)\n", gps_week, gps_tow_sec, 36 | gps_time_sec); 37 | printf(" Attitude (YPR): %.2f, %.2f, %.2f (deg, deg, deg)\n", 38 | contents.ypr_deg[0], contents.ypr_deg[1], contents.ypr_deg[2]); 39 | printf(" Velocity (Body): %.2f, %.2f, %.2f (m/s, m/s, m/s)\n", 40 | contents.velocity_body_mps[0], contents.velocity_body_mps[1], 41 | contents.velocity_body_mps[2]); 42 | printf(" Position Std Dev (ENU): %.2f, %.2f, %.2f (m, m, m)\n", 43 | contents.position_std_enu_m[0], contents.position_std_enu_m[1], 44 | contents.position_std_enu_m[2]); 45 | printf(" Attitude Std Dev (YPR): %.2f, %.2f, %.2f (deg, deg, deg)\n", 46 | contents.ypr_std_deg[0], contents.ypr_std_deg[1], 47 | contents.ypr_std_deg[2]); 48 | printf(" Velocity Std Dev (Body): %.2f, %.2f, %.2f (m/s, m/s, m/s)\n", 49 | contents.velocity_std_body_mps[0], contents.velocity_std_body_mps[1], 50 | contents.velocity_std_body_mps[2]); 51 | printf(" Protection Levels:\n"); 52 | printf(" Aggregate: %.2f m\n", contents.aggregate_protection_level_m); 53 | printf(" Horizontal: %.2f m\n", contents.horizontal_protection_level_m); 54 | printf(" Vertical: %.2f m\n", contents.vertical_protection_level_m); 55 | } else if (header.message_type == MessageType::GNSS_INFO) { 56 | auto& contents = *reinterpret_cast(payload); 57 | 58 | double p1_time_sec = 59 | contents.p1_time.seconds + (contents.p1_time.fraction_ns * 1e-9); 60 | double gps_time_sec = 61 | contents.gps_time.seconds + (contents.gps_time.fraction_ns * 1e-9); 62 | 63 | printf( 64 | "GNSS info message @ P1 time %.3f seconds. [sequence=%u, size=%zu B]\n", 65 | p1_time_sec, header.sequence_number, message_size); 66 | printf(" GPS time: %.3f\n", gps_time_sec); 67 | printf(" GPS time std dev: %.2e sec\n", contents.gps_time_std_sec); 68 | printf(" Leap second: %d\n", 69 | contents.leap_second == GNSSInfoMessage::INVALID_LEAP_SECOND 70 | ? -1 71 | : contents.leap_second); 72 | printf(" # SVs: %d\n", contents.num_svs); 73 | printf(" Reference station: %s\n", 74 | contents.reference_station_id == 75 | GNSSInfoMessage::INVALID_REFERENCE_STATION 76 | ? "none" 77 | : std::to_string(contents.reference_station_id).c_str()); 78 | printf(" Corrections age: %.1f sec\n", 79 | contents.corrections_age == GNSSInfoMessage::INVALID_AGE 80 | ? NAN 81 | : contents.leap_second * 0.1); 82 | printf(" Baseline distance: %.2f km\n", 83 | contents.baseline_distance == GNSSInfoMessage::INVALID_DISTANCE 84 | ? NAN 85 | : contents.baseline_distance * 0.01); 86 | printf(" GDOP: %.1f PDOP: %.1f\n", contents.gdop, contents.pdop); 87 | printf(" HDOP: %.1f VDOP: %.1f\n", contents.hdop, contents.vdop); 88 | } else if (header.message_type == MessageType::GNSS_SATELLITE) { 89 | auto& contents = *reinterpret_cast(payload); 90 | payload += sizeof(contents); 91 | 92 | double p1_time_sec = 93 | contents.p1_time.seconds + (contents.p1_time.fraction_ns * 1e-9); 94 | 95 | printf( 96 | "GNSS satellite message @ P1 time %.3f seconds. [sequence=%u, " 97 | "size=%zu B, %u svs]\n", 98 | p1_time_sec, header.sequence_number, message_size, 99 | contents.num_satellites); 100 | 101 | for (unsigned i = 0; i < contents.num_satellites; ++i) { 102 | auto& sv = *reinterpret_cast(payload); 103 | payload += sizeof(sv); 104 | 105 | printf(" %s PRN %u:\n", to_string(sv.system), sv.prn); 106 | printf(" Elevation/azimuth: (%.1f, %.1f) deg\n", sv.elevation_deg, 107 | sv.azimuth_deg); 108 | printf(" In solution: %s\n", sv.usage > 0 ? "yes" : "no"); 109 | } 110 | } else if (header.message_type == MessageType::VERSION_INFO) { 111 | auto& contents = *reinterpret_cast(payload); 112 | payload += sizeof(contents); 113 | 114 | double system_time = contents.system_time_ns * 1e-9; 115 | printf( 116 | "Version info message @ System time %.3f seconds. [sequence=%u, " 117 | "size=%zu B]\n", 118 | system_time, header.sequence_number, message_size); 119 | printf(" Firmware version: %.*s\n", contents.fw_version_length, 120 | reinterpret_cast(payload)); 121 | } else { 122 | printf("Received message type %s. [sequence=%u, %zu bytes]\n", 123 | to_string(header.message_type), header.sequence_number, 124 | message_size); 125 | } 126 | } 127 | 128 | /******************************************************************************/ 129 | void PrintHex(const void* data, size_t data_len_bytes) { 130 | const uint8_t* data_ptr = static_cast(data); 131 | for (size_t i = 0; i < data_len_bytes; ++i) { 132 | printf("%02x", data_ptr[i]); 133 | if (i < data_len_bytes - 1) { 134 | printf(" "); 135 | } 136 | } 137 | } 138 | 139 | } // namespace examples 140 | } // namespace fusion_engine 141 | } // namespace point_one 142 | -------------------------------------------------------------------------------- /examples/common/print_message.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Common print function used by example applications. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | namespace point_one { 11 | namespace fusion_engine { 12 | namespace examples { 13 | 14 | /** 15 | * @brief Print the contents of a received FusionEngine message. 16 | * 17 | * @param header The message header. 18 | * @param payload The message payload. 19 | */ 20 | void PrintMessage(const fusion_engine::messages::MessageHeader& header, 21 | const void* payload); 22 | 23 | void PrintHex(const void* data, size_t data_len_bytes); 24 | 25 | } // namespace examples 26 | } // namespace fusion_engine 27 | } // namespace point_one 28 | -------------------------------------------------------------------------------- /examples/external_cmake_project/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # This is a simple example of how to import the fusion-engine-client library in 3 | # your own project using the CMake FetchContent feature. We strongly encourage 4 | # you to use FetchContent to download fusion-engine-client from the publicly 5 | # available source code. 6 | # 7 | # Alternatively, you may choose to use a git submodule to import the source code 8 | # into your repository. We do not recommend copying the fusion-engine-client 9 | # source code directly into your repository. Doing so makes it much more 10 | # difficult to receive updates as new features and improvements are released. 11 | ################################################################################ 12 | 13 | cmake_minimum_required(VERSION 3.12) 14 | 15 | set(CMAKE_CXX_STANDARD 14) 16 | 17 | project(fusion_engine_usage_example C CXX) 18 | 19 | # Use FetchContent to import the fusion-engine-client C++ library using Git. 20 | # 21 | # Note that we always recommend using a specific version of the library in your 22 | # code by specifying a release zip file or a git tag (e.g., `GIT_TAG vA.B.C`), 23 | # and updating that as new versions are released. That way, you can be sure that 24 | # your code is always built with a known version of fusion-engine-client. If you 25 | # prefer, however, you can set the GIT_TAG to track the latest changes by 26 | # setting `GIT_TAG master` below. 27 | # 28 | # We explicitly disable example applications from the fusion-engine-client 29 | # library by setting P1_FE_BUILD_EXAMPLES to OFF below. We only want to build 30 | # the library and make the fusion_engine_client CMake target available here. By 31 | # default, if we do not tell it otherwise, FetchContent_MakeAvailable() will 32 | # also import all of the example applications in fusion-engine-client/examples/. 33 | # 34 | # It is important to specify it as an INTERNAL variable. If you do not do this, 35 | # the option definition in the fusion-engine-client CMakeLists.txt file will 36 | # override this value and enable the example applications anyway. This is a 37 | # result of CMP0077, which was added in CMake 3.13. 38 | include(FetchContent) 39 | FetchContent_Declare( 40 | fusion_engine_client 41 | # Recommended: 42 | URL https://github.com/PointOneNav/fusion-engine-client/archive/refs/tags/v1.22.3.zip 43 | URL_HASH MD5=cfe1de319725822a1b825cd3421fb6b1 44 | # Or alternatively: 45 | # GIT_REPOSITORY https://github.com/PointOneNav/fusion-engine-client.git 46 | # GIT_TAG v1.22.3 47 | ) 48 | set(P1_FE_BUILD_EXAMPLES OFF CACHE INTERNAL "") 49 | FetchContent_MakeAvailable(fusion_engine_client) 50 | 51 | # Now we define an example application that uses the fusion-engine-client 52 | # library. In your own code, you can link any add_executable() or add_library() 53 | # target with fusion-engine-client by calling target_link_libraries() as shown. 54 | add_executable(example_app main.cc) 55 | target_link_libraries(example_app PUBLIC fusion_engine_client) 56 | -------------------------------------------------------------------------------- /examples/external_cmake_project/main.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Simple example of linking against the fusion-engine-client library 3 | * using CMake. 4 | * 5 | * This application does not do anything very interesting. It is meant only as an 6 | * example of how to import fusion-engine-client in CMake using FetchContent. See 7 | * the accompanying CMakeLists.txt file. 8 | * 9 | * @file 10 | ******************************************************************************/ 11 | 12 | #include 13 | 14 | #include 15 | 16 | using namespace point_one::fusion_engine::messages; 17 | 18 | int main(int argc, const char* argv[]) { 19 | // Populate a pose message with some content. 20 | PoseMessage pose_message; 21 | 22 | pose_message.p1_time.seconds = 123; 23 | pose_message.p1_time.fraction_ns = 456000000; 24 | 25 | pose_message.gps_time.seconds = 1282677727; 26 | pose_message.gps_time.fraction_ns = 200000000; 27 | 28 | pose_message.solution_type = SolutionType::RTKFixed; 29 | pose_message.lla_deg[0] = 37.795137; 30 | pose_message.lla_deg[1] = -122.402754; 31 | pose_message.lla_deg[2] = 40.8; 32 | 33 | // Print out the LLA position. 34 | printf("LLA: %.6f, %.6f, %.2f\n", pose_message.lla_deg[0], 35 | pose_message.lla_deg[1], pose_message.lla_deg[2]); 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /examples/generate_data/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_binary( 4 | name = "generate_data", 5 | srcs = [ 6 | "generate_data.cc", 7 | ], 8 | deps = [ 9 | "@fusion_engine_client", 10 | ], 11 | ) 12 | -------------------------------------------------------------------------------- /examples/generate_data/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(generate_data generate_data.cc) 2 | target_link_libraries(generate_data PUBLIC fusion_engine_client) 3 | -------------------------------------------------------------------------------- /examples/lband_decode/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_binary( 4 | name = "lband_decode", 5 | srcs = [ 6 | "lband_decode.cc", 7 | ], 8 | deps = [ 9 | "@fusion_engine_client", 10 | ], 11 | ) 12 | -------------------------------------------------------------------------------- /examples/lband_decode/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(lband_decode lband_decode.cc) 2 | target_link_libraries(lband_decode PUBLIC fusion_engine_client) 3 | -------------------------------------------------------------------------------- /examples/lband_decode/lband_decode.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Example of decoding FusionEngine messages from a recorded file. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace point_one::fusion_engine::messages; 15 | using namespace point_one::fusion_engine::parsers; 16 | using namespace point_one::rtcm; 17 | 18 | static constexpr size_t READ_SIZE_BYTES = 1024; 19 | // The max FE L-band message is (FE header + message) + 504 B. 20 | static constexpr size_t FE_FRAMER_BUFFER_BYTES = 600; 21 | // The max RTCM message size is (RTCM header) + 1023 B. 22 | static constexpr size_t RTCM_FRAMER_BUFFER_BYTES = 1030; 23 | static RTCMFramer rtcm_framer(RTCM_FRAMER_BUFFER_BYTES); 24 | 25 | // This is the callback for handling decoded FusionEngine messages. 26 | /******************************************************************************/ 27 | void OnFEMessage(const MessageHeader& header, const void* data) { 28 | // If the FusionEngine message is L-band data, process its payload. 29 | if (header.message_type == LBandFrameMessage::MESSAGE_TYPE) { 30 | auto frame = reinterpret_cast(data); 31 | auto lband_data = 32 | static_cast(data) + sizeof(LBandFrameMessage); 33 | printf("Decoded %u L-band bytes.\n", frame->user_data_size_bytes); 34 | rtcm_framer.OnData(lband_data, frame->user_data_size_bytes); 35 | } 36 | } 37 | 38 | // This is the callback for handling decoded RTCM messages. 39 | /******************************************************************************/ 40 | void OnRTCMMessage(uint16_t message_type, const void* data, size_t data_len) { 41 | // Don't warn unused. 42 | (void)data; 43 | printf("Decoded RTCM message. [type=%hu, size=%zu B]\n", message_type, 44 | data_len); 45 | } 46 | 47 | /******************************************************************************/ 48 | int main(int argc, const char* argv[]) { 49 | if (argc != 2) { 50 | printf("Usage: %s FILE\n", argv[0]); 51 | printf("Decode L-band corrections and write contents to 'lband.bin'."); 52 | return 0; 53 | } 54 | 55 | char buffer[READ_SIZE_BYTES]; 56 | FusionEngineFramer fe_framer(FE_FRAMER_BUFFER_BYTES); 57 | // Set a callback to handle the decoded L-band data. 58 | fe_framer.SetMessageCallback(&OnFEMessage); 59 | 60 | // Set a callback to handle the decoded RTCM in the decoded L-band data. 61 | rtcm_framer.SetMessageCallback(&OnRTCMMessage); 62 | 63 | std::ifstream in_stream(argv[1], std::ifstream::binary); 64 | if (!in_stream.is_open()) { 65 | printf("Error opening file '%s'.\n", argv[1]); 66 | return 1; 67 | } 68 | 69 | while (true) { 70 | in_stream.read(buffer, READ_SIZE_BYTES); 71 | if (in_stream.eof()) { 72 | break; 73 | } 74 | 75 | // Feed the FusionEngine data into the decoder. 76 | fe_framer.OnData(reinterpret_cast(buffer), sizeof(buffer)); 77 | } 78 | printf("Decoded %u messages successfully and had %u decoding errors.\n", 79 | rtcm_framer.GetNumDecodedMessages(), rtcm_framer.GetNumErrors()); 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /examples/message_decode/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_binary( 4 | name = "message_decode", 5 | srcs = [ 6 | "message_decode.cc", 7 | ], 8 | data = [ 9 | ":example_data", 10 | ], 11 | deps = [ 12 | "//common:print_message", 13 | "@fusion_engine_client", 14 | ], 15 | ) 16 | 17 | filegroup( 18 | name = "example_data", 19 | srcs = ["example_data.p1log"], 20 | ) 21 | -------------------------------------------------------------------------------- /examples/message_decode/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(message_decode message_decode.cc) 2 | target_link_libraries(message_decode PUBLIC fusion_engine_client) 3 | target_link_libraries(message_decode PUBLIC print_message) 4 | -------------------------------------------------------------------------------- /examples/message_decode/example_data.p1log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/examples/message_decode/example_data.p1log -------------------------------------------------------------------------------- /examples/message_decode/message_decode.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Example of decoding FusionEngine messages from a recorded file. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include "../common/print_message.h" 14 | 15 | using namespace point_one::fusion_engine::examples; 16 | using namespace point_one::fusion_engine::messages; 17 | using namespace point_one::fusion_engine::parsers; 18 | 19 | /******************************************************************************/ 20 | int main(int argc, const char* argv[]) { 21 | if (argc != 2) { 22 | printf("Usage: %s FILE\n", argv[0]); 23 | printf(R"EOF( 24 | Decode platform pose messages from a binary file containing FusionEngine data. 25 | )EOF"); 26 | return 0; 27 | } 28 | 29 | // Open the file. 30 | std::ifstream stream(argv[1], std::ifstream::binary); 31 | if (!stream) { 32 | printf("Error opening file '%s'.\n", argv[1]); 33 | return 1; 34 | } 35 | 36 | // Create a decoder and configure it to print when messaes arrive. 37 | FusionEngineFramer framer(MessageHeader::MAX_MESSAGE_SIZE_BYTES); 38 | framer.SetMessageCallback(PrintMessage); 39 | 40 | // Read the file in chunks and decode any messages that are found. 41 | uint8_t buffer[4096]; 42 | while (!stream.eof()) { 43 | stream.read(reinterpret_cast(buffer), sizeof(buffer)); 44 | size_t bytes_read = stream.gcount(); 45 | framer.OnData(buffer, bytes_read); 46 | } 47 | 48 | // Close the file. 49 | stream.close(); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /examples/raw_message_decode/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_binary( 4 | name = "raw_message_decode", 5 | srcs = [ 6 | "raw_message_decode.cc", 7 | ], 8 | data = [ 9 | "//message_decode:example_data", 10 | ], 11 | deps = [ 12 | "//common:print_message", 13 | "@fusion_engine_client", 14 | ], 15 | ) 16 | -------------------------------------------------------------------------------- /examples/raw_message_decode/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(raw_message_decode raw_message_decode.cc) 2 | target_link_libraries(raw_message_decode PUBLIC fusion_engine_client) 3 | target_link_libraries(raw_message_decode PUBLIC print_message) 4 | -------------------------------------------------------------------------------- /examples/raw_message_decode/raw_message_decode.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Example of decoding FusionEngine messages from a recorded file directly 3 | * without the use of the @ref FusionEngineDecoder class. 4 | * @file 5 | ******************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include "../common/print_message.h" 15 | 16 | using namespace point_one::fusion_engine::examples; 17 | using namespace point_one::fusion_engine::messages; 18 | 19 | /******************************************************************************/ 20 | bool DecodeMessage(std::ifstream& stream, size_t available_bytes) { 21 | static uint32_t expected_sequence_number = 0; 22 | 23 | // Enforce a 4-byte aligned address. 24 | alignas(4) uint8_t storage[4096]; 25 | char* buffer = reinterpret_cast(storage); 26 | 27 | // Read the message header. 28 | if (available_bytes < sizeof(MessageHeader)) { 29 | printf("Not enough data: cannot read header. [%zu bytes < %zu bytes]\n", 30 | available_bytes, sizeof(MessageHeader)); 31 | return false; 32 | } 33 | 34 | stream.read(buffer, sizeof(MessageHeader)); 35 | if (!stream) { 36 | printf("Unexpected error reading header.\n"); 37 | return false; 38 | } 39 | 40 | available_bytes -= sizeof(MessageHeader); 41 | 42 | auto& header = *reinterpret_cast(buffer); 43 | buffer += sizeof(MessageHeader); 44 | 45 | // Read the message payload. 46 | if (available_bytes < header.payload_size_bytes) { 47 | printf("Not enough data: cannot read payload. [%zu bytes < %u bytes]\n", 48 | available_bytes, header.payload_size_bytes); 49 | return false; 50 | } 51 | 52 | stream.read(buffer, header.payload_size_bytes); 53 | if (!stream) { 54 | printf("Unexpected error reading payload.\n"); 55 | return false; 56 | } 57 | 58 | // Verify the message checksum. 59 | size_t message_size = sizeof(MessageHeader) + header.payload_size_bytes; 60 | if (!IsValid(storage)) { 61 | printf( 62 | "CRC failure. [type=%s (%u), size=%zu bytes (payload size=%u bytes], " 63 | "sequence=%u, expected_crc=0x%08x, calculated_crc=0x%08x]\n", 64 | to_string(header.message_type), 65 | static_cast(header.message_type), message_size, 66 | header.payload_size_bytes, header.sequence_number, header.crc, 67 | CalculateCRC(storage)); 68 | return false; 69 | } 70 | 71 | // Check that the sequence number increments as expected. 72 | if (header.sequence_number != expected_sequence_number) { 73 | printf( 74 | "Warning: unexpected sequence number. [type=%s (%u), size=%zu bytes " 75 | "(payload size=%u bytes], crc=0x%08x, expected_sequence=%u, " 76 | "received_sequence=%u]\n", 77 | to_string(header.message_type), 78 | static_cast(header.message_type), message_size, 79 | header.payload_size_bytes, header.crc, expected_sequence_number, 80 | header.sequence_number); 81 | } 82 | 83 | expected_sequence_number = header.sequence_number + 1; 84 | 85 | // Interpret the payload. 86 | PrintMessage(header, buffer); 87 | 88 | return true; 89 | } 90 | 91 | /******************************************************************************/ 92 | int main(int argc, const char* argv[]) { 93 | if (argc != 2) { 94 | printf("Usage: %s FILE\n", argv[0]); 95 | printf(R"EOF( 96 | Decode platform pose messages from a binary file containing FusionEngine data. 97 | )EOF"); 98 | return 0; 99 | } 100 | 101 | // Open the file. 102 | std::ifstream stream(argv[1], std::ifstream::binary); 103 | if (!stream) { 104 | printf("Error opening file '%s'.\n", argv[1]); 105 | return 1; 106 | } 107 | 108 | // Determine the file size. 109 | stream.seekg(0, stream.end); 110 | std::streampos file_size_bytes = stream.tellg(); 111 | stream.seekg(0, stream.beg); 112 | 113 | // Decode all messages in the file. 114 | int return_code = 0; 115 | while (stream.tellg() != file_size_bytes) { 116 | if (!DecodeMessage(stream, 117 | static_cast(file_size_bytes - stream.tellg()))) { 118 | return_code = 1; 119 | break; 120 | } 121 | } 122 | 123 | // Close the file. 124 | stream.close(); 125 | 126 | return return_code; 127 | } 128 | -------------------------------------------------------------------------------- /examples/request_version/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | cc_binary( 4 | name = "request_version", 5 | srcs = [ 6 | "request_version.cc", 7 | ], 8 | deps = [ 9 | "//common:print_message", 10 | "@fusion_engine_client", 11 | ], 12 | ) 13 | -------------------------------------------------------------------------------- /examples/request_version/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(request_version request_version.cc) 2 | target_link_libraries(request_version PUBLIC fusion_engine_client) 3 | target_link_libraries(request_version PUBLIC print_message) 4 | -------------------------------------------------------------------------------- /examples/request_version/request_version.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Simulate sending a request for a version info message, and waiting for 3 | * a response. 4 | * @file 5 | ******************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "../common/print_message.h" 16 | 17 | using namespace point_one::fusion_engine::examples; 18 | using namespace point_one::fusion_engine::messages; 19 | using namespace point_one::fusion_engine::parsers; 20 | 21 | bool message_found = false; 22 | 23 | // Enforce a 4-byte aligned address. 24 | alignas(4) uint8_t storage[4096]; 25 | 26 | // Fake Send/Receive functions. 27 | /******************************************************************************/ 28 | void SendData(void* data, size_t data_len_bytes) { 29 | (void)data; 30 | (void)data_len_bytes; 31 | } 32 | 33 | /******************************************************************************/ 34 | size_t ReceiveData(uint8_t* buffer, size_t read_size) { 35 | static size_t offset = 0; 36 | if (offset + read_size < sizeof(storage)) { 37 | // We're using this data as if it were received from the device. 38 | memcpy(buffer, storage + offset, read_size); 39 | offset += read_size; 40 | return read_size; 41 | } else { 42 | return 0; 43 | } 44 | } 45 | 46 | /******************************************************************************/ 47 | int main(int argc, const char* argv[]) { 48 | if (argc != 1) { 49 | printf("Usage: %s\n", argv[0]); 50 | printf(R"EOF( 51 | Simulate sending a version request, and parsing the response. 52 | )EOF"); 53 | return 0; 54 | } 55 | 56 | ////////////////////////////////////////////////////////////////////////////// 57 | // Write a VersionInfoMessage request. 58 | ////////////////////////////////////////////////////////////////////////////// 59 | 60 | uint8_t* buffer = storage; 61 | auto header = reinterpret_cast(buffer); 62 | buffer += sizeof(MessageHeader); 63 | *header = MessageHeader(); 64 | 65 | header->sequence_number = 0; 66 | header->message_type = MessageType::MESSAGE_REQUEST; 67 | header->payload_size_bytes = sizeof(MessageRequest); 68 | 69 | auto req_message = reinterpret_cast(buffer); 70 | *req_message = MessageRequest(); 71 | 72 | req_message->message_type = VersionInfoMessage::MESSAGE_TYPE; 73 | 74 | header->crc = CalculateCRC(storage); 75 | 76 | printf("Sending VersionInfoMessage request:\n "); 77 | PrintHex(storage, sizeof(MessageHeader) + sizeof(MessageRequest)); 78 | // This data would be sent over serial to the device. 79 | SendData(storage, sizeof(MessageHeader) + sizeof(MessageRequest)); 80 | 81 | printf("\n"); 82 | 83 | ////////////////////////////////////////////////////////////////////////////// 84 | // Generate an example response of the data a device would send back. 85 | ////////////////////////////////////////////////////////////////////////////// 86 | 87 | static constexpr char VERSION_STR[] = {'t', 'e', 's', 't'}; 88 | 89 | // @ref ReceiveData will read data from `storage`. 90 | buffer = storage; 91 | header = reinterpret_cast(buffer); 92 | buffer += sizeof(MessageHeader); 93 | *header = MessageHeader(); 94 | 95 | header->sequence_number = 0; 96 | header->message_type = MessageType::VERSION_INFO; 97 | header->payload_size_bytes = sizeof(VersionInfoMessage) + sizeof(VERSION_STR); 98 | 99 | auto version_message = reinterpret_cast(buffer); 100 | *version_message = VersionInfoMessage(); 101 | version_message->fw_version_length = sizeof(VERSION_STR); 102 | buffer += sizeof(VersionInfoMessage); 103 | 104 | char* version_str_ptr = reinterpret_cast(buffer); 105 | // NOTE: Not NULL terminated. 106 | memcpy(version_str_ptr, VERSION_STR, sizeof(VERSION_STR)); 107 | 108 | header->crc = CalculateCRC(storage); 109 | 110 | ////////////////////////////////////////////////////////////////////////////// 111 | // Receive example response. 112 | ////////////////////////////////////////////////////////////////////////////// 113 | 114 | printf("Waiting for response\n"); 115 | // In a real application, you'd need to do the bookkeeping to trigger a 116 | // timeout if no response is received after a couple seconds. 117 | bool has_timed_out = false; 118 | uint8_t read_buffer[10]; 119 | 120 | FusionEngineFramer framer(1024); 121 | framer.SetMessageCallback( 122 | [](const MessageHeader& header, const void* payload) { 123 | // Ignore messages besides the expected response type. 124 | if (header.message_type == VersionInfoMessage::MESSAGE_TYPE) { 125 | PrintMessage(header, payload); 126 | message_found = true; 127 | } 128 | }); 129 | 130 | while (!has_timed_out && !message_found) { 131 | size_t data_read = ReceiveData(read_buffer, sizeof(read_buffer)); 132 | framer.OnData(read_buffer, data_read); 133 | } 134 | 135 | printf("Response received.\n"); 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /examples/tcp_client/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | # TCP client application currently supported for Linux only. 4 | cc_binary( 5 | name = "tcp_client", 6 | deps = select({ 7 | "@bazel_tools//src/conditions:linux_x86_64": [ 8 | ":linux_tcp_client", 9 | ], 10 | "//conditions:default": [], 11 | }), 12 | ) 13 | 14 | cc_library( 15 | name = "linux_tcp_client", 16 | srcs = [ 17 | "linux_tcp_client.cc", 18 | ], 19 | deps = [ 20 | "//common:print_message", 21 | "@fusion_engine_client", 22 | ], 23 | ) 24 | -------------------------------------------------------------------------------- /examples/tcp_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TCP client application currently supported for Linux only. 2 | if (UNIX) 3 | add_executable(tcp_client linux_tcp_client.cc) 4 | target_link_libraries(tcp_client PUBLIC fusion_engine_client) 5 | target_link_libraries(tcp_client PUBLIC print_message) 6 | endif() 7 | -------------------------------------------------------------------------------- /examples/tcp_client/linux_tcp_client.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Simple Linux TCP client example. 3 | * @file 4 | * 5 | * @note 6 | * This is a minimal TCP client implementation, meant as an example of how to 7 | * connect to a device and decode incoming data. It is not robust to network 8 | * outages, socket reconnects, or other typical network errors. Production 9 | * implementations must check for and handle these cases appropriately (by 10 | * checking expected `errno` values, etc.). 11 | ******************************************************************************/ 12 | 13 | #include 14 | #include // For lround() 15 | #include // For signal() 16 | #include // For fprintf() 17 | #include // For memcpy() 18 | #include // For stoi() and strerror() 19 | 20 | #include // For gethostbyname() and hostent 21 | #include // For IPPROTO_* macros and htons() 22 | #include // For socket support. 23 | #include // For close() 24 | 25 | #include 26 | 27 | #include "../common/print_message.h" 28 | 29 | using namespace point_one::fusion_engine::examples; 30 | using namespace point_one::fusion_engine::parsers; 31 | 32 | static bool shutdown_pending_ = false; 33 | 34 | /******************************************************************************/ 35 | void HandleSignal(int signal) { 36 | if (signal == SIGINT || signal == SIGTERM) { 37 | std::signal(signal, SIG_DFL); 38 | shutdown_pending_ = true; 39 | } 40 | } 41 | 42 | /******************************************************************************/ 43 | int main(int argc, const char* argv[]) { 44 | // Parse arguments. 45 | if (argc < 2 || argc > 3) { 46 | printf(R"EOF( 47 | Usage: %s HOSTNAME [PORT] 48 | 49 | Connect to an Atlas device over TCP and print out the incoming message 50 | contents. 51 | )EOF", 52 | argv[0]); 53 | return 0; 54 | } 55 | 56 | const char* hostname = argv[1]; 57 | int port = argc > 2 ? std::stoi(argv[2]) : 30201; 58 | 59 | // Perform a hostname lookup/translate the string IP address. 60 | hostent* host_info = gethostbyname(hostname); 61 | if (host_info == NULL) { 62 | printf("Error: IP address lookup failed for hostname '%s'.\n", hostname); 63 | return 1; 64 | } 65 | 66 | sockaddr_in addr; 67 | addr.sin_family = AF_INET; 68 | addr.sin_port = htons(port); 69 | memcpy(&addr.sin_addr, host_info->h_addr_list[0], host_info->h_length); 70 | 71 | // Connect the socket. 72 | int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 73 | if (sock < 0) { 74 | printf("Error creating socket.\n"); 75 | return 2; 76 | } 77 | 78 | int ret = connect(sock, (sockaddr*)&addr, sizeof(addr)); 79 | if (ret < 0) { 80 | printf("Error connecting to target device: %s (%d)\n", std::strerror(errno), 81 | errno); 82 | close(sock); 83 | return 3; 84 | } 85 | 86 | // Listen for SIGINT (Ctrl-C) or SIGTERM and shutdown gracefully. 87 | std::signal(SIGINT, HandleSignal); 88 | std::signal(SIGTERM, HandleSignal); 89 | 90 | // Receive incoming data. 91 | FusionEngineFramer framer(1024); 92 | framer.SetMessageCallback(PrintMessage); 93 | 94 | uint8_t buffer[1024]; 95 | size_t total_bytes_read = 0; 96 | ret = 0; 97 | while (!shutdown_pending_) { 98 | ssize_t bytes_read = recv(sock, buffer, sizeof(buffer), 0); 99 | if (bytes_read == 0) { 100 | printf("Socket closed remotely.\n"); 101 | break; 102 | } else if (bytes_read < 0) { 103 | printf("Error reading from socket: %s (%d)\n", std::strerror(errno), 104 | errno); 105 | ret = 4; 106 | break; 107 | } else { 108 | total_bytes_read += bytes_read; 109 | framer.OnData(buffer, (size_t)bytes_read); 110 | } 111 | } 112 | 113 | // Done. 114 | close(sock); 115 | 116 | printf("Finished. %zu bytes read.\n", total_bytes_read); 117 | 118 | return ret; 119 | } 120 | -------------------------------------------------------------------------------- /examples/udp_client/BUILD: -------------------------------------------------------------------------------- 1 | package(default_visibility = ["//visibility:public"]) 2 | 3 | # UDP client application currently supported for Linux only. 4 | cc_binary( 5 | name = "udp_client", 6 | deps = select({ 7 | "@bazel_tools//src/conditions:linux_x86_64": [ 8 | ":linux_udp_client", 9 | ], 10 | "//conditions:default": [], 11 | }), 12 | ) 13 | 14 | cc_library( 15 | name = "linux_udp_client", 16 | srcs = [ 17 | "linux_udp_client.cc", 18 | ], 19 | deps = [ 20 | "//common:print_message", 21 | "@fusion_engine_client", 22 | ], 23 | ) 24 | -------------------------------------------------------------------------------- /examples/udp_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # UDP client application currently supported for Linux only. 2 | if (UNIX) 3 | add_executable(udp_client linux_udp_client.cc) 4 | target_link_libraries(udp_client PUBLIC fusion_engine_client) 5 | target_link_libraries(udp_client PUBLIC print_message) 6 | endif() 7 | -------------------------------------------------------------------------------- /examples/udp_client/linux_udp_client.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * @brief Simple Linux UDP client example. 3 | * @file 4 | * 5 | * @note 6 | * This is a minimal UDP client implementation, meant as an example of how to 7 | * connect to a device and decode incoming data. It is not robust to network 8 | * outages, socket reconnects, or other typical network errors. Production 9 | * implementations must check for and handle these cases appropriately (by 10 | * checking expected `errno` values, etc.). 11 | ******************************************************************************/ 12 | 13 | #include 14 | #include // For lround() 15 | #include // For signal() 16 | #include // For fprintf() 17 | #include // For memcpy() 18 | #include // For stoi() and strerror() 19 | 20 | #include 21 | #include // For gethostbyname() and hostent 22 | #include // For IPPROTO_* macros and htons() 23 | #include // For socket support. 24 | #include // For close() 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include "../common/print_message.h" 31 | 32 | #define DEBUG_ON 0 33 | 34 | using namespace point_one::fusion_engine::examples; 35 | using namespace point_one::fusion_engine::parsers; 36 | 37 | static bool shutdown_pending_ = false; 38 | 39 | /******************************************************************************/ 40 | void HandleSignal(int signal) { 41 | if (signal == SIGINT || signal == SIGTERM) { 42 | std::signal(signal, SIG_DFL); 43 | shutdown_pending_ = true; 44 | } 45 | } 46 | 47 | /******************************************************************************/ 48 | void* GetInAddr(struct sockaddr* sa) { 49 | if (sa->sa_family == AF_INET) { 50 | return &(((struct sockaddr_in*)sa)->sin_addr); 51 | } 52 | return &(((struct sockaddr_in6*)sa)->sin6_addr); 53 | } 54 | 55 | /******************************************************************************/ 56 | int main(int argc, const char* argv[]) { 57 | // Parse arguments. 58 | if (argc > 2) { 59 | printf(R"EOF( 60 | Usage: %s [PORT] 61 | 62 | Receive incoming data from an Atlas device over UDP and print out the incoming message 63 | contents. 64 | )EOF", 65 | argv[0]); 66 | return 0; 67 | } 68 | 69 | int port = (argc == 2) ? std::stoi(argv[1]) : 12345; 70 | 71 | // Create UDP socket. 72 | int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 73 | if (sock < 0) { 74 | printf("Error creating socket.\n"); 75 | return 2; 76 | } 77 | 78 | // Bind socket to port. 79 | sockaddr_in addr; 80 | addr.sin_family = AF_INET; 81 | addr.sin_port = htons(port); 82 | addr.sin_addr.s_addr = INADDR_ANY; // Any local address. 83 | int ret = bind(sock, (struct sockaddr*)&addr, sizeof(addr)); 84 | if (ret < 0) { 85 | close(sock); 86 | printf("Error binding.\n"); 87 | return 3; 88 | } 89 | 90 | // Listen for SIGINT (Ctrl-C) or SIGTERM and shutdown gracefully. 91 | std::signal(SIGINT, HandleSignal); 92 | std::signal(SIGTERM, HandleSignal); 93 | 94 | // Receive incoming data. 95 | FusionEngineFramer framer(1024); 96 | framer.SetMessageCallback(PrintMessage); 97 | 98 | uint8_t buffer[1024]; 99 | size_t total_bytes_read = 0; 100 | struct sockaddr_storage their_addr; // Address of recieved packet. 101 | socklen_t addr_len = sizeof(their_addr); 102 | ret = 0; 103 | char their_ip[INET6_ADDRSTRLEN]; 104 | while (!shutdown_pending_) { 105 | ssize_t bytes_read = recvfrom(sock, buffer, sizeof(buffer), 0, 106 | (struct sockaddr*)&their_addr, &addr_len); 107 | 108 | if (bytes_read < 0) { 109 | printf("Error reading from socket: %s (%d)\n", std::strerror(errno), 110 | errno); 111 | ret = 4; 112 | break; 113 | } else if (bytes_read == 0) { 114 | printf("Socket closed remotely.\n"); 115 | break; 116 | } 117 | 118 | inet_ntop(their_addr.ss_family, GetInAddr((struct sockaddr*)&their_addr), 119 | their_ip, sizeof(their_ip)); 120 | 121 | #if DEBUG_ON 122 | // Enable to debug udp server to client problems. 123 | printf("listener: received raw packet [%zu bytes] from %s\n", bytes_read, 124 | their_ip); 125 | #endif 126 | 127 | total_bytes_read += bytes_read; 128 | framer.OnData(buffer, (size_t)bytes_read); 129 | } 130 | 131 | // Done. 132 | close(sock); 133 | 134 | printf("Finished. %zu bytes read.\n", total_bytes_read); 135 | 136 | return ret; 137 | } -------------------------------------------------------------------------------- /python/.pep8: -------------------------------------------------------------------------------- 1 | [pycodestyle] 2 | max_line_length = 120 3 | aggressive = 3 4 | recursive = true 5 | 6 | # - E265,E266 - Do not reformat double-# Doxygen comment blocks. 7 | # - E402 - Do not reorder imports to the top of the file (fusion_engine_client 8 | # imports in the example applications must come after the sys.path.append() 9 | # call). 10 | ignore = E265,E266,E402 11 | -------------------------------------------------------------------------------- /python/examples/analyze_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 7 | sys.path.append(root_dir) 8 | 9 | from fusion_engine_client.analysis.data_loader import DataLoader 10 | from fusion_engine_client.messages.core import * 11 | from fusion_engine_client.utils import trace as logging 12 | from fusion_engine_client.utils.argument_parser import ArgumentParser 13 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 14 | 15 | 16 | if __name__ == "__main__": 17 | parser = ArgumentParser(description="""\ 18 | Compute the average LLA position for the data contained in a *.p1log file. 19 | """) 20 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 21 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 22 | "specified.") 23 | parser.add_argument('log', 24 | help="The log to be read. May be one of:\n" 25 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 26 | "content\n" 27 | "- The path to a FusionEngine log directory\n" 28 | "- A pattern matching a FusionEngine log directory under the specified base directory " 29 | "(see find_fusion_engine_log() and --log-base-dir)") 30 | options = parser.parse_args() 31 | 32 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 33 | logger = logging.getLogger('point_one.fusion_engine') 34 | logger.setLevel(logging.INFO) 35 | 36 | # Locate the input file and set the output directory. 37 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 38 | log_base_dir=options.log_base_dir) 39 | if input_path is None: 40 | sys.exit(1) 41 | 42 | if log_id is None: 43 | logger.info('Loading %s.' % os.path.basename(input_path)) 44 | else: 45 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 46 | 47 | # Read pose data from the file. 48 | # 49 | # Note that we explicitly ask the DataLoader to return the data converted to numpy arrays so we can analyze it 50 | # below. 51 | reader = DataLoader(input_path) 52 | result = reader.read(message_types=[PoseMessage], show_progress=True, return_numpy=True) 53 | 54 | # Print out the messages that were read. 55 | pose_data = result[PoseMessage.MESSAGE_TYPE] 56 | for message in pose_data.messages: 57 | logger.info(str(message)) 58 | 59 | # Compute and print the average LLA value. Limit only to valid solutions. 60 | idx = pose_data.solution_type != SolutionType.Invalid 61 | mean_lla_deg = np.mean(pose_data.lla_deg[:, idx], axis=1) 62 | logger.info('Average position: %.6f, %.6f, %.3f' % tuple(mean_lla_deg)) 63 | -------------------------------------------------------------------------------- /python/examples/binary_message_decode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages import MessageHeader, MessagePayload, message_type_to_class 12 | from fusion_engine_client.parsers import FusionEngineDecoder 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.argument_parser import ArgumentParser 15 | 16 | from examples.message_decode import print_message 17 | 18 | 19 | if __name__ == "__main__": 20 | parser = ArgumentParser(description="""\ 21 | Decode and print the contents of one or more FusionEngine messages contained 22 | in a binary string or file. For example: 23 | 24 | > python3 binary_message_decode.py \\ 25 | 2e31 0000 0acf ee8f 0200 ca32 0000 0000 0400 0000 0000 0000 ff0f 0001 26 | 27 | Successfully decoded 1 FusionEngine messages. 28 | Header: RESET_REQUEST (13002) Message (version 0): 29 | Sequence #: 0 30 | Payload: 4 B 31 | Source: 0 32 | CRC: 0x8feecf0a 33 | Payload: Reset Request [mask=0x01000fff] 34 | 35 | or 36 | 37 | > python3 binary_message_decode.py my_file.p1log 38 | """) 39 | parser.add_argument('-v', '--verbose', action='count', default=0, 40 | help="Print verbose/trace debugging messages.") 41 | parser.add_argument('contents', nargs='+', 42 | help="Binary FusionEngine message contents, specified as a hex string, or the path to a binary " 43 | "file to be read. All spaces in the hex string will be ignored.") 44 | options = parser.parse_args() 45 | 46 | # Configure logging. 47 | if options.verbose >= 1: 48 | logging.basicConfig(level=logging.INFO, format='%(levelname)s - %(name)s:%(lineno)d - %(message)s', 49 | stream=sys.stdout) 50 | if options.verbose == 1: 51 | logging.getLogger('point_one.fusion_engine').setLevel(logging.DEBUG) 52 | else: 53 | logging.getLogger('point_one.fusion_engine').setLevel( 54 | logging.getTraceLevel(depth=options.verbose - 1)) 55 | else: 56 | logging.basicConfig(level=logging.INFO, format='%(message)s', stream=sys.stdout) 57 | 58 | logger = logging.getLogger('point_one.fusion_engine') 59 | 60 | # Concatenate all hex characters and convert to bytes. 61 | if len(options.contents) == 1 and os.path.exists(options.contents[0]): 62 | with open(options.contents[0], 'rb') as f: 63 | contents = f.read() 64 | else: 65 | byte_str_array = [b if len(b) % 2 == 0 else f'0{b}' for b in options.contents] 66 | contents_str = ''.join(byte_str_array).replace(' ', '') 67 | if len(contents_str) % 2 != 0: 68 | logger.error("Error: Contents must contain an even number of hex characters.") 69 | sys.exit(1) 70 | 71 | contents = bytes.fromhex(contents_str) 72 | 73 | # Decode the incoming data and print the contents of any complete messages. 74 | decoder = FusionEngineDecoder(warn_on_error=FusionEngineDecoder.WarnOnError.ALL, warn_on_unrecognized=True) 75 | messages = decoder.on_data(contents) 76 | if len(messages) > 0: 77 | logger.info("Successfully decoded %d FusionEngine messages." % len(messages)) 78 | for i, (header, message) in enumerate(messages): 79 | if i > 0: 80 | logger.info('\n') 81 | 82 | logger.info("Header: " + str(header)) 83 | if isinstance(message, MessagePayload): 84 | logger.info("Payload: " + str(message)) 85 | else: 86 | # If we didn't detect any complete messages, see if maybe they didn't provide enough bytes? 87 | if len(contents) < MessageHeader.calcsize(): 88 | logger.warning("Warning: Specified byte string too small to contain a valid FusionEngine message. " 89 | "[size=%d B, minimum=%d B]" % (len(contents), MessageHeader.calcsize())) 90 | else: 91 | try: 92 | # Try to decode a message header anyway. 93 | header = MessageHeader() 94 | header.unpack(contents, validate_crc=False) 95 | if len(contents) < header.get_message_size(): 96 | logger.warning('Warning: Specified byte string too small. [expected=%d B, got=%d B]' % 97 | (header.get_message_size(), len(contents))) 98 | logger.info("Header: " + str(header)) 99 | 100 | # If that succeeds, try to determine the payload type and print out the expected size. If we have enough 101 | # bytes, try to decode the payload. 102 | payload_cls = message_type_to_class.get(header.message_type, None) 103 | if payload_cls is None: 104 | logger.warning("Unrecognized message type %s." % str(header.message_type)) 105 | else: 106 | payload = payload_cls() 107 | 108 | # Calculate the minimum size for this message type. 109 | try: 110 | min_payload_size_bytes = payload.calcsize() 111 | min_message_size_bytes = MessageHeader.calcsize() + min_payload_size_bytes 112 | 113 | logger.info("Minimum size for this message:") 114 | logger.info(" Payload: %d B" % min_payload_size_bytes) 115 | logger.info(" Complete message: %d B" % min_message_size_bytes) 116 | except TypeError: 117 | # SetConfig and ConfigResponse messages cannot be packed or compute size if a configuration 118 | # payload has not been specified. 119 | min_message_size_bytes = 0 120 | 121 | # Try to decode the message payload. If we cannot calculate the minimum size above, we'll just try 122 | # to decode anyway. 123 | if len(contents) >= min_message_size_bytes: 124 | try: 125 | payload.unpack(buffer=contents, offset=header.calcsize(), 126 | message_version=header.message_version) 127 | logger.info("Decoded payload contents: %s" % str(payload)) 128 | except ValueError as e: 129 | logger.warning(str(e)) 130 | logger.warning("Unable to decode payload contents.") 131 | else: 132 | logger.warning("Not enough data to decode payload.") 133 | except ValueError as e: 134 | logger.warning(str(e)) 135 | logger.warning("No valid FusionEngine messages decoded.") 136 | 137 | sys.exit(2) 138 | -------------------------------------------------------------------------------- /python/examples/encode_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages.core import * 12 | from fusion_engine_client.parsers import FusionEngineEncoder 13 | from fusion_engine_client.utils.argument_parser import ArgumentParser 14 | 15 | if __name__ == "__main__": 16 | parser = ArgumentParser(description="""\ 17 | Generate a .p1log file containing a few fixed FusionEngine messages as an 18 | example of using the FusionEngineEncoder class to serialize data. Serialized 19 | messages can be saved to disk, or sent to a FusionEngine device in real time. 20 | 21 | The generated file can be used with any of the example analysis or extraction 22 | scripts. 23 | """) 24 | 25 | parser.add_argument('output', metavar='FILE', nargs='?', default='test_data.p1log', 26 | help='The path to the .p1log file to be generated.') 27 | 28 | options = parser.parse_args() 29 | 30 | p1i_path = os.path.splitext(options.output)[0] + '.p1i' 31 | if os.path.exists(p1i_path): 32 | os.remove(p1i_path) 33 | 34 | print("Creating '%s'." % options.output) 35 | f = open(options.output, 'wb') 36 | 37 | encoder = FusionEngineEncoder() 38 | 39 | # P1 time 1.0 40 | message = PoseMessage() 41 | message.p1_time = Timestamp(1.0) 42 | message.solution_type = SolutionType.DGPS 43 | message.lla_deg = np.array([37.776417, -122.417711, 0.0]) 44 | message.ypr_deg = np.array([45.0, 0.0, 0.0]) 45 | f.write(encoder.encode_message(message)) 46 | 47 | message = PoseAuxMessage() 48 | message.p1_time = Timestamp(1.0) 49 | message.velocity_enu_mps = np.array([0.1, 0.2, 0.3]) 50 | f.write(encoder.encode_message(message)) 51 | 52 | message = GNSSSatelliteMessage() 53 | message.p1_time = Timestamp(1.0) 54 | sv = SatelliteInfo() 55 | sv.system = SatelliteType.GPS 56 | sv.prn = 3 57 | message.svs.append(sv) 58 | sv = SatelliteInfo() 59 | sv.system = SatelliteType.GPS 60 | sv.prn = 4 61 | message.svs.append(sv) 62 | f.write(encoder.encode_message(message)) 63 | 64 | # P1 time 2.0 65 | message = PoseMessage() 66 | message.p1_time = Timestamp(2.0) 67 | message.solution_type = SolutionType.DGPS 68 | message.lla_deg = np.array([37.776466, -122.417502, 0.1]) 69 | message.ypr_deg = np.array([0.0, 0.0, 0.0]) 70 | f.write(encoder.encode_message(message)) 71 | 72 | # Note: Intentionally skipping this message so the file may be used when demonstrating time-aligned file reading. 73 | # message = PoseAuxMessage() 74 | # message.p1_time = Timestamp(2.0) 75 | # message.velocity_enu_mps = np.array([0.2, 0.3, 0.4]) 76 | # f.write(encoder.encode_message(message)) 77 | 78 | message = GNSSSatelliteMessage() 79 | message.p1_time = Timestamp(2.0) 80 | sv = SatelliteInfo() 81 | sv.system = SatelliteType.GPS 82 | sv.prn = 3 83 | message.svs.append(sv) 84 | sv = SatelliteInfo() 85 | sv.system = SatelliteType.GPS 86 | sv.prn = 4 87 | message.svs.append(sv) 88 | sv = SatelliteInfo() 89 | sv.system = SatelliteType.GPS 90 | sv.prn = 5 91 | message.svs.append(sv) 92 | f.write(encoder.encode_message(message)) 93 | 94 | # P1 time 3.0 95 | message = PoseMessage() 96 | message.p1_time = Timestamp(3.0) 97 | message.solution_type = SolutionType.RTKFixed 98 | message.lla_deg = np.array([37.776407, -122.417369, 0.2]) 99 | message.ypr_deg = np.array([-45.0, 0.0, 0.0]) 100 | f.write(encoder.encode_message(message)) 101 | 102 | message = PoseAuxMessage() 103 | message.p1_time = Timestamp(3.0) 104 | message.velocity_enu_mps = np.array([0.4, 0.5, 0.6]) 105 | f.write(encoder.encode_message(message)) 106 | 107 | # Note: Intentionally skipping this message so the file may be used when demonstrating time-aligned file reading. 108 | # message = GNSSSatelliteMessage() 109 | # message.p1_time = Timestamp(3.0) 110 | # sv = SatelliteInfo() 111 | # sv.system = SatelliteType.GPS 112 | # sv.prn = 3 113 | # message.svs.append(sv) 114 | # sv = SatelliteInfo() 115 | # sv.system = SatelliteType.GPS 116 | # sv.prn = 4 117 | # message.svs.append(sv) 118 | # sv = SatelliteInfo() 119 | # sv.system = SatelliteType.GPS 120 | # sv.prn = 5 121 | # message.svs.append(sv) 122 | # f.write(encoder.encode_message(message)) 123 | 124 | # P1 time 4.0 125 | message = PoseMessage() 126 | message.p1_time = Timestamp(4.0) 127 | message.solution_type = SolutionType.RTKFixed 128 | message.lla_deg = np.array([37.776331, -122.417256, 0.3]) 129 | message.ypr_deg = np.array([-45.0, 0.0, 0.0]) 130 | f.write(encoder.encode_message(message)) 131 | 132 | message = PoseAuxMessage() 133 | message.p1_time = Timestamp(4.0) 134 | message.velocity_enu_mps = np.array([0.5, 0.6, 0.7]) 135 | f.write(encoder.encode_message(message)) 136 | 137 | message = GNSSSatelliteMessage() 138 | message.p1_time = Timestamp(4.0) 139 | sv = SatelliteInfo() 140 | sv.system = SatelliteType.GPS 141 | sv.prn = 3 142 | message.svs.append(sv) 143 | sv = SatelliteInfo() 144 | sv.system = SatelliteType.GPS 145 | sv.prn = 4 146 | message.svs.append(sv) 147 | sv = SatelliteInfo() 148 | sv.system = SatelliteType.GPS 149 | sv.prn = 5 150 | message.svs.append(sv) 151 | f.write(encoder.encode_message(message)) 152 | 153 | f.close() 154 | -------------------------------------------------------------------------------- /python/examples/encode_message.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages import * 12 | from fusion_engine_client.parsers import FusionEngineEncoder 13 | from fusion_engine_client.utils.argument_parser import ArgumentParser 14 | from fusion_engine_client.utils.bin_utils import bytes_to_hex 15 | 16 | if __name__ == "__main__": 17 | parser = ArgumentParser(description="""\ 18 | Encode a FusionEngine message and print the resulting binary content to the 19 | console. 20 | """) 21 | 22 | options = parser.parse_args() 23 | 24 | # Enable FusionEngine PoseMessage output on UART1 25 | message = SetMessageRate(output_interface=InterfaceID(TransportType.SERIAL, 1), 26 | protocol=ProtocolType.FUSION_ENGINE, 27 | message_id=MessageType.POSE, 28 | rate=MessageRate.ON_CHANGE) 29 | print(message) 30 | 31 | encoder = FusionEngineEncoder() 32 | encoded_data = encoder.encode_message(message) 33 | 34 | print('') 35 | print(bytes_to_hex(encoded_data, bytes_per_row=16, bytes_per_col=2)) 36 | -------------------------------------------------------------------------------- /python/examples/extract_imu_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.analysis.data_loader import DataLoader 12 | from fusion_engine_client.messages.core import * 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 15 | from fusion_engine_client.utils.argument_parser import ArgumentParser 16 | 17 | if __name__ == "__main__": 18 | # Parse arguments. 19 | parser = ArgumentParser(description="""\ 20 | Extract IMU accelerometer and gyroscope measurements. 21 | """) 22 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 23 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 24 | "specified.") 25 | parser.add_argument('log', 26 | help="The log to be read. May be one of:\n" 27 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 28 | "content\n" 29 | "- The path to a FusionEngine log directory\n" 30 | "- A pattern matching a FusionEngine log directory under the specified base directory " 31 | "(see find_fusion_engine_log() and --log-base-dir)") 32 | options = parser.parse_args() 33 | 34 | # Configure logging. 35 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 36 | logger = logging.getLogger('point_one.fusion_engine') 37 | logger.setLevel(logging.INFO) 38 | 39 | # Locate the input file and set the output directory. 40 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 41 | log_base_dir=options.log_base_dir) 42 | if input_path is None: 43 | sys.exit(1) 44 | 45 | if log_id is None: 46 | logger.info('Loading %s.' % os.path.basename(input_path)) 47 | else: 48 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 49 | 50 | # Read satellite data from the file. 51 | reader = DataLoader(input_path) 52 | result = reader.read(message_types=[IMUOutput, RawIMUOutput], show_progress=True, return_numpy=True) 53 | imu_data = result[IMUOutput.MESSAGE_TYPE] 54 | raw_imu_data = result[RawIMUOutput.MESSAGE_TYPE] 55 | if len(imu_data.p1_time) == 0 and len(raw_imu_data.p1_time) == 0: 56 | logger.warning('No IMU data found in log file.') 57 | sys.exit(2) 58 | 59 | output_prefix = os.path.join(output_dir, os.path.splitext(os.path.basename(input_path))[0]) 60 | 61 | # Generate a CSV file for corrected IMU data. 62 | if len(imu_data.p1_time) != 0: 63 | path = f'{output_prefix}.imu.csv' 64 | logger.info("Generating '%s'." % path) 65 | gps_time = reader.convert_to_gps_time(imu_data.p1_time) 66 | with open(path, 'w') as f: 67 | f.write('P1 Time (sec), GPS Time (sec), Accel X (m/s^2), Y, Z, Gyro X (rad/s), Y, Z\n') 68 | np.savetxt(f, np.concatenate([imu_data.p1_time[:, None], 69 | gps_time[:, None], 70 | imu_data.accel_mps2.T, 71 | imu_data.gyro_rps.T], axis=1), fmt='%.6f') 72 | else: 73 | logger.info("No corrected IMU data.") 74 | 75 | # Generate a CSV file for raw IMU data. 76 | if len(raw_imu_data.p1_time) != 0: 77 | path = f'{output_prefix}.raw_imu.csv' 78 | logger.info("Generating '%s'." % path) 79 | gps_time = reader.convert_to_gps_time(raw_imu_data.p1_time) 80 | with open(path, 'w') as f: 81 | f.write('P1 Time (sec), GPS Time (sec), Accel X (m/s^2), Y, Z, Gyro X (rad/s), Y, Z, Temp(C)\n') 82 | np.savetxt(f, np.concatenate([raw_imu_data.p1_time[:, None], 83 | gps_time[:, None], 84 | raw_imu_data.accel_mps2.T, 85 | raw_imu_data.gyro_rps.T, 86 | raw_imu_data.temperature_degc[:, None]], axis=1), fmt='%.6f') 87 | else: 88 | logger.info("No raw IMU data.") 89 | 90 | logger.info("Output stored in '%s'." % os.path.abspath(output_dir)) 91 | -------------------------------------------------------------------------------- /python/examples/extract_satellite_info.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.analysis.data_loader import DataLoader 12 | from fusion_engine_client.messages.core import * 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.argument_parser import ArgumentParser 15 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 16 | 17 | if __name__ == "__main__": 18 | # Parse arguments. 19 | parser = ArgumentParser(description="""\ 20 | Extract satellite azimuth, elevation, and L1 signal C/N0 data to a CSV file. 21 | """) 22 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 23 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 24 | "specified.") 25 | parser.add_argument('log', 26 | help="The log to be read. May be one of:\n" 27 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 28 | "content\n" 29 | "- The path to a FusionEngine log directory\n" 30 | "- A pattern matching a FusionEngine log directory under the specified base directory " 31 | "(see find_fusion_engine_log() and --log-base-dir)") 32 | options = parser.parse_args() 33 | 34 | # Configure logging. 35 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 36 | logger = logging.getLogger('point_one.fusion_engine') 37 | logger.setLevel(logging.INFO) 38 | 39 | # Locate the input file and set the output directory. 40 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 41 | log_base_dir=options.log_base_dir) 42 | if input_path is None: 43 | sys.exit(1) 44 | 45 | if log_id is None: 46 | logger.info('Loading %s.' % os.path.basename(input_path)) 47 | else: 48 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 49 | 50 | # Read satellite data from the file. 51 | reader = DataLoader(input_path) 52 | result = reader.read(message_types=[GNSSSatelliteMessage], show_progress=True) 53 | satellite_data = result[GNSSSatelliteMessage.MESSAGE_TYPE] 54 | if len(satellite_data.messages) == 0: 55 | logger.warning('No satellite data found in log file.') 56 | sys.exit(2) 57 | 58 | output_prefix = os.path.join(output_dir, os.path.splitext(os.path.basename(input_path))[0]) 59 | 60 | # Generate a CSV file. 61 | path = f'{output_prefix}.satellite_info.csv' 62 | logger.info("Generating '%s'." % path) 63 | with open(path, 'w') as f: 64 | f.write('P1 Time (sec), GPS Time (sec), System, PRN, Azimuth (deg), Elevation (deg), C/N0 (dB-Hz)\n') 65 | for message in satellite_data.messages: 66 | if message.gps_time: 67 | for sv in message.svs: 68 | f.write('%.6f, %.6f, %d, %d, %.1f, %.1f, %f\n' % 69 | (float(message.p1_time), float(message.gps_time), 70 | int(sv.system), sv.prn, sv.azimuth_deg, sv.elevation_deg, sv.cn0_dbhz)) 71 | 72 | logger.info("Output stored in '%s'." % os.path.abspath(output_dir)) 73 | -------------------------------------------------------------------------------- /python/examples/extract_vehicle_speed_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.analysis.data_loader import DataLoader 12 | from fusion_engine_client.messages.core import * 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 15 | from fusion_engine_client.utils.argument_parser import ArgumentParser 16 | 17 | if __name__ == "__main__": 18 | # Parse arguments. 19 | parser = ArgumentParser(description="""\ 20 | Extract wheel speed data. 21 | """) 22 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 23 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 24 | "specified.") 25 | parser.add_argument('log', 26 | help="The log to be read. May be one of:\n" 27 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 28 | "content\n" 29 | "- The path to a FusionEngine log directory\n" 30 | "- A pattern matching a FusionEngine log directory under the specified base directory " 31 | "(see find_fusion_engine_log() and --log-base-dir)") 32 | options = parser.parse_args() 33 | 34 | # Configure logging. 35 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 36 | logger = logging.getLogger('point_one.fusion_engine') 37 | logger.setLevel(logging.INFO) 38 | 39 | # Locate the input file and set the output directory. 40 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 41 | log_base_dir=options.log_base_dir) 42 | if input_path is None: 43 | sys.exit(1) 44 | 45 | if log_id is None: 46 | logger.info('Loading %s.' % os.path.basename(input_path)) 47 | else: 48 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 49 | 50 | # Read satellite data from the file. 51 | reader = DataLoader(input_path) 52 | result = reader.read( 53 | message_types=[ 54 | WheelSpeedOutput, 55 | RawWheelSpeedOutput, 56 | VehicleSpeedOutput, 57 | RawVehicleSpeedOutput], 58 | show_progress=True, 59 | return_numpy=True) 60 | if all(len(d.p1_time) == 0 for d in result.values()): 61 | logger.warning('No speed data found in log file.') 62 | sys.exit(2) 63 | 64 | output_prefix = os.path.join(output_dir, os.path.splitext(os.path.basename(input_path))[0]) 65 | 66 | # Generate a CSV file for corrected wheel speed data. 67 | wheel_speed_data = result[WheelSpeedOutput.MESSAGE_TYPE] 68 | if len(wheel_speed_data.p1_time) != 0: 69 | path = f'{output_prefix}.wheel_speed.csv' 70 | logger.info("Generating '%s'." % path) 71 | gps_time = reader.convert_to_gps_time(wheel_speed_data.p1_time) 72 | with open(path, 'w') as f: 73 | f.write('P1 Time (sec), GPS Time (sec), Front Left Speed (m/s), Front Right Speed (m/s), Back Left Speed (m/s), Back Right Speed (m/s), Gear\n') 74 | np.savetxt(f, np.stack([wheel_speed_data.p1_time, 75 | gps_time, 76 | wheel_speed_data.front_left_speed_mps, 77 | wheel_speed_data.front_right_speed_mps, 78 | wheel_speed_data.rear_left_speed_mps, 79 | wheel_speed_data.rear_right_speed_mps, 80 | wheel_speed_data.gear], axis=1), fmt=['%.6f'] * 6 + ['%d']) 81 | else: 82 | logger.info("No corrected wheel speed data.") 83 | 84 | # Generate a CSV file for raw wheel speed data. 85 | raw_wheel_speed_data = result[RawWheelSpeedOutput.MESSAGE_TYPE] 86 | if len(raw_wheel_speed_data.p1_time) != 0: 87 | path = f'{output_prefix}.raw_wheel_speed.csv' 88 | logger.info("Generating '%s'." % path) 89 | gps_time = reader.convert_to_gps_time(raw_wheel_speed_data.p1_time) 90 | with open(path, 'w') as f: 91 | f.write('P1 Time (sec), GPS Time (sec), Front Left Speed (m/s), Front Right Speed (m/s), Back Left Speed (m/s), Back Right Speed (m/s), Gear\n') 92 | np.savetxt(f, np.stack([raw_wheel_speed_data.p1_time, 93 | gps_time, 94 | raw_wheel_speed_data.front_left_speed_mps, 95 | raw_wheel_speed_data.front_right_speed_mps, 96 | raw_wheel_speed_data.rear_left_speed_mps, 97 | raw_wheel_speed_data.rear_right_speed_mps, 98 | raw_wheel_speed_data.gear], axis=1), fmt=['%.6f'] * 6 + ['%d']) 99 | else: 100 | logger.info("No raw wheel speed data.") 101 | 102 | # Generate a CSV file for corrected vehicle speed data. 103 | vehicle_speed_data = result[VehicleSpeedOutput.MESSAGE_TYPE] 104 | if len(vehicle_speed_data.p1_time) != 0: 105 | path = f'{output_prefix}.vehicle_speed.csv' 106 | logger.info("Generating '%s'." % path) 107 | gps_time = reader.convert_to_gps_time(vehicle_speed_data.p1_time) 108 | with open(path, 'w') as f: 109 | f.write('P1 Time (sec), GPS Time (sec), Vehicle Speed (m/s), Gear\n') 110 | np.savetxt(path, np.stack([vehicle_speed_data.p1_time, 111 | gps_time, 112 | vehicle_speed_data.vehicle_speed_mps, 113 | vehicle_speed_data.gear], axis=1), fmt=['%.6f'] * 3 + ['%d']) 114 | else: 115 | logger.info("No corrected vehicle speed data.") 116 | 117 | # Generate a CSV file for raw vehicle speed data. 118 | raw_vehicle_speed_data = result[RawVehicleSpeedOutput.MESSAGE_TYPE] 119 | if len(raw_vehicle_speed_data.p1_time) != 0: 120 | path = f'{output_prefix}.raw_vehicle_speed.csv' 121 | logger.info("Generating '%s'." % path) 122 | gps_time = reader.convert_to_gps_time(raw_vehicle_speed_data.p1_time) 123 | with open(path, 'w') as f: 124 | f.write('P1 Time (sec), GPS Time (sec), Vehicle Speed (m/s), Gear\n') 125 | np.savetxt(path, np.stack([raw_vehicle_speed_data.p1_time, 126 | gps_time, 127 | raw_vehicle_speed_data.vehicle_speed_mps, 128 | raw_vehicle_speed_data.gear], axis=1), fmt=['%.6f'] * 3 + ['%d']) 129 | else: 130 | logger.info("No raw vehicle speed data.") 131 | 132 | logger.info("Output stored in '%s'." % os.path.abspath(output_dir)) 133 | -------------------------------------------------------------------------------- /python/examples/manual_message_decode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages.core import * 12 | from fusion_engine_client.messages import ros 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.argument_parser import ArgumentParser 15 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 16 | 17 | from examples.message_decode import print_message 18 | 19 | 20 | logger = logging.getLogger('point_one.fusion_engine') 21 | 22 | 23 | def decode_message(header, data, offset): 24 | # Validate the message length and CRC. 25 | if len(data) != header.calcsize() + header.payload_size_bytes: 26 | return False 27 | else: 28 | header.validate_crc(data) 29 | 30 | # Check that the sequence number increments as expected. 31 | if header.sequence_number != decode_message.expected_sequence_number: 32 | logger.info('Warning: unexpected sequence number. [expected=%d, received=%d]' % 33 | (decode_message.expected_sequence_number, header.sequence_number)) 34 | 35 | decode_message.expected_sequence_number = header.sequence_number + 1 36 | 37 | # Deserialize and print the message contents. 38 | # 39 | # Note: This could also be done more generally using the fusion_engine_client.message_type_to_class dictionary. 40 | # We do it explicitly here for sake of example. 41 | if header.message_type == PoseMessage.MESSAGE_TYPE: 42 | contents = PoseMessage() 43 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 44 | elif header.message_type == GNSSInfoMessage.MESSAGE_TYPE: 45 | contents = GNSSInfoMessage() 46 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 47 | elif header.message_type == GNSSSatelliteMessage.MESSAGE_TYPE: 48 | contents = GNSSSatelliteMessage() 49 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 50 | elif header.message_type == ros.ROSPoseMessage.MESSAGE_TYPE: 51 | contents = ros.ROSPoseMessage() 52 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 53 | elif header.message_type == ros.ROSGPSFixMessage.MESSAGE_TYPE: 54 | contents = ros.ROSGPSFixMessage() 55 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 56 | elif header.message_type == ros.ROSIMUMessage.MESSAGE_TYPE: 57 | contents = ros.ROSIMUMessage() 58 | contents.unpack(buffer=data, offset=offset, message_version=header.message_version) 59 | else: 60 | contents = None 61 | 62 | print_message(header, contents) 63 | 64 | return True 65 | 66 | 67 | decode_message.expected_sequence_number = 0 68 | 69 | 70 | if __name__ == "__main__": 71 | parser = ArgumentParser(description="""\ 72 | Manually decode and print the contents of messages contained in a *.p1log file. 73 | 74 | This example application is similar to message_decode.py, but here we 75 | demonstrate manually decoding message headers and payloads, similar to what you 76 | might do for an incoming FusionEngine binary data stream. 77 | """) 78 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 79 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 80 | "specified.") 81 | parser.add_argument('log', 82 | help="The log to be read. May be one of:\n" 83 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 84 | "content\n" 85 | "- The path to a FusionEngine log directory\n" 86 | "- A pattern matching a FusionEngine log directory under the specified base directory " 87 | "(see find_fusion_engine_log() and --log-base-dir)") 88 | options = parser.parse_args() 89 | 90 | # Configure logging. 91 | logging.basicConfig(format='%(message)s', stream=sys.stdout) 92 | logger.setLevel(logging.INFO) 93 | 94 | # Locate the input file and set the output directory. 95 | # 96 | # Note that, unlike the message_decode.py example, here we _do_ ask locate_log() to create a *.p1log file for us if 97 | # it finds an input file containing a mix of FusionEngine messages and other data. The loop below assumes that the 98 | # file we are reading contains _only_ FusionEngine messages. 99 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 100 | log_base_dir=options.log_base_dir) 101 | if input_path is None: 102 | sys.exit(1) 103 | 104 | if log_id is None: 105 | logger.info('Loading %s.' % os.path.basename(input_path)) 106 | else: 107 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 108 | 109 | # Read one FusionEngine message at a time from the binary file. If the file contains any data that is not part of a 110 | # valid FusionEngine message, we will exit immediately. 111 | f = open(input_path, 'rb') 112 | 113 | while True: 114 | # Read the next message header. 115 | data = f.read(MessageHeader.calcsize()) 116 | if len(data) == 0: 117 | break 118 | 119 | # Deserialize the header. 120 | try: 121 | header = MessageHeader() 122 | offset = header.unpack(buffer=data) 123 | except Exception as e: 124 | logger.error('Decode error: %s' % str(e)) 125 | break 126 | 127 | # Read the message payload and append it to the header. 128 | data += f.read(header.payload_size_bytes) 129 | 130 | try: 131 | if not decode_message(header, data, offset): 132 | break 133 | except Exception as e: 134 | logger.error('Decode error: %s' % str(e)) 135 | break 136 | -------------------------------------------------------------------------------- /python/examples/manual_tcp_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import socket 5 | import sys 6 | 7 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 8 | # if this application is being run directly out of the repository and is not installed as a pip package. 9 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 10 | sys.path.insert(0, root_dir) 11 | 12 | from fusion_engine_client.messages.core import * 13 | from fusion_engine_client.utils.argument_parser import ArgumentParser 14 | 15 | from examples.manual_message_decode import decode_message 16 | 17 | 18 | if __name__ == "__main__": 19 | parser = ArgumentParser(description="""\ 20 | Connect to an Point One device over TCP and print out the incoming message 21 | contents and/or log the messages to disk. 22 | 23 | This example interprets the incoming data directly, and does not use the 24 | FusionEngineDecoder class. The incoming data stream must contain only 25 | FusionEngine messages. See also tcp_client.py. 26 | """) 27 | parser.add_argument('-p', '--port', type=int, default=30201, 28 | help="The FusionEngine TCP port on the data source.") 29 | parser.add_argument('hostname', 30 | help="The IP address or hostname of the data source.") 31 | options = parser.parse_args() 32 | 33 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 34 | sock.connect((socket.gethostbyname(options.hostname), options.port)) 35 | 36 | received_data = b'' 37 | current_header = None 38 | current_offset = 0 39 | while True: 40 | # Read some data. 41 | try: 42 | new_data = sock.recv(1024) 43 | except KeyboardInterrupt: 44 | break 45 | 46 | received_data += new_data 47 | 48 | # Process all complete packets in the stream. 49 | while True: 50 | # Wait for a complete message header. 51 | if current_header is None: 52 | if len(received_data) - current_offset < MessageHeader._SIZE: 53 | # No more complete packets in the buffer. Discard any data that was already processed. 54 | received_data = received_data[current_offset:] 55 | current_offset = 0 56 | break 57 | else: 58 | current_header = MessageHeader() 59 | current_offset += current_header.unpack(buffer=received_data, offset=current_offset) 60 | 61 | # Now wait for the message payload. 62 | if len(received_data) - current_offset < current_header.payload_size_bytes: 63 | break 64 | 65 | # Finally, decode the message. This function will verify the CRC, but in theory at least, this should not be 66 | # necessary for a TCP stream. 67 | decode_message(current_header, received_data, current_offset) 68 | current_offset += current_header.payload_size_bytes 69 | current_header = None 70 | 71 | sock.close() 72 | -------------------------------------------------------------------------------- /python/examples/message_decode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages.core import MessagePayload 12 | from fusion_engine_client.parsers import FusionEngineDecoder 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.argument_parser import ArgumentParser 15 | from fusion_engine_client.utils.log import locate_log, DEFAULT_LOG_BASE_DIR 16 | 17 | 18 | logger = logging.getLogger('point_one.fusion_engine') 19 | 20 | 21 | def print_message(header, contents): 22 | if isinstance(contents, MessagePayload): 23 | parts = str(contents).split('\n') 24 | parts[0] += ' [sequence=%d, size=%d B]' % (header.sequence_number, header.get_message_size()) 25 | logger.info('\n'.join(parts)) 26 | else: 27 | logger.info('Decoded %s message [sequence=%d, size=%d B]' % 28 | (header.get_type_string(), header.sequence_number, header.get_message_size())) 29 | 30 | 31 | if __name__ == "__main__": 32 | parser = ArgumentParser(description="""\ 33 | Decode and print the contents of messages contained in a *.p1log file or other 34 | binary file containing FusionEngine messages. The binary file may also contain 35 | other types of data. 36 | """) 37 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 38 | help="The base directory containing FusionEngine logs to be searched if a log pattern is " 39 | "specified.") 40 | parser.add_argument('log', 41 | help="The log to be read. May be one of:\n" 42 | "- The path to a .p1log file or a file containing FusionEngine messages and other " 43 | "content\n" 44 | "- The path to a FusionEngine log directory\n" 45 | "- A pattern matching a FusionEngine log directory under the specified base directory " 46 | "(see find_fusion_engine_log() and --log-base-dir)") 47 | options = parser.parse_args() 48 | 49 | # Configure logging. 50 | logging.basicConfig(format='%(message)s', stream=sys.stdout) 51 | logger.setLevel(logging.INFO) 52 | 53 | # Locate the input file and set the output directory. 54 | # 55 | # Note here that we intentionally tell locate_log() _not_ to convert the input file to a *.p1log file 56 | # (extract_fusion_engine_data=False). That way, if it finds a file containing a mix of FusionEngine messages and 57 | # other data, we can use that file directly to demonstrate how the FusionEngineDecoder class operates. 58 | input_path, output_dir, log_id = locate_log(options.log, return_output_dir=True, return_log_id=True, 59 | log_base_dir=options.log_base_dir, extract_fusion_engine_data=False) 60 | if input_path is None: 61 | sys.exit(1) 62 | 63 | if log_id is None: 64 | logger.info('Loading %s.' % os.path.basename(input_path)) 65 | else: 66 | logger.info('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 67 | 68 | # Read binary data from the file a 1 KB chunk at a time and decode any FusionEngine messages in the stream. Any data 69 | # that is not part of a FusionEngine message will be ignored. 70 | f = open(input_path, 'rb') 71 | 72 | decoder = FusionEngineDecoder() 73 | while True: 74 | # Read the next message header. 75 | data = f.read(1024) 76 | if len(data) == 0: 77 | break 78 | 79 | # Decode the incoming data and print the contents of any complete messages. 80 | messages = decoder.on_data(data) 81 | for (header, message) in messages: 82 | print_message(header, message) 83 | -------------------------------------------------------------------------------- /python/examples/send_command.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 7 | # if this application is being run directly out of the repository and is not installed as a pip package. 8 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 9 | sys.path.insert(0, root_dir) 10 | 11 | from fusion_engine_client.messages import * 12 | from fusion_engine_client.parsers import FusionEngineDecoder, FusionEngineEncoder 13 | from fusion_engine_client.utils import trace as logging 14 | from fusion_engine_client.utils.argument_parser import ArgumentParser 15 | from fusion_engine_client.utils.bin_utils import bytes_to_hex 16 | from fusion_engine_client.utils.transport_utils import * 17 | 18 | 19 | if __name__ == "__main__": 20 | parser = ArgumentParser(description="""\ 21 | Send a command to a Point One device and wait for a response. 22 | """) 23 | 24 | parser.add_argument( 25 | '-i', '--ignore-response', action='store_true', 26 | help="Do not wait for a response from the device.") 27 | 28 | parser.add_argument( 29 | '-v', '--verbose', action='count', default=0, 30 | help="Print verbose/trace debugging messages.") 31 | 32 | parser.add_argument( 33 | 'transport', type=str, 34 | help=TRANSPORT_HELP_STRING) 35 | 36 | options = parser.parse_args() 37 | 38 | # Configure output logging. 39 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 40 | logger = logging.getLogger('point_one.fusion_engine.app') 41 | 42 | if options.verbose == 0: 43 | logger.setLevel(logging.INFO) 44 | elif options.verbose == 1: 45 | logger.setLevel(logging.DEBUG) 46 | elif options.verbose == 2: 47 | logger.setLevel(logging.DEBUG) 48 | logging.getLogger('point_one.fusion_engine.parsers').setLevel(logging.DEBUG) 49 | else: 50 | logger.setLevel(logging.DEBUG) 51 | logging.getLogger('point_one.fusion_engine.parsers').setLevel( 52 | logging.getTraceLevel(depth=options.verbose - 1)) 53 | 54 | # Connect to the device using the specified transport. 55 | response_timeout_sec = 3.0 56 | 57 | try: 58 | transport = create_transport(options.transport, timeout_sec=response_timeout_sec, print_func=logger.info) 59 | except Exception as e: 60 | logger.error(str(e)) 61 | sys.exit(1) 62 | 63 | # Specify the message to be sent. 64 | message = ResetRequest(reset_mask=ResetRequest.HOT_START) 65 | # message = SetConfigMessage(GNSSLeverArmConfig(0.4, 0.0, 1.2)) 66 | # message = GetConfigMessage(GNSSLeverArmConfig) 67 | # message = SetConfigMessage(EnabledGNSSSystemsConfig(SatelliteType.GPS, SatelliteType.GALILEO)) 68 | # message = SetMessageRate(output_interface=InterfaceID(TransportType.SERIAL, 1), 69 | # protocol=ProtocolType.FUSION_ENGINE, 70 | # message_id=MessageType.POSE, 71 | # rate=MessageRate.ON_CHANGE) 72 | # message = FaultControlMessage(payload=FaultControlMessage.EnableGNSS(False)) 73 | 74 | # Send the command. 75 | logger.info("Sending message:\n%s" % str(message)) 76 | 77 | encoder = FusionEngineEncoder() 78 | encoded_data = encoder.encode_message(message) 79 | logger.debug(bytes_to_hex(encoded_data, bytes_per_row=16, bytes_per_col=2)) 80 | 81 | transport.send(encoded_data) 82 | 83 | # Listen for the response. 84 | decoder = FusionEngineDecoder(warn_on_unrecognized=False, return_bytes=True) 85 | response = Response.OK if options.ignore_response else None 86 | start_time = datetime.now() 87 | while response is None: 88 | if (datetime.now() - start_time).total_seconds() > response_timeout_sec: 89 | logger.error("Timed out waiting for a response.") 90 | break 91 | 92 | try: 93 | received_data = transport.recv(1024) 94 | except socket.timeout: 95 | logger.error("Timed out waiting for a response.") 96 | break 97 | except KeyboardInterrupt: 98 | break 99 | 100 | messages = decoder.on_data(received_data) 101 | for header, message, encoded_data in messages: 102 | if isinstance(message, (CommandResponseMessage, ConfigResponseMessage)): 103 | logger.info("Received response:\n%s" % str(message)) 104 | logger.debug(bytes_to_hex(encoded_data, bytes_per_row=16, bytes_per_col=2)) 105 | response = message.response 106 | break 107 | 108 | transport.close() 109 | 110 | if response == Response.OK: 111 | sys.exit(0) 112 | else: 113 | sys.exit(3) 114 | -------------------------------------------------------------------------------- /python/examples/send_vehicle_speed.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | import time 6 | 7 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 8 | # if this application is being run directly out of the repository and is not installed as a pip package. 9 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 10 | sys.path.insert(0, root_dir) 11 | 12 | from fusion_engine_client.messages import * 13 | from fusion_engine_client.parsers import FusionEngineEncoder 14 | from fusion_engine_client.utils import trace as logging 15 | from fusion_engine_client.utils.argument_parser import ArgumentParser 16 | from fusion_engine_client.utils.bin_utils import bytes_to_hex 17 | from fusion_engine_client.utils.transport_utils import * 18 | 19 | 20 | if __name__ == "__main__": 21 | parser = ArgumentParser(description="""\ 22 | Send example vehicle/wheel speed measurements to a Point One device at a fixed 23 | rate. 24 | """) 25 | 26 | parser.add_argument( 27 | '-i', '--interval', type=float, default=1.0, 28 | help="The message interval (in seconds).") 29 | parser.add_argument( 30 | '-t', '--type', choices=('vehicle', 'wheel', 'one_wheel'), default='vehicle', 31 | help="""\ 32 | Specify the type of speed message to be sent to the device: 33 | - on_wheel - Send a WheelSpeedInput message with a single speed for the 34 | front-right wheel 35 | - vehicle - Send a VehicleSpeedInput message with a single speed value 36 | - wheel - Send a WheelSpeedInput message with a speed values for each wheel 37 | """) 38 | parser.add_argument( 39 | '-v', '--verbose', action='count', default=0, 40 | help="Print verbose/trace debugging messages.") 41 | 42 | parser.add_argument( 43 | 'transport', type=str, 44 | help=TRANSPORT_HELP_STRING) 45 | 46 | options = parser.parse_args() 47 | 48 | # Configure output logging. 49 | logging.basicConfig(format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', stream=sys.stdout) 50 | logger = logging.getLogger('point_one.fusion_engine.app') 51 | 52 | if options.verbose == 0: 53 | logger.setLevel(logging.INFO) 54 | elif options.verbose == 1: 55 | logger.setLevel(logging.DEBUG) 56 | elif options.verbose == 2: 57 | logger.setLevel(logging.DEBUG) 58 | logging.getLogger('point_one.fusion_engine.parsers').setLevel(logging.DEBUG) 59 | else: 60 | logger.setLevel(logging.DEBUG) 61 | logging.getLogger('point_one.fusion_engine.parsers').setLevel( 62 | logging.getTraceLevel(depth=options.verbose - 1)) 63 | 64 | # Connect to the device using the specified transport. 65 | try: 66 | transport = create_transport(options.transport, print_func=logger.info, timeout_sec=3.0) 67 | except Exception as e: 68 | logger.error(str(e)) 69 | sys.exit(1) 70 | 71 | # Specify the message to be sent. 72 | if options.type == 'vehicle': 73 | message = VehicleSpeedInput() 74 | message.vehicle_speed_mps = 1 75 | elif options.type == 'wheel': 76 | message = WheelSpeedInput() 77 | message.front_right_speed_mps = 1 78 | message.front_left_speed_mps = 1 79 | message.rear_right_speed_mps = 1 80 | message.rear_left_speed_mps = 1 81 | elif options.type == 'one_wheel': 82 | message = WheelSpeedInput() 83 | message.front_right_speed_mps = 1 84 | else: 85 | logger.error(f"Unrecognized message type '{options.type}'.") 86 | sys.exit(2) 87 | 88 | encoder = FusionEngineEncoder() 89 | 90 | try: 91 | while True: 92 | logger.info("Sending message:\n%s" % str(message)) 93 | 94 | encoded_data = encoder.encode_message(message) 95 | logger.debug(bytes_to_hex(encoded_data, bytes_per_row=16, bytes_per_col=2)) 96 | 97 | transport.send(encoded_data) 98 | time.sleep(options.interval) 99 | except KeyboardInterrupt: 100 | pass 101 | finally: 102 | transport.close() 103 | -------------------------------------------------------------------------------- /python/examples/serial_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | try: 7 | import serial 8 | except ImportError: 9 | print("This application requires pyserial. Please install (pip install pyserial) and run again.") 10 | sys.exit(1) 11 | 12 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 13 | # if this application is being run directly out of the repository and is not installed as a pip package. 14 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 15 | sys.path.insert(0, root_dir) 16 | 17 | from fusion_engine_client.utils.argument_parser import ArgumentParser 18 | from fusion_engine_client.messages import MessagePayload 19 | from fusion_engine_client.parsers import FusionEngineDecoder 20 | 21 | 22 | if __name__ == "__main__": 23 | # Parse command-line arguments. 24 | parser = ArgumentParser(description="""\ 25 | Connect to a Point One device over serial and print out the incoming message 26 | contents. 27 | """) 28 | parser.add_argument('-b', '--baud', type=int, default=460800, 29 | help="The serial baud rate to be used.") 30 | parser.add_argument('port', 31 | help="The serial device to use (e.g., /dev/ttyUSB0, COM1)") 32 | options = parser.parse_args() 33 | 34 | # Connect to the device. 35 | transport = serial.Serial(port=options.port, baudrate=options.baud) 36 | 37 | # Listen for incoming data and parse FusionEngine messages. 38 | try: 39 | decoder = FusionEngineDecoder() 40 | while True: 41 | received_data = transport.read(1024) 42 | messages = decoder.on_data(received_data) 43 | for header, message in messages: 44 | if isinstance(message, MessagePayload): 45 | print(str(message)) 46 | else: 47 | print(f'{header.message_type} message (not supported)') 48 | except KeyboardInterrupt: 49 | pass 50 | except serial.SerialException as e: 51 | print('Unexpected error reading from device:\r%s' % str(e)) 52 | 53 | # Close the transport when finished. 54 | transport.close() 55 | -------------------------------------------------------------------------------- /python/examples/tcp_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import socket 5 | import sys 6 | 7 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 8 | # if this application is being run directly out of the repository and is not installed as a pip package. 9 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 10 | sys.path.insert(0, root_dir) 11 | 12 | from fusion_engine_client.utils.argument_parser import ArgumentParser 13 | from fusion_engine_client.messages import MessagePayload 14 | from fusion_engine_client.parsers import FusionEngineDecoder 15 | 16 | 17 | if __name__ == "__main__": 18 | # Parse command-line arguments. 19 | parser = ArgumentParser(description="""\ 20 | Connect to a Point One device over TCP and print out the incoming message 21 | contents. 22 | """) 23 | parser.add_argument('-p', '--port', type=int, default=30201, 24 | help="The FusionEngine TCP port on the data source.") 25 | parser.add_argument('hostname', 26 | help="The IP address or hostname of the data source.") 27 | options = parser.parse_args() 28 | 29 | # Connect to the device. 30 | transport = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 31 | transport.connect((socket.gethostbyname(options.hostname), options.port)) 32 | 33 | # Listen for incoming data and parse FusionEngine messages. 34 | try: 35 | decoder = FusionEngineDecoder() 36 | while True: 37 | received_data = transport.recv(1024) 38 | messages = decoder.on_data(received_data) 39 | for header, message in messages: 40 | if isinstance(message, MessagePayload): 41 | print(str(message)) 42 | else: 43 | print(f'{header.message_type} message (not supported)') 44 | except KeyboardInterrupt: 45 | pass 46 | 47 | # Close the transport when finished. 48 | transport.close() 49 | -------------------------------------------------------------------------------- /python/examples/udp_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import socket 5 | import sys 6 | 7 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 8 | # if this application is being run directly out of the repository and is not installed as a pip package. 9 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 10 | sys.path.insert(0, root_dir) 11 | 12 | from fusion_engine_client.utils.argument_parser import ArgumentParser 13 | from fusion_engine_client.messages import MessagePayload 14 | from fusion_engine_client.parsers import FusionEngineDecoder 15 | 16 | 17 | if __name__ == "__main__": 18 | # Parse command-line arguments. 19 | parser = ArgumentParser(description="""\ 20 | Connect to a Point One device over UDP and print out the incoming message 21 | contents. 22 | 23 | When using UDP, you must configure the device to send data to your machine. 24 | """) 25 | parser.add_argument('-p', '--port', type=int, default=30400, 26 | help="The FusionEngine UDP port on the data source.") 27 | options = parser.parse_args() 28 | 29 | # Connect to the device. 30 | transport = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 31 | transport.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 32 | transport.bind(('', options.port)) 33 | 34 | # Listen for incoming data and parse FusionEngine messages. 35 | try: 36 | decoder = FusionEngineDecoder() 37 | while True: 38 | received_data = transport.recv(1024) 39 | messages = decoder.on_data(received_data) 40 | for header, message in messages: 41 | if isinstance(message, MessagePayload): 42 | print(str(message)) 43 | else: 44 | print(f'{header.message_type} message (not supported)') 45 | except KeyboardInterrupt: 46 | pass 47 | 48 | # Close the transport when finished. 49 | transport.close() 50 | -------------------------------------------------------------------------------- /python/fusion_engine_client/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ['analysis', 'applications', 'messages', 'parsers', 'utils'] 2 | __version__ = '1.24.1' 3 | __author__ = 'Point One Navigation' 4 | -------------------------------------------------------------------------------- /python/fusion_engine_client/analysis/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/python/fusion_engine_client/analysis/__init__.py -------------------------------------------------------------------------------- /python/fusion_engine_client/analysis/attitude.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def get_ned_rotation_matrix(latitude, longitude, deg=False): 5 | """! 6 | @brief Get the rotation matrix resolving from ECEF to the local NED frame. 7 | 8 | @param latitude The local latitude (in rad). 9 | @param longitude The local longitude (in rad). 10 | @param deg If @c True, interpret @c latitude and @c longitude in degrees instead of radians. 11 | 12 | @return A 3x3 rotation matrix as an @c np.array. 13 | """ 14 | if deg: 15 | latitude = np.deg2rad(latitude) 16 | longitude = np.deg2rad(longitude) 17 | 18 | cos_lat = np.cos(latitude) 19 | sin_lat = np.sin(latitude) 20 | cos_lon = np.cos(longitude) 21 | sin_lon = np.sin(longitude) 22 | 23 | result = np.zeros([3, 3]) 24 | result[0, 0] = -sin_lat * cos_lon 25 | result[0, 1] = -sin_lat * sin_lon 26 | result[0, 2] = cos_lat 27 | 28 | result[1, 0] = -sin_lon 29 | result[1, 1] = cos_lon 30 | 31 | result[2, 0] = -cos_lat * cos_lon 32 | result[2, 1] = -cos_lat * sin_lon 33 | result[2, 2] = -sin_lat 34 | 35 | return result 36 | 37 | 38 | def get_enu_rotation_matrix(latitude, longitude, deg=False): 39 | """! 40 | @brief Get the rotation matrix resolving from ECEF to the local ENU frame. 41 | 42 | @param latitude The local latitude (in rad). 43 | @param longitude The local longitude (in rad). 44 | @param deg If @c True, interpret @c latitude and @c longitude in degrees instead of radians. 45 | 46 | @return A 3x3 rotation matrix as an @c np.array. 47 | """ 48 | C_ned_e = get_ned_rotation_matrix(latitude=latitude, longitude=longitude, deg=deg) 49 | C_enu_e = C_ned_e[(1, 0, 2), :] 50 | C_enu_e[2, :] = -C_enu_e[2, :] 51 | return C_enu_e 52 | -------------------------------------------------------------------------------- /python/fusion_engine_client/applications/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/python/fusion_engine_client/applications/__init__.py -------------------------------------------------------------------------------- /python/fusion_engine_client/applications/import_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | 5 | def enable_relative_imports(name, file, package=None): 6 | """! 7 | @brief Enable relative imports for a Python script run as `python script.py`. 8 | 9 | When running an application directly as a script (e.g., python p1_*.py), rather than as an application installed by 10 | pip (e.g., p1_*), Python will not set the application's `__package__` setting, and will not include the module 11 | root directory (fusion-engine-client/python/) in the import search path. That means that by default, scripts cannot 12 | perform relative imports to other files in the same module. This function sets both of those things so that an 13 | application may perform either relative or absolute imports as needed. 14 | 15 | @note 16 | Note that Python will include the parent directory of the script itself in the search path, so we do not 17 | need to do that in order to do the _absolute_ import `from import_utils ...` below. However, if the file is being 18 | imported within another file and not executed directly (for example, called as `p1_*` from an entry point script 19 | installed by `pip`), its parent directory is _not_ guaranteed to be present on the search path so the absolute 20 | import will fail. It is highly recommended that you check the value of `__package__` before importing this function. 21 | 22 | For example, imagine an application `fusion_engine_client/applications/p1_my_app.py` and run as 23 | `python p1_my_app.py`: 24 | ```py 25 | if __package__ is None or __package__ == "": 26 | from import_utils import enable_relative_imports 27 | __package__ = enable_relative_imports(__name__, __file__, __package__) 28 | 29 | # Can now do a relative import (recommended): 30 | from ..messages import PoseMessage 31 | # or an absolute import: 32 | from fusion_engine_client.messages import PoseMessage 33 | ``` 34 | """ 35 | if name == "__main__": 36 | # Note that the root directory is fixed relative to the path to _this_ file, not the caller's file. 37 | root_dir = os.path.normpath(os.path.join(os.path.abspath(os.path.dirname(__file__)), '../..')) 38 | sys.path.insert(0, root_dir) 39 | 40 | if package is None or package == "": 41 | package = os.path.dirname(os.path.relpath(file, root_dir)).replace('/', '.') 42 | return package 43 | -------------------------------------------------------------------------------- /python/fusion_engine_client/applications/p1_display.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | if __package__ is None or __package__ == "": 4 | from import_utils import enable_relative_imports 5 | __package__ = enable_relative_imports(__name__, __file__) 6 | 7 | from ..analysis.analyzer import main as analyzer_main 8 | 9 | 10 | def main(): 11 | analyzer_main() 12 | 13 | 14 | if __name__ == "__main__": 15 | main() 16 | -------------------------------------------------------------------------------- /python/fusion_engine_client/applications/p1_extract.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | if __package__ is None or __package__ == "": 7 | from import_utils import enable_relative_imports 8 | __package__ = enable_relative_imports(__name__, __file__) 9 | 10 | from ..utils import trace as logging 11 | from ..utils.argument_parser import ArgumentParser 12 | from ..utils.log import extract_fusion_engine_log, find_log_file, CANDIDATE_LOG_FILES, DEFAULT_LOG_BASE_DIR 13 | 14 | 15 | def main(): 16 | parser = ArgumentParser(description="""\ 17 | Extract FusionEngine message contents from a binary file containing mixed data 18 | (e.g., interleaved RTCM and FusionEngine messages). 19 | """) 20 | 21 | parser.add_argument('--log-base-dir', metavar='DIR', default=DEFAULT_LOG_BASE_DIR, 22 | help="The base directory containing FusionEngine logs to be searched if a log pattern is" 23 | "specified.") 24 | parser.add_argument('-c', '--candidate-files', type=str, metavar='DIR', 25 | help="An optional comma-separated list of candidate input filenames to search within the log " 26 | "directory.") 27 | parser.add_argument('-o', '--output', type=str, metavar='DIR', 28 | help="The directory where output will be stored. Defaults to the parent directory of the input" 29 | "file, or to the log directory if reading from a log.") 30 | parser.add_argument('-p', '--prefix', type=str, 31 | help="Use the specified prefix for the output file: `.p1log`. Otherwise, use the " 32 | "filename of the input data file (e.g., `input.p1log`), or `fusion_engine` if reading " 33 | "from a log (e.g., `fusion_engine.p1log`).") 34 | parser.add_argument('-v', '--verbose', action='count', default=0, 35 | help="Print verbose/trace debugging messages.") 36 | 37 | parser.add_argument('log', 38 | help="The log to be read. May be one of:\n" 39 | "- The path to a binary data file\n" 40 | "- The path to a FusionEngine log directory containing a candidate binary data file\n" 41 | "- A pattern matching a FusionEngine log directory under the specified base directory " 42 | "(see find_fusion_engine_log() and --log-base-dir)") 43 | 44 | options = parser.parse_args() 45 | 46 | # Configure logging. 47 | logging.basicConfig(level=logging.INFO, format='%(message)s', stream=sys.stdout) 48 | logger = logging.getLogger('point_one.fusion_engine') 49 | if options.verbose == 1: 50 | logger.setLevel(logging.DEBUG) 51 | elif options.verbose > 1: 52 | logger.setLevel(logging.getTraceLevel(depth=options.verbose - 1)) 53 | 54 | # Locate the input file and set the output directory. 55 | try: 56 | if options.candidate_files is None: 57 | candidate_files = CANDIDATE_LOG_FILES 58 | else: 59 | candidate_files = options.candidate_files.split(',') 60 | 61 | input_path, output_dir, log_id = find_log_file(options.log, candidate_files=candidate_files, 62 | return_output_dir=True, return_log_id=True, 63 | log_base_dir=options.log_base_dir) 64 | 65 | if log_id is None: 66 | print('Loading %s.' % os.path.basename(input_path)) 67 | else: 68 | print('Loading %s from log %s.' % (os.path.basename(input_path), log_id)) 69 | 70 | if options.output is not None: 71 | output_dir = options.output 72 | except FileNotFoundError as e: 73 | print(str(e)) 74 | sys.exit(1) 75 | 76 | # Read through the data file, searching for valid FusionEngine messages to extract and store in 77 | # 'output_dir/.p1log'. 78 | if options.prefix is not None: 79 | prefix = options.prefix 80 | elif log_id is not None: 81 | prefix = 'fusion_engine' 82 | else: 83 | prefix = os.path.splitext(os.path.basename(input_path))[0] 84 | output_path = os.path.join(output_dir, prefix + '.p1log') 85 | 86 | valid_count = extract_fusion_engine_log(input_path, output_path) 87 | if options.verbose == 0: 88 | # If verbose > 0, extract_fusion_engine_log() will log messages. 89 | if valid_count > 0: 90 | logger.info('Found %d valid FusionEngine messages.' % valid_count) 91 | else: 92 | logger.debug('No FusionEngine messages found.') 93 | 94 | logger.info(f"Output stored in '{output_path}'.") 95 | 96 | 97 | if __name__ == "__main__": 98 | main() 99 | -------------------------------------------------------------------------------- /python/fusion_engine_client/applications/p1_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from datetime import datetime 4 | import os 5 | import sys 6 | import time 7 | 8 | # Since stdout is used for data stream, don't write any print statements to stdout. 9 | # Done here to avoid any log/print statements triggered by imports. 10 | original_stdout = sys.stdout 11 | sys.stdout = sys.stderr 12 | 13 | # Add the Python root directory (fusion-engine-client/python/) to the import search path to enable FusionEngine imports 14 | # if this application is being run directly out of the repository and is not installed as a pip package. 15 | root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..')) 16 | sys.path.insert(0, root_dir) 17 | 18 | from fusion_engine_client.messages import MessagePayload, message_type_by_name 19 | from fusion_engine_client.parsers import FusionEngineDecoder 20 | from fusion_engine_client.utils.argument_parser import ArgumentParser, ExtendedBooleanAction 21 | 22 | 23 | if __name__ == "__main__": 24 | parser = ArgumentParser(description="""\ 25 | Filter FusionEngine data coming through stdin. Examples: 26 | netcat 192.168.1.138 30210 | \ 27 | ./p1_filter.py --blacklist -m GNSSSatellite --display > /tmp/out.p1log 28 | cat /tmp/out.p1log | ./p1_filter.py -m Pose > /tmp/pose_out.p1log 29 | stty -F /dev/ttyUSB0 speed 460800 cs8 \ 30 | -cstopb -parenb -icrnl -ixon -ixoff -opost -isig -icanon -echo && \ 31 | cat /dev/ttyUSB0 | \ 32 | ./p1_filter.py -m Pose > /tmp/pose_out.p1log 33 | """) 34 | 35 | parser.add_argument( 36 | '-m', '--message-type', type=str, action='append', 37 | help="An list of class names corresponding with the message types to forward or discard (see --blacklist).\n" 38 | "\n" 39 | "May be specified multiple times (-m Pose -m PoseAux), or as a comma-separated list (-m Pose,PoseAux). " 40 | "All matches are case-insensitive.\n" 41 | "\n" 42 | "If a partial name is specified, the best match will be returned. Use the wildcard '*' to match multiple " 43 | "message types.\n" 44 | "\n" 45 | "Supported types:\n%s" % '\n'.join(['- %s' % c for c in message_type_by_name.keys()])) 46 | parser.add_argument( 47 | '--blacklist', action=ExtendedBooleanAction, 48 | help="""\ 49 | If specified, discard all message types specified with --message-type and output everything else. 50 | 51 | By default, all specified message types are output and all others are discarded.""") 52 | parser.add_argument( 53 | '--display', action=ExtendedBooleanAction, 54 | help="Periodically print status on stderr.") 55 | options = parser.parse_args() 56 | 57 | # If the user specified a set of message names, lookup their type values. Below, we will limit the printout to only 58 | # those message types. 59 | message_types = set() 60 | if options.message_type is not None: 61 | # Pattern match to any of: 62 | # -m Type1 63 | # -m Type1 -m Type2 64 | # -m Type1,Type2 65 | # -m Type1,Type2 -m Type3 66 | # -m Type* 67 | try: 68 | message_types = MessagePayload.find_matching_message_types(options.message_type) 69 | if len(message_types) == 0: 70 | # find_matching_message_types() will print an error. 71 | sys.exit(1) 72 | except ValueError as e: 73 | print(str(e)) 74 | sys.exit(1) 75 | 76 | start_time = datetime.now() 77 | last_print_time = datetime.now() 78 | bytes_received = 0 79 | bytes_forwarded = 0 80 | messages_received = 0 81 | messages_forwarded = 0 82 | 83 | # Listen for incoming data. 84 | decoder = FusionEngineDecoder(return_bytes=True) 85 | try: 86 | while True: 87 | # Need to specify read size or read waits for end of file character. 88 | # This returns immediately even if 0 bytes are available. 89 | received_data = sys.stdin.buffer.read(64) 90 | if len(received_data) == 0: 91 | time.sleep(0.1) 92 | else: 93 | bytes_received += len(received_data) 94 | messages = decoder.on_data(received_data) 95 | for (header, message, raw_data) in messages: 96 | messages_received += 1 97 | pass_through_message = (options.blacklist and header.message_type not in message_types) or ( 98 | not options.blacklist and header.message_type in message_types) 99 | if pass_through_message: 100 | messages_forwarded += 1 101 | bytes_forwarded += len(raw_data) 102 | original_stdout.buffer.write(raw_data) 103 | 104 | if options.display: 105 | now = datetime.now() 106 | if (now - last_print_time).total_seconds() > 5.0: 107 | print('Status: [bytes_in=%d, msgs_in=%d, bytes_out=%d, msgs_out=%d, elapsed_time=%d sec]' % 108 | (bytes_received, messages_received, bytes_forwarded, 109 | messages_forwarded, (now - start_time).total_seconds())) 110 | last_print_time = now 111 | 112 | except KeyboardInterrupt: 113 | pass 114 | -------------------------------------------------------------------------------- /python/fusion_engine_client/messages/__init__.py: -------------------------------------------------------------------------------- 1 | from .core import * 2 | from .fault_control import * 3 | from . import ros 4 | from . import sta5635 5 | 6 | message_type_to_class = MessagePayload.message_type_to_class 7 | message_type_by_name = MessagePayload.message_type_by_name 8 | 9 | messages_with_p1_time = set([t for t, c in message_type_to_class.items() if hasattr(c(), 'p1_time')]) 10 | messages_with_gps_time = set([t for t, c in message_type_to_class.items() if hasattr(c(), 'gps_time')]) 11 | messages_with_system_time = set([t for t, c in message_type_to_class.items() if hasattr(c(), 'system_time_ns')]) 12 | -------------------------------------------------------------------------------- /python/fusion_engine_client/messages/core.py: -------------------------------------------------------------------------------- 1 | from .configuration import * 2 | from .control import * 3 | from .defs import * 4 | from .device import * 5 | from .gnss_corrections import * 6 | from .measurements import * 7 | from .solution import * 8 | -------------------------------------------------------------------------------- /python/fusion_engine_client/messages/gnss_corrections.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | from construct import (Struct, Float32l, Int8ul, Int16ul, Int64sl, Padding, Bytes, this) 4 | 5 | from ..utils.construct_utils import construct_message_to_string 6 | from .defs import * 7 | 8 | 9 | class LBandFrameMessage(MessagePayload): 10 | """! 11 | @brief L-band frame message. 12 | """ 13 | MESSAGE_TYPE = MessageType.LBAND_FRAME 14 | MESSAGE_VERSION = 0 15 | 16 | LBandFrameMessageConstruct = Struct( 17 | "system_time_ns" / Int64sl, 18 | "user_data_size_bytes" / Int16ul, 19 | "bit_error_count" / Int16ul, 20 | "signal_power_db" / Int8ul, 21 | Padding(3), 22 | "doppler_hz" / Float32l, 23 | "data_payload" / Bytes(this.user_data_size_bytes), 24 | ) 25 | 26 | def __init__(self): 27 | self.system_time_ns = 0 28 | self.bit_error_count = 0 29 | self.signal_power_db = 0 30 | self.doppler_hz = math.nan 31 | self.data_payload = bytes() 32 | 33 | def pack(self, buffer: Optional[bytes] = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): 34 | values = vars(self) 35 | values['user_data_size_bytes'] = len(self.data_payload) 36 | packed_data = self.LBandFrameMessageConstruct.build(values) 37 | return PackedDataToBuffer(packed_data, buffer, offset, return_buffer) 38 | 39 | def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: 40 | parsed = self.LBandFrameMessageConstruct.parse(buffer[offset:]) 41 | self.__dict__.update(parsed) 42 | del self.__dict__['_io'] 43 | del self.__dict__['user_data_size_bytes'] 44 | return parsed._io.tell() 45 | 46 | def __repr__(self): 47 | result = super().__repr__()[:-1] 48 | result += f', errors={self.bit_error_count}, power={self.signal_power_db}, doppler={self.doppler_hz}]' 49 | return result 50 | 51 | def __str__(self): 52 | string = f'L-band Frame @ %s\n' % system_time_to_str(self.system_time_ns) 53 | string += f' Bit Error Count: {self.bit_error_count}\n' 54 | string += f' Signal Power: {self.signal_power_db} dB\n' 55 | string += f' Doppler: {self.doppler_hz} Hz' 56 | return string 57 | 58 | def calcsize(self) -> int: 59 | return len(self.pack()) 60 | -------------------------------------------------------------------------------- /python/fusion_engine_client/messages/measurement_details.py: -------------------------------------------------------------------------------- 1 | import struct 2 | 3 | from construct import Adapter, Struct, Int8ul, Padding 4 | import numpy as np 5 | 6 | from .timestamp import Timestamp, TimestampConstruct 7 | from ..utils.construct_utils import AutoEnum, ClassAdapter 8 | from ..utils.enum_utils import IntEnum 9 | 10 | 11 | class SensorDataSource(IntEnum): 12 | """! 13 | @brief The source of received sensor measurements, if known. 14 | """ 15 | ## Data source not known. 16 | UNKNOWN = 0 17 | ## Sensor data captured internal to the device (embedded IMU, GNSS receiver, etc.). 18 | INTERNAL = 1 19 | ## Sensor data generated via hardware voltage signal (wheel tick, external event, etc.). 20 | HARDWARE_IO = 2 21 | ## Sensor data captured from a vehicle CAN bus. 22 | CAN = 3 23 | ## Sensor data provided over a serial connection. 24 | SERIAL = 4 25 | ## Sensor data provided over a network connection. 26 | NETWORK = 5 27 | 28 | 29 | class SystemTimeSource(IntEnum): 30 | """! 31 | @brief The source of a @ref point_one::fusion_engine::messages::Timestamp used to represent the time of 32 | applicability of an incoming sensor measurement. 33 | """ 34 | ## Timestamp not valid. 35 | INVALID = 0 36 | ## Message timestamped in P1 time. 37 | P1_TIME = 1 38 | ## Message timestamped in system time, generated when received by the device. 39 | TIMESTAMPED_ON_RECEPTION = 2 40 | ## Message timestamp was generated from a monotonic clock of an external system. 41 | SENDER_SYSTEM_TIME = 3 42 | ## Message timestamped in GPS time, referenced to 1980/1/6. 43 | GPS_TIME = 4 44 | 45 | 46 | class MeasurementDetails(object): 47 | """! 48 | @brief The time of applicability and additional information for an incoming sensor measurement. 49 | """ 50 | Construct = Struct( 51 | "measurement_time" / TimestampConstruct, 52 | "measurement_time_source" / AutoEnum(Int8ul, SystemTimeSource), 53 | "data_source" / AutoEnum(Int8ul, SensorDataSource), 54 | Padding(2), 55 | "p1_time" / TimestampConstruct, 56 | ) 57 | 58 | _STRUCT = struct.Struct(' (bytes, int): 67 | if buffer is None: 68 | buffer = bytearray(self.calcsize()) 69 | offset = 0 70 | 71 | initial_offset = offset 72 | 73 | offset += self.measurement_time.pack(buffer, offset, return_buffer=False) 74 | self._STRUCT.pack_into(buffer, offset, int(self.measurement_time_source), int(self.data_source)) 75 | offset += self._STRUCT.size 76 | offset += self.p1_time.pack(buffer, offset, return_buffer=False) 77 | 78 | if return_buffer: 79 | return buffer 80 | else: 81 | return offset - initial_offset 82 | 83 | def unpack(self, buffer: bytes, offset: int = 0) -> int: 84 | initial_offset = offset 85 | 86 | offset += self.measurement_time.unpack(buffer, offset) 87 | (measurement_time_source_int, data_source_int) = self._STRUCT.unpack_from(buffer, offset) 88 | offset += self._STRUCT.size 89 | offset += self.p1_time.unpack(buffer, offset) 90 | 91 | self.measurement_time_source = SystemTimeSource(measurement_time_source_int) 92 | self.data_source = SensorDataSource(data_source_int) 93 | 94 | return offset - initial_offset 95 | 96 | @classmethod 97 | def calcsize(cls) -> int: 98 | return 2 * Timestamp.calcsize() + cls._STRUCT.size 99 | 100 | def __str__(self): 101 | if self.measurement_time_source == SystemTimeSource.P1_TIME or \ 102 | self.measurement_time_source == SystemTimeSource.GPS_TIME: 103 | measurement_time_str = str(self.measurement_time) 104 | else: 105 | measurement_time_str = 'System: %.3f sec' % self.measurement_time 106 | string = f'Measurement time: {measurement_time_str} ' \ 107 | f'(source: {self.measurement_time_source.to_string()})\n' 108 | if self.measurement_time_source != SystemTimeSource.P1_TIME: 109 | string += f'P1 time: {str(self.p1_time)}\n' 110 | string += f'Data source: {str(self.data_source)}' 111 | return string 112 | 113 | @classmethod 114 | def to_numpy(cls, messages): 115 | time_source = np.array([int(m.measurement_time_source) for m in messages], dtype=int) 116 | data_source = np.array([int(m.data_source) for m in messages], dtype=int) 117 | measurement_time = np.array([float(m.measurement_time) for m in messages]) 118 | 119 | # If the p1_time field is not set _and_ the incoming measurement time source is explicitly set to P1 time (i.e., 120 | # the data provider is synchronized to P1 time), use the measurement_time value. Note that we always prefer the 121 | # p1_time value if it is present -- the value in measurement_time may be adjusted internally by the device, and 122 | # the adjusted result will be stored in p1_time (measurement_time will never be modified). 123 | p1_time = np.array([float(m.p1_time) for m in messages]) 124 | idx = np.logical_and(time_source == SystemTimeSource.P1_TIME, np.isnan(p1_time)) 125 | p1_time[idx] = measurement_time[idx] 126 | 127 | result = { 128 | 'measurement_time': measurement_time, 129 | 'measurement_time_source': time_source, 130 | 'data_source': data_source, 131 | 'p1_time': p1_time, 132 | } 133 | 134 | idx = time_source == SystemTimeSource.GPS_TIME 135 | if np.any(idx): 136 | gps_time = np.full_like(time_source, np.nan) 137 | gps_time[idx] = measurement_time[idx] 138 | result['gps_time'] = gps_time 139 | 140 | idx = time_source == SystemTimeSource.TIMESTAMPED_ON_RECEPTION 141 | if np.any(idx): 142 | system_time = np.full_like(time_source, np.nan) 143 | system_time[idx] = measurement_time[idx] 144 | result['system_time'] = system_time 145 | 146 | return result 147 | 148 | 149 | MeasurementDetailsConstruct = ClassAdapter(MeasurementDetails, MeasurementDetails.Construct) 150 | -------------------------------------------------------------------------------- /python/fusion_engine_client/messages/sta5635.py: -------------------------------------------------------------------------------- 1 | from construct import (GreedyBytes, Padding, Struct, Int8ul, Int32ul, Int64sl, Bytes) 2 | 3 | from ..utils.construct_utils import construct_message_to_string 4 | from .defs import * 5 | 6 | 7 | class STA5635Command(MessagePayload): 8 | """! 9 | @brief A command to be sent to an attached STA5635 RF front-end. 10 | """ 11 | MESSAGE_TYPE = MessageType.STA5635_COMMAND 12 | MESSAGE_VERSION = 0 13 | 14 | Construct = Struct( 15 | "command" / Int8ul, 16 | "address" / Int8ul, 17 | "data" / Bytes(2), 18 | ) 19 | 20 | def __init__(self): 21 | self.command = 0 22 | self.address = 0 23 | self.data = bytes() 24 | 25 | def pack(self, buffer: Optional[bytes] = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): 26 | values = vars(self) 27 | packed_data = self.Construct.build(values) 28 | return PackedDataToBuffer(packed_data, buffer, offset, return_buffer) 29 | 30 | def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: 31 | parsed = self.Construct.parse(buffer[offset:]) 32 | self.__dict__.update(parsed) 33 | del self.__dict__['_io'] 34 | return parsed._io.tell() 35 | 36 | def calcsize(self) -> int: 37 | return self.Construct.sizeof() 38 | 39 | def __repr__(self): 40 | result = super().__repr__()[:-1] 41 | result += f', command=0x{self.command:02X}, address=0x{self.address:02X}, data={self.data}]' 42 | return result 43 | 44 | def __str__(self): 45 | return construct_message_to_string(message=self, value_to_string={ 46 | 'command': lambda x: f'0x{x:02X}', 47 | 'address': lambda x: f'0x{x:02X}', 48 | }) 49 | 50 | 51 | class STA5635CommandResponse(MessagePayload): 52 | """! 53 | @brief Result from an STA5635 sent in response to an @ref STA5635Command. 54 | """ 55 | MESSAGE_TYPE = MessageType.STA5635_COMMAND_RESPONSE 56 | MESSAGE_VERSION = 0 57 | 58 | Construct = Struct( 59 | "system_time_ns" / Int64sl, 60 | "command_sequence_number" / Int32ul, 61 | "data" / Bytes(4), 62 | ) 63 | 64 | def __init__(self): 65 | self.system_time_ns = 0 66 | self.command_sequence_number = 0 67 | self.data = bytes() 68 | 69 | def pack(self, buffer: Optional[bytes] = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): 70 | values = vars(self) 71 | packed_data = self.Construct.build(values) 72 | return PackedDataToBuffer(packed_data, buffer, offset, return_buffer) 73 | 74 | def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: 75 | parsed = self.Construct.parse(buffer[offset:]) 76 | self.__dict__.update(parsed) 77 | del self.__dict__['_io'] 78 | return parsed._io.tell() 79 | 80 | def calcsize(self) -> int: 81 | return self.Construct.sizeof() 82 | 83 | def __repr__(self): 84 | result = super().__repr__()[:-1] 85 | result += f', seq={self.command_sequence_number}, data={self.data}]' 86 | return result 87 | 88 | 89 | class STA5635IQData(MessagePayload): 90 | """! 91 | @brief Wrapper for IQ Samples from a STA5635. 92 | """ 93 | MESSAGE_TYPE = MessageType.STA5635_IQ_DATA 94 | MESSAGE_VERSION = 0 95 | 96 | Construct = Struct( 97 | Padding(4), 98 | # NOTE: Since this message does no capture the expected data size, the Construct relies on the size of the 99 | # Python buffer passed to `unpack`` to infer the size of the data. This is the behavior of @ref GreedyBytes. 100 | "data" / GreedyBytes 101 | ) 102 | 103 | def __init__(self): 104 | self.data = bytes() 105 | 106 | def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): 107 | ret = MessagePayload.pack(self, buffer, offset, return_buffer) 108 | return ret 109 | 110 | def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: 111 | ret = MessagePayload.unpack(self, buffer, offset, message_version) 112 | return ret 113 | 114 | def __repr__(self): 115 | result = super().__repr__()[:-1] 116 | result += f', data_len={len(self.data)}]' 117 | return result 118 | 119 | def __str__(self): 120 | return construct_message_to_string(message=self, construct=self.Construct, 121 | value_to_string={ 122 | 'data': lambda x: f'{len(x)} B payload', 123 | }, 124 | title=f'IQ Samples') 125 | 126 | def calcsize(self) -> int: 127 | return len(self.pack()) 128 | -------------------------------------------------------------------------------- /python/fusion_engine_client/parsers/__init__.py: -------------------------------------------------------------------------------- 1 | from .encoder import FusionEngineEncoder 2 | from .decoder import FusionEngineDecoder 3 | from .file_index import FileIndex 4 | from .mixed_log_reader import MixedLogReader 5 | -------------------------------------------------------------------------------- /python/fusion_engine_client/parsers/encoder.py: -------------------------------------------------------------------------------- 1 | from ..messages import MessageHeader, MessagePayload 2 | 3 | 4 | class FusionEngineEncoder: 5 | """! 6 | @brief Helper class for serializing FusionEngine messages. 7 | 8 | This class takes instances of @ref MessagePayload and returns the serialized message content (header + payload) as a 9 | byte array (`bytes`). It attaches sequence numbers and CRCs automatically to the outbound byte stream. 10 | """ 11 | 12 | def __init__(self): 13 | """! 14 | @brief Construct a new encoder instance. 15 | """ 16 | self.sequence_number = 0 17 | 18 | def encode_message(self, message: MessagePayload, source_identifier: int = 0) -> (bytes): 19 | """! 20 | @brief Serialize a message with valid header and payload. 21 | 22 | Construct a header for the specified payload, automatically setting the message type and version from the 23 | payload object, and populating the message sequence number, source identifier, and CRC. Then serialize both the 24 | header and payload into a `bytes` object. 25 | 26 | @param message The MessagePayload to serialize. 27 | @param source_identifier A numeric source identifier to associate with this message (optional). 28 | 29 | @return A `bytes` object containing the serialized message. 30 | """ 31 | header = MessageHeader(message.get_type()) 32 | header.message_version = message.get_version() 33 | header.sequence_number = self.sequence_number 34 | header.source_identifier = source_identifier 35 | self.sequence_number += 1 36 | 37 | message_data = message.pack() 38 | 39 | return header.pack(payload=message_data) 40 | -------------------------------------------------------------------------------- /python/fusion_engine_client/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PointOneNav/fusion-engine-client/f512983ab0da10fa88477e45e44666920bdeef81/python/fusion_engine_client/utils/__init__.py -------------------------------------------------------------------------------- /python/fusion_engine_client/utils/bin_utils.py: -------------------------------------------------------------------------------- 1 | import textwrap 2 | 3 | 4 | def bytes_to_hex(buffer, bytes_per_row=32, bytes_per_col=1): 5 | if bytes_per_col == 1: 6 | if bytes_per_row > 0: 7 | return '\n'.join(textwrap.wrap(' '.join(['%02X' % b for b in buffer]), (3 * bytes_per_row - 1))) 8 | else: 9 | return ' '.join(['%02X' % b for b in buffer]) 10 | else: 11 | byte_string = '' 12 | for i, b in enumerate(buffer): 13 | if i > 0: 14 | if bytes_per_row > 0 and (i % bytes_per_row) == 0: 15 | byte_string += '\n' 16 | elif (i % bytes_per_col) == 0: 17 | byte_string += ' ' 18 | 19 | byte_string += '%02x' % b 20 | return byte_string 21 | -------------------------------------------------------------------------------- /python/fusion_engine_client/utils/numpy_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def find_first(arr: np.ndarray): 4 | """! 5 | @brief Get the first index of input array which is `True`. 6 | 7 | @param arr A boolean array. 8 | 9 | @return First index of input array which is `True`. If all elements are `False`, returns -1. 10 | """ 11 | if arr.dtype != bool: 12 | raise ValueError('Input array is not a boolean array.') 13 | elif len(arr) == 0: 14 | return -1 15 | else: 16 | idx = np.nanargmax(arr) 17 | if idx == 0 and not arr[0]: 18 | return -1 19 | else: 20 | return idx 21 | -------------------------------------------------------------------------------- /python/fusion_engine_client/utils/transport_utils.py: -------------------------------------------------------------------------------- 1 | import re 2 | import socket 3 | from typing import Callable, Union 4 | 5 | try: 6 | # pySerial is optional. 7 | import serial 8 | serial_supported = True 9 | 10 | # The Serial class has read() and write() functions. For convenience, we add recv() and send() aliases, consistent 11 | # with the Python socket class. 12 | def __recv(self, size, flags=None): 13 | data = self.read(size) 14 | if len(data) == 0 and size > 0: 15 | raise socket.timeout('Serial read timed out.') 16 | else: 17 | return data 18 | def __send(self, data, flags=None): 19 | self.write(data) 20 | serial.Serial.recv = __recv 21 | serial.Serial.send = __send 22 | except ImportError: 23 | serial_supported = False 24 | # Dummy stand-in if pySerial is not installed. 25 | class serial: 26 | class Serial: pass 27 | class SerialException: pass 28 | 29 | TRANSPORT_HELP_STRING = """\ 30 | The method used to communicate with the target device: 31 | - tcp://HOSTNAME[:PORT] - Connect to the specified hostname (or IP address) and 32 | port over TCP (e.g., tty://192.168.0.3:30202); defaults to port 30200 33 | - udp://:PORT - Listen for incoming data on the specified UDP port (e.g., 34 | udp://:12345) 35 | Note: When using UDP, you must configure the device to send data to your 36 | machine. 37 | - unix://FILENAME - Connect to the specified UNIX domain socket file 38 | - [(serial|tty)://]DEVICE:BAUD - Connect to a serial device with the specified 39 | baud rate (e.g., tty:///dev/ttyUSB0:460800 or /dev/ttyUSB0:460800) 40 | """ 41 | 42 | 43 | def create_transport(descriptor: str, timeout_sec: float = None, print_func: Callable = None) -> \ 44 | Union[socket.socket, serial.Serial]: 45 | m = re.match(r'^tcp://([a-zA-Z0-9-_.]+)?(?::([0-9]+))?$', descriptor) 46 | if m: 47 | hostname = m.group(1) 48 | ip_address = socket.gethostbyname(hostname) 49 | port = 30200 if m.group(2) is None else int(m.group(2)) 50 | if print_func is not None: 51 | print_func(f'Connecting to tcp://{ip_address}:{port}.') 52 | 53 | transport = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 54 | if timeout_sec is not None: 55 | transport.settimeout(timeout_sec) 56 | try: 57 | transport.connect((ip_address, port)) 58 | except socket.timeout: 59 | raise socket.timeout(f'Timed out connecting to tcp://{ip_address}:{port}.') 60 | return transport 61 | 62 | m = re.match(r'^udp://:([0-9]+)$', descriptor) 63 | if m: 64 | port = int(m.group(1)) 65 | if print_func is not None: 66 | print_func(f'Connecting to udp://:{port}.') 67 | 68 | transport = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 69 | transport.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 70 | if timeout_sec is not None: 71 | transport.settimeout(timeout_sec) 72 | transport.bind(('', port)) 73 | return transport 74 | 75 | m = re.match(r'^unix://([a-zA-Z0-9-_./]+)$', descriptor) 76 | if m: 77 | path = m.group(1) 78 | if print_func is not None: 79 | print_func(f'Connecting to unix://{path}.') 80 | 81 | transport = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) 82 | if timeout_sec is not None: 83 | transport.settimeout(timeout_sec) 84 | transport.connect(path) 85 | return transport 86 | 87 | m = re.match(r'^(?:(?:serial|tty)://)?([^:]+)(:([0-9]+))?$', descriptor) 88 | if m: 89 | if serial_supported: 90 | path = m.group(1) 91 | if m.group(2) is None: 92 | raise ValueError('Serial baud rate not specified.') 93 | else: 94 | baud_rate = int(m.group(2)) 95 | if print_func is not None: 96 | print_func(f'Connecting to tty://{path}:{baud_rate}.') 97 | 98 | transport = serial.Serial(port=path, baudrate=baud_rate, timeout=timeout_sec) 99 | return transport 100 | else: 101 | raise RuntimeError( 102 | "This application requires pyserial. Please install (pip install pyserial) and run again.") 103 | 104 | raise ValueError('Unsupported transport descriptor.') 105 | -------------------------------------------------------------------------------- /python/requirements.txt: -------------------------------------------------------------------------------- 1 | aenum>=3.1.1 2 | # Note: Using the Point One fork of gpstime until the patch to download the leap 3 | # seconds file from alternate servers is merged: 4 | # https://gitlab.com/jrollins/gpstime/-/merge_requests/2 5 | p1-gpstime>=0.6.3.dev1 6 | numpy>=1.16.0 7 | construct>=2.10.0 8 | 9 | # Required for analysis and example applications only. Not used by the `messages` package. 10 | argparse-formatter>=1.4 11 | colorama>=0.4.4 12 | palettable>=3.3.0 13 | plotly>=4.0.0 14 | pymap3d>=2.4.3 15 | scipy>=1.5.0 16 | 17 | # Required for development only. 18 | packaging>=21.0.0 19 | -------------------------------------------------------------------------------- /python/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | 4 | from setuptools import setup, find_packages 5 | 6 | 7 | def find_version(*file_paths): 8 | with open(os.path.join(*file_paths), 'rt') as f: 9 | version_file = f.read() 10 | version_match = re.search(r"^__version__ = ['\"]([^'\"]*)['\"]", version_file, re.M) 11 | if version_match: 12 | return version_match.group(1) 13 | raise RuntimeError("Unable to find version string.") 14 | 15 | 16 | version = find_version('fusion_engine_client', '__init__.py') 17 | 18 | message_requirements = set([ 19 | 'aenum>=3.1.1', 20 | # Note: Using the Point One fork of gpstime until the patch to download the leap 21 | # seconds file from alternate servers is merged: 22 | # https://gitlab.com/jrollins/gpstime/-/merge_requests/2 23 | 'p1-gpstime>=0.6.3.dev1', 24 | 'numpy>=1.16.0', 25 | 'construct>=2.10.0', 26 | ]) 27 | 28 | tools_requirements = set([ 29 | 'argparse-formatter>=1.4', 30 | 'scipy>=1.5.0', 31 | ]) 32 | 33 | display_requirements = set([ 34 | 'colorama>=0.4.4', 35 | 'palettable>=3.3.0', 36 | 'plotly>=4.0.0', 37 | 'pymap3d>=2.4.3', 38 | ]) | tools_requirements 39 | 40 | dev_requirements = set([ 41 | 'packaging>=21.0.0', 42 | ]) 43 | 44 | all_requirements = message_requirements | tools_requirements | display_requirements | dev_requirements 45 | 46 | setup( 47 | name='fusion-engine-client', 48 | version=version, 49 | description='Point One FusionEngine Library', 50 | long_description="""\ 51 | Point One FusionEngine protocol support for real-time interaction and control, plus post-processing data analysis tools. 52 | 53 | See https://github.com/PointOneNav/fusion-engine-client for full details. See https://pointonenav.com/docs/ 54 | for the latest FusionEngine message specification. 55 | """, 56 | long_description_content_type='text/markdown', 57 | author='Point One Navigation', 58 | author_email='support@pointonenav.com', 59 | license='MIT', 60 | classifiers=[ 61 | 'Development Status :: 5 - Production/Stable', 62 | 'Intended Audience :: Developers', 63 | 'Intended Audience :: End Users/Desktop', 64 | 'Intended Audience :: Science/Research', 65 | 'License :: OSI Approved :: MIT License', 66 | 'Operating System :: MacOS :: MacOS X', 67 | 'Operating System :: Microsoft :: Windows', 68 | 'Operating System :: POSIX', 69 | 'Programming Language :: Python', 70 | 'Programming Language :: Python :: 3', 71 | 'Programming Language :: Python :: 3.6', 72 | 'Programming Language :: Python :: 3.7', 73 | 'Programming Language :: Python :: 3.8', 74 | 'Programming Language :: Python :: 3.9', 75 | 'Programming Language :: Python :: 3.10', 76 | 'Programming Language :: Python :: 3.11', 77 | 'Topic :: Software Development :: Libraries', 78 | 'Topic :: Software Development :: Libraries :: Python Modules', 79 | ], 80 | url='https://github.com/PointOneNav/fusion-engine-client', 81 | download_url=f'https://github.com/PointOneNav/fusion-engine-client/archive/refs/tags/v{version}.tar.gz', 82 | packages=find_packages(where='.'), 83 | entry_points={ 84 | 'console_scripts': [ 85 | 'p1_capture = fusion_engine_client.applications.p1_capture:main', 86 | 'p1_display = fusion_engine_client.applications.p1_display:main', 87 | 'p1_extract = fusion_engine_client.applications.p1_extract:main', 88 | 'p1_lband_extract = fusion_engine_client.applications.p1_lband_extract:main', 89 | 'p1_print = fusion_engine_client.applications.p1_print:main', 90 | ] 91 | }, 92 | python_requires='>=3.6', 93 | setup_requires=[ 94 | 'wheel>=0.36.2', 95 | ], 96 | install_requires=list(all_requirements), 97 | extras_require={ 98 | # Kept for backwards compatibility. 99 | 'all': [], 100 | 'dev': [], 101 | 'display': [], 102 | 'tools': [], 103 | }, 104 | ) 105 | -------------------------------------------------------------------------------- /python/tests/test_decoder.py: -------------------------------------------------------------------------------- 1 | from fusion_engine_client.messages import PoseMessage, PoseAuxMessage 2 | from fusion_engine_client.messages.defs import MessageHeader, MessageType 3 | from fusion_engine_client.parsers import FusionEngineDecoder 4 | from fusion_engine_client.utils import trace as logging 5 | 6 | logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') 7 | logging.getLogger('point_one').setLevel(logging.DEBUG) 8 | 9 | 10 | P1_POSE_MESSAGE1 = b".1\x00\x00\xb3\x9a\xf0\x7f\x02\x00\x10'\x00\x00\x00\x00\x8c\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x80\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00@\x00\x00\x00\x00\x00\x00\x08@\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 11 | P1_POSE_MESSAGE2 = b".1\x00\x00\x02O\xd6\xef\x02\x00\x10'\x01\x00\x00\x00\x8c\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x80\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00@\x00\x00\x00\x00\x00\x00\x08@\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 12 | P1_POSE_AUX_MESSAGE3 = b".1\x00\x00z\x90\x98t\x02\x00\x13'\x02\x00\x00\x00\xa0\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 13 | 14 | 15 | def test_good_message_at_once(): 16 | decoder = FusionEngineDecoder() 17 | ret = decoder.on_data(P1_POSE_MESSAGE1) 18 | assert len(ret) == 1 19 | assert ret[0][0].message_type == PoseMessage.MESSAGE_TYPE 20 | 21 | 22 | def test_good_message_byte_by_byte(): 23 | decoder = FusionEngineDecoder() 24 | for byte in P1_POSE_MESSAGE1[:-1]: 25 | ret = decoder.on_data(byte) 26 | assert len(ret) == 0 27 | ret = decoder.on_data(P1_POSE_MESSAGE1[-1]) 28 | assert len(ret) == 1 29 | assert ret[0][0].message_type == PoseMessage.MESSAGE_TYPE 30 | 31 | 32 | def test_multiple_good(): 33 | test_bytes = P1_POSE_MESSAGE1 + P1_POSE_MESSAGE2 34 | decoder = FusionEngineDecoder() 35 | ret = decoder.on_data(test_bytes) 36 | assert len(ret) == 2 37 | assert ret[0][0].message_type == PoseMessage.MESSAGE_TYPE 38 | assert ret[0][0].sequence_number == 0 39 | assert ret[1][0].message_type == PoseMessage.MESSAGE_TYPE 40 | assert ret[1][0].sequence_number == 1 41 | 42 | 43 | def test_sync(): 44 | # Bad preamble 45 | test_bytes = bytearray() + P1_POSE_MESSAGE1 + P1_POSE_MESSAGE2 + P1_POSE_AUX_MESSAGE3 46 | # Bad sync0 for first message 47 | test_bytes[0] = 1 48 | # Bad sync1 for second message 49 | test_bytes[len(P1_POSE_MESSAGE1) + 1] = 1 50 | decoder = FusionEngineDecoder() 51 | ret = decoder.on_data(test_bytes) 52 | assert len(ret) == 1 53 | assert ret[0][0].sequence_number == 2 54 | 55 | # CRC failure 56 | test_bytes = bytearray() + P1_POSE_MESSAGE1 + P1_POSE_MESSAGE2 57 | test_bytes[25] = 1 58 | decoder = FusionEngineDecoder() 59 | ret = decoder.on_data(test_bytes) 60 | assert len(ret) == 1 61 | assert ret[0][0].sequence_number == 1 62 | 63 | 64 | def test_resync(): 65 | test_bytes = P1_POSE_MESSAGE1 + b'.' + P1_POSE_MESSAGE2 + b'.1' + P1_POSE_AUX_MESSAGE3 66 | decoder = FusionEngineDecoder(200) 67 | ret = decoder.on_data(test_bytes) 68 | assert len(ret) == 3 69 | for i in range(3): 70 | assert ret[i][0].sequence_number == i 71 | 72 | 73 | def test_unknown_message(): 74 | header = MessageHeader() 75 | payload = b"1234" 76 | header.message_type = MessageType.RESERVED 77 | test_bytes = header.pack(payload=payload) 78 | decoder = FusionEngineDecoder() 79 | ret = decoder.on_data(test_bytes) 80 | assert len(ret) == 1 81 | assert ret[0][1] == payload 82 | 83 | 84 | def test_callbacks(): 85 | counters = [0, 0, 0, 0] 86 | 87 | def func_helper(header, payload, idx): 88 | counters[idx] += 1 89 | 90 | def func1(header, payload): return func_helper(header, payload, 0) 91 | def func2(header, payload): return func_helper(header, payload, 1) 92 | def func3(header, payload): return func_helper(header, payload, 2) 93 | def func4(header, payload): return func_helper(header, payload, 3) 94 | 95 | test_bytes = P1_POSE_MESSAGE1 + P1_POSE_MESSAGE2 + P1_POSE_AUX_MESSAGE3 96 | decoder = FusionEngineDecoder() 97 | decoder.add_callback(PoseMessage.MESSAGE_TYPE, func1) 98 | decoder.add_callback(PoseAuxMessage.MESSAGE_TYPE, func2) 99 | decoder.add_callback(None, func3) 100 | decoder.add_callback(PoseMessage.MESSAGE_TYPE, func4) 101 | decoder.add_callback(None, func4) 102 | 103 | decoder.on_data(test_bytes) 104 | 105 | assert counters[0] == 2 106 | assert counters[1] == 1 107 | assert counters[2] == 3 108 | assert counters[3] == 5 109 | 110 | 111 | def test_seq_backwards_warning(caplog): 112 | test_bytes = P1_POSE_MESSAGE1 + P1_POSE_MESSAGE2 + P1_POSE_MESSAGE1 113 | decoder = FusionEngineDecoder(warn_on_error=True) 114 | decoder.on_data(test_bytes) 115 | assert "Sequence number went backwards on POSE (10000) message." in caplog.text 116 | 117 | 118 | def test_seq_skip_warning(caplog): 119 | test_bytes = P1_POSE_MESSAGE1 + P1_POSE_AUX_MESSAGE3 120 | decoder = FusionEngineDecoder(warn_on_gap=True) 121 | decoder.on_data(test_bytes) 122 | assert "Gap detected in FusionEngine message sequence numbers. [expected=1, received=2]." in caplog.text 123 | -------------------------------------------------------------------------------- /python/tests/test_encoder.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from fusion_engine_client.messages import PoseMessage, PoseAuxMessage 4 | from fusion_engine_client.parsers import FusionEngineEncoder 5 | from fusion_engine_client.utils import trace as logging 6 | 7 | logging.basicConfig(level=logging.INFO, 8 | format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') 9 | logging.getLogger('point_one').setLevel(logging.DEBUG) 10 | 11 | 12 | P1_POSE_MESSAGE1 = b".1\x00\x00\xb1&\xc8\xfe\x02\x02\x10'\x00\x00\x00\x00\x8c\x00\x00\x00\x00\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x80\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00@\x00\x00\x00\x00\x00\x00\x08@\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 13 | P1_POSE_MESSAGE2 = b".1\x00\x00\x00\xf3\xeen\x02\x02\x10'\x01\x00\x00\x00\x8c\x00\x00\x00\x00\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x80\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00@\x00\x00\x00\x00\x00\x00\x08@\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 14 | P1_POSE_AUX_MESSAGE3 = b".1\x00\x00\xac\xa4\x08\x94\x02\x00\x13'\x02\x00\x00\x00\xa0\x00\x00\x00\x00\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\x00\x00\x00\x00\xf8\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f\x00\x00\xc0\x7f" 15 | 16 | 17 | def test_pose_encode(): 18 | encoder = FusionEngineEncoder() 19 | pose = PoseMessage() 20 | pose.velocity_body_mps = np.array([1.0, 2.0, 3.0]) 21 | pose_aux = PoseAuxMessage() 22 | 23 | data = encoder.encode_message(pose) 24 | assert data == P1_POSE_MESSAGE1 25 | data = encoder.encode_message(pose) 26 | assert data == P1_POSE_MESSAGE2 27 | data = encoder.encode_message(pose_aux) 28 | assert data == P1_POSE_AUX_MESSAGE3 29 | -------------------------------------------------------------------------------- /python/tests/test_enum_utils.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | 3 | from fusion_engine_client.utils.enum_utils import IntEnum, enum_bitmask 4 | 5 | 6 | @pytest.fixture 7 | def Enum(): 8 | class TestEnum(IntEnum): 9 | A = 1 10 | B = 2 11 | return TestEnum 12 | 13 | 14 | def test_str(Enum): 15 | assert str(Enum.A) == 'A' 16 | 17 | 18 | def test_repr(Enum): 19 | assert repr(Enum.A) == '' 20 | 21 | 22 | def test_to_string(Enum): 23 | assert Enum.A.to_string(include_value=True) == 'A (1)' 24 | assert Enum.A.to_string(include_value=False) == 'A' 25 | 26 | 27 | def test_getitem(Enum): 28 | assert Enum['A'] == Enum.A 29 | # Not natively supported by Enum - we added these for convenience. 30 | assert Enum['a'] == Enum.A 31 | assert Enum[Enum.A] == Enum.A 32 | assert Enum[1] == Enum.A 33 | 34 | 35 | def test_call(Enum): 36 | assert Enum(Enum.A) == Enum.A 37 | assert Enum(1) == Enum.A 38 | # Not natively supported by Enum - we added these for convenience. 39 | assert Enum('A') == Enum.A 40 | assert Enum('a') == Enum.A 41 | 42 | 43 | def test_iter(Enum): 44 | assert list(Enum) == [Enum.A, Enum.B] 45 | assert len(Enum) == 2 46 | 47 | 48 | def test_unrecognized(Enum): 49 | with pytest.raises(ValueError): 50 | Enum(8, raise_on_unrecognized=True) 51 | Enum(9, raise_on_unrecognized=True) 52 | 53 | value = Enum(10, raise_on_unrecognized=False) 54 | assert int(value) == 10 55 | assert str(value) == '(Unrecognized)' 56 | assert repr(value) == '' 57 | assert value == 10 58 | 59 | with pytest.raises(ValueError): 60 | Enum(10, raise_on_unrecognized=True) 61 | 62 | # Unrecognized values are not included in the iter() or len() output. 63 | assert list(Enum) == [Enum.A, Enum.B] 64 | assert len(Enum) == 2 65 | 66 | with pytest.raises(KeyError): 67 | assert Enum['Q'] 68 | assert Enum('Q', raise_on_unrecognized=True) 69 | 70 | value = Enum('Q', raise_on_unrecognized=False) 71 | assert int(value) == -1 72 | assert Enum['Q'] == -1 73 | 74 | value = Enum('W', raise_on_unrecognized=False) 75 | assert int(value) == -2 76 | 77 | 78 | def test_bitmask_decorator(Enum): 79 | @enum_bitmask(Enum) 80 | class EnumMask: pass 81 | expected_values = [Enum.A, Enum.B] 82 | expected_mask = (1 << int(Enum.A)) | (1 << int(Enum.B)) 83 | assert EnumMask.to_bitmask(expected_values) == expected_mask 84 | assert EnumMask.to_values(expected_mask) == expected_values 85 | assert EnumMask.to_string(expected_mask) == 'A, B' 86 | 87 | 88 | def test_bitmask_decorator_extended(Enum): 89 | @enum_bitmask(Enum) 90 | class EnumMask: 91 | ALL = 0xFF 92 | 93 | expected_values = [Enum.A, Enum.B] 94 | expected_mask = (1 << int(Enum.A)) | (1 << int(Enum.B)) 95 | assert EnumMask.to_bitmask(expected_values) == expected_mask 96 | assert EnumMask.to_values(expected_mask) == expected_values 97 | assert EnumMask.to_string(expected_mask) == 'A, B' 98 | 99 | assert EnumMask.to_values(0xFF) == expected_values 100 | assert EnumMask[0xFF] == EnumMask.ALL 101 | 102 | 103 | def test_bitmask_decorator_offset(Enum): 104 | @enum_bitmask(Enum, offset=1) 105 | class EnumMask: pass 106 | assert EnumMask.B == (1 << (Enum.B - 1)) 107 | 108 | expected_values = [Enum.A, Enum.B] 109 | expected_mask = (1 << (int(Enum.A) - 1)) | (1 << (int(Enum.B) - 1)) 110 | assert EnumMask.to_bitmask(expected_values) == expected_mask 111 | assert EnumMask.to_values(expected_mask) == expected_values 112 | assert EnumMask.to_string(expected_mask) == 'A, B' 113 | 114 | 115 | def test_bitmask_decorator_predicate(Enum): 116 | @enum_bitmask(Enum, predicate=lambda x: str(x) == 'A') 117 | class EnumMask: pass 118 | assert hasattr(EnumMask, 'A') 119 | assert not hasattr(EnumMask, 'B') 120 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/common/logging.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief 3 | ******************************************************************************/ 4 | 5 | #include "point_one/fusion_engine/common/logging.h" 6 | 7 | using namespace point_one::fusion_engine::common; 8 | 9 | NullStream NullMessage::stream_; 10 | NullMessage NullMessage::instance_; 11 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/common/logging.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief API wrapper for optional compilation of logging support. 3 | * @file 4 | * 5 | * To enable logging support, include 6 | * [Google glog](https://github.com/google/glog) in your project, and define 7 | * the following macro: 8 | * ```cpp 9 | * #define P1_HAVE_GLOG 1 10 | * ``` 11 | * 12 | * This is typically done during compilation by specifying the 13 | * `-DP1_HAVE_GLOG=1` command-line argument to the compiler. 14 | ******************************************************************************/ 15 | 16 | #pragma once 17 | 18 | #include "point_one/fusion_engine/common/portability.h" 19 | 20 | // Use Google Logging Library (glog). 21 | #if P1_HAVE_GLOG && !P1_NO_LOGGING 22 | # include 23 | 24 | // For internal use only. 25 | #elif P1_HAVE_PORTABLE_LOGGING && !P1_NO_LOGGING 26 | # include "point_one/common/portability/logging.h" 27 | 28 | // Disable logging support at compile time. 29 | #else // Logging disabled 30 | 31 | # include // For abort(). 32 | 33 | # if !P1_NO_LOGGING 34 | # undef P1_NO_LOGGING 35 | # define P1_NO_LOGGING 1 36 | # endif // !P1_NO_LOGGING 37 | 38 | namespace point_one { 39 | namespace fusion_engine { 40 | namespace common { 41 | 42 | class NullStream : public p1_ostream { 43 | public: 44 | # if P1_HAVE_STD_OSTREAM 45 | NullStream() : p1_ostream(nullptr) {} 46 | # endif 47 | }; 48 | 49 | template 50 | inline NullStream& operator<<(NullStream& stream, const T&) { 51 | return stream; 52 | } 53 | 54 | class NullMessage { 55 | public: 56 | static NullStream stream_; 57 | static NullMessage instance_; 58 | 59 | NullStream& stream() { return stream_; } 60 | }; 61 | 62 | } // namespace common 63 | } // namespace fusion_engine 64 | } // namespace point_one 65 | 66 | # define P1_NULL_STREAM point_one::fusion_engine::common::NullMessage::stream_ 67 | # define P1_NULL_MESSAGE \ 68 | point_one::fusion_engine::common::NullMessage::instance_ 69 | 70 | # define COMPACT_GOOGLE_LOG_INFO P1_NULL_MESSAGE 71 | # define COMPACT_GOOGLE_LOG_WARNING P1_NULL_MESSAGE 72 | # define COMPACT_GOOGLE_LOG_ERROR P1_NULL_MESSAGE 73 | # define COMPACT_GOOGLE_LOG_FATAL \ 74 | abort(); \ 75 | P1_NULL_MESSAGE 76 | 77 | # define LOG(severity) COMPACT_GOOGLE_LOG_##severity.stream() 78 | # define LOG_IF(severity, condition) COMPACT_GOOGLE_LOG_##severity.stream() 79 | # define LOG_EVERY_N(verboselevel, n) COMPACT_GOOGLE_LOG_##severity.stream() 80 | # define LOG_IF_EVERY_N(verboselevel, condition, n) \ 81 | COMPACT_GOOGLE_LOG_##severity.stream() 82 | 83 | # define VLOG_IS_ON(verboselevel) false 84 | # define COMPACT_GOOGLE_VLOG(verboselevel) P1_NULL_MESSAGE 85 | 86 | # define VLOG_IF(verboselevel, condition) \ 87 | COMPACT_GOOGLE_VLOG(verboselevel).stream() 88 | # define VLOG(verboselevel) COMPACT_GOOGLE_VLOG(verboselevel).stream() 89 | # define VLOG_EVERY_N(verboselevel, n) \ 90 | COMPACT_GOOGLE_VLOG(verboselevel).stream() 91 | # define VLOG_IF_EVERY_N(verboselevel, condition, n) \ 92 | COMPACT_GOOGLE_VLOG(verboselevel).stream() 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/common/portability.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Library portability helper definitions. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | // References: 11 | // - https://gcc.gnu.org/wiki/Visibility. 12 | // - https://github.com/protocolbuffers/protobuf/blob/master/src/google/protobuf/port_def.inc 13 | #if defined(_WIN32) || defined(_MSC_VER) || defined(__CYGWIN__) 14 | # ifdef BUILDING_DLL 15 | # ifdef __GNUC__ 16 | # define P1_EXPORT __attribute__((dllexport)) 17 | # else 18 | # define P1_EXPORT __declspec(dllexport) 19 | # endif 20 | # else 21 | # ifdef __GNUC__ 22 | # define P1_EXPORT __attribute__((dllimport)) 23 | # else 24 | # define P1_EXPORT __declspec(dllimport) 25 | # endif 26 | # endif 27 | # define P1_HIDDEN 28 | #else 29 | # if __GNUC__ >= 4 30 | # define P1_EXPORT __attribute__((visibility("default"))) 31 | # define P1_HIDDEN __attribute__((visibility("hidden"))) 32 | # else 33 | # define P1_EXPORT 34 | # define P1_HIDDEN 35 | # endif 36 | #endif 37 | 38 | // Support ARM CC quirks 39 | #ifdef __CC_ARM 40 | // The cstdint included with Keil ARM CC does not appear to include stdint.h 41 | // or to define most of the stdint types (uint8_t, etc.), even though it should. 42 | # include 43 | // Fixes bug in macro used Keil's math.h header where they define NAN (see 44 | // http://www.keil.com/forum/60227/). 45 | # define __ESCAPE__(__x) (__x) 46 | # define P1_HAVE_STD_FUNCTION 0 47 | # define P1_HAVE_STD_OSTREAM 0 48 | # define P1_HAVE_STD_SMART_PTR 0 49 | # define P1_NO_LOGGING 1 50 | #endif 51 | 52 | // Different compilers support different ways of specifying default struct 53 | // alignment. Since there's no universal method, a macro is used instead. 54 | #ifdef __CC_ARM 55 | # define P1_ALIGNAS(N) __attribute__((aligned(N))) 56 | #else 57 | # define P1_ALIGNAS(N) alignas(N) 58 | #endif 59 | 60 | // ssize_t is a POSIX extension and is not supported on Windows/ARM CC. 61 | #if defined(_WIN32) || defined(__CC_ARM) 62 | typedef int32_t p1_ssize_t; 63 | #elif defined(_MSC_VER) 64 | typedef int64_t p1_ssize_t; 65 | #else 66 | # include // For ssize_t 67 | typedef ssize_t p1_ssize_t; 68 | #endif 69 | 70 | #ifndef P1_HAVE_STD_OSTREAM 71 | # define P1_HAVE_STD_OSTREAM 1 72 | #endif 73 | #if P1_HAVE_STD_OSTREAM 74 | # include 75 | using p1_ostream = std::ostream; 76 | #else 77 | class p1_ostream { 78 | public: 79 | p1_ostream() = default; 80 | }; 81 | template 82 | inline p1_ostream& operator<<(p1_ostream& stream, const T&) { 83 | return stream; 84 | } 85 | #endif 86 | 87 | #ifndef P1_HAVE_STD_FUNCTION 88 | # define P1_HAVE_STD_FUNCTION 1 89 | #endif 90 | 91 | #ifndef P1_HAVE_STD_SMART_PTR 92 | # define P1_HAVE_STD_SMART_PTR 1 93 | #endif 94 | 95 | // Support for multi-statement constexpr functions was not added until C++14. 96 | // When compiling with C++11, we'll simply use inline instead. 97 | #ifndef P1_HAVE_MULTILINE_CONSTEXPR_FUNC 98 | # define P1_HAVE_MULTILINE_CONSTEXPR_FUNC (__cplusplus >= 201402L) 99 | #endif 100 | 101 | #ifndef P1_CONSTEXPR_FUNC 102 | # if P1_HAVE_MULTILINE_CONSTEXPR_FUNC 103 | # define P1_CONSTEXPR_FUNC constexpr 104 | # else 105 | # define P1_CONSTEXPR_FUNC inline 106 | # endif 107 | #endif 108 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/common/version.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Library version macros. 3 | ******************************************************************************/ 4 | 5 | #pragma once 6 | 7 | #define P1_FUSION_ENGINE_VERSION_STRING "1.24.1" 8 | #define P1_FUSION_ENGINE_VERSION_MAJOR 1 9 | #define P1_FUSION_ENGINE_VERSION_MINOR 24 10 | #define P1_FUSION_ENGINE_VERSION_PATCH 1 11 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/core.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Core Point One FusionEngine message definitions. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include "point_one/fusion_engine/common/version.h" 9 | #include "point_one/fusion_engine/messages/configuration.h" 10 | #include "point_one/fusion_engine/messages/control.h" 11 | #include "point_one/fusion_engine/messages/defs.h" 12 | #include "point_one/fusion_engine/messages/device.h" 13 | #include "point_one/fusion_engine/messages/measurements.h" 14 | #include "point_one/fusion_engine/messages/solution.h" 15 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/crc.cc: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Utility functions. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #include // For offsetof() 7 | 8 | #include "point_one/fusion_engine/messages/crc.h" 9 | 10 | namespace { 11 | 12 | /******************************************************************************/ 13 | const uint32_t* GetCRCTable() { 14 | // Note: This is the CRC-32 polynomial. 15 | static constexpr uint32_t polynomial = 0xEDB88320; 16 | 17 | static bool is_initialized = false; 18 | static uint32_t crc_table[256]; 19 | 20 | if (!is_initialized) { 21 | for (uint32_t i = 0; i < 256; i++) { 22 | uint32_t c = i; 23 | for (size_t j = 0; j < 8; j++) { 24 | if (c & 1) { 25 | c = polynomial ^ (c >> 1); 26 | } else { 27 | c >>= 1; 28 | } 29 | } 30 | crc_table[i] = c; 31 | } 32 | 33 | is_initialized = true; 34 | } 35 | 36 | return crc_table; 37 | } 38 | } // namespace 39 | 40 | namespace point_one { 41 | namespace fusion_engine { 42 | namespace messages { 43 | 44 | /******************************************************************************/ 45 | uint32_t CalculateCRC(const void* buffer, size_t length, 46 | uint32_t initial_value) { 47 | static const uint32_t* crc_table = ::GetCRCTable(); 48 | uint32_t c = initial_value ^ 0xFFFFFFFF; 49 | const uint8_t* u = static_cast(buffer); 50 | for (size_t i = 0; i < length; ++i) { 51 | c = crc_table[(c ^ u[i]) & 0xFF] ^ (c >> 8); 52 | } 53 | return c ^ 0xFFFFFFFF; 54 | } 55 | 56 | /******************************************************************************/ 57 | uint32_t CalculateCRC(const void* buffer) { 58 | static constexpr size_t offset = offsetof(MessageHeader, protocol_version); 59 | const MessageHeader& header = *static_cast(buffer); 60 | size_t size_bytes = 61 | (sizeof(MessageHeader) - offset) + header.payload_size_bytes; 62 | return CalculateCRC(reinterpret_cast(&header) + offset, 63 | size_bytes); 64 | } 65 | 66 | } // namespace messages 67 | } // namespace fusion_engine 68 | } // namespace point_one 69 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/crc.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Message CRC support. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | 11 | #include "point_one/fusion_engine/common/portability.h" 12 | #include "point_one/fusion_engine/messages/defs.h" 13 | 14 | namespace point_one { 15 | namespace fusion_engine { 16 | namespace messages { 17 | 18 | /** 19 | * @defgroup crc_support CRC Calculation/Message Validation Support 20 | * @{ 21 | */ 22 | 23 | /** 24 | * @brief Calculate the CRC for the message (header + payload) contained in the 25 | * buffer. 26 | * 27 | * @param buffer A byte buffer containing a @ref MessageHeader and payload. 28 | * 29 | * @return The calculated CRC value. 30 | */ 31 | P1_EXPORT uint32_t CalculateCRC(const void* buffer); 32 | 33 | /** 34 | * @brief Calculate the CRC for the message (payload) contained in the buffer. 35 | * 36 | * @param buffer A byte buffer containing a payload. 37 | * @param length The length of the buffer. 38 | * @param initial_value The seed value of the CRC calculation. 39 | * 40 | * @return The calculated CRC value. 41 | */ 42 | P1_EXPORT uint32_t CalculateCRC(const void* buffer, size_t length, 43 | uint32_t initial_value = 0); 44 | 45 | /** 46 | * @brief Check if the message contained in the buffer has a valid CRC. 47 | * 48 | * @param buffer A byte buffer containing a @ref MessageHeader and payload. 49 | * 50 | * @return `true` if the CRC value in the header matches the CRC computed from 51 | * the current contents. 52 | */ 53 | inline bool IsValid(const void* buffer) { 54 | // Sanity check the message payload length before calculating the CRC. 55 | const MessageHeader& header = *static_cast(buffer); 56 | if (sizeof(MessageHeader) + header.payload_size_bytes > 57 | MessageHeader::MAX_MESSAGE_SIZE_BYTES) { 58 | return false; 59 | } else { 60 | return header.crc == CalculateCRC(buffer); 61 | } 62 | } 63 | 64 | /** @} */ 65 | 66 | } // namespace messages 67 | } // namespace fusion_engine 68 | } // namespace point_one 69 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/data_version.cc: -------------------------------------------------------------------------------- 1 | #include "point_one/fusion_engine/messages/data_version.h" 2 | 3 | #include 4 | 5 | namespace point_one { 6 | namespace fusion_engine { 7 | namespace messages { 8 | 9 | p1_ostream& operator<<(p1_ostream& stream, const DataVersion& ver) { 10 | if (ver.IsValid()) { 11 | return stream << (int)ver.major_version << "." << ver.minor_version; 12 | } else { 13 | return stream << ""; 14 | } 15 | } 16 | 17 | std::string ToString(const DataVersion& ver) { 18 | if (ver.IsValid()) { 19 | return std::to_string(ver.major_version) + "." + 20 | std::to_string(ver.minor_version); 21 | } else { 22 | return ""; 23 | } 24 | } 25 | 26 | DataVersion FromString(const char* str) { 27 | char* end_c = nullptr; 28 | long tmp = 0; 29 | DataVersion version; 30 | 31 | tmp = strtol(str, &end_c, 10); 32 | if (end_c == str || tmp > 0xFF || tmp < 0) { 33 | return INVALID_DATA_VERSION; 34 | } 35 | version.major_version = (uint8_t)tmp; 36 | 37 | const char* minor_str = end_c + 1; 38 | 39 | tmp = strtol(minor_str, &end_c, 10); 40 | if (end_c == minor_str || tmp > 0xFFFF || tmp < 0) { 41 | return INVALID_DATA_VERSION; 42 | } 43 | version.minor_version = (uint16_t)tmp; 44 | 45 | return version; 46 | } 47 | 48 | } // namespace messages 49 | } // namespace fusion_engine 50 | } // namespace point_one 51 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/data_version.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "point_one/fusion_engine/common/portability.h" 7 | 8 | namespace point_one { 9 | namespace fusion_engine { 10 | namespace messages { 11 | 12 | // Enforce 4-byte alignment and packing of all data structures and values. 13 | // Floating point values are aligned on platforms that require it. This is done 14 | // with a combination of setting struct attributes, and manual alignment 15 | // within the definitions. See the "Message Packing" section of the README. 16 | #pragma pack(push, 1) 17 | 18 | /** 19 | * @brief A struct representing the version of a data object. 20 | * 21 | * The version is considered invalid if @ref major_version is 0xFF and @ref minor_version is 22 | * 0xFFFF. 23 | */ 24 | struct P1_ALIGNAS(4) DataVersion { 25 | // The reserved bytes must be 0xFF for backward compatibility. 26 | uint8_t reserved = 0xFF; 27 | uint8_t major_version = 0xFF; 28 | uint16_t minor_version = 0xFFFF; 29 | 30 | constexpr DataVersion() = default; 31 | constexpr DataVersion(uint8_t major, uint16_t minor) 32 | : major_version{major}, minor_version{minor} {} 33 | 34 | /** 35 | * @brief Returns whether the stored version is valid. 36 | * 37 | * @return `true` if the version is valid, `false` otherwise. 38 | */ 39 | bool IsValid() const { 40 | return major_version != 0xFF || minor_version != 0xFFFF; 41 | } 42 | }; 43 | 44 | #pragma pack(pop) 45 | 46 | constexpr DataVersion INVALID_DATA_VERSION; 47 | 48 | inline constexpr bool operator==(const DataVersion& a, const DataVersion& b) { 49 | return a.major_version == b.major_version && 50 | a.minor_version == b.minor_version; 51 | } 52 | 53 | inline constexpr bool operator!=(const DataVersion& a, const DataVersion& b) { 54 | return !(a == b); 55 | } 56 | 57 | inline constexpr bool operator<(const DataVersion& a, const DataVersion& b) { 58 | return a.major_version < b.major_version || 59 | (a.major_version == b.major_version && 60 | a.minor_version < b.minor_version); 61 | } 62 | 63 | inline constexpr bool operator>(const DataVersion& a, const DataVersion& b) { 64 | return b < a; 65 | } 66 | 67 | inline constexpr bool operator<=(const DataVersion& a, const DataVersion& b) { 68 | return !(a > b); 69 | } 70 | 71 | inline constexpr bool operator>=(const DataVersion& a, const DataVersion& b) { 72 | return !(a < b); 73 | } 74 | 75 | /** 76 | * @brief Helper class for printing out X.Y form of @ref DataVersion. 77 | * 78 | * ```cpp 79 | * DataVersion ver{3, 2}; 80 | * std::cout << "Ver: " << ver; 81 | * // Ver: 3.2 82 | * ``` 83 | */ 84 | p1_ostream& operator<<(p1_ostream& stream, const DataVersion& ver); 85 | 86 | std::string ToString(const DataVersion& ver); 87 | 88 | DataVersion FromString(const char* str); 89 | 90 | inline DataVersion FromString(std::string str) { 91 | return FromString(str.c_str()); 92 | } 93 | 94 | } // namespace messages 95 | } // namespace fusion_engine 96 | } // namespace point_one 97 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/gnss_corrections.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief GNSS corrections messages. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include "point_one/fusion_engine/messages/defs.h" 9 | 10 | namespace point_one { 11 | namespace fusion_engine { 12 | namespace messages { 13 | 14 | // Enforce 4-byte alignment and packing of all data structures and values. 15 | // Floating point values are aligned on platforms that require it. This is done 16 | // with a combination of setting struct attributes, and manual alignment 17 | // within the definitions. See the "Message Packing" section of the README. 18 | #pragma pack(push, 1) 19 | 20 | /** 21 | * @defgroup gnss_corrections GNSS Corrections Message Definitions 22 | * @brief Messages containing GNSS corrections. 23 | * @ingroup messages 24 | * 25 | * See also @ref messages. 26 | */ 27 | 28 | /** 29 | * @brief L-band frame contents (@ref MessageType::LBAND_FRAME, version 1.0). 30 | * @ingroup gnss_corrections 31 | */ 32 | struct P1_ALIGNAS(4) LBandFrameMessage : public MessagePayload { 33 | static constexpr MessageType MESSAGE_TYPE = MessageType::LBAND_FRAME; 34 | static constexpr uint8_t MESSAGE_VERSION = 0; 35 | 36 | /** 37 | * The system time when the frame was received (in nanoseconds). Note that 38 | * this is not synchronized to other P1 systems or GNSS. 39 | */ 40 | int64_t system_time_ns = 0; 41 | 42 | /** Number of bytes in this data payload. */ 43 | uint16_t user_data_size_bytes = 0; 44 | 45 | /** Count of bit errors found in the data frame. */ 46 | uint16_t bit_error_count = 0; 47 | 48 | /** Power of the signal (dB). */ 49 | uint8_t signal_power_db = 0; 50 | 51 | uint8_t reserved[3] = {0}; 52 | 53 | /** 54 | * The offset from the center frequency (Hz). This includes effects from user 55 | * motion, receiver clock, and satellite clock errors. 56 | */ 57 | float doppler_hz = 0; 58 | 59 | /** 60 | * The beginning of the demodulated L-band frame data. 61 | */ 62 | // uint8_t data_payload[0]; 63 | }; 64 | 65 | #pragma pack(pop) 66 | 67 | } // namespace messages 68 | } // namespace fusion_engine 69 | } // namespace point_one 70 | -------------------------------------------------------------------------------- /src/point_one/fusion_engine/messages/sta5635.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief Command/control support for an attached STA5635 RF front-end. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include "point_one/fusion_engine/common/portability.h" 9 | #include "point_one/fusion_engine/messages/defs.h" 10 | 11 | namespace point_one { 12 | namespace fusion_engine { 13 | namespace messages { 14 | 15 | // Enforce 4-byte alignment and packing of all data structures and values. 16 | // Floating point values are aligned on platforms that require it. This is done 17 | // with a combination of setting struct attributes, and manual alignment 18 | // within the definitions. See the "Message Packing" section of the README. 19 | #pragma pack(push, 1) 20 | 21 | /**************************************************************************/ /** 22 | * @defgroup sta5635 STA5635 Command/Control Messages 23 | * @brief Messages for interacting with an attached STA5635 RF front-end device. 24 | * @ingroup device_control 25 | * 26 | * These messages are intended to be used only for devices with an 27 | * STMicroelectronics STA5635 RF front-end where direct user control of the 28 | * front-end is needed. This is not common and should not be used on most 29 | * platforms. 30 | * 31 | * For platforms using an STA5635, the device will output @ref LBandFrameMessage 32 | * containing I/Q samples from the RF front-end. The format and use of the I/Q 33 | * samples is platform-specific. 34 | * 35 | * See also @ref messages. 36 | ******************************************************************************/ 37 | 38 | /** 39 | * @brief A command to be sent to an attached STA5635 RF front-end. (@ref 40 | * MessageType::STA5635_COMMAND, version 1.0). 41 | * @ingroup sta5635 42 | * 43 | * See the STA5635 data sheet for the allowed command, address, and data values. 44 | */ 45 | struct P1_ALIGNAS(4) STA5635Command : public MessagePayload { 46 | static constexpr MessageType MESSAGE_TYPE = MessageType::STA5635_COMMAND; 47 | static constexpr uint8_t MESSAGE_VERSION = 0; 48 | 49 | /** The STA5635 command code to be issued. */ 50 | uint8_t command = 0; 51 | /** The address of the STA5635 register to be accessed. */ 52 | uint8_t address = 0; 53 | /** 54 | * The value to be sent to the device, where `data[0]` contains the MSB. 55 | */ 56 | uint8_t data[2] = {0}; 57 | }; 58 | 59 | /** 60 | * @brief Result from an STA5635 sent in response to an @ref STA5635Command. 61 | * (@ref MessageType::STA5635_COMMAND_RESPONSE, version 1.0). 62 | * @ingroup sta5635 63 | */ 64 | struct P1_ALIGNAS(4) STA5635CommandResponse : public MessagePayload { 65 | static constexpr MessageType MESSAGE_TYPE = 66 | MessageType::STA5635_COMMAND_RESPONSE; 67 | static constexpr uint8_t MESSAGE_VERSION = 0; 68 | 69 | /** 70 | * The system time when the response was received (in nanoseconds). Note that 71 | * this is not synchronized to P1 or GNSS time. 72 | */ 73 | int64_t system_time_ns = 0; 74 | /** 75 | * The sequence number contained in the @ref STA5635Command to which this 76 | * response belongs. 77 | */ 78 | uint32_t command_sequence_number = 0; 79 | /** 80 | * The response from the device, where `data[0]` contains the first byte in 81 | * the response. 82 | */ 83 | uint8_t data[4] = {0}; 84 | }; 85 | 86 | /** 87 | * @brief IQ sample data from an STA5635 88 | * (@ref MessageType::STA5635_IQ_DATA, version 1.0). 89 | * @ingroup sta5635 90 | * @note 91 | * The rest of this message contains the wrapped payload data. The size of 92 | * the data is found by subtracting the size of the other fields in this 93 | * message from the header `payload_size_bytes` (i.e. `size_t content_size = 94 | * header->payload_size_bytes - sizeof(STA5635IQData)`). 95 | */ 96 | struct P1_ALIGNAS(4) STA5635IQData : public MessagePayload { 97 | static constexpr MessageType MESSAGE_TYPE = MessageType::STA5635_IQ_DATA; 98 | static constexpr uint8_t MESSAGE_VERSION = 0; 99 | 100 | uint8_t reserved[4] = {0}; 101 | }; 102 | 103 | #pragma pack(pop) 104 | 105 | } // namespace messages 106 | } // namespace fusion_engine 107 | } // namespace point_one 108 | -------------------------------------------------------------------------------- /src/point_one/rtcm/rtcm_framer.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************/ /** 2 | * @brief RTCM 3 message framer. 3 | * @file 4 | ******************************************************************************/ 5 | 6 | #pragma once 7 | 8 | #include // For size_t 9 | #include 10 | 11 | #include "point_one/fusion_engine/common/portability.h" // For macros. 12 | 13 | namespace point_one { 14 | namespace rtcm { 15 | 16 | /** 17 | * @brief Frame and validate incoming RTCM 3 messages. 18 | * 19 | * This class locates and validates RTCM 3 messages within a stream of binary 20 | * data. Data may be stored in an internally allocated buffer, or in an external 21 | * buffer supplied by the user. 22 | * 23 | * The callback function provided to @ref SetMessageCallback() will be called 24 | * each time a complete message is received. Any messages that do not pass the 25 | * CRC check, or that are too big to be stored in the data buffer, will be 26 | * discarded. 27 | * 28 | * Example usage: 29 | * ```cpp 30 | * void MessageReceived(uint16_t message_type, const void* data, size_t data_len) { 31 | * ... 32 | * } 33 | * 34 | * RTCMFramer framer(1024); 35 | * framer.SetMessageCallback(MessageReceived); 36 | * framer.OnData(my_data, my_data_size); 37 | * ``` 38 | */ 39 | class P1_EXPORT RTCMFramer { 40 | public: 41 | using MessageCallback = void (*)(uint16_t, const void*, size_t); 42 | 43 | /** 44 | * @brief Construct a framer instance with no buffer allocated. 45 | * 46 | * @note 47 | * You must call @ref SetBuffer() to assign a buffer, otherwise all incoming 48 | * data will be discarded. 49 | */ 50 | RTCMFramer() = default; 51 | 52 | /** 53 | * @brief Construct a framer instance with an internally allocated buffer. 54 | * 55 | * @param capacity_bytes The maximum framing buffer capacity (in bytes). 56 | */ 57 | explicit RTCMFramer(size_t capacity_bytes) 58 | : RTCMFramer(nullptr, capacity_bytes) {} 59 | 60 | /** 61 | * @brief Construct a framer instance with a user-specified buffer. 62 | * 63 | * @post 64 | * `buffer` must exist for the lifetime of this instance. 65 | * 66 | * @param buffer The framing buffer to use. Set to `nullptr` to allocate a 67 | * buffer internally. 68 | * @param capacity_bytes The maximum framing buffer capacity (in bytes). 69 | */ 70 | RTCMFramer(void* buffer, size_t capacity_bytes); 71 | 72 | ~RTCMFramer(); 73 | 74 | // Don't allow copying or moving to avoid issues with managed buffer_. 75 | RTCMFramer(const RTCMFramer&) = delete; // Copy constructor 76 | RTCMFramer(RTCMFramer&&) = delete; // Move constructor 77 | RTCMFramer& operator=(const RTCMFramer&) = delete; // Copy assignment operator 78 | RTCMFramer& operator=(RTCMFramer&&) = delete; // Move assignment operator 79 | 80 | /** 81 | * @brief Set the buffer to use for message framing. 82 | * 83 | * @post 84 | * `buffer` must exist for the lifetime of this instance. 85 | * 86 | * @param buffer The framing buffer to use. Set to `nullptr` to allocate a 87 | * buffer internally. 88 | * @param capacity_bytes The maximum framing buffer capacity (in bytes). 89 | */ 90 | void SetBuffer(void* buffer, size_t capacity_bytes); 91 | 92 | /** 93 | * @brief Enable/disable warnings for CRC and "message too large" failures. 94 | * 95 | * This is typically used when the incoming stream has multiple types of 96 | * binary content (e.g., interleaved FusionEngine and RTCM messages), and the 97 | * RTCM message preamble is expected to appear in the non-RTCM content 98 | * occasionally. 99 | * 100 | * @param enabled If `true`, issue warnings on errors. 101 | */ 102 | void WarnOnError(bool enabled) { warn_on_error_ = enabled; } 103 | 104 | /** 105 | * @brief Specify a function to be called when a message is framed. 106 | * 107 | * @param callback The function to be called with the message header and a 108 | * pointer to the message payload. 109 | */ 110 | void SetMessageCallback(MessageCallback callback) { callback_ = callback; } 111 | 112 | /** 113 | * @brief Reset the framer and discard all pending data. 114 | */ 115 | void Reset(); 116 | 117 | /** 118 | * @brief Process incoming data. 119 | * 120 | * @param buffer A buffer containing data to be framed. 121 | * @param length_bytes The number of bytes to be framed. 122 | * 123 | * @return The total size of all valid, complete messages, or 0 if no messages 124 | * were completed. 125 | */ 126 | size_t OnData(const uint8_t* buffer, size_t length_bytes); 127 | 128 | /** 129 | * @brief Get the number of decoded messages. 130 | * 131 | * @return The number of RTCM messages successfully decoded. 132 | */ 133 | uint32_t GetNumDecodedMessages() const { return decoded_msg_count_; } 134 | 135 | /** 136 | * @brief Get the number of preamble synchronizations that resulted in errors. 137 | * 138 | * This is not an accurate count of failed messages since the RTCM preamble is 139 | * not unique and may appear anywhere in the data stream, but gives an 140 | * approximate count. 141 | * 142 | * @return The number of length or CRC failures found in decoding so far. 143 | */ 144 | uint32_t GetNumErrors() const { return error_count_; } 145 | 146 | private: 147 | enum class State { 148 | SYNC = 0, 149 | HEADER = 1, 150 | DATA = 2, 151 | }; 152 | 153 | MessageCallback callback_ = nullptr; 154 | 155 | bool warn_on_error_ = true; 156 | bool is_buffer_managed_ = false; 157 | uint8_t* buffer_{nullptr}; 158 | uint32_t capacity_bytes_{0}; 159 | 160 | State state_{State::SYNC}; 161 | uint32_t next_byte_index_{0}; 162 | size_t current_message_size_{0}; 163 | 164 | uint32_t error_count_{0}; 165 | uint32_t decoded_msg_count_{0}; 166 | 167 | /** 168 | * @brief Process a single byte. 169 | * 170 | * @pre 171 | * The byte must be located at `buffer_[next_byte_index_ - 1]`. 172 | * 173 | * @param quiet If `true`, suppress failure warning messages. 174 | * 175 | * @return The total size of all valid, complete messages, 0 if no messages 176 | * were completed, or <0 CRC or "message too large" error. 177 | */ 178 | int32_t OnByte(bool quiet); 179 | 180 | /** 181 | * @brief Perform a resynchronization operation starting at `buffer_[1]`. 182 | * 183 | * @return The total size of all valid, complete messages, or 0 if no messages 184 | * were completed. 185 | */ 186 | uint32_t Resync(); 187 | 188 | /** 189 | * @brief Free the @ref buffer_ if it's being managed internally. 190 | */ 191 | void ClearManagedBuffer(); 192 | }; 193 | 194 | } // namespace rtcm 195 | } // namespace point_one 196 | --------------------------------------------------------------------------------