├── .clang-format ├── .clang-tidy ├── .gitignore ├── LICENSE ├── README.md ├── assets ├── angle.png ├── d_img_1.png ├── d_img_2.png ├── d_img_3.png ├── d_img_4.png ├── d_img_5.png ├── d_img_6.png ├── d_img_7.png ├── full_1.png ├── full_2.png ├── img1.png ├── img2.png ├── img3.png ├── img4.png ├── img5.png ├── img6.png ├── img7.png ├── img8.png ├── serial.png ├── 全功能同步板快速开始.md ├── 时间同步原理.md ├── 核心板快速开始.md └── 相机购买指南.md ├── data ├── full.stl └── mini.stl ├── demo ├── customised_camera │ ├── CMakeLists.txt │ ├── ReadMe.md │ ├── customised_camera.cpp │ ├── customised_camera.h │ └── main_ros.cpp ├── uart_demo │ ├── CMakeLists.txt │ ├── ReadMe.md │ └── main_ros.cpp ├── udp_demo │ ├── CMakeLists.txt │ ├── ReadMe.md │ └── main_ros.cpp └── zmq_demo │ ├── CMakeLists.txt │ ├── ReadMe.md │ ├── cmake │ └── FindZeroMQ.cmake │ ├── main_zmq.cpp │ ├── proto_msg │ ├── CMakeLists.txt │ ├── msg_header.proto │ ├── msg_image.proto │ └── msg_imu.proto │ └── zmq.hpp ├── firmware ├── README.md ├── flash_board_tool.uf2 ├── full_v2_24_10_16_release.uf2 ├── full_v3_24_10_23_release.uf2 ├── full_v3_24_11_16_release.uf2 ├── full_v3_24_11_25_release.uf2 ├── mini_24_11_13_release.uf2 ├── mini_24_11_16_release.uf2 └── mini_24_11_25_release.uf2 └── sdk ├── README.md └── infinite_sense_core ├── arm ├── include │ ├── CameraParams.h │ ├── Fusion.h │ ├── FusionAhrs.h │ ├── FusionAxes.h │ ├── FusionCalibration.h │ ├── FusionCompass.h │ ├── FusionConvention.h │ ├── FusionMath.h │ ├── FusionOffset.h │ ├── MvCameraControl.h │ ├── MvErrorDefine.h │ ├── MvISPErrorDefine.h │ ├── MvObsoleteInterfaces.h │ ├── ObsoleteCamParams.h │ ├── PixelType.h │ ├── atomicops.h │ ├── cam_manager.h │ ├── crc.h │ ├── data_manager.h │ ├── practical_socket.h │ ├── readerwriterqueue.h │ ├── serial.h │ ├── serial_manager.h │ ├── udp_manager.h │ ├── unix.h │ ├── v8stdint.h │ └── win.h └── lib │ ├── CommonParameters.ini │ ├── MvProducerGEV.cti │ ├── MvProducerU3V.cti │ ├── libFormatConversion.so │ ├── libMVGigEVisionSDK.so │ ├── libMVGigEVisionSDK.so.4.4.1.2 │ ├── libMVRender.so │ ├── libMediaProcess.so │ ├── libMvCameraControl.so │ ├── libMvCameraControl.so.4.4.1.3 │ ├── libMvCameraControlWrapper.so │ ├── libMvCameraControlWrapper.so.2.4.0.6 │ ├── libMvSDKVersion.so │ ├── libMvUsb3vTL.so │ ├── libMvUsb3vTL.so.4.4.1.2 │ ├── libavutil.so │ ├── libfusion.a │ ├── libinfinite_sense_core.so │ ├── libopencv_core.so │ ├── libopencv_core.so.4.2 │ ├── libopencv_core.so.4.2.0 │ ├── libopencv_imgproc.so │ ├── libopencv_imgproc.so.4.2 │ ├── libopencv_imgproc.so.4.2.0 │ ├── libserial.a │ ├── libswscale.so │ ├── libudp.a │ └── libusb-1.0.so.0 └── x86 ├── include ├── CameraParams.h ├── Fusion.h ├── FusionAhrs.h ├── FusionAxes.h ├── FusionCalibration.h ├── FusionCompass.h ├── FusionConvention.h ├── FusionMath.h ├── FusionOffset.h ├── MvCameraControl.h ├── MvErrorDefine.h ├── MvISPErrorDefine.h ├── MvObsoleteInterfaces.h ├── ObsoleteCamParams.h ├── PixelType.h ├── atomicops.h ├── cam_manager.h ├── crc.h ├── data_manager.h ├── practical_socket.h ├── readerwriterqueue.h ├── serial.h ├── serial_manager.h ├── udp_manager.h ├── unix.h ├── v8stdint.h └── win.h └── lib ├── CommonParameters.ini ├── MvProducerGEV.cti ├── MvProducerU3V.cti ├── libCLAllSerial_gcc447_v3_0.so ├── libCLProtocol_gcc447_v3_0.so ├── libCLSerCOM.so ├── libFormatConversion.so ├── libGCBase_gcc447_v3_0.so ├── libGenCP_gcc447_v3_0.so ├── libLog_gcc447_v3_0.so ├── libMVGigEVisionSDK.so ├── libMVGigEVisionSDK.so.4.1.2.2 ├── libMVRender.so ├── libMediaProcess.so ├── libMvCamLVision.so ├── libMvCamLVision.so.4.1.0.3 ├── libMvCameraControl.so ├── libMvCameraControl.so.4.1.2.2 ├── libMvCameraControlWrapper.so ├── libMvCameraControlWrapper.so.1.0.1.0 ├── libMvUsb3vTL.so ├── libMvUsb3vTL.so.4.1.2.2 ├── libavutil.so ├── libfusion.a ├── libinfinite_sense_core.so ├── liblog4cpp_gcc447_v3_0.so ├── libopencv_core.so ├── libopencv_core.so.4.8.0 ├── libopencv_core.so.408 ├── libopencv_highgui.so ├── libopencv_highgui.so.4.8.0 ├── libopencv_highgui.so.408 ├── libopencv_imgcodecs.so ├── libopencv_imgcodecs.so.4.8.0 ├── libopencv_imgcodecs.so.408 ├── libopencv_imgproc.so ├── libopencv_imgproc.so.4.8.0 ├── libopencv_imgproc.so.408 ├── libserial.a ├── libswscale.so ├── libudp.a └── libusb-1.0.so.0 /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignArrayOfStructures: None 7 | AlignConsecutiveMacros: None 8 | AlignConsecutiveAssignments: None 9 | AlignConsecutiveBitFields: None 10 | AlignConsecutiveDeclarations: None 11 | AlignEscapedNewlines: Left 12 | AlignOperands: Align 13 | AlignTrailingComments: true 14 | AllowAllArgumentsOnNextLine: true 15 | AllowAllConstructorInitializersOnNextLine: true 16 | AllowAllParametersOfDeclarationOnNextLine: true 17 | AllowShortEnumsOnASingleLine: true 18 | AllowShortBlocksOnASingleLine: Never 19 | AllowShortCaseLabelsOnASingleLine: false 20 | AllowShortFunctionsOnASingleLine: All 21 | AllowShortLambdasOnASingleLine: All 22 | AllowShortIfStatementsOnASingleLine: WithoutElse 23 | AllowShortLoopsOnASingleLine: true 24 | AlwaysBreakAfterDefinitionReturnType: None 25 | AlwaysBreakAfterReturnType: None 26 | AlwaysBreakBeforeMultilineStrings: true 27 | AlwaysBreakTemplateDeclarations: Yes 28 | BinPackArguments: true 29 | BinPackParameters: true 30 | BraceWrapping: 31 | AfterCaseLabel: false 32 | AfterClass: false 33 | AfterControlStatement: Never 34 | AfterEnum: false 35 | AfterFunction: false 36 | AfterNamespace: false 37 | AfterObjCDeclaration: false 38 | AfterStruct: false 39 | AfterUnion: false 40 | AfterExternBlock: false 41 | BeforeCatch: false 42 | BeforeElse: false 43 | BeforeLambdaBody: false 44 | BeforeWhile: false 45 | IndentBraces: false 46 | SplitEmptyFunction: true 47 | SplitEmptyRecord: true 48 | SplitEmptyNamespace: true 49 | BreakBeforeBinaryOperators: None 50 | BreakBeforeConceptDeclarations: true 51 | BreakBeforeBraces: Attach 52 | BreakBeforeInheritanceComma: false 53 | BreakInheritanceList: BeforeColon 54 | BreakBeforeTernaryOperators: true 55 | BreakConstructorInitializersBeforeComma: false 56 | BreakConstructorInitializers: BeforeColon 57 | BreakAfterJavaFieldAnnotations: false 58 | BreakStringLiterals: true 59 | ColumnLimit: 120 60 | CommentPragmas: '^ IWYU pragma:' 61 | CompactNamespaces: false 62 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 63 | ConstructorInitializerIndentWidth: 4 64 | ContinuationIndentWidth: 4 65 | Cpp11BracedListStyle: true 66 | DeriveLineEnding: true 67 | DerivePointerAlignment: true 68 | DisableFormat: false 69 | EmptyLineAfterAccessModifier: Leave 70 | EmptyLineBeforeAccessModifier: LogicalBlock 71 | ExperimentalAutoDetectBinPacking: false 72 | FixNamespaceComments: true 73 | ForEachMacros: 74 | - foreach 75 | - Q_FOREACH 76 | - BOOST_FOREACH 77 | IncludeBlocks: Regroup 78 | IncludeCategories: 79 | - Regex: '^' 80 | Priority: 2 81 | SortPriority: 0 82 | - Regex: '^<.*\.h>' 83 | Priority: 1 84 | SortPriority: 0 85 | - Regex: '^<.*' 86 | Priority: 2 87 | SortPriority: 0 88 | - Regex: '.*' 89 | Priority: 3 90 | SortPriority: 0 91 | IncludeIsMainRegex: '([-_](test|unittest))?$' 92 | IncludeIsMainSourceRegex: '' 93 | IndentCaseLabels: true 94 | IndentCaseBlocks: false 95 | IndentGotoLabels: true 96 | IndentPPDirectives: None 97 | IndentExternBlock: AfterExternBlock 98 | IndentWidth: 2 99 | IndentWrappedFunctionNames: false 100 | InsertTrailingCommas: None 101 | JavaScriptQuotes: Leave 102 | JavaScriptWrapImports: true 103 | KeepEmptyLinesAtTheStartOfBlocks: false 104 | MacroBlockBegin: '' 105 | MacroBlockEnd: '' 106 | MaxEmptyLinesToKeep: 1 107 | NamespaceIndentation: None 108 | ObjCBinPackProtocolList: Never 109 | ObjCBlockIndentWidth: 2 110 | ObjCBreakBeforeNestedBlockParam: true 111 | ObjCSpaceAfterProperty: false 112 | ObjCSpaceBeforeProtocolList: true 113 | PenaltyBreakAssignment: 2 114 | PenaltyBreakBeforeFirstCallParameter: 1 115 | PenaltyBreakComment: 300 116 | PenaltyBreakFirstLessLess: 120 117 | PenaltyBreakString: 1000 118 | PenaltyBreakTemplateDeclaration: 10 119 | PenaltyExcessCharacter: 1000000 120 | PenaltyReturnTypeOnItsOwnLine: 200 121 | PointerAlignment: Left 122 | PPIndentWidth: -1 123 | RawStringFormats: 124 | - Language: Cpp 125 | Delimiters: 126 | - cc 127 | - CC 128 | - cpp 129 | - Cpp 130 | - CPP 131 | - 'c++' 132 | - 'C++' 133 | CanonicalDelimiter: '' 134 | BasedOnStyle: google 135 | - Language: TextProto 136 | Delimiters: 137 | - pb 138 | - PB 139 | - proto 140 | - PROTO 141 | EnclosingFunctions: 142 | - EqualsProto 143 | - EquivToProto 144 | - PARSE_PARTIAL_TEXT_PROTO 145 | - PARSE_TEST_PROTO 146 | - PARSE_TEXT_PROTO 147 | - ParseTextOrDie 148 | - ParseTextProtoOrDie 149 | - ParseTestProto 150 | - ParsePartialTestProto 151 | CanonicalDelimiter: '' 152 | BasedOnStyle: google 153 | ReferenceAlignment: Pointer 154 | ShortNamespaceLines: 1 155 | ReflowComments: true 156 | SortIncludes: Never 157 | SortUsingDeclarations: true 158 | SpaceAfterCStyleCast: false 159 | SpaceAfterLogicalNot: false 160 | SpaceAfterTemplateKeyword: true 161 | SpaceBeforeAssignmentOperators: true 162 | SpaceBeforeCaseColon: false 163 | SpaceBeforeCpp11BracedList: false 164 | SpaceBeforeCtorInitializerColon: true 165 | SpaceBeforeInheritanceColon: true 166 | SpaceBeforeParens: ControlStatements 167 | SpaceAroundPointerQualifiers: Default 168 | SpaceBeforeRangeBasedForLoopColon: true 169 | SpaceInEmptyBlock: false 170 | SpaceInEmptyParentheses: false 171 | SpacesBeforeTrailingComments: 2 172 | SpacesInAngles: Never 173 | SpacesInConditionalStatement: false 174 | SpacesInContainerLiterals: true 175 | SpacesInCStyleCastParentheses: false 176 | SpacesInParentheses: false 177 | SpacesInSquareBrackets: false 178 | SpaceBeforeSquareBrackets: false 179 | SpacesInLineCommentPrefix: 180 | Minimum: 1 181 | Maximum: 1 182 | Standard: Auto 183 | StatementMacros: 184 | - Q_UNUSED 185 | - QT_REQUIRE_VERSION 186 | TabWidth: 8 187 | UseCRLF: false 188 | UseTab: Never 189 | WhitespaceSensitiveMacros: 190 | - STRINGIZE 191 | - PP_STRINGIZE 192 | - BOOST_PP_STRINGIZE 193 | - NS_SWIFT_NAME 194 | - CF_SWIFT_NAME 195 | ... 196 | 197 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: 'readability-identifier-naming' 2 | CheckOptions: 3 | # ------------------- 变量命名规则 4 | # 变量小写 5 | - key: readability-identifier-naming.VariableCase 6 | value: lower_case 7 | # 全局常量驼峰，加前缀‘k’ 8 | - key: readability-identifier-naming.GlobalConstantCase 9 | value: CamelCase 10 | - key: readability-identifier-naming.GlobalConstantPrefix 11 | value: k 12 | # 局部常量驼峰，加前缀‘k’ 13 | - key: readability-identifier-naming.LocalConstantCase 14 | value: CamelCase 15 | - key: readability-identifier-naming.LocalConstantPrefix 16 | value: k 17 | # 类静态常量驼峰，加前缀‘k’ 18 | - key: readability-identifier-naming.ClassConstantCase 19 | value: CamelCase 20 | - key: readability-identifier-naming.ClassConstantPrefix 21 | value: k 22 | # 枚举常量驼峰，加前缀‘k’ 23 | - key: readability-identifier-naming.EnumConstantCase 24 | value: CamelCase 25 | - key: readability-identifier-naming.EnumConstantPrefix 26 | value: k 27 | # 成员变量小写 28 | - key: readability-identifier-naming.MemberCase 29 | value: lower_case 30 | # 私有成员变量小写 31 | - key: readability-identifier-naming.PrivateMemberCase 32 | value: lower_case 33 | # 私有成员变量小写，后缀加‘_’ 34 | - key: readability-identifier-naming.PrivateMemberSuffix 35 | value: _ 36 | 37 | # ------------------- 类型，函数命名规则 38 | # 类名称驼峰 39 | - key: readability-identifier-naming.ClassCase 40 | value: CamelCase 41 | # 结构体名称驼峰 42 | - key: readability-identifier-naming.StructCase 43 | value: CamelCase 44 | # 枚举类型名称驼峰 45 | - key: readability-identifier-naming.EnumCase 46 | value: CamelCase 47 | # 函数名称驼峰 48 | - key: readability-identifier-naming.FunctionCase 49 | value: CamelCase 50 | # 宏名称全大写 51 | - key: readability-identifier-naming.MacroDefinitionCase 52 | value: UPPER_CASE 53 | # 命名空间小写 54 | - key: readability-identifier-naming.NamespaceCase 55 | value: lower_case 56 | 57 | # ------------------ 命名空间规则 58 | # llvmlibc-implementation-in-namespace 59 | 60 | # ------------------- 其他规则 61 | # google-explicit-constructor 隐式类型转换 62 | # modernize-use-override 检查到final或overrider修饰的函数是否有virtual修饰 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.dylib 16 | 17 | # Fortran module files 18 | *.mod 19 | *.smod 20 | 21 | # Compiled Static libraries 22 | *.lai 23 | *.la 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | .idea/ 32 | cmake-build-*/ 33 | .vscode/ 34 | build/ 35 | log/ 36 | .clang-format 37 | .clang-tidy 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Lin Yicheng 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NOTE!!!! 2 | 3 | 本项目的新地址在：[https://github.com/InfiniteSenseLab/Multi-Sensor-Time-Synchronisation-System](https://github.com/InfiniteSenseLab/Multi-Sensor-Time-Synchronisation-System) ，后续都将在新地址下更新,本地址将不再维护。 4 | 5 | # Multi-Sensor Time Synchronisation System 6 | # 多传感器时间同步系统 7 | ![title](./assets/img1.png) 8 | 9 | ## News 10 | 11 | >1. 很多小伙伴咨询接线的问题，我们在[全功能同步板快速开始](./assets/全功能同步板快速开始.md)与[核心板快速开始](./assets/核心板快速开始.md)介绍了MID360与工业相机的接线。 12 | 如果相机/雷达型号特殊或者帮忙接线，可以旺旺联系我们。 13 | >2. 暂时不支持RTK/GPS，RTK/GPS版本要12月底发布，RTK会作为主时钟源，更新主机与传感器时间。 14 | >3. 很多小伙伴咨询同步精度，整体同步精度可以满足VIO，LO，VSLAM，LSLAM等绝大数多的算法应用，至于是否符合您的需求，需要您自己测试。 15 | >4. 全功能版本IP设置好之后，需要更改主机IP(同一网段)。然后更改demo中的IP，才能使用。 16 | 17 | ## 简介 18 | 多传感器的时间同步是一个很重要的问题，尤其对多传感器融合系统。不正确的时间同步会导致数据融合错误，影响系统的性能。 19 | 对于大多数研究人员来说，这是一个很底层又复杂的问题，却不是他们的研究方向。 20 | 更多的精力应该放在设计传感器融合算法上，而不是在时间同步上。因此，我们设计了一个这样一个系统，让时间同步不再是一件难事。 21 | 22 | 23 | ## 如何开始 24 | 25 | ### 开始之前 26 | 你依旧需要一些关于时间同步的基础知识，才能够更好的使用这个系统。 27 | 幸运的是，我们将必要的相关知识整理到了[这里](./assets/时间同步原理.md)。针对不同平台的用户提供基于ROS/ZMQ例程，也提供ARM/X86环境的支持。下表是我们支持的类型 28 | >| 设备类型 | 品牌 |同步方式 | 29 | >|-------------|-----------------------------|--------| 30 | >| 工业相机(网口) | 海康/大华/大恒/京航/... | PWM | 31 | >| 工业相机(USB) | 海康/大华/大恒/京航/... | PWM | 32 | >| 第三方IMU | Xsense全系列/... | PWM | 33 | >| 3D激光 | Mid360/Mid70/RoboSense系列/... | PPS | 34 | >| RTK/GPS | 暂无 | NMEA | 35 | >| 主机(ARM/X86) | Intel/AMD/Jetson/RockChip/... | PTP | 36 | ### 硬件准备 37 | 38 | 准备**全功能时间同步板**或**时间同步核心板**，核心板尺寸较小适合无人机搭载，**全功能时间同步板**与树莓派尺寸相同，安装孔位对应(端子型号GH1.25)。连接好需要同步的传感器和电源。硬件接口的定义如下： 39 | 40 | 全功能时间同步板： 41 | 42 | ![full](./assets/img2.png) 43 | 44 | 时间同步核心板： 45 | 46 | ![core](./assets/img3.png) 47 | 48 | 按照上图的定义，将传感器的同步信号连接到板子上的对应接口，然后连接电源。下面是一个同步线连接的参考示例： 49 | 50 | ### 烧入固件 51 | 在firmware文件夹下，有mini_xx.uf2文件和full_xx.uf2文件，分别对应最小功能固件和全功能固件。其中full表示全功能，V3表示第三代同步板。 52 | 53 | >| 固件类型 | 功能 | 支持硬件版本 | 54 | >|----------------------|---------------|----| 55 | >| flash_board_tool.uf2 | 配置工具(IP,PORT,HZ...) | V3 | 56 | >| full_xx.uf2 | 全功能同步板固件 | V3 | 57 | >| mini_xx.uf2 | 核心同步板固件 | V3 | 58 | 59 | 将对应uf2文件下载，将板子连接通过Type-C连接到电脑上，按住板上的BOOT按键，再按一次RESET按键，最后松开BOOT按键，会弹出一个虚拟U盘。 将uf2文件拖到板子的U盘中，等待固件烧入完成。 60 | 61 | 你可以参考[全功能同步板快速开始](./assets/全功能同步板快速开始.md)与[核心板快速开始](./assets/核心板快速开始.md)进行快速使用。 62 | 63 | ### 运行程序 64 | 65 | #### 权限设置 66 | 将Type-C线连接到Linux系统上，检测到串口设备后，设置串口权限，使用如下命令： 67 | 68 | ```shell 69 | sudo chmod 777 /dev/ttyACM0 70 | ``` 71 | 72 | #### ROS示例程序 73 | 74 | 如果相机设备是**海康**、**大恒**,**大华**等相机，无论是USB3.0接口或者网口接口，那么我们的SDK将自动检测相机数量然后读取图片信息并以ROS消息的格式发布出来。 75 | 如果是其他的相机系统，由于相机时间计算较为复杂，则需要参考高级功能中**自定义相机型号**，自行编写相机读取代码以及时间计算代码。 76 | 77 | 假设您的设备是**海康**、**大恒**,**大华**等相机，运行程序之前请接好设备并按如下操作运行程序。 78 | 79 | 克隆代码 80 | ```shell 81 | 82 | git clone https://github.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System.git 83 | 84 | ``` 85 | 86 | 编译代码, 如果是**全功能**时间同步板 87 | ```shell 88 | 89 | cd demo/udp_demo 90 | mkdir build 91 | cd build 92 | cmake .. 93 | make -j8 94 | 95 | ``` 96 | 97 | 如果是时间同步**核心板** 98 | ```shell 99 | cd demo/uart_demo 100 | mkdir build 101 | cd build 102 | cmake .. 103 | make -j8 104 | ``` 105 | 106 | 运行代码，如果是**全功能**时间同步板 107 | ```shell 108 | ./udp_demo_ros_node 109 | ``` 110 | 111 | 运行代码，如果是时间同步**核心板** 112 | ```shell 113 | ./uart_demo_ros_node 114 | ``` 115 | 116 | 正常运行则可以通过`rostopic list`的方法得到传感器信息，进一步的可以通过`rostopic echo /imu_sync_board`查看IMU数据 117 | 118 | ![full](./assets/d_img_7.png) 119 | 120 | #### ZMQ示例程序 121 | 122 | 与ROS程序相同，ZMQ示例程序提供了非ROS环境下数据发送方法。通过将图像消息与IMU消息进行序列化(ProtoBuf)后通过ZMQ发布。 123 | 124 | ```shell 125 | cd demo/zmq_demo 126 | mkdir build 127 | cd build 128 | cmake .. 129 | make -j8 130 | ``` 131 | 132 | 运行代码，与ROS相同，用户可以根据同步板类型使用不同的同步模式 133 | 134 | ```shell 135 | ./zmq_demo_zmq_node 136 | ``` 137 | 138 | ## 高级功能 139 | 140 | ### 网口通信同步 141 | 全功能时间同步板提供了一个百兆网口，能够更高效、更准确的进行时间同步。网口提供了更快的数据传输速度和更加稳定的通讯质量，在实现网口传输数据的同时，还额外加入精简版的[PTP对时协议](https://en.wikipedia.org/wiki/Precision_Time_Protocol)，从而实现了与主机时间同步，进而传感器数据采集时间与主机时间一致。如果要使用网口通信功能则需要按如下步骤进行。 142 | 143 | ### 参数配置 144 | 全功能时间同步板出厂的默认IP地址为192.168.1.188端口号为8888。为了方便用户自定义组网。可通过以下方式配置同步板的IP地址与端口。 145 | IP设置： 146 | 例如：设置网络IP号为192.168.192.188，使用Typc连接线连接同步板和电脑，安装任意版本的串口助手，串口助手发送以下数数据给同步板： 147 | ``` 148 | IP192.168.192.188\n 149 | ``` 150 | 其中`\n`是回车换行符，转换成16进制为0D 0A, 有的串口调试助手发送时自带换行，有的不带需要自己手动添加。 151 | 端口设置： 152 | 例如：设置网络端口号为8888，串口助手发送以下数数据： 153 | ``` 154 | IO8888\n 155 | ``` 156 | 时间同步核心板与全功能时间同步板都支持更改相机触发帧率。默认多相机之间与多相机与IMU的同步帧率为25HZ，自定义帧率可通过以下方式进行配置。目前最大同步帧率为400HZ。最大相机同步帧率与相机性能有关，过高的同步帧率会造成相机丢帧。日常使用中建议25HZ。 157 | 相机帧率设置： 158 | 例如：设置相机帧率为25HZ，串口助手发送以下数数据： 159 | ``` 160 | HZ25\n 161 | ``` 162 | 上述所有设置后，需要重新上电重启，或者按下Reset按键等待系统重启。 163 | 164 | PTP同步只有在全功能时间同步板中才会启用，启动方式使用UDP协议开启数据传输即可。在代码中可以按照以下方式打开： 165 | ```c++ 166 | // 开启数据传输和PTP同步 167 | auto udp_manager = std::make_shared("192.168.192.168", 8888); 168 | udp_manager->Start(); 169 | // 关闭数据传输和PTP同步 170 | udp_manager->Stop(); 171 | ``` 172 | ### 姿态解算 173 | 时间同步板搭载了最新推出的[ICM42688P]([./assets/相机购买指南.md](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/))，为了方便非紧耦合算法的使用，我们提供基于6轴IMU的航姿解算功能。具体实现可以参考`demo/udp_demo`中的实现。 174 | ```c++ 175 | void PublishIMUData(const ros::Publisher& pub, const ImuData& imudata) { 176 | FusionVector gyroscope = {imudata.gx, imudata.gy, imudata.gz}; 177 | FusionVector accelerometer = {imudata.ax, imudata.ay, imudata.az}; 178 | FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 0.0025f); 179 | FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs); 180 | 181 | sensor_msgs::Imu imu_msg_data; 182 | ... 183 | imu_msg_data.orientation.w = q.array[0]; 184 | imu_msg_data.orientation.x = q.array[1]; 185 | imu_msg_data.orientation.y = q.array[2]; 186 | imu_msg_data.orientation.z = q.array[3]; 187 | pub.publish(imu_msg_data); 188 | } 189 | ``` 190 | ![full](./assets/angle.png) 191 | ## 自定义相机型号 192 | 193 | 如果使用的相机型号不是指定厂商的，那么需要自己进行一定的编程，如果需要我们协助支持，请私信或者提出相关的issue。在`demo/CustomisedCamera`文件夹下有一个示例代码，可以参考这个示例代码进行开发。 194 | 这里简单介绍一下开发步骤： 195 | 196 | 1. 新建头文件和源文件，例如 `customised_camera.h` 和 `customised_camera.cpp`。 197 | 2. 在头文件中定义一个类，例如`CustCamManger`，定义如下的接口函数 198 | 199 | ```c++ 200 | #pragma once 201 | #include 202 | #include 203 | 204 | class CustCamManger { 205 | public: 206 | static CustCamManger& GetInstance() { 207 | static CustCamManger instance; 208 | return instance; 209 | } 210 | CustCamManger(const CustCamManger&) = delete; 211 | CustCamManger& operator=(const CustCamManger&) = delete; 212 | 213 | // 初始化相机函数 214 | bool Initialization(); 215 | // 停止相机读取线程函数 216 | void Stop(); 217 | // 开始相机读取线程函数 218 | void Start(); 219 | // 启用相机函数 220 | void Enable() { is_running_ = true; } 221 | // 禁用相机函数 222 | void Disable() { is_running_ = false; } 223 | // 相机线程，读取相机数据 224 | void Receive(const std::string&) const; 225 | 226 | private: 227 | 228 | CustCamManger(); 229 | ~CustCamManger(); 230 | // 相机线程向量，一个相机一个线程读取 231 | std::vector cam_threads_; 232 | // 是否正在运行标志位 233 | bool is_running_{false}; 234 | }; 235 | ``` 236 | 237 | 3. 在源文件中实现这些函数，其中关键点在于实现如下几个关键部分： 238 | 1. 初始化相机线程，一个相机一个线程 239 | 2. 结束相机线程，关闭相机 240 | 3. 相机读取线程函数，读取相机图像数据，曝光数据，计算时间戳，存入DataManger 241 | 242 | 如下是一个示例代码： 243 | ```c++ 244 | // 根据相机数量，构造对应数量的线程，读取相机 245 | void CustCamManger::Start() { 246 | // 假设有两个相机 247 | const std::string cam1 = "cam1"; 248 | const std::string cam2 = "cam2"; 249 | cam_threads_.emplace_back(&CustCamManger::Receive, this, cam1); 250 | cam_threads_.emplace_back(&CustCamManger::Receive, this, cam2); 251 | Enable(); 252 | } 253 | 254 | void CustCamManger::Stop() { 255 | // 1. 关闭相机线程 256 | Disable(); 257 | std::this_thread::sleep_for(std::chrono::milliseconds{500}); 258 | for (auto &cam_thread : cam_threads_) { 259 | while (cam_thread.joinable()) { 260 | cam_thread.join(); 261 | } 262 | } 263 | cam_threads_.clear(); 264 | cam_threads_.shrink_to_fit(); 265 | // 2. 关闭相机, 自定义相机关闭 266 | } 267 | 268 | // 相机读取线程函数 269 | void CustCamManger::Receive(const std::string &name) const { 270 | ImgData img_data; 271 | while (is_running_) { 272 | // 1. 自定义相机获取图像数据 273 | // 2. 将图像数据存入img_data TODO 274 | // 3. 自定义相机获取曝光时间 275 | float expose_time_us = 0; 276 | // 4. 将曝光时间存入img_data t = t_trigger + t_expose / 2 277 | img_data.time_stamp_us = DataManger::GetInstance().GetLastTiggerTime() + static_cast(expose_time_us / 2.); 278 | // 5. 设置相机名字 279 | img_data.camera_name = name; 280 | // 6. 将img_data存入DataManger 281 | DataManger::GetInstance().AddCamData(name, img_data); 282 | std::this_thread::sleep_for(std::chrono::milliseconds{10}); 283 | } 284 | } 285 | ``` 286 | 如果您手中没有相机，需要购买相机或者镜头，以及搭建自己的多相机模组，我们这里有恰好有一些建议：[这里](./assets/相机购买指南.md)。 287 | 如果您手中没有雷达，需要雷达，以及搭建自己的基于雷达的多传感器融合模组，我们这里建议选择支持PPS秒脉冲同步功能的雷达设备。 288 | 289 | # 购买与咨询 290 | [【淘宝】Access denied MF3543 「多相机IMU同步板网口串口同步工业相机六轴姿态」 291 | 点击链接直接打开 或者 淘宝搜索直接打开](https://item.taobao.com/item.htm?ft=t&id=832624497202) 292 | 293 | 294 | [//]: # (![photo](./assets/img4.png)) 295 | 296 | 297 | -------------------------------------------------------------------------------- /assets/angle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/angle.png -------------------------------------------------------------------------------- /assets/d_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_1.png -------------------------------------------------------------------------------- /assets/d_img_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_2.png -------------------------------------------------------------------------------- /assets/d_img_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_3.png -------------------------------------------------------------------------------- /assets/d_img_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_4.png -------------------------------------------------------------------------------- /assets/d_img_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_5.png -------------------------------------------------------------------------------- /assets/d_img_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_6.png -------------------------------------------------------------------------------- /assets/d_img_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/d_img_7.png -------------------------------------------------------------------------------- /assets/full_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/full_1.png -------------------------------------------------------------------------------- /assets/full_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/full_2.png -------------------------------------------------------------------------------- /assets/img1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img1.png -------------------------------------------------------------------------------- /assets/img2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img2.png -------------------------------------------------------------------------------- /assets/img3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img3.png -------------------------------------------------------------------------------- /assets/img4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img4.png -------------------------------------------------------------------------------- /assets/img5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img5.png -------------------------------------------------------------------------------- /assets/img6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img6.png -------------------------------------------------------------------------------- /assets/img7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img7.png -------------------------------------------------------------------------------- /assets/img8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/img8.png -------------------------------------------------------------------------------- /assets/serial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/assets/serial.png -------------------------------------------------------------------------------- /assets/全功能同步板快速开始.md: -------------------------------------------------------------------------------- 1 | ## 1. 更新固件 2 | 3 | 1. 接好Type-C线，在Linux下的串口设备名称为 /dev/ttyACM0。 如果没有，请更换Type-C线，部分Type-C线材是不支持的。 4 | 2. 长按同步板中间的RESET号按键不松开，然后短按一下BOOT号按键之后松开，会弹出一个虚拟U盘。 5 | 3. 下载 firmware 目录下的 full_v3_xx_xx_xx_release.uf2 固件。将固件拖入U盘中，U盘自动消失，多出来一个串口设备即为成功。固件是最新发布的固件，按日期使用最新的。 6 | 7 | ![按键说明](./full_1.png) 8 | 9 | ## 2. 设置同步频率 10 | 11 | 1. 更新固件为 full_v3_xx_xx_xx_release.uf2 固件 12 | 2. 连接串口助手，这里以win10自带的串口助手为例，配置参数为波特率 460800，数据位8， 校验位None，停止位1 13 | 14 | ![串口参数](./serial.png) 15 | 16 | 3. 发送触发频率指令 `HZx\n`,其中x在1-60之间。注意在后面是有一个回车的，这里以发送`HZ10`为例,发送完成后重启，如果设置成功会显示当前设置的值与"commit ok"。 17 | 18 | ![串口参数](./img6.png) 19 | 20 | 4. 发送IP更改指令,例如更改IP为 `IP192.168.1.168\n` 如果设置成功会显示当前设置的值与"commit ok"，更改完成后重新对同步板上电。*该IP地址与主机地址必须在同一个网段下*，例如，同步板设置的IP地址为192.168.1.168，则主机必须要设置的IP地址为192.168.1.x。启动前确认可以通过ping功能测试连接。注意在后面是有一个回车的，发送完成后重启。如果不能确认是否修改成功。可以打开串口后重新启动同步板，串口会打印当前的相关配置。 21 | 22 | 6. 发送端口号更改指令 `IO8888\n` 即更改端口号为8888，如果设置成功会显示当前设置的值与"commit ok"。结合IP更改指令与端口号更改指令，可以方便的更改全功能板的网络地址。发送完成后重启。 23 | 24 | ## 3. 如何接线 25 | ![接口定义](./full_2.png) 26 | 27 | 1. 以工业相机为例。相机只需要四根线，一个触发线CAM、一个GND，一个12V相机电源，一个相机电源地，在这上面图片中已经非常清晰。 28 | 2. 雷达接线通常需要三根线，分别是同步版上面的GND,TX, IO。这里以常用的MID360为例展示接线。 29 | 30 | ## 4. 运行同步程序 31 | 32 | 1. 设置串口权限 33 | 2. 在终端运行`roscore` 34 | 2. 运行`demo`目录下的`udp_demo` 35 | 36 | ``` 37 | cd demo/udp_demo 38 | mkdir build 39 | cd build 40 | cmake .. 41 | make -j8 42 | ./udp_demo_ros_node 43 | ``` 44 | 45 | 3. 在另一个终端中观察IMU数据 46 | 47 | ``` 48 | 49 | rostopic list 50 | rostopic echo /imu_sync_board 51 | 52 | ``` 53 | 54 | 看到不断更新的IMU数据即为正常 55 | 56 | ## 5. 同步相机、雷达 57 | 58 | 当第四步完成之后，同步版将以1hz频率发送雷达同步信号，在雷达软件中可以看到同步效果。这里以MID360为例。 59 | 60 | ![雷达同步](./img8.png) 61 | 62 | 63 | 相机的同步的测试可以回到参数设置，将相机同步频率设置为1，然后在相机软件中观察多个相机的采集频率是否为1Hz,并且观察多个相机采集的时刻是否为同一个时刻。确保没问题后，再将参数修改会目标值，正常使用。 64 | 65 | -------------------------------------------------------------------------------- /assets/时间同步原理.md: -------------------------------------------------------------------------------- 1 | ## 时间同步原理 2 | 以工控机的时间为基准，对雷达、IMU和相机时间戳进行精确的对齐。这三种传感器的时间定义方式各不相同，因此首先我们要知道传感器采集时间定义。其次，我们针对它们的时间定义，给出了一套完整的时间同步方案，能够达到us级的同步精度。 3 | ### 传感器时间定义 4 | #### 相机 5 | 相机采集图像分为三个过程，其中第一步是采集触发，第二步是曝光，第三步是将模拟图像信号转换为数字信号。在上升沿的瞬间，相机打开快门，开始采集图像，在曝光时间内采集这个时间内的所有光信号，然后最后将光信号转换为数字信号。 6 | 7 | ![相机采集图像过程](./d_img_1.png) 8 | 9 | 图像采集得到的信息是开始曝光到结束曝光这段时间内的，因此我们将图像采集的时间定义为触发时间加上一半的曝光时间 10 | 11 | $$ 12 | t_{cam}=t_{trigger}+\frac{t_{exposure}}{2} 13 | $$ 14 | 15 | 其中 $t_{cam}$ 是定义的相机时间， $t_{trigger}$ 是触发信号的时间， $t_{exposure}$ 是曝光时间。而如果采用USB相机读取图像的时刻作为图像时间，则和真实的采集时刻有一个较大的偏差。 16 | 17 | #### IMU 18 | 对于VIO系统和LIO系统来说，IMU数据的频率应该越高越好例如1000Hz或者2000Hz，并且尽量不使用低通滤波器，或者配置尽量高频率的低通滤波器。然而实际使用时并不需要那么高的频率，大多数导航系统能够处理的IMU频率在200-400Hz左右。因此，通常会在IMU内部设置一个降采样的功能，自动插值得到对应频率的IMU信号，并通过引脚状态反馈。 19 | 20 | ![IMU时间定义](./d_img_5.png) 21 | 22 | 如上图所示，是典型的MEMS的IMU传感器工作流程，IMU系统内部包含晶振时间系统，以2000Hz的频率进行内部数据采集，然后将其下采样到400Hz，进行自动的差值得到低频率的传感器数据。在IMU内部的数据准备好了之后，IMU的一个引脚被短时间拉高，被采集用于获得IMU的时间。所以IMU的传感器数据时间被定义为IMU特定引脚上升沿的时刻。 23 | 24 | #### 雷达 25 | 雷达内部包含了自动的激光采集时间计算方式，即激光束的发射时间和接收时间的平均值。因此，我们只需要将工控机的时间戳发送给雷达即可，一般有如下两种方式 26 | 1. PPS+GPRMC，模拟GNSS信息给雷达发送当前时刻的信号（优势，精度高，需要额外的硬件实现） 27 | 2. PTP协议，利用网络通信中的时间同步机制进行同步（优势，实现简单不需要任何额外设备） 28 | 29 | ![雷达时间定义](./d_img_2.png) 30 | 31 | PPS+GPRMC的方式，利用一个IO口的上升沿标志当前时刻的，然后用串口发送对应时刻的时间戳信息，从而实现雷达系统和外部的时间同步。 32 | 33 | ### 传感器的时间同步逻辑 34 | 为了实现整体系统的时间同步，我们设计了如下的时间同步方式。它以上位机主控的时间戳为基准，首先和时间同步版内部时钟保持同步，然后时间同步板基于此时间获取IMU采集时间、获取相机曝光触发时间并向激光雷达发送当前时刻，然后在上位机读取的过程中获得的IMU时间和雷达时间均在同一个时间源下，而相机的采集时间则需要通过获得当前帧的曝光时间，并与相机触发时间结合求得。 35 | 36 | ![时间同步逻辑](./d_img_3.png) 37 | 38 | ### 时间同步板硬件设计 39 | 为了实现相机、IMU和雷达之间的同步，我们设计了如下硬件结构，包括一个内置的IMU，通过特定引脚电平和数据传输接口获取其数据和时间戳，包括一个PPS信号和一个GPRMC信号接口，用于模拟GNSS给雷达发送信号，以及一个USB2.0的接口连接上位机，同时实现系统供电和模拟串口通信。 40 | 41 | ![时间同步逻辑](./d_img_4.png) 42 | 43 | 1. IMU数据同步的实现 44 | 上位机通过USB2.0模拟串口以低频率发送系统时间后，反馈得到的IMU数据即为时间同步后的IMU数据 45 | 2. 雷达数据同步的实现 46 | 如果雷达支持PPS+GPRMC协议的时间同步，则将时间同步版的GPS信号与PPS相连，TX与雷达的RX连接，即可对雷达设备进行时间同步。 47 | 如果雷达支持PTP协议，则只需要在PC上开启PTP网络同步授时即可。 48 | 3. 相机时间同步 49 | 1. 从USB2.0反馈中获得上一次相机的触发时刻 50 | 2. 从相机中采集图像，并获得相机曝光时间 51 | 3. 计算相机时间戳为， $t_{cam} = t_{trigger} + \frac{t_{exposure}}{2}$ 52 | 53 | -------------------------------------------------------------------------------- /assets/核心板快速开始.md: -------------------------------------------------------------------------------- 1 | ## 1. 更新固件 2 | 3 | 1. 接好Type-C线，电脑将弹出一个USB串口设备或者一个128M的小U盘，在Linux下的串口设备名称为 /dev/ttyACM0。 如果没有这一现象，请更换Type-C线，部分Type-C线材是不支持的，最好使用USB2.0的线。 4 | 2. 长按图中所示的1号按键不松开，然后短按一下2号按键之后松开，会弹出一个128M的U盘。 5 | 3. 下载 firmware 目录下的 mini_xxxx.uf2 或者 flash_board_tool.uf2 固件。将固件拖入U盘中，则U盘消失，多出来一个串口设备即为成功。其中 flash_board_tool.uf2 是用于后面设置参数的固件，mini_xxxx.uf2固件是最新发布的固件，按日期使用最新的。 6 | 7 | ![按键说明](./img5.png) 8 | 9 | ## 2. 设置同步频率 10 | 11 | 1. 按照步骤1更新固件为mini_flash.uf2 固件 12 | 2. 连接串口助手，这里以win10自带的串口助手为例，配置参数为波特率 460800，数据位8， 校验位None，停止位1 13 | 14 | ![串口参数](./serial.png) 15 | 16 | 3. 发送触发频率指令 `HZx\n`,其中x在1-60之间。注意在后面是有一个回车的，这里以发送`HZ10`为例 17 | 18 | ![串口参数](./img6.png) 19 | 20 | 4. 重启，按照步骤1刷入最新固件。 21 | 22 | ## 3. 如何接线 23 | 24 | 1. 相机只需要两根线，一个触发线CAM、一个GND，在这下面图片中已经非常清晰。其中由于核心板只能提供3.3V电源，因此相机需要额外设备单独供电。 25 | 26 | ![接口定义](./img3.png) 27 | 28 | 2. 雷达接线通常需要三根线，分别是同步版上面的GND,TX, IO。这里以常用的MID360为例展示接线，与相机相似，Mid360需要额外供电 29 | 30 | ![接口定义](./img7.png) 31 | 32 | 33 | ## 4. 运行同步程序 34 | 35 | 1. 在linux系统上设置串口权限 36 | 2. 在终端运行ros，确保ros安装正确 `roscore` 37 | 2. 运行`demo`目录下的`uart_demo` 38 | 39 | ``` 40 | cd xxx/uart_demo 41 | mkdir build 42 | cd build 43 | cmake .. 44 | make -j8 45 | ./uart_demo_ros_node 46 | ``` 47 | 48 | 3. 在另一个终端中观察IMU数据 49 | 50 | ``` 51 | rostopic list 52 | rostopic echo /imu_sync_board 53 | ``` 54 | 55 | 看到不断更新的IMU数据即为正常 56 | 57 | ## 5. 同步相机、雷达 58 | 59 | 当第四步完成之后，同步版将以1hz频率发送雷达同步信号，在雷达软件中可以看到同步效果。这里以MID360为例。 60 | 61 | ![雷达同步](./img8.png) 62 | 63 | 64 | 相机的同步的测试可以回到参数设置，将相机同步频率设置为1，然后在相机软件中观察多个相机的采集频率是否为1Hz,并且观察多个相机采集的时刻是否为同一个时刻。确保没问题后，再将参数修改会目标值，正常使用。 65 | 66 | -------------------------------------------------------------------------------- /assets/相机购买指南.md: -------------------------------------------------------------------------------- 1 | ## 相机购买指南 2 | 3 | ### 相机基本要求 4 | 5 | ### 相机配置与设置 6 | 7 | ### 相机型号推荐 8 | 9 | ### 镜头与镜框 -------------------------------------------------------------------------------- /data/full.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/data/full.stl -------------------------------------------------------------------------------- /data/mini.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/data/mini.stl -------------------------------------------------------------------------------- /demo/customised_camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(CustomisedCamera) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE Release) 8 | endif () 9 | set(CMAKE_CXX_FLAGS_RELEASE -Ofast) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 11 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 12 | 13 | set(SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../sdk/infinite_sense_core/x86) 14 | 15 | find_package(catkin QUIET) 16 | 17 | if (catkin_FOUND) 18 | message(STATUS "ROS found, Compiling ROS nodes") 19 | find_package(catkin REQUIRED COMPONENTS 20 | roscpp 21 | sensor_msgs 22 | cv_bridge 23 | image_transport 24 | geometry_msgs 25 | ) 26 | file(GLOB_RECURSE SOURCE_FILES 27 | main_ros.cpp 28 | customised_camera.cpp 29 | ) 30 | add_executable(${PROJECT_NAME}_ros_node ${SOURCE_FILES}) 31 | target_link_directories(${PROJECT_NAME}_ros_node PRIVATE 32 | ${SDK_PATH}/lib 33 | ) 34 | target_link_libraries(${PROJECT_NAME}_ros_node 35 | ${catkin_LIBRARIES} 36 | infinite_sense_core 37 | ) 38 | target_include_directories(${PROJECT_NAME}_ros_node PRIVATE 39 | ${catkin_INCLUDE_DIRS} 40 | ${SDK_PATH}/include 41 | ) 42 | target_compile_definitions(${PROJECT_NAME}_ros_node PUBLIC -DROS_PLATFORM) 43 | endif () 44 | 45 | -------------------------------------------------------------------------------- /demo/customised_camera/ReadMe.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/demo/customised_camera/ReadMe.md -------------------------------------------------------------------------------- /demo/customised_camera/customised_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "customised_camera.h" 2 | #include 3 | #include "data_manager.h" 4 | 5 | CustCamManger::CustCamManger() = default; 6 | 7 | CustCamManger::~CustCamManger() { Stop(); } 8 | 9 | bool CustCamManger::Initialization() { 10 | // 自定义相机初始化 11 | return true; 12 | } 13 | 14 | // 根据相机数量，构造对应数量的线程，读取相机 15 | void CustCamManger::Start() { 16 | // 假设有两个相机 17 | const std::string cam_1 = "cam1"; 18 | const std::string cam_2 = "cam2"; 19 | cam_threads_.emplace_back(&CustCamManger::Receive, this, cam_1); 20 | cam_threads_.emplace_back(&CustCamManger::Receive, this, cam_2); 21 | Enable(); 22 | } 23 | 24 | void CustCamManger::Stop() { 25 | // 1. 关闭相机线程 26 | Disable(); 27 | std::this_thread::sleep_for(std::chrono::milliseconds{500}); 28 | for (auto &cam_thread : cam_threads_) { 29 | while (cam_thread.joinable()) { 30 | cam_thread.join(); 31 | } 32 | } 33 | cam_threads_.clear(); 34 | cam_threads_.shrink_to_fit(); 35 | // 2. 关闭相机, 自定义相机关闭 36 | } 37 | 38 | void CustCamManger::Receive(const std::string &name) const { 39 | ImgData img_data; 40 | while (is_running_) { 41 | // 1. 自定义相机获取图像数据 42 | // 2. 将图像数据存入img_data 43 | // 3. 自定义相机获取曝光时间 44 | float expose_time_us = 0; 45 | // 4. 将曝光时间存入img_data t = t_trigger + t_expose / 2 46 | img_data.time_stamp_us = DataManger::GetInstance().GetLastTiggerTime() + static_cast(expose_time_us / 2.); 47 | // 5. 设置相机名字 48 | img_data.camera_name = name; 49 | // img_data.image 是要存储的图像 50 | // 6. 将img_data存入DataManger 51 | DataManger::GetInstance().AddCamData(name, img_data); 52 | std::this_thread::sleep_for(std::chrono::milliseconds{5}); 53 | } 54 | } 55 | 56 | -------------------------------------------------------------------------------- /demo/customised_camera/customised_camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | class CustCamManger { 6 | public: 7 | static CustCamManger& GetInstance() { 8 | static CustCamManger instance; 9 | return instance; 10 | } 11 | CustCamManger(const CustCamManger&) = delete; 12 | CustCamManger& operator=(const CustCamManger&) = delete; 13 | 14 | // 初始化相机函数 15 | bool Initialization(); 16 | // 停止相机读取线程函数 17 | void Stop(); 18 | // 开始相机读取线程函数 19 | void Start(); 20 | // 启用相机函数 21 | void Enable() { is_running_ = true; } 22 | // 禁用相机函数 23 | void Disable() { is_running_ = false; } 24 | // 相机线程，读取相机数据 25 | void Receive(const std::string&) const; 26 | 27 | private: 28 | 29 | CustCamManger(); 30 | ~CustCamManger(); 31 | // 相机线程向量，一个相机一个线程读取 32 | std::vector cam_threads_; 33 | // 是否正在运行标志位 34 | bool is_running_{false}; 35 | }; 36 | -------------------------------------------------------------------------------- /demo/customised_camera/main_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "customised_camera.h" 7 | #include "data_manager.h" 8 | 9 | inline ros::Time CreateRosTimestamp(const uint64_t timestamp_micoseconds) { 10 | static constexpr uint32_t kNanosecondsPerSecond = 1e9; 11 | const auto kTimestampU64 = timestamp_micoseconds * 1000; 12 | const uint32_t kTimestampSec = kTimestampU64 / kNanosecondsPerSecond; 13 | const uint32_t kRosTimestampNsec = kTimestampU64 - (kTimestampSec * kNanosecondsPerSecond); 14 | return {kTimestampSec, kRosTimestampNsec}; 15 | } 16 | 17 | void PublishIMUData(const ros::Publisher& pub, const ImuData& imudata) { 18 | sensor_msgs::Imu imu_msg_data; 19 | imu_msg_data.header.frame_id = "/base_imu_link"; 20 | imu_msg_data.header.stamp = CreateRosTimestamp(imudata.time_stamp_us); 21 | 22 | imu_msg_data.angular_velocity.x = imudata.gx; 23 | imu_msg_data.angular_velocity.y = imudata.gy; 24 | imu_msg_data.angular_velocity.z = imudata.gz; 25 | 26 | imu_msg_data.linear_acceleration.x = imudata.ax; 27 | imu_msg_data.linear_acceleration.y = imudata.ay; 28 | imu_msg_data.linear_acceleration.z = imudata.az; 29 | 30 | pub.publish(imu_msg_data); 31 | } 32 | 33 | int main(int argc, char** argv) { 34 | // 等待5秒 35 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 36 | // ROS 初始化 37 | ros::init(argc, argv, "CIS",ros::init_options::NoSigintHandler); 38 | ros::NodeHandle node; 39 | 40 | // IMU数据发布 41 | ros::Publisher imu_pub = node.advertise("/imu_sync_board", 1000); 42 | 43 | // 相机线程初始化 44 | CustCamManger::GetInstance().Initialization(); 45 | CustCamManger::GetInstance().Start(); 46 | 47 | // 等待相机启动 48 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 49 | 50 | // 遍历所有相机，新建发布者 51 | image_transport::ImageTransport it(node); 52 | std::vector all_cam_names; 53 | DataManger::GetInstance().GetAllCamNames(all_cam_names); 54 | std::cout << "Number of cameras detected " << all_cam_names.size() << std::endl; 55 | std::map pub_list; 56 | for (auto& cam : all_cam_names) { 57 | pub_list[cam] = it.advertise(cam, 30); 58 | } 59 | 60 | // ROS循环发布传感器数据 61 | ros::Rate loop_rate(1000); 62 | ImuData imudata{}; 63 | 64 | while (node.ok()) { 65 | // 获取IMU数据 66 | while (DataManger::GetInstance().GetNewImuData(imudata)) { 67 | // 发布IMU数据 68 | PublishIMUData(imu_pub, imudata); 69 | } 70 | // 遍历所有相机，发布图像数据 71 | ImgData img_data{}; 72 | for (auto& cam : all_cam_names) { 73 | // 获取图像数据 74 | if (DataManger::GetInstance().GetNewCamData(cam, img_data)) { 75 | // 发布图像数据 76 | sensor_msgs::ImagePtr msg = 77 | cv_bridge::CvImage(std_msgs::Header(), "mono8", img_data.image.clone()).toImageMsg(); 78 | msg->header.stamp = CreateRosTimestamp(img_data.time_stamp_us); 79 | pub_list[cam].publish(msg); 80 | } 81 | } 82 | loop_rate.sleep(); 83 | } 84 | // 释放资源 85 | // serial_manager->Stop(); 86 | CustCamManger::GetInstance().Stop(); 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /demo/uart_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(uart_demo) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE Release) 8 | endif () 9 | set(CMAKE_CXX_FLAGS_RELEASE -Ofast) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 11 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 12 | 13 | set(SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../sdk/infinite_sense_core/x86) 14 | 15 | find_package(catkin QUIET) 16 | if (catkin_FOUND) 17 | message(STATUS "ROS found, Compiling ROS nodes") 18 | find_package(catkin REQUIRED COMPONENTS 19 | roscpp 20 | sensor_msgs 21 | cv_bridge 22 | image_transport 23 | geometry_msgs 24 | ) 25 | file(GLOB_RECURSE SOURCE_FILES 26 | main_ros.cpp 27 | ) 28 | add_executable(${PROJECT_NAME}_ros_node ${SOURCE_FILES}) 29 | target_link_directories(${PROJECT_NAME}_ros_node PRIVATE 30 | ${SDK_PATH}/lib 31 | ) 32 | target_link_libraries(${PROJECT_NAME}_ros_node 33 | infinite_sense_core 34 | ${catkin_LIBRARIES} 35 | ) 36 | target_include_directories(${PROJECT_NAME}_ros_node PRIVATE 37 | ${catkin_INCLUDE_DIRS} 38 | ${SDK_PATH}/include 39 | ) 40 | target_compile_definitions(${PROJECT_NAME}_ros_node PUBLIC -DROS_PLATFORM) 41 | endif () 42 | 43 | -------------------------------------------------------------------------------- /demo/uart_demo/ReadMe.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/demo/uart_demo/ReadMe.md -------------------------------------------------------------------------------- /demo/uart_demo/main_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "cam_manager.h" 7 | #include "serial_manager.h" 8 | #include "data_manager.h" 9 | 10 | inline ros::Time CreateRosTimestamp(const uint64_t timestamp_micoseconds) { 11 | static constexpr uint32_t kNanosecondsPerSecond = 1e9; 12 | const auto kTimestampU64 = timestamp_micoseconds * 1000; 13 | const uint32_t kTimestampSec = kTimestampU64 / kNanosecondsPerSecond; 14 | const uint32_t kRosTimestampNsec = kTimestampU64 - (kTimestampSec * kNanosecondsPerSecond); 15 | return {kTimestampSec, kRosTimestampNsec}; 16 | } 17 | 18 | void PublishIMUData(const ros::Publisher& pub, const ImuData& imudata) { 19 | sensor_msgs::Imu imu_msg_data; 20 | imu_msg_data.header.frame_id = "/base_imu_link"; 21 | imu_msg_data.header.stamp = CreateRosTimestamp(imudata.time_stamp_us); 22 | 23 | imu_msg_data.angular_velocity.x = imudata.gx; 24 | imu_msg_data.angular_velocity.y = imudata.gy; 25 | imu_msg_data.angular_velocity.z = imudata.gz; 26 | 27 | imu_msg_data.linear_acceleration.x = imudata.ax; 28 | imu_msg_data.linear_acceleration.y = imudata.ay; 29 | imu_msg_data.linear_acceleration.z = imudata.az; 30 | 31 | pub.publish(imu_msg_data); 32 | } 33 | void SigIntHandler(int sig) { 34 | ros::shutdown(); // 让ROS节点安全退出 35 | } 36 | int main(int argc, char** argv) { 37 | // Wait 5 seconds for the camera to start up. 38 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 39 | // ROS 初始化 40 | ros::init(argc, argv, "CIS",ros::init_options::NoSigintHandler); 41 | ros::NodeHandle node; 42 | // 串口线程初始化 43 | auto serial_manager =std::make_shared("/dev/ttyACM0"); 44 | serial_manager->Start(); 45 | 46 | // IMU数据发布 47 | ros::Publisher imu_pub = node.advertise("/imu_sync_board", 1000); 48 | 49 | // 相机线程初始化 50 | CamManger::GetInstance().Initialization(); 51 | CamManger::GetInstance().Start(); 52 | 53 | // 等待相机启动 54 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 55 | 56 | // 遍历所有相机，新建发布者 57 | image_transport::ImageTransport it(node); 58 | std::vector all_cam_names; 59 | DataManger::GetInstance().GetAllCamNames(all_cam_names); 60 | std::cout << "Number of cameras detected " << all_cam_names.size() << std::endl; 61 | std::map pub_list; 62 | for (auto& cam : all_cam_names) { 63 | pub_list[cam] = it.advertise(cam, 30); 64 | } 65 | 66 | // ROS循环发布传感器数据 67 | ros::Rate loop_rate(1000); 68 | ImuData imudata{}; 69 | 70 | while (node.ok()) { 71 | // 获取IMU数据 72 | while (DataManger::GetInstance().GetNewImuData(imudata)) { 73 | // 发布IMU数据 74 | PublishIMUData(imu_pub, imudata); 75 | } 76 | // 遍历所有相机，发布图像数据 77 | ImgData img_data{}; 78 | for (auto& cam : all_cam_names) { 79 | // 获取图像数据 80 | while (DataManger::GetInstance().GetNewCamData(cam, img_data)) { 81 | // 发布图像数据 82 | sensor_msgs::ImagePtr msg = 83 | cv_bridge::CvImage(std_msgs::Header(), "mono8", img_data.image.clone()).toImageMsg(); 84 | msg->header.stamp = CreateRosTimestamp(img_data.time_stamp_us); 85 | pub_list[cam].publish(msg); 86 | } 87 | } 88 | loop_rate.sleep(); 89 | } 90 | // 释放资源 91 | serial_manager->Stop(); 92 | CamManger::GetInstance().Stop(); 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /demo/udp_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(udp_demo) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE Release) 8 | endif () 9 | 10 | set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 11 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 12 | 13 | set(SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../sdk/infinite_sense_core/x86) 14 | 15 | find_package(catkin QUIET) 16 | if (catkin_FOUND) 17 | message(STATUS "ROS found, Compiling ROS nodes") 18 | find_package(catkin REQUIRED COMPONENTS 19 | roscpp 20 | sensor_msgs 21 | cv_bridge 22 | image_transport 23 | geometry_msgs 24 | ) 25 | file(GLOB_RECURSE SOURCE_FILES 26 | main_ros.cpp 27 | ) 28 | add_executable(${PROJECT_NAME}_ros_node ${SOURCE_FILES}) 29 | target_link_directories(${PROJECT_NAME}_ros_node PRIVATE 30 | BEFORE ${SDK_PATH}/lib 31 | ) 32 | target_link_libraries(${PROJECT_NAME}_ros_node 33 | MvCameraControl 34 | fusion 35 | infinite_sense_core 36 | ${catkin_LIBRARIES} 37 | 38 | ) 39 | target_include_directories(${PROJECT_NAME}_ros_node PRIVATE 40 | ${catkin_INCLUDE_DIRS} 41 | ${SDK_PATH}/include 42 | ) 43 | target_compile_definitions(${PROJECT_NAME}_ros_node PUBLIC -DROS_PLATFORM) 44 | endif () 45 | 46 | -------------------------------------------------------------------------------- /demo/udp_demo/ReadMe.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/demo/udp_demo/ReadMe.md -------------------------------------------------------------------------------- /demo/udp_demo/main_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "udp_manager.h" 7 | #include "cam_manager.h" 8 | #include "data_manager.h" 9 | #include "Fusion.h" 10 | 11 | FusionAhrs ahrs; 12 | 13 | inline ros::Time CreateRosTimestamp(const uint64_t timestamp_micoseconds) { 14 | static constexpr uint32_t kNanosecondsPerSecond = 1e9; 15 | const auto kTimestampU64 = timestamp_micoseconds * 1000; 16 | const uint32_t kTimestampSec = kTimestampU64 / kNanosecondsPerSecond; 17 | const uint32_t kRosTimestampNsec = kTimestampU64 - (kTimestampSec * kNanosecondsPerSecond); 18 | return {kTimestampSec, kRosTimestampNsec}; 19 | } 20 | 21 | void PublishIMUData(const ros::Publisher& pub, const ImuData& imudata) { 22 | 23 | FusionVector gyroscope = {imudata.gx, imudata.gy, imudata.gz}; 24 | FusionVector accelerometer = {imudata.ax, imudata.ay, imudata.az}; 25 | FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 0.0025f); 26 | FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs); 27 | 28 | sensor_msgs::Imu imu_msg_data; 29 | imu_msg_data.header.frame_id = "/base_imu_link"; 30 | imu_msg_data.header.stamp = CreateRosTimestamp(imudata.time_stamp_us); 31 | 32 | imu_msg_data.angular_velocity.x = imudata.gx; 33 | imu_msg_data.angular_velocity.y = imudata.gy; 34 | imu_msg_data.angular_velocity.z = imudata.gz; 35 | 36 | imu_msg_data.linear_acceleration.x = imudata.ax; 37 | imu_msg_data.linear_acceleration.y = imudata.ay; 38 | imu_msg_data.linear_acceleration.z = imudata.az; 39 | 40 | imu_msg_data.orientation.w = q.array[0]; 41 | imu_msg_data.orientation.x = q.array[1]; 42 | imu_msg_data.orientation.y = q.array[2]; 43 | imu_msg_data.orientation.z = q.array[3]; 44 | 45 | pub.publish(imu_msg_data); 46 | } 47 | void SigIntHandler(int sig) { 48 | ros::shutdown(); // 让ROS节点安全退出 49 | } 50 | int main(int argc, char** argv) { 51 | ros::init(argc, argv, "CIS",ros::init_options::NoSigintHandler); 52 | ros::NodeHandle node; 53 | FusionAhrsInitialise(&ahrs); 54 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 55 | /* 56 | 本机IP地址应该在192.168.1.X网段下。 57 | 192.168.1.168是同步板的IP地址 58 | The host IP address should be in the 192.168.1.X network segment. 59 | 192.168.1.168 is the IP address of the synchronization board. 60 | */ 61 | auto udp_manager = std::make_shared("192.168.1.168", 8888); 62 | udp_manager->Start(); 63 | 64 | ros::Publisher imu_pub = node.advertise("/imu_sync_board", 1000); 65 | CamManger::GetInstance().Initialization(); 66 | CamManger::GetInstance().Start(); 67 | std::this_thread::sleep_for(std::chrono::milliseconds(5000)); 68 | image_transport::ImageTransport it(node); 69 | std::vector all_cam_names; 70 | DataManger::GetInstance().GetAllCamNames(all_cam_names); 71 | std::cout << "Number of cameras detected " << all_cam_names.size() << std::endl; 72 | std::map pub_list; 73 | for (auto& cam : all_cam_names) { 74 | pub_list[cam] = it.advertise(cam, 30); 75 | } 76 | ros::Rate loop_rate(1000); 77 | ImuData imudata{}; 78 | while (node.ok()) { 79 | while (DataManger::GetInstance().GetNewImuData(imudata)) { 80 | PublishIMUData(imu_pub, imudata); 81 | } 82 | ImgData img_data{}; 83 | for (auto& cam : all_cam_names) { 84 | while (DataManger::GetInstance().GetNewCamData(cam, img_data)) { 85 | sensor_msgs::ImagePtr msg = 86 | cv_bridge::CvImage(std_msgs::Header(), "mono8", img_data.image.clone()).toImageMsg(); 87 | msg->header.stamp = CreateRosTimestamp(img_data.time_stamp_us); 88 | pub_list[cam].publish(msg); 89 | } 90 | } 91 | ros::spinOnce(); // 确保处理ROS回调 92 | loop_rate.sleep(); 93 | } 94 | udp_manager->Stop(); 95 | CamManger::GetInstance().Stop(); 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /demo/zmq_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(zmq_demo) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE Release) 8 | endif () 9 | 10 | set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 11 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") 12 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake CACHE INTERNAL "" FORCE) 13 | 14 | 15 | set(SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../sdk/infinite_sense_core/x86) 16 | find_package(OpenCV REQUIRED) 17 | find_package(ZeroMQ QUIET) 18 | if (ZeroMQ_FOUND) 19 | message(STATUS "ZMQ found") 20 | add_subdirectory(proto_msg) 21 | find_package(ZeroMQ REQUIRED) 22 | add_executable(${PROJECT_NAME}_zmq_node 23 | main_zmq.cpp 24 | ) 25 | target_link_directories(${PROJECT_NAME}_zmq_node PRIVATE 26 | BEFORE ${SDK_PATH}/lib 27 | ) 28 | target_include_directories(${PROJECT_NAME}_zmq_node PRIVATE 29 | ${ZeroMQ_INCLUDE_DIRS} 30 | ${SDK_PATH}/include 31 | ${OpenCV_INCLUDE_DIRS} 32 | ) 33 | target_link_libraries(${PROJECT_NAME}_zmq_node PRIVATE 34 | ${ZeroMQ_LIBRARIES} 35 | infinite_sense_core 36 | proto_msg 37 | ${OpenCV_LIBS} 38 | ) 39 | endif () 40 | -------------------------------------------------------------------------------- /demo/zmq_demo/ReadMe.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/demo/zmq_demo/ReadMe.md -------------------------------------------------------------------------------- /demo/zmq_demo/cmake/FindZeroMQ.cmake: -------------------------------------------------------------------------------- 1 | set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH ON) 2 | find_package(PkgConfig) 3 | pkg_check_modules(PC_LIBZMQ QUIET libzmq) 4 | 5 | set(ZeroMQ_VERSION ${PC_LIBZMQ_VERSION}) 6 | 7 | find_path(ZeroMQ_INCLUDE_DIR zmq.h 8 | PATHS ${ZeroMQ_DIR}/include 9 | ${PC_LIBZMQ_INCLUDE_DIRS}) 10 | 11 | find_library(ZeroMQ_LIBRARY 12 | NAMES zmq 13 | PATHS ${ZeroMQ_DIR}/lib 14 | ${PC_LIBZMQ_LIBDIR} 15 | ${PC_LIBZMQ_LIBRARY_DIRS}) 16 | 17 | if (ZeroMQ_LIBRARY) 18 | set(ZeroMQ_FOUND ON) 19 | endif () 20 | 21 | set(ZeroMQ_LIBRARIES ${ZeroMQ_LIBRARY}) 22 | set(ZeroMQ_INCLUDE_DIRS ${ZeroMQ_INCLUDE_DIR}) 23 | 24 | if (NOT TARGET libzmq) 25 | add_library(libzmq UNKNOWN IMPORTED) 26 | set_target_properties(libzmq PROPERTIES 27 | IMPORTED_LOCATION ${ZeroMQ_LIBRARIES} 28 | INTERFACE_INCLUDE_DIRECTORIES ${ZeroMQ_INCLUDE_DIRS}) 29 | endif () 30 | 31 | include(FindPackageHandleStandardArgs) 32 | # handle the QUIETLY and REQUIRED arguments and set ZeroMQ_FOUND to TRUE 33 | # if all listed variables are TRUE 34 | find_package_handle_standard_args(ZeroMQ DEFAULT_MSG ZeroMQ_LIBRARIES ZeroMQ_INCLUDE_DIRS) 35 | -------------------------------------------------------------------------------- /demo/zmq_demo/main_zmq.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "udp_manager.h" 7 | #include "cam_manager.h" 8 | #include "data_manager.h" 9 | #include "zmq.hpp" 10 | #include "msg_imu.pb.h" 11 | #include "msg_image.pb.h" 12 | 13 | void Pub(zmq::socket_t *pub, const std::string &topic, const std::string &metadata) { 14 | zmq::message_t topic_msg(topic.size()); 15 | memcpy(topic_msg.data(), topic.c_str(), topic.size()); 16 | pub->send(topic_msg, zmq::send_flags::sndmore); // Send the topic firs 17 | 18 | zmq::message_t query(metadata.length()); 19 | memcpy(query.data(), metadata.c_str(), metadata.size()); 20 | pub->send(query, zmq::send_flags::dontwait); 21 | } 22 | 23 | int main() { 24 | std::this_thread::sleep_for(std::chrono::milliseconds(3000)); 25 | zmq::context_t ctx(10); 26 | zmq::socket_t zmq_publisher(ctx, ZMQ_PUB); 27 | zmq_publisher.bind("tcp://127.0.0.1:5555"); 28 | /* 29 | 本机IP地址应该在192.168.1.X网段下。 30 | 192.168.1.168是同步板的IP地址 31 | The host IP address should be in the 192.168.1.X network segment. 32 | 192.168.1.168 is the IP address of the synchronization board. 33 | */ 34 | auto udp_manager = std::make_shared("192.168.1.168", 8888); 35 | udp_manager->Start(); 36 | CamManger::GetInstance().Initialization(); 37 | CamManger::GetInstance().Start(); 38 | std::this_thread::sleep_for(std::chrono::milliseconds(3000)); 39 | std::vector all_cam_names; 40 | DataManger::GetInstance().GetAllCamNames(all_cam_names); 41 | std::cout << "Number of cameras detected " << all_cam_names.size() << std::endl; 42 | ImuData imu_data{}; 43 | while (zmq_publisher.connected()) { 44 | while (DataManger::GetInstance().GetNewImuData(imu_data)) { 45 | auto imu = std::make_shared(); 46 | imu->mutable_header()->set_stamp(imu_data.time_stamp_us * 1000); 47 | imu->mutable_header()->set_sensor_name("imu_sync_board"); 48 | imu->add_angular_velocity(imu_data.gx); 49 | imu->add_angular_velocity(imu_data.gy); 50 | imu->add_angular_velocity(imu_data.gz); 51 | 52 | imu->add_linear_acceleration(imu_data.ax); 53 | imu->add_linear_acceleration(imu_data.ay); 54 | imu->add_linear_acceleration(imu_data.az); 55 | auto serialized_msg = imu->SerializeAsString(); 56 | Pub(&zmq_publisher, "imu", serialized_msg); 57 | } 58 | protocol::Image image; 59 | ImgData image_data{}; 60 | for (auto &cam : all_cam_names) { 61 | while (DataManger::GetInstance().GetNewCamData(cam, image_data)) { 62 | if (image_data.image.empty()) { 63 | continue; 64 | } 65 | image.mutable_header()->set_stamp(image_data.time_stamp_us * 1000); 66 | image.mutable_header()->set_sensor_name(cam); 67 | image.set_name(cam); 68 | protocol::Mat mat; 69 | mat.set_rows(image_data.image.rows); 70 | mat.set_cols(image_data.image.cols); 71 | mat.set_channels(image_data.image.channels()); 72 | mat.set_elt_type(image_data.image.type()); 73 | mat.set_elt_size((int)image_data.image.elemSize()); 74 | size_t data_size = image_data.image.rows * image_data.image.cols * image_data.image.elemSize(); 75 | mat.set_data(image_data.image.data, data_size); 76 | image.set_name(image_data.camera_name); 77 | image.mutable_mat()->MergeFrom(mat); 78 | std::string serialized_msg = image.SerializeAsString(); 79 | Pub(&zmq_publisher, "image", serialized_msg); 80 | } 81 | } 82 | std::this_thread::sleep_for(std::chrono::milliseconds{1}); 83 | } 84 | udp_manager->Stop(); 85 | CamManger::GetInstance().Stop(); 86 | zmq_publisher.close(); 87 | return 0; 88 | } -------------------------------------------------------------------------------- /demo/zmq_demo/proto_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(proto_msg) 2 | 3 | set(CMAKE_BUILD_TYPE "RelWithDebInfo") 4 | 5 | if (WIN32) 6 | find_package(Protobuf REQUIRED) 7 | else () 8 | find_package(Protobuf 3.6.1 REQUIRED) 9 | endif () 10 | 11 | message(STATUS "Protobuf_VERSION: " ${Protobuf_VERSION}) 12 | message(STATUS "Protobuf_INCLUDE_DIR: " ${Protobuf_INCLUDE_DIR}) 13 | 14 | if (NOT MSVC) 15 | set(PROTO_META_BASE_DIR ${CMAKE_BINARY_DIR}/message) 16 | if (EXISTS "${CMAKE_BINARY_DIR}/message" AND IS_DIRECTORY "${CMAKE_BINARY_DIR}/message") 17 | message(STATUS "${PROTO_META_BASE_DIR} Check ok!") 18 | else () 19 | file(MAKE_DIRECTORY ${PROTO_META_BASE_DIR}) 20 | endif () 21 | 22 | message(STATUS "proto_msg meta files path: ${PROTO_META_BASE_DIR}") 23 | 24 | list(APPEND PROTO_FLAGS -I${CMAKE_CURRENT_SOURCE_DIR}) 25 | file(GLOB_RECURSE MSG_PROTOS ${CMAKE_CURRENT_SOURCE_DIR}/*.proto) 26 | 27 | set(MESSAGE_HDRS "") 28 | set(MESSAGE_SRC "") 29 | 30 | foreach (msg ${MSG_PROTOS}) 31 | get_filename_component(FIL_WE ${msg} NAME_WE) 32 | 33 | list(APPEND MESSAGE_SRC "${PROTO_META_BASE_DIR}/${FIL_WE}.pb.cc") 34 | list(APPEND MESSAGE_HDRS "${PROTO_META_BASE_DIR}/${FIL_WE}.pb.h") 35 | 36 | add_custom_command( 37 | OUTPUT "${PROTO_META_BASE_DIR}/${FIL_WE}.pb.cc" 38 | "${PROTO_META_BASE_DIR}/${FIL_WE}.pb.h" 39 | COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} ${PROTO_FLAGS} --cpp_out=${PROTO_META_BASE_DIR} ${msg} 40 | DEPENDS ${msg} 41 | COMMENT "Running C++ protocol buffer compiler on ${msg}" 42 | VERBATIM 43 | ) 44 | endforeach () 45 | 46 | set_source_files_properties(${MESSAGE_SRC} ${MESSAGE_HDRS} PROPERTIES GENERATED TRUE) 47 | 48 | add_custom_target(generate_message ALL 49 | DEPENDS ${MESSAGE_SRC} ${MESSAGE_HDRS} 50 | COMMENT "generate message target" 51 | VERBATIM 52 | ) 53 | 54 | else () 55 | set(PROTO_META_BASE_DIR .) 56 | 57 | file(GLOB_RECURSE MESSAGE_SRC ${PROTO_META_BASE_DIR}/*.pb.cc) 58 | message(STATUS "MESSAGE_SRC: ${MESSAGE_SRC}") 59 | 60 | endif () 61 | 62 | add_library(${PROJECT_NAME} SHARED 63 | ${MESSAGE_SRC} 64 | ) 65 | if (NOT MSVC) 66 | else () 67 | target_compile_definitions(${PROJECT_NAME} 68 | PRIVATE 69 | RoProtobuf_LIBRARY 70 | ) 71 | endif () 72 | 73 | target_include_directories(${PROJECT_NAME} 74 | PUBLIC 75 | ${PROTO_META_BASE_DIR} 76 | ${Protobuf_INCLUDE_DIR} 77 | ) 78 | 79 | target_link_libraries(${PROJECT_NAME} 80 | PUBLIC 81 | ${Protobuf_LIBRARY_RELEASE} 82 | ) 83 | message(STATUS "create proto_msg OBJECT") 84 | 85 | -------------------------------------------------------------------------------- /demo/zmq_demo/proto_msg/msg_header.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | package protocol; 3 | 4 | message Header { 5 | int64 stamp = 1; // 传感器时间，单位ns 6 | uint32 seq = 2; 7 | string frame_id = 3; 8 | string sensor_name = 4; 9 | } 10 | -------------------------------------------------------------------------------- /demo/zmq_demo/proto_msg/msg_image.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | package protocol; 3 | import "msg_header.proto"; 4 | 5 | 6 | message Mat { 7 | int32 rows = 1; 8 | int32 cols = 2; 9 | int32 channels = 3; 10 | int32 elt_type = 4; 11 | int32 elt_size = 5; 12 | bytes data = 6; 13 | } 14 | 15 | message Image { 16 | Header header = 1; 17 | string name = 2; 18 | Mat mat = 3; 19 | } 20 | 21 | message Images { 22 | repeated Image images = 1; 23 | } 24 | 25 | 26 | -------------------------------------------------------------------------------- /demo/zmq_demo/proto_msg/msg_imu.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | package protocol; 3 | import "msg_header.proto"; 4 | 5 | message Quaternion { 6 | double x = 1; 7 | double y = 2; 8 | double z = 3; 9 | double w = 4; 10 | } 11 | message Imu { 12 | Header header = 1; 13 | repeated double angular_velocity = 2; 14 | repeated double linear_acceleration = 3; 15 | Quaternion orientation = 4; 16 | } 17 | 18 | message ImuArray { 19 | repeated Imu imu_data = 1; 20 | } 21 | 22 | -------------------------------------------------------------------------------- /firmware/README.md: -------------------------------------------------------------------------------- 1 | # 更新日志 2 | ## 24.11.16 3 | 1. 已知BUG优化 4 | 2. 更加稳定的PPS同步机制 5 | 3. 加入刷机固件，主要针对MINI版本配置设置 6 | -------------------------------------------------------------------------------- /firmware/flash_board_tool.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/flash_board_tool.uf2 -------------------------------------------------------------------------------- /firmware/full_v2_24_10_16_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/full_v2_24_10_16_release.uf2 -------------------------------------------------------------------------------- /firmware/full_v3_24_10_23_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/full_v3_24_10_23_release.uf2 -------------------------------------------------------------------------------- /firmware/full_v3_24_11_16_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/full_v3_24_11_16_release.uf2 -------------------------------------------------------------------------------- /firmware/full_v3_24_11_25_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/full_v3_24_11_25_release.uf2 -------------------------------------------------------------------------------- /firmware/mini_24_11_13_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/mini_24_11_13_release.uf2 -------------------------------------------------------------------------------- /firmware/mini_24_11_16_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/mini_24_11_16_release.uf2 -------------------------------------------------------------------------------- /firmware/mini_24_11_25_release.uf2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/firmware/mini_24_11_25_release.uf2 -------------------------------------------------------------------------------- /sdk/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/README.md -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/Fusion.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Fusion.h 3 | * @author Seb Madgwick 4 | * @brief Main header file for the Fusion library. This is the only file that 5 | * needs to be included when using the library. 6 | */ 7 | 8 | #ifndef FUSION_H 9 | #define FUSION_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "FusionAhrs.h" 19 | #include "FusionAxes.h" 20 | #include "FusionCalibration.h" 21 | #include "FusionCompass.h" 22 | #include "FusionConvention.h" 23 | #include "FusionMath.h" 24 | #include "FusionOffset.h" 25 | 26 | #ifdef __cplusplus 27 | } 28 | #endif 29 | 30 | #endif 31 | //------------------------------------------------------------------------------ 32 | // End of file 33 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionAhrs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionAhrs.h 3 | * @author Seb Madgwick 4 | * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer 5 | * measurements into a single measurement of orientation relative to the Earth. 6 | */ 7 | 8 | #ifndef FUSION_AHRS_H 9 | #define FUSION_AHRS_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionConvention.h" 15 | #include "FusionMath.h" 16 | #include 17 | 18 | //------------------------------------------------------------------------------ 19 | // Definitions 20 | 21 | /** 22 | * @brief AHRS algorithm settings. 23 | */ 24 | typedef struct { 25 | FusionConvention convention; 26 | float gain; 27 | float gyroscopeRange; 28 | float accelerationRejection; 29 | float magneticRejection; 30 | unsigned int recoveryTriggerPeriod; 31 | } FusionAhrsSettings; 32 | 33 | /** 34 | * @brief AHRS algorithm structure. Structure members are used internally and 35 | * must not be accessed by the application. 36 | */ 37 | typedef struct { 38 | FusionAhrsSettings settings; 39 | FusionQuaternion quaternion; 40 | FusionVector accelerometer; 41 | bool initialising; 42 | float rampedGain; 43 | float rampedGainStep; 44 | bool angularRateRecovery; 45 | FusionVector halfAccelerometerFeedback; 46 | FusionVector halfMagnetometerFeedback; 47 | bool accelerometerIgnored; 48 | int accelerationRecoveryTrigger; 49 | int accelerationRecoveryTimeout; 50 | bool magnetometerIgnored; 51 | int magneticRecoveryTrigger; 52 | int magneticRecoveryTimeout; 53 | } FusionAhrs; 54 | 55 | /** 56 | * @brief AHRS algorithm internal states. 57 | */ 58 | typedef struct { 59 | float accelerationError; 60 | bool accelerometerIgnored; 61 | float accelerationRecoveryTrigger; 62 | float magneticError; 63 | bool magnetometerIgnored; 64 | float magneticRecoveryTrigger; 65 | } FusionAhrsInternalStates; 66 | 67 | /** 68 | * @brief AHRS algorithm flags. 69 | */ 70 | typedef struct { 71 | bool initialising; 72 | bool angularRateRecovery; 73 | bool accelerationRecovery; 74 | bool magneticRecovery; 75 | } FusionAhrsFlags; 76 | 77 | //------------------------------------------------------------------------------ 78 | // Function declarations 79 | 80 | void FusionAhrsInitialise(FusionAhrs *const ahrs); 81 | 82 | void FusionAhrsReset(FusionAhrs *const ahrs); 83 | 84 | void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); 85 | 86 | void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); 87 | 88 | void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); 89 | 90 | void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); 91 | 92 | FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); 93 | 94 | void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion); 95 | 96 | FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs); 97 | 98 | FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); 99 | 100 | FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); 101 | 102 | FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); 103 | 104 | FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs); 105 | 106 | void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); 107 | 108 | #endif 109 | 110 | //------------------------------------------------------------------------------ 111 | // End of file 112 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionAxes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionAxes.h 3 | * @author Seb Madgwick 4 | * @brief Swaps sensor axes for alignment with the body axes. 5 | */ 6 | 7 | #ifndef FUSION_AXES_H 8 | #define FUSION_AXES_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include "FusionMath.h" 14 | 15 | //------------------------------------------------------------------------------ 16 | // Definitions 17 | 18 | /** 19 | * @brief Axes alignment describing the sensor axes relative to the body axes. 20 | * For example, if the body X axis is aligned with the sensor Y axis and the 21 | * body Y axis is aligned with sensor X axis but pointing the opposite direction 22 | * then alignment is +Y-X+Z. 23 | */ 24 | typedef enum { 25 | FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ 26 | FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ 27 | FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ 28 | FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ 29 | FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ 30 | FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ 31 | FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ 32 | FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ 33 | FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ 34 | FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ 35 | FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ 36 | FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ 37 | FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ 38 | FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ 39 | FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ 40 | FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ 41 | FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ 42 | FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ 43 | FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ 44 | FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ 45 | FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ 46 | FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ 47 | FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ 48 | FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ 49 | } FusionAxesAlignment; 50 | 51 | //------------------------------------------------------------------------------ 52 | // Inline functions 53 | 54 | /** 55 | * @brief Swaps sensor axes for alignment with the body axes. 56 | * @param sensor Sensor axes. 57 | * @param alignment Axes alignment. 58 | * @return Sensor axes aligned with the body axes. 59 | */ 60 | static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { 61 | FusionVector result; 62 | switch (alignment) { 63 | case FusionAxesAlignmentPXPYPZ: 64 | break; 65 | case FusionAxesAlignmentPXNZPY: 66 | result.axis.x = +sensor.axis.x; 67 | result.axis.y = -sensor.axis.z; 68 | result.axis.z = +sensor.axis.y; 69 | return result; 70 | case FusionAxesAlignmentPXNYNZ: 71 | result.axis.x = +sensor.axis.x; 72 | result.axis.y = -sensor.axis.y; 73 | result.axis.z = -sensor.axis.z; 74 | return result; 75 | case FusionAxesAlignmentPXPZNY: 76 | result.axis.x = +sensor.axis.x; 77 | result.axis.y = +sensor.axis.z; 78 | result.axis.z = -sensor.axis.y; 79 | return result; 80 | case FusionAxesAlignmentNXPYNZ: 81 | result.axis.x = -sensor.axis.x; 82 | result.axis.y = +sensor.axis.y; 83 | result.axis.z = -sensor.axis.z; 84 | return result; 85 | case FusionAxesAlignmentNXPZPY: 86 | result.axis.x = -sensor.axis.x; 87 | result.axis.y = +sensor.axis.z; 88 | result.axis.z = +sensor.axis.y; 89 | return result; 90 | case FusionAxesAlignmentNXNYPZ: 91 | result.axis.x = -sensor.axis.x; 92 | result.axis.y = -sensor.axis.y; 93 | result.axis.z = +sensor.axis.z; 94 | return result; 95 | case FusionAxesAlignmentNXNZNY: 96 | result.axis.x = -sensor.axis.x; 97 | result.axis.y = -sensor.axis.z; 98 | result.axis.z = -sensor.axis.y; 99 | return result; 100 | case FusionAxesAlignmentPYNXPZ: 101 | result.axis.x = +sensor.axis.y; 102 | result.axis.y = -sensor.axis.x; 103 | result.axis.z = +sensor.axis.z; 104 | return result; 105 | case FusionAxesAlignmentPYNZNX: 106 | result.axis.x = +sensor.axis.y; 107 | result.axis.y = -sensor.axis.z; 108 | result.axis.z = -sensor.axis.x; 109 | return result; 110 | case FusionAxesAlignmentPYPXNZ: 111 | result.axis.x = +sensor.axis.y; 112 | result.axis.y = +sensor.axis.x; 113 | result.axis.z = -sensor.axis.z; 114 | return result; 115 | case FusionAxesAlignmentPYPZPX: 116 | result.axis.x = +sensor.axis.y; 117 | result.axis.y = +sensor.axis.z; 118 | result.axis.z = +sensor.axis.x; 119 | return result; 120 | case FusionAxesAlignmentNYPXPZ: 121 | result.axis.x = -sensor.axis.y; 122 | result.axis.y = +sensor.axis.x; 123 | result.axis.z = +sensor.axis.z; 124 | return result; 125 | case FusionAxesAlignmentNYNZPX: 126 | result.axis.x = -sensor.axis.y; 127 | result.axis.y = -sensor.axis.z; 128 | result.axis.z = +sensor.axis.x; 129 | return result; 130 | case FusionAxesAlignmentNYNXNZ: 131 | result.axis.x = -sensor.axis.y; 132 | result.axis.y = -sensor.axis.x; 133 | result.axis.z = -sensor.axis.z; 134 | return result; 135 | case FusionAxesAlignmentNYPZNX: 136 | result.axis.x = -sensor.axis.y; 137 | result.axis.y = +sensor.axis.z; 138 | result.axis.z = -sensor.axis.x; 139 | return result; 140 | case FusionAxesAlignmentPZPYNX: 141 | result.axis.x = +sensor.axis.z; 142 | result.axis.y = +sensor.axis.y; 143 | result.axis.z = -sensor.axis.x; 144 | return result; 145 | case FusionAxesAlignmentPZPXPY: 146 | result.axis.x = +sensor.axis.z; 147 | result.axis.y = +sensor.axis.x; 148 | result.axis.z = +sensor.axis.y; 149 | return result; 150 | case FusionAxesAlignmentPZNYPX: 151 | result.axis.x = +sensor.axis.z; 152 | result.axis.y = -sensor.axis.y; 153 | result.axis.z = +sensor.axis.x; 154 | return result; 155 | case FusionAxesAlignmentPZNXNY: 156 | result.axis.x = +sensor.axis.z; 157 | result.axis.y = -sensor.axis.x; 158 | result.axis.z = -sensor.axis.y; 159 | return result; 160 | case FusionAxesAlignmentNZPYPX: 161 | result.axis.x = -sensor.axis.z; 162 | result.axis.y = +sensor.axis.y; 163 | result.axis.z = +sensor.axis.x; 164 | return result; 165 | case FusionAxesAlignmentNZNXPY: 166 | result.axis.x = -sensor.axis.z; 167 | result.axis.y = -sensor.axis.x; 168 | result.axis.z = +sensor.axis.y; 169 | return result; 170 | case FusionAxesAlignmentNZNYNX: 171 | result.axis.x = -sensor.axis.z; 172 | result.axis.y = -sensor.axis.y; 173 | result.axis.z = -sensor.axis.x; 174 | return result; 175 | case FusionAxesAlignmentNZPXNY: 176 | result.axis.x = -sensor.axis.z; 177 | result.axis.y = +sensor.axis.x; 178 | result.axis.z = -sensor.axis.y; 179 | return result; 180 | } 181 | return sensor; // avoid compiler warning 182 | } 183 | 184 | #endif 185 | 186 | //------------------------------------------------------------------------------ 187 | // End of file 188 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionCalibration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionCalibration.h 3 | * @author Seb Madgwick 4 | * @brief Gyroscope, accelerometer, and magnetometer calibration models. 5 | */ 6 | 7 | #ifndef FUSION_CALIBRATION_H 8 | #define FUSION_CALIBRATION_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include "FusionMath.h" 14 | 15 | //------------------------------------------------------------------------------ 16 | // Inline functions 17 | 18 | /** 19 | * @brief Gyroscope and accelerometer calibration model. 20 | * @param uncalibrated Uncalibrated measurement. 21 | * @param misalignment Misalignment matrix. 22 | * @param sensitivity Sensitivity. 23 | * @param offset Offset. 24 | * @return Calibrated measurement. 25 | */ 26 | static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { 27 | return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); 28 | } 29 | 30 | /** 31 | * @brief Magnetometer calibration model. 32 | * @param uncalibrated Uncalibrated measurement. 33 | * @param softIronMatrix Soft-iron matrix. 34 | * @param hardIronOffset Hard-iron offset. 35 | * @return Calibrated measurement. 36 | */ 37 | static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { 38 | return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset)); 39 | } 40 | 41 | #endif 42 | 43 | //------------------------------------------------------------------------------ 44 | // End of file 45 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionCompass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionCompass.h 3 | * @author Seb Madgwick 4 | * @brief Tilt-compensated compass to calculate the magnetic heading using 5 | * accelerometer and magnetometer measurements. 6 | */ 7 | 8 | #ifndef FUSION_COMPASS_H 9 | #define FUSION_COMPASS_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionConvention.h" 15 | #include "FusionMath.h" 16 | 17 | //------------------------------------------------------------------------------ 18 | // Function declarations 19 | 20 | float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer); 21 | 22 | #endif 23 | 24 | //------------------------------------------------------------------------------ 25 | // End of file 26 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionConvention.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionConvention.h 3 | * @author Seb Madgwick 4 | * @brief Earth axes convention. 5 | */ 6 | 7 | #ifndef FUSION_CONVENTION_H 8 | #define FUSION_CONVENTION_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Definitions 12 | 13 | /** 14 | * @brief Earth axes convention. 15 | */ 16 | typedef enum { 17 | FusionConventionNwu, /* North-West-Up */ 18 | FusionConventionEnu, /* East-North-Up */ 19 | FusionConventionNed, /* North-East-Down */ 20 | } FusionConvention; 21 | 22 | #endif 23 | 24 | //------------------------------------------------------------------------------ 25 | // End of file 26 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionMath.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionMath.h 3 | * @author Seb Madgwick 4 | * @brief Math library. 5 | */ 6 | 7 | #ifndef FUSION_MATH_H 8 | #define FUSION_MATH_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include // M_PI, sqrtf, atan2f, asinf 14 | #include 15 | #include 16 | 17 | //------------------------------------------------------------------------------ 18 | // Definitions 19 | 20 | /** 21 | * @brief 3D vector. 22 | */ 23 | typedef union { 24 | float array[3]; 25 | 26 | struct { 27 | float x; 28 | float y; 29 | float z; 30 | } axis; 31 | } FusionVector; 32 | 33 | /** 34 | * @brief Quaternion. 35 | */ 36 | typedef union { 37 | float array[4]; 38 | 39 | struct { 40 | float w; 41 | float x; 42 | float y; 43 | float z; 44 | } element; 45 | } FusionQuaternion; 46 | 47 | /** 48 | * @brief 3x3 matrix in row-major order. 49 | * See http://en.wikipedia.org/wiki/Row-major_order 50 | */ 51 | typedef union { 52 | float array[3][3]; 53 | 54 | struct { 55 | float xx; 56 | float xy; 57 | float xz; 58 | float yx; 59 | float yy; 60 | float yz; 61 | float zx; 62 | float zy; 63 | float zz; 64 | } element; 65 | } FusionMatrix; 66 | 67 | /** 68 | * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around 69 | * X, Y, and Z respectively. 70 | */ 71 | typedef union { 72 | float array[3]; 73 | 74 | struct { 75 | float roll; 76 | float pitch; 77 | float yaw; 78 | } angle; 79 | } FusionEuler; 80 | 81 | /** 82 | * @brief Vector of zeros. 83 | */ 84 | #define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) 85 | 86 | /** 87 | * @brief Vector of ones. 88 | */ 89 | #define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) 90 | 91 | /** 92 | * @brief Identity quaternion. 93 | */ 94 | #define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) 95 | 96 | /** 97 | * @brief Identity matrix. 98 | */ 99 | #define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) 100 | 101 | /** 102 | * @brief Euler angles of zero. 103 | */ 104 | #define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) 105 | 106 | /** 107 | * @brief Pi. May not be defined in math.h. 108 | */ 109 | #ifndef M_PI 110 | #define M_PI (3.14159265358979323846) 111 | #endif 112 | 113 | /** 114 | * @brief Include this definition or add as a preprocessor definition to use 115 | * normal square root operations. 116 | */ 117 | //#define FUSION_USE_NORMAL_SQRT 118 | 119 | //------------------------------------------------------------------------------ 120 | // Inline functions - Degrees and radians conversion 121 | 122 | /** 123 | * @brief Converts degrees to radians. 124 | * @param degrees Degrees. 125 | * @return Radians. 126 | */ 127 | static inline float FusionDegreesToRadians(const float degrees) { 128 | return degrees * ((float) M_PI / 180.0f); 129 | } 130 | 131 | /** 132 | * @brief Converts radians to degrees. 133 | * @param radians Radians. 134 | * @return Degrees. 135 | */ 136 | static inline float FusionRadiansToDegrees(const float radians) { 137 | return radians * (180.0f / (float) M_PI); 138 | } 139 | 140 | //------------------------------------------------------------------------------ 141 | // Inline functions - Arc sine 142 | 143 | /** 144 | * @brief Returns the arc sine of the value. 145 | * @param value Value. 146 | * @return Arc sine of the value. 147 | */ 148 | static inline float FusionAsin(const float value) { 149 | if (value <= -1.0f) { 150 | return (float) M_PI / -2.0f; 151 | } 152 | if (value >= 1.0f) { 153 | return (float) M_PI / 2.0f; 154 | } 155 | return asinf(value); 156 | } 157 | 158 | //------------------------------------------------------------------------------ 159 | // Inline functions - Fast inverse square root 160 | 161 | #ifndef FUSION_USE_NORMAL_SQRT 162 | 163 | /** 164 | * @brief Calculates the reciprocal of the square root. 165 | * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ 166 | * @param x Operand. 167 | * @return Reciprocal of the square root of x. 168 | */ 169 | static inline float FusionFastInverseSqrt(const float x) { 170 | 171 | typedef union { 172 | float f; 173 | int32_t i; 174 | } Union32; 175 | 176 | Union32 union32 = {.f = x}; 177 | union32.i = 0x5F1F1412 - (union32.i >> 1); 178 | return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); 179 | } 180 | 181 | #endif 182 | 183 | //------------------------------------------------------------------------------ 184 | // Inline functions - Vector operations 185 | 186 | /** 187 | * @brief Returns true if the vector is zero. 188 | * @param vector Vector. 189 | * @return True if the vector is zero. 190 | */ 191 | static inline bool FusionVectorIsZero(const FusionVector vector) { 192 | return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); 193 | } 194 | 195 | /** 196 | * @brief Returns the sum of two vectors. 197 | * @param vectorA Vector A. 198 | * @param vectorB Vector B. 199 | * @return Sum of two vectors. 200 | */ 201 | static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { 202 | const FusionVector result = {.axis = { 203 | .x = vectorA.axis.x + vectorB.axis.x, 204 | .y = vectorA.axis.y + vectorB.axis.y, 205 | .z = vectorA.axis.z + vectorB.axis.z, 206 | }}; 207 | return result; 208 | } 209 | 210 | /** 211 | * @brief Returns vector B subtracted from vector A. 212 | * @param vectorA Vector A. 213 | * @param vectorB Vector B. 214 | * @return Vector B subtracted from vector A. 215 | */ 216 | static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { 217 | const FusionVector result = {.axis = { 218 | .x = vectorA.axis.x - vectorB.axis.x, 219 | .y = vectorA.axis.y - vectorB.axis.y, 220 | .z = vectorA.axis.z - vectorB.axis.z, 221 | }}; 222 | return result; 223 | } 224 | 225 | /** 226 | * @brief Returns the sum of the elements. 227 | * @param vector Vector. 228 | * @return Sum of the elements. 229 | */ 230 | static inline float FusionVectorSum(const FusionVector vector) { 231 | return vector.axis.x + vector.axis.y + vector.axis.z; 232 | } 233 | 234 | /** 235 | * @brief Returns the multiplication of a vector by a scalar. 236 | * @param vector Vector. 237 | * @param scalar Scalar. 238 | * @return Multiplication of a vector by a scalar. 239 | */ 240 | static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { 241 | const FusionVector result = {.axis = { 242 | .x = vector.axis.x * scalar, 243 | .y = vector.axis.y * scalar, 244 | .z = vector.axis.z * scalar, 245 | }}; 246 | return result; 247 | } 248 | 249 | /** 250 | * @brief Calculates the Hadamard product (element-wise multiplication). 251 | * @param vectorA Vector A. 252 | * @param vectorB Vector B. 253 | * @return Hadamard product. 254 | */ 255 | static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { 256 | const FusionVector result = {.axis = { 257 | .x = vectorA.axis.x * vectorB.axis.x, 258 | .y = vectorA.axis.y * vectorB.axis.y, 259 | .z = vectorA.axis.z * vectorB.axis.z, 260 | }}; 261 | return result; 262 | } 263 | 264 | /** 265 | * @brief Returns the cross product. 266 | * @param vectorA Vector A. 267 | * @param vectorB Vector B. 268 | * @return Cross product. 269 | */ 270 | static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { 271 | #define A vectorA.axis 272 | #define B vectorB.axis 273 | const FusionVector result = {.axis = { 274 | .x = A.y * B.z - A.z * B.y, 275 | .y = A.z * B.x - A.x * B.z, 276 | .z = A.x * B.y - A.y * B.x, 277 | }}; 278 | return result; 279 | #undef A 280 | #undef B 281 | } 282 | 283 | /** 284 | * @brief Returns the dot product. 285 | * @param vectorA Vector A. 286 | * @param vectorB Vector B. 287 | * @return Dot product. 288 | */ 289 | static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) { 290 | return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); 291 | } 292 | 293 | /** 294 | * @brief Returns the vector magnitude squared. 295 | * @param vector Vector. 296 | * @return Vector magnitude squared. 297 | */ 298 | static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { 299 | return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); 300 | } 301 | 302 | /** 303 | * @brief Returns the vector magnitude. 304 | * @param vector Vector. 305 | * @return Vector magnitude. 306 | */ 307 | static inline float FusionVectorMagnitude(const FusionVector vector) { 308 | return sqrtf(FusionVectorMagnitudeSquared(vector)); 309 | } 310 | 311 | /** 312 | * @brief Returns the normalised vector. 313 | * @param vector Vector. 314 | * @return Normalised vector. 315 | */ 316 | static inline FusionVector FusionVectorNormalise(const FusionVector vector) { 317 | #ifdef FUSION_USE_NORMAL_SQRT 318 | const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); 319 | #else 320 | const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); 321 | #endif 322 | return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); 323 | } 324 | 325 | //------------------------------------------------------------------------------ 326 | // Inline functions - Quaternion operations 327 | 328 | /** 329 | * @brief Returns the sum of two quaternions. 330 | * @param quaternionA Quaternion A. 331 | * @param quaternionB Quaternion B. 332 | * @return Sum of two quaternions. 333 | */ 334 | static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { 335 | const FusionQuaternion result = {.element = { 336 | .w = quaternionA.element.w + quaternionB.element.w, 337 | .x = quaternionA.element.x + quaternionB.element.x, 338 | .y = quaternionA.element.y + quaternionB.element.y, 339 | .z = quaternionA.element.z + quaternionB.element.z, 340 | }}; 341 | return result; 342 | } 343 | 344 | /** 345 | * @brief Returns the multiplication of two quaternions. 346 | * @param quaternionA Quaternion A (to be post-multiplied). 347 | * @param quaternionB Quaternion B (to be pre-multiplied). 348 | * @return Multiplication of two quaternions. 349 | */ 350 | static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { 351 | #define A quaternionA.element 352 | #define B quaternionB.element 353 | const FusionQuaternion result = {.element = { 354 | .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, 355 | .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, 356 | .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, 357 | .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, 358 | }}; 359 | return result; 360 | #undef A 361 | #undef B 362 | } 363 | 364 | /** 365 | * @brief Returns the multiplication of a quaternion with a vector. This is a 366 | * normal quaternion multiplication where the vector is treated a 367 | * quaternion with a W element value of zero. The quaternion is post- 368 | * multiplied by the vector. 369 | * @param quaternion Quaternion. 370 | * @param vector Vector. 371 | * @return Multiplication of a quaternion with a vector. 372 | */ 373 | static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { 374 | #define Q quaternion.element 375 | #define V vector.axis 376 | const FusionQuaternion result = {.element = { 377 | .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, 378 | .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, 379 | .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, 380 | .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, 381 | }}; 382 | return result; 383 | #undef Q 384 | #undef V 385 | } 386 | 387 | /** 388 | * @brief Returns the normalised quaternion. 389 | * @param quaternion Quaternion. 390 | * @return Normalised quaternion. 391 | */ 392 | static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { 393 | #define Q quaternion.element 394 | #ifdef FUSION_USE_NORMAL_SQRT 395 | const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); 396 | #else 397 | const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); 398 | #endif 399 | const FusionQuaternion result = {.element = { 400 | .w = Q.w * magnitudeReciprocal, 401 | .x = Q.x * magnitudeReciprocal, 402 | .y = Q.y * magnitudeReciprocal, 403 | .z = Q.z * magnitudeReciprocal, 404 | }}; 405 | return result; 406 | #undef Q 407 | } 408 | 409 | //------------------------------------------------------------------------------ 410 | // Inline functions - Matrix operations 411 | 412 | /** 413 | * @brief Returns the multiplication of a matrix with a vector. 414 | * @param matrix Matrix. 415 | * @param vector Vector. 416 | * @return Multiplication of a matrix with a vector. 417 | */ 418 | static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { 419 | #define R matrix.element 420 | const FusionVector result = {.axis = { 421 | .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, 422 | .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, 423 | .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, 424 | }}; 425 | return result; 426 | #undef R 427 | } 428 | 429 | //------------------------------------------------------------------------------ 430 | // Inline functions - Conversion operations 431 | 432 | /** 433 | * @brief Converts a quaternion to a rotation matrix. 434 | * @param quaternion Quaternion. 435 | * @return Rotation matrix. 436 | */ 437 | static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { 438 | #define Q quaternion.element 439 | const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations 440 | const float qwqx = Q.w * Q.x; 441 | const float qwqy = Q.w * Q.y; 442 | const float qwqz = Q.w * Q.z; 443 | const float qxqy = Q.x * Q.y; 444 | const float qxqz = Q.x * Q.z; 445 | const float qyqz = Q.y * Q.z; 446 | const FusionMatrix matrix = {.element = { 447 | .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), 448 | .xy = 2.0f * (qxqy - qwqz), 449 | .xz = 2.0f * (qxqz + qwqy), 450 | .yx = 2.0f * (qxqy + qwqz), 451 | .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), 452 | .yz = 2.0f * (qyqz - qwqx), 453 | .zx = 2.0f * (qxqz - qwqy), 454 | .zy = 2.0f * (qyqz + qwqx), 455 | .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), 456 | }}; 457 | return matrix; 458 | #undef Q 459 | } 460 | 461 | /** 462 | * @brief Converts a quaternion to ZYX Euler angles in degrees. 463 | * @param quaternion Quaternion. 464 | * @return Euler angles in degrees. 465 | */ 466 | static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { 467 | #define Q quaternion.element 468 | const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations 469 | const FusionEuler euler = {.angle = { 470 | .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), 471 | .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), 472 | .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), 473 | }}; 474 | return euler; 475 | #undef Q 476 | } 477 | 478 | #endif 479 | 480 | //------------------------------------------------------------------------------ 481 | // End of file 482 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/FusionOffset.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionOffset.h 3 | * @author Seb Madgwick 4 | * @brief Gyroscope offset correction algorithm for run-time calibration of the 5 | * gyroscope offset. 6 | */ 7 | 8 | #ifndef FUSION_OFFSET_H 9 | #define FUSION_OFFSET_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionMath.h" 15 | 16 | //------------------------------------------------------------------------------ 17 | // Definitions 18 | 19 | /** 20 | * @brief Gyroscope offset algorithm structure. Structure members are used 21 | * internally and must not be accessed by the application. 22 | */ 23 | typedef struct { 24 | float filterCoefficient; 25 | unsigned int timeout; 26 | unsigned int timer; 27 | FusionVector gyroscopeOffset; 28 | } FusionOffset; 29 | 30 | //------------------------------------------------------------------------------ 31 | // Function declarations 32 | 33 | void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); 34 | 35 | FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); 36 | 37 | #endif 38 | 39 | //------------------------------------------------------------------------------ 40 | // End of file 41 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/MvCameraControl.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/include/MvCameraControl.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/MvErrorDefine.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/include/MvErrorDefine.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/MvISPErrorDefine.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/include/MvISPErrorDefine.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/ObsoleteCamParams.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/include/ObsoleteCamParams.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/cam_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "MvCameraControl.h" 6 | 7 | class CamManger { 8 | public: 9 | static CamManger& GetInstance() { 10 | static CamManger instance; 11 | return instance; 12 | } 13 | CamManger(const CamManger&) = delete; 14 | CamManger& operator=(const CamManger&) = delete; 15 | 16 | bool Initialization(); 17 | void Stop(); 18 | void Start(); 19 | void Enable() { is_running_ = true; } 20 | void Disable() { is_running_ = false; } 21 | void Restart(); 22 | private: 23 | 24 | CamManger(); 25 | ~CamManger(); 26 | void Receive(void* handle, const std::string&) const; 27 | static bool PrintDeviceInfo(MV_CC_DEVICE_INFO* pstMVDevInfo); 28 | 29 | std::vector rets_; 30 | std::vector handles_; 31 | std::vector cam_threads_; 32 | bool is_running_{false}; 33 | }; 34 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/crc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | class Crc { 4 | public: 5 | Crc(); 6 | ~Crc(); 7 | static uint16_t Crc16(const uint8_t *ptr, uint32_t len); 8 | static constexpr uint8_t start_package[3] = {0x80, 0x81, 0x82}; 9 | static constexpr uint8_t stop_package[3] = {0x83, 0x84, 0x85}; 10 | private: 11 | static uint16_t crc_tabs_[16]; 12 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/data_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "readerwriterqueue.h" 9 | 10 | struct __attribute__ ((packed)) IMU { 11 | uint64_t time_stamp; 12 | float count; 13 | float ax; 14 | float ay; 15 | float az; 16 | float gx; 17 | float gy; 18 | float gz; 19 | float tmp; 20 | float trigger; 21 | }; 22 | 23 | struct ImuData { 24 | uint64_t time_stamp_us; 25 | float ax; 26 | float ay; 27 | float az; 28 | float gx; 29 | float gy; 30 | float gz; 31 | float tmp; 32 | }; 33 | 34 | struct __attribute__ ((packed))TimeStampData { 35 | uint16_t head; 36 | uint64_t mark_1; 37 | uint64_t mark_2; 38 | }; 39 | 40 | struct ImgData { 41 | std::string camera_name; 42 | uint64_t time_stamp_us; 43 | cv::Mat image; 44 | }; 45 | 46 | class DataManger { 47 | public: 48 | static DataManger &GetInstance() { 49 | static DataManger instance; 50 | return instance; 51 | } 52 | DataManger(const DataManger &) = delete; 53 | DataManger &operator=(const DataManger &) = delete; 54 | 55 | void AddCamData(const std::string &name, const ImgData &data); 56 | bool GetNewCamData(const std::string &name, ImgData &data); 57 | void AddImuData(const ImuData &data); 58 | bool GetNewImuData(ImuData &data); 59 | void GetAllCamNames(std::vector &names) const; 60 | void SetLastTiggerTime(const uint64_t &time); 61 | uint64_t GetLastTiggerTime(); 62 | 63 | private: 64 | DataManger(); 65 | ~DataManger(); 66 | std::unordered_map>> all_cams_data_{}; 67 | moodycamel::ReaderWriterQueue imu_data_{512}; 68 | std::mutex last_trigger_lock_{}; 69 | std::atomic last_imu_trigger_time_{0}; 70 | }; 71 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/practical_socket.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // 异常类 4 | #include // 字符串类 5 | 6 | using namespace std; 7 | 8 | /** 9 | * 表示与套接字调用执行有关的问题。 10 | */ 11 | class SocketException : public exception { 12 | public: 13 | /** 14 | * 使用说明性消息构造 SocketException。 15 | * @param message 说明性消息 16 | * @param incSysMsg 如果为 true，则将系统消息（来自 strerror(errno)）附加到用户提供的消息后面 17 | */ 18 | SocketException(const string &message, bool inclSysMsg = false) throw(); 19 | 20 | /** 21 | * 为了确保不抛出任何异常而提供。 22 | */ 23 | ~SocketException() throw(); 24 | 25 | /** 26 | * 获取异常消息 27 | * @return 异常消息 28 | */ 29 | const char *what() const throw(); 30 | 31 | private: 32 | string userMessage; // 异常消息 33 | }; 34 | 35 | /** 36 | * 表示基本通信端点的基类 37 | */ 38 | class Socket { 39 | public: 40 | /** 41 | * 关闭和释放此套接字 42 | */ 43 | ~Socket(); 44 | 45 | /** 46 | * 获取本地地址 47 | * @return 套接字的本地地址 48 | * @exception SocketException 如果获取失败则抛出 49 | */ 50 | string getLocalAddress() noexcept(false); 51 | 52 | /** 53 | * 获取本地端口 54 | * @return 套接字的本地端口 55 | * @exception SocketException 如果获取失败则抛出 56 | */ 57 | unsigned short getLocalPort() const noexcept(false); 58 | 59 | /** 60 | * 将本地端口设置为指定的端口，并将本地地址设置为任何接口 61 | * @param localPort 本地端口 62 | * @exception SocketException 如果设置本地端口失败则抛出 63 | */ 64 | void setLocalPort(unsigned short localPort) noexcept(false); 65 | 66 | /** 67 | * 将本地端口设置为指定的端口，并将本地地址设置为指定的地址。 68 | * 如果省略端口，则将选择一个随机端口。 69 | * @param localAddress 本地地址 70 | * @param localPort 本地端口 71 | * @exception SocketException 如果设置本地端口或地址失败则抛出 72 | */ 73 | void setLocalAddressAndPort(const string &localAddress, unsigned short localPort = 0) noexcept(false); 74 | 75 | /** 76 | * 如果是 WinSock，则卸载 WinSock DLL；否则什么也不做。 77 | * 我们在示例客户端代码中忽略这一点，但在库中包含它是为了完整性。 78 | * 如果您在 Windows 上运行并且担心 DLL 资源消耗，请在所有 Socket 实例完成后调用此函数。 79 | * 如果您在某个 Socket 实例存在时在 Windows 上执行此操作，则可能会出问题。 80 | * 为了客户端代码的可移植性，在非 Windows 平台上，这是一个空函数，因此您总是可以包含它。 81 | * @param buffer 接收数据的缓冲区 82 | * @param bufferLen 最大读入缓冲区的字节数 83 | * @return 读取的字节数，EOF 为 0，错误为 -1 84 | * @exception SocketException 如果 WinSock 清理失败则抛出 85 | */ 86 | static void cleanUp() noexcept(false); 87 | 88 | /** 89 | * 将指定协议的服务解析为主机字节顺序中的相应端口号 90 | * @param service 要解析的服务（例如 "http"） 91 | * @param protocol 服务的协议。默认为 "tcp"。 92 | */ 93 | static unsigned short resolveService(const string &service, const string &protocol = "tcp"); 94 | 95 | private: 96 | // 防止用户在此对象上尝试使用值语义 97 | Socket(const Socket &sock); 98 | void operator=(const Socket &sock); 99 | 100 | protected: 101 | int sockDesc; // 套接字描述符 102 | Socket(int type, int protocol) noexcept(false); 103 | Socket(int sockDesc); 104 | }; 105 | 106 | /** 107 | * 能够连接、发送和接收的套接字 108 | */ 109 | class CommunicatingSocket : public Socket { 110 | public: 111 | /** 112 | * 与给定的远程地址和端口建立套接字连接 113 | * @param foreignAddress 对方地址（IP 地址或名称） 114 | * @param foreignPort 对方端口 115 | * @exception SocketException 如果无法建立连接则抛出 116 | */ 117 | void connect(const string &foreignAddress, unsigned short foreignPort) noexcept(false); 118 | 119 | /** 120 | * 将给定的缓冲区写入此套接字。在调用 send() 之前调用 connect() 121 | * @param buffer 要写入的缓冲区 122 | * @param bufferLen 要写入缓冲区的字节数 123 | * @exception SocketException 如果无法发送数据则抛出 124 | */ 125 | void send(const void *buffer, int bufferLen) noexcept(false); 126 | 127 | /** 128 | * 从此套接字读取最多 bufferLen 字节数据到给定的缓冲区中。在调用 recv() 之前调用 connect() 129 | * @param buffer 接收数据的缓冲区 130 | * @param bufferLen 最大要读入缓冲区的字节数 131 | * @return 读取的字节数，EOF 为 0，错误为 -1 132 | * @exception SocketException 如果无法接收数据则抛出 133 | */ 134 | int recv(void *buffer, int bufferLen) noexcept(false); 135 | 136 | /** 137 | * 获取对方地址。在调用 recv() 之前调用 connect() 138 | * @return 对方地址 139 | * @exception SocketException 如果无法获取对方地址则抛出 140 | */ 141 | string getForeignAddress() noexcept(false); 142 | 143 | /** 144 | * 获取对方端口。在调用 recv() 之前调用 connect() 145 | * @return 对方端口 146 | * @exception SocketException 如果无法获取对方端口则抛出 147 | */ 148 | unsigned short getForeignPort() noexcept(false); 149 | 150 | protected: 151 | CommunicatingSocket(int type, int protocol) noexcept(false); 152 | CommunicatingSocket(int newConnSD); 153 | }; 154 | 155 | /** 156 | * 用于与其他 TCP 套接字通信的 TCP 套接字 157 | */ 158 | class TCPSocket : public CommunicatingSocket { 159 | public: 160 | /** 161 | * 构造一个没有连接的 TCP 套接字 162 | * @exception SocketException 如果无法创建 TCP 套接字则抛出 163 | */ 164 | TCPSocket() noexcept(false); 165 | 166 | /** 167 | * 构造一个连接到给定远程地址和端口的 TCP 套接字 168 | * @param foreignAddress 对方地址（IP 地址或名称） 169 | * @param foreignPort 对方端口 170 | * @exception SocketException 如果无法创建 TCP 套接字则抛出 171 | */ 172 | TCPSocket(const string &foreignAddress, unsigned short foreignPort) noexcept(false); 173 | 174 | private: 175 | // 用于 TCPServerSocket::accept() 连接创建的访问 176 | friend class TCPServerSocket; 177 | TCPSocket(int newConnSD); 178 | }; 179 | 180 | /** 181 | * 服务器使用的 TCP 套接字类 182 | */ 183 | class TCPServerSocket : public Socket { 184 | public: 185 | /** 186 | 187 | 188 | * 为服务器构造一个 TCP 套接字，接受在指定端口上任何接口上的连接 189 | * @param localPort 服务器套接字的本地端口，值为零将给出一个系统分配的未使用端口 190 | * @param queueLen 最大挂起连接请求的队列长度（默认为 5） 191 | * @exception SocketException 如果无法创建 TCP 服务器套接字则抛出 192 | */ 193 | TCPServerSocket(unsigned short localPort, int queueLen = 5) noexcept(false); 194 | 195 | /** 196 | * 为服务器构造一个 TCP 套接字，接受在指定地址的指定端口上的连接 197 | * @param localAddress 服务器套接字的本地接口（地址） 198 | * @param localPort 服务器套接字的本地端口 199 | * @param queueLen 最大挂起连接请求的队列长度（默认为 5） 200 | * @exception SocketException 如果无法创建 TCP 服务器套接字则抛出 201 | */ 202 | TCPServerSocket(const string &localAddress, unsigned short localPort, int queueLen = 5) noexcept(false); 203 | 204 | /** 205 | * 阻塞，直到在此套接字上建立一个新连接或出现错误 206 | * @return 新连接套接字 207 | * @exception SocketException 如果尝试接受新连接失败则抛出 208 | */ 209 | TCPSocket *accept() noexcept(false); 210 | 211 | private: 212 | void setListen(int queueLen) noexcept(false); 213 | }; 214 | 215 | /** 216 | * UDP 套接字类 217 | */ 218 | class UDPSocket : public CommunicatingSocket { 219 | public: 220 | /** 221 | * 构造一个 UDP 套接字 222 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 223 | */ 224 | UDPSocket() noexcept(false); 225 | 226 | /** 227 | * 构造一个具有给定本地端口的 UDP 套接字 228 | * @param localPort 本地端口 229 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 230 | */ 231 | UDPSocket(unsigned short localPort) noexcept(false); 232 | 233 | /** 234 | * 构造一个具有给定本地端口和地址的 UDP 套接字 235 | * @param localAddress 本地地址 236 | * @param localPort 本地端口 237 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 238 | */ 239 | UDPSocket(const string &localAddress, unsigned short localPort) noexcept(false); 240 | 241 | /** 242 | * 取消关联对方地址和端口 243 | * @return 如果解除关联成功则返回 true 244 | * @exception SocketException 如果无法断开 UDP 套接字的关联则抛出 245 | */ 246 | void disconnect() noexcept(false); 247 | 248 | /** 249 | * 将给定缓冲区作为 UDP 数据报发送到指定的地址/端口 250 | * @param buffer 要写入的缓冲区 251 | * @param bufferLen 要写入的字节数 252 | * @param foreignAddress 要发送到的地址（IP 地址或名称） 253 | * @param foreignPort 要发送到的端口号 254 | * @return 如果发送成功则返回 true 255 | * @exception SocketException 如果无法发送数据报则抛出 256 | */ 257 | void sendTo(const void *buffer, int bufferLen, const string &foreignAddress, 258 | unsigned short foreignPort) noexcept(false); 259 | 260 | /** 261 | * 从此套接字读取最多 bufferLen 字节数据。数据将被放置在给定的缓冲区中 262 | * @param buffer 接收数据的缓冲区 263 | * @param bufferLen 最多接收的字节数 264 | * @param sourceAddress 数据报源的地址 265 | * @param sourcePort 数据源的端口 266 | * @return 接收的字节数和错误为 -1 267 | * @exception SocketException 如果无法接收数据报则抛出 268 | */ 269 | int recvFrom(void *buffer, int bufferLen, string &sourceAddress, unsigned short &sourcePort) noexcept(false); 270 | 271 | /** 272 | * 设置多播 TTL 273 | * @param multicastTTL 多播 TTL 274 | * @exception SocketException 如果无法设置 TTL 则抛出 275 | */ 276 | void setMulticastTTL(unsigned char multicastTTL) noexcept(false); 277 | 278 | /** 279 | * 加入指定的多播组 280 | * @param multicastGroup 要加入的多播组地址 281 | * @exception SocketException 如果无法加入组则抛出 282 | */ 283 | void joinGroup(const string &multicastGroup) noexcept(false); 284 | 285 | /** 286 | * 离开指定的多播组 287 | * @param multicastGroup 要离开的多播组地址 288 | * @exception SocketException 如果无法离开组则抛出 289 | */ 290 | void leaveGroup(const string &multicastGroup) noexcept(false); 291 | 292 | private: 293 | void setBroadcast(); 294 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/serial_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "serial.h" 7 | #include "crc.h" 8 | 9 | class SerialManager { 10 | static constexpr size_t IMU_DATA_NUM = 52; 11 | static constexpr size_t BUF_LEN = 65540; 12 | 13 | public: 14 | explicit SerialManager(const std::string &port); 15 | 16 | ~SerialManager(); 17 | 18 | [[nodiscard]] bool is_available() const { return serial_.isOpen(); } 19 | void Start(); 20 | void Stop(); 21 | 22 | private: 23 | 24 | void Receive(); 25 | 26 | void TimeStampSynchronization(); 27 | Crc crc_; 28 | std::string port_; 29 | serial::Serial serial_; 30 | std::thread rx_thread_, tx_thread_; 31 | uint64_t time_t1_{0}; 32 | uint64_t time_t2_{0}; 33 | bool updated_t1_t2_{false}; 34 | 35 | uint64_t frame_count_{0}; 36 | 37 | bool started_{false}; 38 | }; 39 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/udp_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "data_manager.h" 5 | #include "crc.h" 6 | 7 | #include "practical_socket.h" 8 | 9 | class UdpManager { 10 | static constexpr size_t IMU_DATA_NUM = 52; 11 | static constexpr size_t BUF_LEN = 65540; 12 | 13 | public: 14 | explicit UdpManager(std::string target_ip, uint16_t port,bool use_sync_time = true); 15 | 16 | ~UdpManager(); 17 | void Start(); 18 | void Stop(); 19 | private: 20 | void Receive(); 21 | void TimeStampSynchronization(); 22 | 23 | std::thread rx_thread_, tx_thread_; 24 | std::atomic udp_connected_{false}; 25 | unsigned short port_{}; 26 | std::string target_ip_; 27 | UDPSocket sock_; 28 | Crc crc_; 29 | 30 | uint64_t time_t1_{0}; 31 | uint64_t time_t2_{0}; 32 | bool updated_t1_t2_{false}; 33 | 34 | uint64_t frame_count_{0}; 35 | 36 | bool started_{false}; 37 | bool use_sync_time_{true}; 38 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/unix.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file serial/impl/unix.h 3 | * \author William Woodall 4 | * \author John Harrison 5 | * \version 0.1 6 | * 7 | * \section LICENSE 8 | * 9 | * The MIT License 10 | * 11 | * Copyright (c) 2012 William Woodall, John Harrison 12 | * 13 | * Permission is hereby granted, free of charge, to any person obtaining a 14 | * copy of this software and associated documentation files (the "Software"), 15 | * to deal in the Software without restriction, including without limitation 16 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 17 | * and/or sell copies of the Software, and to permit persons to whom the 18 | * Software is furnished to do so, subject to the following conditions: 19 | * 20 | * The above copyright notice and this permission notice shall be included in 21 | * all copies or substantial portions of the Software. 22 | * 23 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 28 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | * DEALINGS IN THE SOFTWARE. 30 | * 31 | * \section DESCRIPTION 32 | * 33 | * This provides a unix based pimpl for the Serial class. This implementation is 34 | * based off termios.h and uses select for multiplexing the IO ports. 35 | * 36 | */ 37 | 38 | #if !defined(_WIN32) 39 | 40 | #ifndef SERIAL_IMPL_UNIX_H 41 | #define SERIAL_IMPL_UNIX_H 42 | 43 | #include "serial.h" 44 | 45 | #include 46 | 47 | namespace serial { 48 | 49 | using std::invalid_argument; 50 | using std::size_t; 51 | using std::string; 52 | 53 | using serial::IOException; 54 | using serial::SerialException; 55 | 56 | class MillisecondTimer { 57 | public: 58 | MillisecondTimer(const uint32_t millis); 59 | int64_t remaining(); 60 | 61 | private: 62 | static timespec timespec_now(); 63 | timespec expiry; 64 | }; 65 | 66 | class serial::Serial::SerialImpl { 67 | public: 68 | SerialImpl(const string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity, stopbits_t stopbits, 69 | flowcontrol_t flowcontrol); 70 | 71 | virtual ~SerialImpl(); 72 | 73 | void open(); 74 | 75 | void close(); 76 | 77 | bool isOpen() const; 78 | 79 | size_t available(); 80 | 81 | bool waitReadable(uint32_t timeout); 82 | 83 | void waitByteTimes(size_t count); 84 | 85 | size_t read(uint8_t *buf, size_t size = 1); 86 | 87 | size_t write(const uint8_t *data, size_t length); 88 | 89 | void flush(); 90 | 91 | void flushInput(); 92 | 93 | void flushOutput(); 94 | 95 | void sendBreak(int duration); 96 | 97 | void setBreak(bool level); 98 | 99 | void setRTS(bool level); 100 | 101 | void setDTR(bool level); 102 | 103 | bool waitForChange(); 104 | 105 | bool getCTS(); 106 | 107 | bool getDSR(); 108 | 109 | bool getRI(); 110 | 111 | bool getCD(); 112 | 113 | void setPort(const string &port); 114 | 115 | string getPort() const; 116 | 117 | void setTimeout(Timeout &timeout); 118 | 119 | Timeout getTimeout() const; 120 | 121 | void setBaudrate(unsigned long baudrate); 122 | 123 | unsigned long getBaudrate() const; 124 | 125 | void setBytesize(bytesize_t bytesize); 126 | 127 | bytesize_t getBytesize() const; 128 | 129 | void setParity(parity_t parity); 130 | 131 | parity_t getParity() const; 132 | 133 | void setStopbits(stopbits_t stopbits); 134 | 135 | stopbits_t getStopbits() const; 136 | 137 | void setFlowcontrol(flowcontrol_t flowcontrol); 138 | 139 | flowcontrol_t getFlowcontrol() const; 140 | 141 | void readLock(); 142 | 143 | void readUnlock(); 144 | 145 | void writeLock(); 146 | 147 | void writeUnlock(); 148 | 149 | protected: 150 | void reconfigurePort(); 151 | 152 | private: 153 | string port_; // Path to the file descriptor 154 | int fd_; // The current file descriptor 155 | 156 | bool is_open_; 157 | bool xonxoff_; 158 | bool rtscts_; 159 | 160 | Timeout timeout_; // Timeout for read operations 161 | unsigned long baudrate_; // Baudrate 162 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 163 | 164 | parity_t parity_; // Parity 165 | bytesize_t bytesize_; // Size of the bytes 166 | stopbits_t stopbits_; // Stop Bits 167 | flowcontrol_t flowcontrol_; // Flow Control 168 | 169 | // Mutex used to lock the read functions 170 | pthread_mutex_t read_mutex; 171 | // Mutex used to lock the write functions 172 | pthread_mutex_t write_mutex; 173 | }; 174 | 175 | } // namespace serial 176 | 177 | #endif // SERIAL_IMPL_UNIX_H 178 | 179 | #endif // !defined(_WIN32) 180 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/v8stdint.h: -------------------------------------------------------------------------------- 1 | // This header is from the v8 google project: 2 | // http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h 3 | 4 | // Copyright 2012 the V8 project authors. All rights reserved. 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above 12 | // copyright notice, this list of conditions and the following 13 | // disclaimer in the documentation and/or other materials provided 14 | // with the distribution. 15 | // * Neither the name of Google Inc. nor the names of its 16 | // contributors may be used to endorse or promote products derived 17 | // from this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | // Load definitions of standard types. 32 | 33 | #ifndef V8STDINT_H_ 34 | #define V8STDINT_H_ 35 | 36 | #include 37 | #include 38 | 39 | #if defined(_WIN32) && !defined(__MINGW32__) 40 | 41 | typedef signed char int8_t; 42 | typedef unsigned char uint8_t; 43 | typedef short int16_t; // NOLINT 44 | typedef unsigned short uint16_t; // NOLINT 45 | typedef int int32_t; 46 | typedef unsigned int uint32_t; 47 | typedef __int64 int64_t; 48 | typedef unsigned __int64 uint64_t; 49 | // intptr_t and friends are defined in crtdefs.h through stdio.h. 50 | 51 | #else 52 | 53 | #include 54 | 55 | #endif 56 | 57 | #endif // V8STDINT_H_ 58 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/include/win.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file serial/impl/windows.h 3 | * \author William Woodall 4 | * \author John Harrison 5 | * \version 0.1 6 | * 7 | * \section LICENSE 8 | * 9 | * The MIT License 10 | * 11 | * Copyright (c) 2012 William Woodall, John Harrison 12 | * 13 | * Permission is hereby granted, free of charge, to any person obtaining a 14 | * copy of this software and associated documentation files (the "Software"), 15 | * to deal in the Software without restriction, including without limitation 16 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 17 | * and/or sell copies of the Software, and to permit persons to whom the 18 | * Software is furnished to do so, subject to the following conditions: 19 | * 20 | * The above copyright notice and this permission notice shall be included in 21 | * all copies or substantial portions of the Software. 22 | * 23 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 28 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | * DEALINGS IN THE SOFTWARE. 30 | * 31 | * \section DESCRIPTION 32 | * 33 | * This provides a windows implementation of the Serial class interface. 34 | * 35 | */ 36 | 37 | #if defined(_WIN32) 38 | 39 | #ifndef SERIAL_IMPL_WINDOWS_H 40 | #define SERIAL_IMPL_WINDOWS_H 41 | 42 | #include "serial/serial.h" 43 | 44 | #include "windows.h" 45 | 46 | namespace serial { 47 | 48 | using std::invalid_argument; 49 | using std::string; 50 | using std::wstring; 51 | 52 | using serial::IOException; 53 | using serial::SerialException; 54 | 55 | class serial::Serial::SerialImpl { 56 | public: 57 | SerialImpl(const string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity, stopbits_t stopbits, 58 | flowcontrol_t flowcontrol); 59 | 60 | virtual ~SerialImpl(); 61 | 62 | void open(); 63 | 64 | void close(); 65 | 66 | bool isOpen() const; 67 | 68 | size_t available(); 69 | 70 | bool waitReadable(uint32_t timeout); 71 | 72 | void waitByteTimes(size_t count); 73 | 74 | size_t read(uint8_t *buf, size_t size = 1); 75 | 76 | size_t write(const uint8_t *data, size_t length); 77 | 78 | void flush(); 79 | 80 | void flushInput(); 81 | 82 | void flushOutput(); 83 | 84 | void sendBreak(int duration); 85 | 86 | void setBreak(bool level); 87 | 88 | void setRTS(bool level); 89 | 90 | void setDTR(bool level); 91 | 92 | bool waitForChange(); 93 | 94 | bool getCTS(); 95 | 96 | bool getDSR(); 97 | 98 | bool getRI(); 99 | 100 | bool getCD(); 101 | 102 | void setPort(const string &port); 103 | 104 | string getPort() const; 105 | 106 | void setTimeout(Timeout &timeout); 107 | 108 | Timeout getTimeout() const; 109 | 110 | void setBaudrate(unsigned long baudrate); 111 | 112 | unsigned long getBaudrate() const; 113 | 114 | void setBytesize(bytesize_t bytesize); 115 | 116 | bytesize_t getBytesize() const; 117 | 118 | void setParity(parity_t parity); 119 | 120 | parity_t getParity() const; 121 | 122 | void setStopbits(stopbits_t stopbits); 123 | 124 | stopbits_t getStopbits() const; 125 | 126 | void setFlowcontrol(flowcontrol_t flowcontrol); 127 | 128 | flowcontrol_t getFlowcontrol() const; 129 | 130 | void readLock(); 131 | 132 | void readUnlock(); 133 | 134 | void writeLock(); 135 | 136 | void writeUnlock(); 137 | 138 | protected: 139 | void reconfigurePort(); 140 | 141 | private: 142 | wstring port_; // Path to the file descriptor 143 | HANDLE fd_; 144 | 145 | bool is_open_; 146 | 147 | Timeout timeout_; // Timeout for read operations 148 | unsigned long baudrate_; // Baudrate 149 | 150 | parity_t parity_; // Parity 151 | bytesize_t bytesize_; // Size of the bytes 152 | stopbits_t stopbits_; // Stop Bits 153 | flowcontrol_t flowcontrol_; // Flow Control 154 | 155 | // Mutex used to lock the read functions 156 | HANDLE read_mutex; 157 | // Mutex used to lock the write functions 158 | HANDLE write_mutex; 159 | }; 160 | 161 | } // namespace serial 162 | 163 | #endif // SERIAL_IMPL_WINDOWS_H 164 | 165 | #endif // if defined(_WIN32) 166 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/CommonParameters.ini: -------------------------------------------------------------------------------- 1 | ;@~chinese 2 | ;该配置文件列出了部分可配置的参数，其他可配置参数请参考软件安装路径下 /opt/(软件名)/doc/工业相机SDK可配置化参数表.xlsx 3 | ;修改配置后上层应用程序要重新启动 4 | ;不分设备类型的通用参数 5 | ;@~english 6 | ;The configuration file lists some configurable parameters, other configurable parameters please refer to the software installation path /opt/(Software)/doc/Camera SDK configurable parameter table.xlsx 7 | ;When modifying the configuration, the upper application need restarted 8 | ;Generic parameters that do not distinguish device types 9 | [COMMON] 10 | ;@~chinese 11 | ;设置SDK内部图像缓存节点个数，若不调用接口(MV_CC_SetImageNodeNum)主动设置,默认为1，除双U口相机外 12 | ;双U口相机SDK内部多分配2个节点即ImageNodeNum+2,默认节点个数为3 13 | ;@~english 14 | ;Set up the number of image cache nodes within the SDK, and if you don't call the interface (MV_CC_SetImageNodeNum), the default image node number is 1, except for the double usb camera 15 | ;The double usb camera SDK alloc with extra two nodes, as ImageNodeNum+2, so the default image node number is 3 16 | ImageNodeNum=1 17 | 18 | ;@~chinese 19 | ;网口相机相关参数 20 | ;@~english 21 | ;The parameters of Gige camera 22 | [GIGE] 23 | ;@~chinese 24 | ;设置GVCP命令超时时间，默认500ms，范围：0-10000ms 25 | ;@~english 26 | ;Set GVCP command timeout time, the default value is 500ms, range: 0-10000ms 27 | GvcpTimeout=500 28 | 29 | ;@~chinese 30 | ;U口相机相关参数 31 | ;@~english 32 | ;The parameters of U3V camera 33 | [U3V] 34 | ;@~chinese 35 | ;设置U3V的传输包大小，Byte，默认为1M，rang：>=0x400 36 | ;@~english 37 | ;Set transfer size of U3V device, the unit is Byte, default: 1M，rang: >=0x400 38 | TransferSize=1048576 39 | ;@~chinese 40 | ;设置流包间隔超时时间，默认20ms 41 | ;@~english 42 | ;Set stream payload timeout, the default value is 20ms 43 | StreamPayloadTimeout=20 44 | ;@~chinese 45 | ;设置出流寄存器读写超时时间，默认30ms 46 | ;@~english 47 | ;Set stream control register timeout, the default value is 30ms 48 | SIControlRegTimeout=30 49 | ;@~chinese 50 | ;设置控制寄存器读写超时时间，默认1000ms 51 | ;除SI寄存器外 52 | ;@~english 53 | ;Set control Reg timeout ms,default 1000ms 54 | ;Except SI Reg 55 | SyncTimeout=1000 56 | 57 | ;@~chinese 58 | ;CameraLink相机相关参数 59 | ;@~english 60 | ;The parameters of CameraLink camera 61 | [CAML] 62 | 63 | ;@~chinese 64 | ;图像处理相关的参数 65 | ;@~english 66 | ;The parameters of image processing 67 | [IMG_PROCESSING] 68 | ;@~chinese 69 | ;设置插值算法类型，0-快速 1-均衡 2-最优 3-最优+（默认为均衡） 70 | ;@~english 71 | ;Interpolation algorithm type setting, 0-Fast 1-Equilibrium 2-Optimal 3-Optimal+(the default value is 1-Equilibrium) 72 | BayerCvtQuality=1 73 | ;@~chinese 74 | ;设置插值算法处理线程个数，0-自适应 其他-具体线程个数(1,2,3,...)（默认线程个数为4） 75 | ;@~english 76 | ;Set the interpolation algorithm of thread handle count, 0-self-adapting, other-number of specific thread count(1,2,3,...) (the default thread count is 4) 77 | BayerCvtThreadNum=4 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/MvProducerGEV.cti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/MvProducerGEV.cti -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/MvProducerU3V.cti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/MvProducerU3V.cti -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libFormatConversion.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libFormatConversion.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMVGigEVisionSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMVGigEVisionSDK.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMVGigEVisionSDK.so.4.4.1.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMVGigEVisionSDK.so.4.4.1.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMVRender.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMVRender.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMediaProcess.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMediaProcess.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvCameraControl.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvCameraControl.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvCameraControl.so.4.4.1.3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvCameraControl.so.4.4.1.3 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvCameraControlWrapper.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvCameraControlWrapper.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvCameraControlWrapper.so.2.4.0.6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvCameraControlWrapper.so.2.4.0.6 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvSDKVersion.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvSDKVersion.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvUsb3vTL.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvUsb3vTL.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libMvUsb3vTL.so.4.4.1.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libMvUsb3vTL.so.4.4.1.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libavutil.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libavutil.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libfusion.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libfusion.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libinfinite_sense_core.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libinfinite_sense_core.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_core.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_core.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_core.so.4.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_core.so.4.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_core.so.4.2.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_core.so.4.2.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so.4.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so.4.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so.4.2.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libopencv_imgproc.so.4.2.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libserial.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libserial.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libswscale.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libswscale.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libudp.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libudp.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/arm/lib/libusb-1.0.so.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/arm/lib/libusb-1.0.so.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/Fusion.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Fusion.h 3 | * @author Seb Madgwick 4 | * @brief Main header file for the Fusion library. This is the only file that 5 | * needs to be included when using the library. 6 | */ 7 | 8 | #ifndef FUSION_H 9 | #define FUSION_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "FusionAhrs.h" 19 | #include "FusionAxes.h" 20 | #include "FusionCalibration.h" 21 | #include "FusionCompass.h" 22 | #include "FusionConvention.h" 23 | #include "FusionMath.h" 24 | #include "FusionOffset.h" 25 | 26 | #ifdef __cplusplus 27 | } 28 | #endif 29 | 30 | #endif 31 | //------------------------------------------------------------------------------ 32 | // End of file 33 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionAhrs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionAhrs.h 3 | * @author Seb Madgwick 4 | * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer 5 | * measurements into a single measurement of orientation relative to the Earth. 6 | */ 7 | 8 | #ifndef FUSION_AHRS_H 9 | #define FUSION_AHRS_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionConvention.h" 15 | #include "FusionMath.h" 16 | #include 17 | 18 | //------------------------------------------------------------------------------ 19 | // Definitions 20 | 21 | /** 22 | * @brief AHRS algorithm settings. 23 | */ 24 | typedef struct { 25 | FusionConvention convention; 26 | float gain; 27 | float gyroscopeRange; 28 | float accelerationRejection; 29 | float magneticRejection; 30 | unsigned int recoveryTriggerPeriod; 31 | } FusionAhrsSettings; 32 | 33 | /** 34 | * @brief AHRS algorithm structure. Structure members are used internally and 35 | * must not be accessed by the application. 36 | */ 37 | typedef struct { 38 | FusionAhrsSettings settings; 39 | FusionQuaternion quaternion; 40 | FusionVector accelerometer; 41 | bool initialising; 42 | float rampedGain; 43 | float rampedGainStep; 44 | bool angularRateRecovery; 45 | FusionVector halfAccelerometerFeedback; 46 | FusionVector halfMagnetometerFeedback; 47 | bool accelerometerIgnored; 48 | int accelerationRecoveryTrigger; 49 | int accelerationRecoveryTimeout; 50 | bool magnetometerIgnored; 51 | int magneticRecoveryTrigger; 52 | int magneticRecoveryTimeout; 53 | } FusionAhrs; 54 | 55 | /** 56 | * @brief AHRS algorithm internal states. 57 | */ 58 | typedef struct { 59 | float accelerationError; 60 | bool accelerometerIgnored; 61 | float accelerationRecoveryTrigger; 62 | float magneticError; 63 | bool magnetometerIgnored; 64 | float magneticRecoveryTrigger; 65 | } FusionAhrsInternalStates; 66 | 67 | /** 68 | * @brief AHRS algorithm flags. 69 | */ 70 | typedef struct { 71 | bool initialising; 72 | bool angularRateRecovery; 73 | bool accelerationRecovery; 74 | bool magneticRecovery; 75 | } FusionAhrsFlags; 76 | 77 | //------------------------------------------------------------------------------ 78 | // Function declarations 79 | 80 | void FusionAhrsInitialise(FusionAhrs *const ahrs); 81 | 82 | void FusionAhrsReset(FusionAhrs *const ahrs); 83 | 84 | void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); 85 | 86 | void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); 87 | 88 | void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); 89 | 90 | void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); 91 | 92 | FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); 93 | 94 | void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion); 95 | 96 | FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs); 97 | 98 | FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); 99 | 100 | FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); 101 | 102 | FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); 103 | 104 | FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs); 105 | 106 | void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); 107 | 108 | #endif 109 | 110 | //------------------------------------------------------------------------------ 111 | // End of file 112 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionAxes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionAxes.h 3 | * @author Seb Madgwick 4 | * @brief Swaps sensor axes for alignment with the body axes. 5 | */ 6 | 7 | #ifndef FUSION_AXES_H 8 | #define FUSION_AXES_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include "FusionMath.h" 14 | 15 | //------------------------------------------------------------------------------ 16 | // Definitions 17 | 18 | /** 19 | * @brief Axes alignment describing the sensor axes relative to the body axes. 20 | * For example, if the body X axis is aligned with the sensor Y axis and the 21 | * body Y axis is aligned with sensor X axis but pointing the opposite direction 22 | * then alignment is +Y-X+Z. 23 | */ 24 | typedef enum { 25 | FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ 26 | FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ 27 | FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ 28 | FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ 29 | FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ 30 | FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ 31 | FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ 32 | FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ 33 | FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ 34 | FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ 35 | FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ 36 | FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ 37 | FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ 38 | FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ 39 | FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ 40 | FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ 41 | FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ 42 | FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ 43 | FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ 44 | FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ 45 | FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ 46 | FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ 47 | FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ 48 | FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ 49 | } FusionAxesAlignment; 50 | 51 | //------------------------------------------------------------------------------ 52 | // Inline functions 53 | 54 | /** 55 | * @brief Swaps sensor axes for alignment with the body axes. 56 | * @param sensor Sensor axes. 57 | * @param alignment Axes alignment. 58 | * @return Sensor axes aligned with the body axes. 59 | */ 60 | static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { 61 | FusionVector result; 62 | switch (alignment) { 63 | case FusionAxesAlignmentPXPYPZ: 64 | break; 65 | case FusionAxesAlignmentPXNZPY: 66 | result.axis.x = +sensor.axis.x; 67 | result.axis.y = -sensor.axis.z; 68 | result.axis.z = +sensor.axis.y; 69 | return result; 70 | case FusionAxesAlignmentPXNYNZ: 71 | result.axis.x = +sensor.axis.x; 72 | result.axis.y = -sensor.axis.y; 73 | result.axis.z = -sensor.axis.z; 74 | return result; 75 | case FusionAxesAlignmentPXPZNY: 76 | result.axis.x = +sensor.axis.x; 77 | result.axis.y = +sensor.axis.z; 78 | result.axis.z = -sensor.axis.y; 79 | return result; 80 | case FusionAxesAlignmentNXPYNZ: 81 | result.axis.x = -sensor.axis.x; 82 | result.axis.y = +sensor.axis.y; 83 | result.axis.z = -sensor.axis.z; 84 | return result; 85 | case FusionAxesAlignmentNXPZPY: 86 | result.axis.x = -sensor.axis.x; 87 | result.axis.y = +sensor.axis.z; 88 | result.axis.z = +sensor.axis.y; 89 | return result; 90 | case FusionAxesAlignmentNXNYPZ: 91 | result.axis.x = -sensor.axis.x; 92 | result.axis.y = -sensor.axis.y; 93 | result.axis.z = +sensor.axis.z; 94 | return result; 95 | case FusionAxesAlignmentNXNZNY: 96 | result.axis.x = -sensor.axis.x; 97 | result.axis.y = -sensor.axis.z; 98 | result.axis.z = -sensor.axis.y; 99 | return result; 100 | case FusionAxesAlignmentPYNXPZ: 101 | result.axis.x = +sensor.axis.y; 102 | result.axis.y = -sensor.axis.x; 103 | result.axis.z = +sensor.axis.z; 104 | return result; 105 | case FusionAxesAlignmentPYNZNX: 106 | result.axis.x = +sensor.axis.y; 107 | result.axis.y = -sensor.axis.z; 108 | result.axis.z = -sensor.axis.x; 109 | return result; 110 | case FusionAxesAlignmentPYPXNZ: 111 | result.axis.x = +sensor.axis.y; 112 | result.axis.y = +sensor.axis.x; 113 | result.axis.z = -sensor.axis.z; 114 | return result; 115 | case FusionAxesAlignmentPYPZPX: 116 | result.axis.x = +sensor.axis.y; 117 | result.axis.y = +sensor.axis.z; 118 | result.axis.z = +sensor.axis.x; 119 | return result; 120 | case FusionAxesAlignmentNYPXPZ: 121 | result.axis.x = -sensor.axis.y; 122 | result.axis.y = +sensor.axis.x; 123 | result.axis.z = +sensor.axis.z; 124 | return result; 125 | case FusionAxesAlignmentNYNZPX: 126 | result.axis.x = -sensor.axis.y; 127 | result.axis.y = -sensor.axis.z; 128 | result.axis.z = +sensor.axis.x; 129 | return result; 130 | case FusionAxesAlignmentNYNXNZ: 131 | result.axis.x = -sensor.axis.y; 132 | result.axis.y = -sensor.axis.x; 133 | result.axis.z = -sensor.axis.z; 134 | return result; 135 | case FusionAxesAlignmentNYPZNX: 136 | result.axis.x = -sensor.axis.y; 137 | result.axis.y = +sensor.axis.z; 138 | result.axis.z = -sensor.axis.x; 139 | return result; 140 | case FusionAxesAlignmentPZPYNX: 141 | result.axis.x = +sensor.axis.z; 142 | result.axis.y = +sensor.axis.y; 143 | result.axis.z = -sensor.axis.x; 144 | return result; 145 | case FusionAxesAlignmentPZPXPY: 146 | result.axis.x = +sensor.axis.z; 147 | result.axis.y = +sensor.axis.x; 148 | result.axis.z = +sensor.axis.y; 149 | return result; 150 | case FusionAxesAlignmentPZNYPX: 151 | result.axis.x = +sensor.axis.z; 152 | result.axis.y = -sensor.axis.y; 153 | result.axis.z = +sensor.axis.x; 154 | return result; 155 | case FusionAxesAlignmentPZNXNY: 156 | result.axis.x = +sensor.axis.z; 157 | result.axis.y = -sensor.axis.x; 158 | result.axis.z = -sensor.axis.y; 159 | return result; 160 | case FusionAxesAlignmentNZPYPX: 161 | result.axis.x = -sensor.axis.z; 162 | result.axis.y = +sensor.axis.y; 163 | result.axis.z = +sensor.axis.x; 164 | return result; 165 | case FusionAxesAlignmentNZNXPY: 166 | result.axis.x = -sensor.axis.z; 167 | result.axis.y = -sensor.axis.x; 168 | result.axis.z = +sensor.axis.y; 169 | return result; 170 | case FusionAxesAlignmentNZNYNX: 171 | result.axis.x = -sensor.axis.z; 172 | result.axis.y = -sensor.axis.y; 173 | result.axis.z = -sensor.axis.x; 174 | return result; 175 | case FusionAxesAlignmentNZPXNY: 176 | result.axis.x = -sensor.axis.z; 177 | result.axis.y = +sensor.axis.x; 178 | result.axis.z = -sensor.axis.y; 179 | return result; 180 | } 181 | return sensor; // avoid compiler warning 182 | } 183 | 184 | #endif 185 | 186 | //------------------------------------------------------------------------------ 187 | // End of file 188 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionCalibration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionCalibration.h 3 | * @author Seb Madgwick 4 | * @brief Gyroscope, accelerometer, and magnetometer calibration models. 5 | */ 6 | 7 | #ifndef FUSION_CALIBRATION_H 8 | #define FUSION_CALIBRATION_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include "FusionMath.h" 14 | 15 | //------------------------------------------------------------------------------ 16 | // Inline functions 17 | 18 | /** 19 | * @brief Gyroscope and accelerometer calibration model. 20 | * @param uncalibrated Uncalibrated measurement. 21 | * @param misalignment Misalignment matrix. 22 | * @param sensitivity Sensitivity. 23 | * @param offset Offset. 24 | * @return Calibrated measurement. 25 | */ 26 | static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { 27 | return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); 28 | } 29 | 30 | /** 31 | * @brief Magnetometer calibration model. 32 | * @param uncalibrated Uncalibrated measurement. 33 | * @param softIronMatrix Soft-iron matrix. 34 | * @param hardIronOffset Hard-iron offset. 35 | * @return Calibrated measurement. 36 | */ 37 | static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { 38 | return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset)); 39 | } 40 | 41 | #endif 42 | 43 | //------------------------------------------------------------------------------ 44 | // End of file 45 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionCompass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionCompass.h 3 | * @author Seb Madgwick 4 | * @brief Tilt-compensated compass to calculate the magnetic heading using 5 | * accelerometer and magnetometer measurements. 6 | */ 7 | 8 | #ifndef FUSION_COMPASS_H 9 | #define FUSION_COMPASS_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionConvention.h" 15 | #include "FusionMath.h" 16 | 17 | //------------------------------------------------------------------------------ 18 | // Function declarations 19 | 20 | float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer); 21 | 22 | #endif 23 | 24 | //------------------------------------------------------------------------------ 25 | // End of file 26 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionConvention.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionConvention.h 3 | * @author Seb Madgwick 4 | * @brief Earth axes convention. 5 | */ 6 | 7 | #ifndef FUSION_CONVENTION_H 8 | #define FUSION_CONVENTION_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Definitions 12 | 13 | /** 14 | * @brief Earth axes convention. 15 | */ 16 | typedef enum { 17 | FusionConventionNwu, /* North-West-Up */ 18 | FusionConventionEnu, /* East-North-Up */ 19 | FusionConventionNed, /* North-East-Down */ 20 | } FusionConvention; 21 | 22 | #endif 23 | 24 | //------------------------------------------------------------------------------ 25 | // End of file 26 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionMath.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionMath.h 3 | * @author Seb Madgwick 4 | * @brief Math library. 5 | */ 6 | 7 | #ifndef FUSION_MATH_H 8 | #define FUSION_MATH_H 9 | 10 | //------------------------------------------------------------------------------ 11 | // Includes 12 | 13 | #include // M_PI, sqrtf, atan2f, asinf 14 | #include 15 | #include 16 | 17 | //------------------------------------------------------------------------------ 18 | // Definitions 19 | 20 | /** 21 | * @brief 3D vector. 22 | */ 23 | typedef union { 24 | float array[3]; 25 | 26 | struct { 27 | float x; 28 | float y; 29 | float z; 30 | } axis; 31 | } FusionVector; 32 | 33 | /** 34 | * @brief Quaternion. 35 | */ 36 | typedef union { 37 | float array[4]; 38 | 39 | struct { 40 | float w; 41 | float x; 42 | float y; 43 | float z; 44 | } element; 45 | } FusionQuaternion; 46 | 47 | /** 48 | * @brief 3x3 matrix in row-major order. 49 | * See http://en.wikipedia.org/wiki/Row-major_order 50 | */ 51 | typedef union { 52 | float array[3][3]; 53 | 54 | struct { 55 | float xx; 56 | float xy; 57 | float xz; 58 | float yx; 59 | float yy; 60 | float yz; 61 | float zx; 62 | float zy; 63 | float zz; 64 | } element; 65 | } FusionMatrix; 66 | 67 | /** 68 | * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around 69 | * X, Y, and Z respectively. 70 | */ 71 | typedef union { 72 | float array[3]; 73 | 74 | struct { 75 | float roll; 76 | float pitch; 77 | float yaw; 78 | } angle; 79 | } FusionEuler; 80 | 81 | /** 82 | * @brief Vector of zeros. 83 | */ 84 | #define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) 85 | 86 | /** 87 | * @brief Vector of ones. 88 | */ 89 | #define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) 90 | 91 | /** 92 | * @brief Identity quaternion. 93 | */ 94 | #define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) 95 | 96 | /** 97 | * @brief Identity matrix. 98 | */ 99 | #define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) 100 | 101 | /** 102 | * @brief Euler angles of zero. 103 | */ 104 | #define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) 105 | 106 | /** 107 | * @brief Pi. May not be defined in math.h. 108 | */ 109 | #ifndef M_PI 110 | #define M_PI (3.14159265358979323846) 111 | #endif 112 | 113 | /** 114 | * @brief Include this definition or add as a preprocessor definition to use 115 | * normal square root operations. 116 | */ 117 | //#define FUSION_USE_NORMAL_SQRT 118 | 119 | //------------------------------------------------------------------------------ 120 | // Inline functions - Degrees and radians conversion 121 | 122 | /** 123 | * @brief Converts degrees to radians. 124 | * @param degrees Degrees. 125 | * @return Radians. 126 | */ 127 | static inline float FusionDegreesToRadians(const float degrees) { 128 | return degrees * ((float) M_PI / 180.0f); 129 | } 130 | 131 | /** 132 | * @brief Converts radians to degrees. 133 | * @param radians Radians. 134 | * @return Degrees. 135 | */ 136 | static inline float FusionRadiansToDegrees(const float radians) { 137 | return radians * (180.0f / (float) M_PI); 138 | } 139 | 140 | //------------------------------------------------------------------------------ 141 | // Inline functions - Arc sine 142 | 143 | /** 144 | * @brief Returns the arc sine of the value. 145 | * @param value Value. 146 | * @return Arc sine of the value. 147 | */ 148 | static inline float FusionAsin(const float value) { 149 | if (value <= -1.0f) { 150 | return (float) M_PI / -2.0f; 151 | } 152 | if (value >= 1.0f) { 153 | return (float) M_PI / 2.0f; 154 | } 155 | return asinf(value); 156 | } 157 | 158 | //------------------------------------------------------------------------------ 159 | // Inline functions - Fast inverse square root 160 | 161 | #ifndef FUSION_USE_NORMAL_SQRT 162 | 163 | /** 164 | * @brief Calculates the reciprocal of the square root. 165 | * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ 166 | * @param x Operand. 167 | * @return Reciprocal of the square root of x. 168 | */ 169 | static inline float FusionFastInverseSqrt(const float x) { 170 | 171 | typedef union { 172 | float f; 173 | int32_t i; 174 | } Union32; 175 | 176 | Union32 union32 = {.f = x}; 177 | union32.i = 0x5F1F1412 - (union32.i >> 1); 178 | return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); 179 | } 180 | 181 | #endif 182 | 183 | //------------------------------------------------------------------------------ 184 | // Inline functions - Vector operations 185 | 186 | /** 187 | * @brief Returns true if the vector is zero. 188 | * @param vector Vector. 189 | * @return True if the vector is zero. 190 | */ 191 | static inline bool FusionVectorIsZero(const FusionVector vector) { 192 | return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); 193 | } 194 | 195 | /** 196 | * @brief Returns the sum of two vectors. 197 | * @param vectorA Vector A. 198 | * @param vectorB Vector B. 199 | * @return Sum of two vectors. 200 | */ 201 | static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { 202 | const FusionVector result = {.axis = { 203 | .x = vectorA.axis.x + vectorB.axis.x, 204 | .y = vectorA.axis.y + vectorB.axis.y, 205 | .z = vectorA.axis.z + vectorB.axis.z, 206 | }}; 207 | return result; 208 | } 209 | 210 | /** 211 | * @brief Returns vector B subtracted from vector A. 212 | * @param vectorA Vector A. 213 | * @param vectorB Vector B. 214 | * @return Vector B subtracted from vector A. 215 | */ 216 | static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { 217 | const FusionVector result = {.axis = { 218 | .x = vectorA.axis.x - vectorB.axis.x, 219 | .y = vectorA.axis.y - vectorB.axis.y, 220 | .z = vectorA.axis.z - vectorB.axis.z, 221 | }}; 222 | return result; 223 | } 224 | 225 | /** 226 | * @brief Returns the sum of the elements. 227 | * @param vector Vector. 228 | * @return Sum of the elements. 229 | */ 230 | static inline float FusionVectorSum(const FusionVector vector) { 231 | return vector.axis.x + vector.axis.y + vector.axis.z; 232 | } 233 | 234 | /** 235 | * @brief Returns the multiplication of a vector by a scalar. 236 | * @param vector Vector. 237 | * @param scalar Scalar. 238 | * @return Multiplication of a vector by a scalar. 239 | */ 240 | static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { 241 | const FusionVector result = {.axis = { 242 | .x = vector.axis.x * scalar, 243 | .y = vector.axis.y * scalar, 244 | .z = vector.axis.z * scalar, 245 | }}; 246 | return result; 247 | } 248 | 249 | /** 250 | * @brief Calculates the Hadamard product (element-wise multiplication). 251 | * @param vectorA Vector A. 252 | * @param vectorB Vector B. 253 | * @return Hadamard product. 254 | */ 255 | static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { 256 | const FusionVector result = {.axis = { 257 | .x = vectorA.axis.x * vectorB.axis.x, 258 | .y = vectorA.axis.y * vectorB.axis.y, 259 | .z = vectorA.axis.z * vectorB.axis.z, 260 | }}; 261 | return result; 262 | } 263 | 264 | /** 265 | * @brief Returns the cross product. 266 | * @param vectorA Vector A. 267 | * @param vectorB Vector B. 268 | * @return Cross product. 269 | */ 270 | static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { 271 | #define A vectorA.axis 272 | #define B vectorB.axis 273 | const FusionVector result = {.axis = { 274 | .x = A.y * B.z - A.z * B.y, 275 | .y = A.z * B.x - A.x * B.z, 276 | .z = A.x * B.y - A.y * B.x, 277 | }}; 278 | return result; 279 | #undef A 280 | #undef B 281 | } 282 | 283 | /** 284 | * @brief Returns the dot product. 285 | * @param vectorA Vector A. 286 | * @param vectorB Vector B. 287 | * @return Dot product. 288 | */ 289 | static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) { 290 | return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); 291 | } 292 | 293 | /** 294 | * @brief Returns the vector magnitude squared. 295 | * @param vector Vector. 296 | * @return Vector magnitude squared. 297 | */ 298 | static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { 299 | return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); 300 | } 301 | 302 | /** 303 | * @brief Returns the vector magnitude. 304 | * @param vector Vector. 305 | * @return Vector magnitude. 306 | */ 307 | static inline float FusionVectorMagnitude(const FusionVector vector) { 308 | return sqrtf(FusionVectorMagnitudeSquared(vector)); 309 | } 310 | 311 | /** 312 | * @brief Returns the normalised vector. 313 | * @param vector Vector. 314 | * @return Normalised vector. 315 | */ 316 | static inline FusionVector FusionVectorNormalise(const FusionVector vector) { 317 | #ifdef FUSION_USE_NORMAL_SQRT 318 | const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); 319 | #else 320 | const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); 321 | #endif 322 | return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); 323 | } 324 | 325 | //------------------------------------------------------------------------------ 326 | // Inline functions - Quaternion operations 327 | 328 | /** 329 | * @brief Returns the sum of two quaternions. 330 | * @param quaternionA Quaternion A. 331 | * @param quaternionB Quaternion B. 332 | * @return Sum of two quaternions. 333 | */ 334 | static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { 335 | const FusionQuaternion result = {.element = { 336 | .w = quaternionA.element.w + quaternionB.element.w, 337 | .x = quaternionA.element.x + quaternionB.element.x, 338 | .y = quaternionA.element.y + quaternionB.element.y, 339 | .z = quaternionA.element.z + quaternionB.element.z, 340 | }}; 341 | return result; 342 | } 343 | 344 | /** 345 | * @brief Returns the multiplication of two quaternions. 346 | * @param quaternionA Quaternion A (to be post-multiplied). 347 | * @param quaternionB Quaternion B (to be pre-multiplied). 348 | * @return Multiplication of two quaternions. 349 | */ 350 | static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { 351 | #define A quaternionA.element 352 | #define B quaternionB.element 353 | const FusionQuaternion result = {.element = { 354 | .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, 355 | .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, 356 | .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, 357 | .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, 358 | }}; 359 | return result; 360 | #undef A 361 | #undef B 362 | } 363 | 364 | /** 365 | * @brief Returns the multiplication of a quaternion with a vector. This is a 366 | * normal quaternion multiplication where the vector is treated a 367 | * quaternion with a W element value of zero. The quaternion is post- 368 | * multiplied by the vector. 369 | * @param quaternion Quaternion. 370 | * @param vector Vector. 371 | * @return Multiplication of a quaternion with a vector. 372 | */ 373 | static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { 374 | #define Q quaternion.element 375 | #define V vector.axis 376 | const FusionQuaternion result = {.element = { 377 | .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, 378 | .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, 379 | .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, 380 | .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, 381 | }}; 382 | return result; 383 | #undef Q 384 | #undef V 385 | } 386 | 387 | /** 388 | * @brief Returns the normalised quaternion. 389 | * @param quaternion Quaternion. 390 | * @return Normalised quaternion. 391 | */ 392 | static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { 393 | #define Q quaternion.element 394 | #ifdef FUSION_USE_NORMAL_SQRT 395 | const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); 396 | #else 397 | const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); 398 | #endif 399 | const FusionQuaternion result = {.element = { 400 | .w = Q.w * magnitudeReciprocal, 401 | .x = Q.x * magnitudeReciprocal, 402 | .y = Q.y * magnitudeReciprocal, 403 | .z = Q.z * magnitudeReciprocal, 404 | }}; 405 | return result; 406 | #undef Q 407 | } 408 | 409 | //------------------------------------------------------------------------------ 410 | // Inline functions - Matrix operations 411 | 412 | /** 413 | * @brief Returns the multiplication of a matrix with a vector. 414 | * @param matrix Matrix. 415 | * @param vector Vector. 416 | * @return Multiplication of a matrix with a vector. 417 | */ 418 | static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { 419 | #define R matrix.element 420 | const FusionVector result = {.axis = { 421 | .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, 422 | .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, 423 | .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, 424 | }}; 425 | return result; 426 | #undef R 427 | } 428 | 429 | //------------------------------------------------------------------------------ 430 | // Inline functions - Conversion operations 431 | 432 | /** 433 | * @brief Converts a quaternion to a rotation matrix. 434 | * @param quaternion Quaternion. 435 | * @return Rotation matrix. 436 | */ 437 | static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { 438 | #define Q quaternion.element 439 | const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations 440 | const float qwqx = Q.w * Q.x; 441 | const float qwqy = Q.w * Q.y; 442 | const float qwqz = Q.w * Q.z; 443 | const float qxqy = Q.x * Q.y; 444 | const float qxqz = Q.x * Q.z; 445 | const float qyqz = Q.y * Q.z; 446 | const FusionMatrix matrix = {.element = { 447 | .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), 448 | .xy = 2.0f * (qxqy - qwqz), 449 | .xz = 2.0f * (qxqz + qwqy), 450 | .yx = 2.0f * (qxqy + qwqz), 451 | .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), 452 | .yz = 2.0f * (qyqz - qwqx), 453 | .zx = 2.0f * (qxqz - qwqy), 454 | .zy = 2.0f * (qyqz + qwqx), 455 | .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), 456 | }}; 457 | return matrix; 458 | #undef Q 459 | } 460 | 461 | /** 462 | * @brief Converts a quaternion to ZYX Euler angles in degrees. 463 | * @param quaternion Quaternion. 464 | * @return Euler angles in degrees. 465 | */ 466 | static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { 467 | #define Q quaternion.element 468 | const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations 469 | const FusionEuler euler = {.angle = { 470 | .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), 471 | .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), 472 | .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), 473 | }}; 474 | return euler; 475 | #undef Q 476 | } 477 | 478 | #endif 479 | 480 | //------------------------------------------------------------------------------ 481 | // End of file 482 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/FusionOffset.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FusionOffset.h 3 | * @author Seb Madgwick 4 | * @brief Gyroscope offset correction algorithm for run-time calibration of the 5 | * gyroscope offset. 6 | */ 7 | 8 | #ifndef FUSION_OFFSET_H 9 | #define FUSION_OFFSET_H 10 | 11 | //------------------------------------------------------------------------------ 12 | // Includes 13 | 14 | #include "FusionMath.h" 15 | 16 | //------------------------------------------------------------------------------ 17 | // Definitions 18 | 19 | /** 20 | * @brief Gyroscope offset algorithm structure. Structure members are used 21 | * internally and must not be accessed by the application. 22 | */ 23 | typedef struct { 24 | float filterCoefficient; 25 | unsigned int timeout; 26 | unsigned int timer; 27 | FusionVector gyroscopeOffset; 28 | } FusionOffset; 29 | 30 | //------------------------------------------------------------------------------ 31 | // Function declarations 32 | 33 | void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); 34 | 35 | FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); 36 | 37 | #endif 38 | 39 | //------------------------------------------------------------------------------ 40 | // End of file 41 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/MvCameraControl.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/include/MvCameraControl.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/MvErrorDefine.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/include/MvErrorDefine.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/MvISPErrorDefine.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/include/MvISPErrorDefine.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/ObsoleteCamParams.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/include/ObsoleteCamParams.h -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/cam_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "MvCameraControl.h" 6 | 7 | class CamManger { 8 | public: 9 | static CamManger& GetInstance() { 10 | static CamManger instance; 11 | return instance; 12 | } 13 | CamManger(const CamManger&) = delete; 14 | CamManger& operator=(const CamManger&) = delete; 15 | 16 | bool Initialization(); 17 | void Stop(); 18 | void Start(); 19 | void Enable() { is_running_ = true; } 20 | void Disable() { is_running_ = false; } 21 | void Restart(); 22 | private: 23 | 24 | CamManger(); 25 | ~CamManger(); 26 | void Receive(void* handle, const std::string&) const; 27 | static bool PrintDeviceInfo(MV_CC_DEVICE_INFO* pstMVDevInfo); 28 | 29 | std::vector rets_; 30 | std::vector handles_; 31 | std::vector cam_threads_; 32 | bool is_running_{false}; 33 | }; 34 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/crc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | class Crc { 4 | public: 5 | Crc(); 6 | ~Crc(); 7 | static uint16_t Crc16(const uint8_t *ptr, uint32_t len); 8 | static constexpr uint8_t start_package[3] = {0x80, 0x81, 0x82}; 9 | static constexpr uint8_t stop_package[3] = {0x83, 0x84, 0x85}; 10 | private: 11 | static uint16_t crc_tabs_[16]; 12 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/data_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "readerwriterqueue.h" 9 | 10 | struct __attribute__ ((packed)) IMU { 11 | uint64_t time_stamp; 12 | float count; 13 | float ax; 14 | float ay; 15 | float az; 16 | float gx; 17 | float gy; 18 | float gz; 19 | float tmp; 20 | float trigger; 21 | }; 22 | 23 | struct ImuData { 24 | uint64_t time_stamp_us; 25 | float ax; 26 | float ay; 27 | float az; 28 | float gx; 29 | float gy; 30 | float gz; 31 | float tmp; 32 | }; 33 | 34 | struct __attribute__ ((packed))TimeStampData { 35 | uint16_t head; 36 | uint64_t mark_1; 37 | uint64_t mark_2; 38 | }; 39 | 40 | struct ImgData { 41 | std::string camera_name; 42 | uint64_t time_stamp_us; 43 | cv::Mat image; 44 | }; 45 | 46 | class DataManger { 47 | public: 48 | static DataManger &GetInstance() { 49 | static DataManger instance; 50 | return instance; 51 | } 52 | DataManger(const DataManger &) = delete; 53 | DataManger &operator=(const DataManger &) = delete; 54 | 55 | void AddCamData(const std::string &name, const ImgData &data); 56 | bool GetNewCamData(const std::string &name, ImgData &data); 57 | void AddImuData(const ImuData &data); 58 | bool GetNewImuData(ImuData &data); 59 | void GetAllCamNames(std::vector &names) const; 60 | void SetLastTiggerTime(const uint64_t &time); 61 | uint64_t GetLastTiggerTime(); 62 | 63 | private: 64 | DataManger(); 65 | ~DataManger(); 66 | std::unordered_map>> all_cams_data_{}; 67 | moodycamel::ReaderWriterQueue imu_data_{512}; 68 | std::mutex last_trigger_lock_{}; 69 | std::atomic last_imu_trigger_time_{0}; 70 | }; 71 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/practical_socket.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // 异常类 4 | #include // 字符串类 5 | 6 | using namespace std; 7 | 8 | /** 9 | * 表示与套接字调用执行有关的问题。 10 | */ 11 | class SocketException : public exception { 12 | public: 13 | /** 14 | * 使用说明性消息构造 SocketException。 15 | * @param message 说明性消息 16 | * @param incSysMsg 如果为 true，则将系统消息（来自 strerror(errno)）附加到用户提供的消息后面 17 | */ 18 | SocketException(const string &message, bool inclSysMsg = false) throw(); 19 | 20 | /** 21 | * 为了确保不抛出任何异常而提供。 22 | */ 23 | ~SocketException() throw(); 24 | 25 | /** 26 | * 获取异常消息 27 | * @return 异常消息 28 | */ 29 | const char *what() const throw(); 30 | 31 | private: 32 | string userMessage; // 异常消息 33 | }; 34 | 35 | /** 36 | * 表示基本通信端点的基类 37 | */ 38 | class Socket { 39 | public: 40 | /** 41 | * 关闭和释放此套接字 42 | */ 43 | ~Socket(); 44 | 45 | /** 46 | * 获取本地地址 47 | * @return 套接字的本地地址 48 | * @exception SocketException 如果获取失败则抛出 49 | */ 50 | string getLocalAddress() noexcept(false); 51 | 52 | /** 53 | * 获取本地端口 54 | * @return 套接字的本地端口 55 | * @exception SocketException 如果获取失败则抛出 56 | */ 57 | unsigned short getLocalPort() const noexcept(false); 58 | 59 | /** 60 | * 将本地端口设置为指定的端口，并将本地地址设置为任何接口 61 | * @param localPort 本地端口 62 | * @exception SocketException 如果设置本地端口失败则抛出 63 | */ 64 | void setLocalPort(unsigned short localPort) noexcept(false); 65 | 66 | /** 67 | * 将本地端口设置为指定的端口，并将本地地址设置为指定的地址。 68 | * 如果省略端口，则将选择一个随机端口。 69 | * @param localAddress 本地地址 70 | * @param localPort 本地端口 71 | * @exception SocketException 如果设置本地端口或地址失败则抛出 72 | */ 73 | void setLocalAddressAndPort(const string &localAddress, unsigned short localPort = 0) noexcept(false); 74 | 75 | /** 76 | * 如果是 WinSock，则卸载 WinSock DLL；否则什么也不做。 77 | * 我们在示例客户端代码中忽略这一点，但在库中包含它是为了完整性。 78 | * 如果您在 Windows 上运行并且担心 DLL 资源消耗，请在所有 Socket 实例完成后调用此函数。 79 | * 如果您在某个 Socket 实例存在时在 Windows 上执行此操作，则可能会出问题。 80 | * 为了客户端代码的可移植性，在非 Windows 平台上，这是一个空函数，因此您总是可以包含它。 81 | * @param buffer 接收数据的缓冲区 82 | * @param bufferLen 最大读入缓冲区的字节数 83 | * @return 读取的字节数，EOF 为 0，错误为 -1 84 | * @exception SocketException 如果 WinSock 清理失败则抛出 85 | */ 86 | static void cleanUp() noexcept(false); 87 | 88 | /** 89 | * 将指定协议的服务解析为主机字节顺序中的相应端口号 90 | * @param service 要解析的服务（例如 "http"） 91 | * @param protocol 服务的协议。默认为 "tcp"。 92 | */ 93 | static unsigned short resolveService(const string &service, const string &protocol = "tcp"); 94 | 95 | private: 96 | // 防止用户在此对象上尝试使用值语义 97 | Socket(const Socket &sock); 98 | void operator=(const Socket &sock); 99 | 100 | protected: 101 | int sockDesc; // 套接字描述符 102 | Socket(int type, int protocol) noexcept(false); 103 | Socket(int sockDesc); 104 | }; 105 | 106 | /** 107 | * 能够连接、发送和接收的套接字 108 | */ 109 | class CommunicatingSocket : public Socket { 110 | public: 111 | /** 112 | * 与给定的远程地址和端口建立套接字连接 113 | * @param foreignAddress 对方地址（IP 地址或名称） 114 | * @param foreignPort 对方端口 115 | * @exception SocketException 如果无法建立连接则抛出 116 | */ 117 | void connect(const string &foreignAddress, unsigned short foreignPort) noexcept(false); 118 | 119 | /** 120 | * 将给定的缓冲区写入此套接字。在调用 send() 之前调用 connect() 121 | * @param buffer 要写入的缓冲区 122 | * @param bufferLen 要写入缓冲区的字节数 123 | * @exception SocketException 如果无法发送数据则抛出 124 | */ 125 | void send(const void *buffer, int bufferLen) noexcept(false); 126 | 127 | /** 128 | * 从此套接字读取最多 bufferLen 字节数据到给定的缓冲区中。在调用 recv() 之前调用 connect() 129 | * @param buffer 接收数据的缓冲区 130 | * @param bufferLen 最大要读入缓冲区的字节数 131 | * @return 读取的字节数，EOF 为 0，错误为 -1 132 | * @exception SocketException 如果无法接收数据则抛出 133 | */ 134 | int recv(void *buffer, int bufferLen) noexcept(false); 135 | 136 | /** 137 | * 获取对方地址。在调用 recv() 之前调用 connect() 138 | * @return 对方地址 139 | * @exception SocketException 如果无法获取对方地址则抛出 140 | */ 141 | string getForeignAddress() noexcept(false); 142 | 143 | /** 144 | * 获取对方端口。在调用 recv() 之前调用 connect() 145 | * @return 对方端口 146 | * @exception SocketException 如果无法获取对方端口则抛出 147 | */ 148 | unsigned short getForeignPort() noexcept(false); 149 | 150 | protected: 151 | CommunicatingSocket(int type, int protocol) noexcept(false); 152 | CommunicatingSocket(int newConnSD); 153 | }; 154 | 155 | /** 156 | * 用于与其他 TCP 套接字通信的 TCP 套接字 157 | */ 158 | class TCPSocket : public CommunicatingSocket { 159 | public: 160 | /** 161 | * 构造一个没有连接的 TCP 套接字 162 | * @exception SocketException 如果无法创建 TCP 套接字则抛出 163 | */ 164 | TCPSocket() noexcept(false); 165 | 166 | /** 167 | * 构造一个连接到给定远程地址和端口的 TCP 套接字 168 | * @param foreignAddress 对方地址（IP 地址或名称） 169 | * @param foreignPort 对方端口 170 | * @exception SocketException 如果无法创建 TCP 套接字则抛出 171 | */ 172 | TCPSocket(const string &foreignAddress, unsigned short foreignPort) noexcept(false); 173 | 174 | private: 175 | // 用于 TCPServerSocket::accept() 连接创建的访问 176 | friend class TCPServerSocket; 177 | TCPSocket(int newConnSD); 178 | }; 179 | 180 | /** 181 | * 服务器使用的 TCP 套接字类 182 | */ 183 | class TCPServerSocket : public Socket { 184 | public: 185 | /** 186 | 187 | 188 | * 为服务器构造一个 TCP 套接字，接受在指定端口上任何接口上的连接 189 | * @param localPort 服务器套接字的本地端口，值为零将给出一个系统分配的未使用端口 190 | * @param queueLen 最大挂起连接请求的队列长度（默认为 5） 191 | * @exception SocketException 如果无法创建 TCP 服务器套接字则抛出 192 | */ 193 | TCPServerSocket(unsigned short localPort, int queueLen = 5) noexcept(false); 194 | 195 | /** 196 | * 为服务器构造一个 TCP 套接字，接受在指定地址的指定端口上的连接 197 | * @param localAddress 服务器套接字的本地接口（地址） 198 | * @param localPort 服务器套接字的本地端口 199 | * @param queueLen 最大挂起连接请求的队列长度（默认为 5） 200 | * @exception SocketException 如果无法创建 TCP 服务器套接字则抛出 201 | */ 202 | TCPServerSocket(const string &localAddress, unsigned short localPort, int queueLen = 5) noexcept(false); 203 | 204 | /** 205 | * 阻塞，直到在此套接字上建立一个新连接或出现错误 206 | * @return 新连接套接字 207 | * @exception SocketException 如果尝试接受新连接失败则抛出 208 | */ 209 | TCPSocket *accept() noexcept(false); 210 | 211 | private: 212 | void setListen(int queueLen) noexcept(false); 213 | }; 214 | 215 | /** 216 | * UDP 套接字类 217 | */ 218 | class UDPSocket : public CommunicatingSocket { 219 | public: 220 | /** 221 | * 构造一个 UDP 套接字 222 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 223 | */ 224 | UDPSocket() noexcept(false); 225 | 226 | /** 227 | * 构造一个具有给定本地端口的 UDP 套接字 228 | * @param localPort 本地端口 229 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 230 | */ 231 | UDPSocket(unsigned short localPort) noexcept(false); 232 | 233 | /** 234 | * 构造一个具有给定本地端口和地址的 UDP 套接字 235 | * @param localAddress 本地地址 236 | * @param localPort 本地端口 237 | * @exception SocketException 如果无法创建 UDP 套接字则抛出 238 | */ 239 | UDPSocket(const string &localAddress, unsigned short localPort) noexcept(false); 240 | 241 | /** 242 | * 取消关联对方地址和端口 243 | * @return 如果解除关联成功则返回 true 244 | * @exception SocketException 如果无法断开 UDP 套接字的关联则抛出 245 | */ 246 | void disconnect() noexcept(false); 247 | 248 | /** 249 | * 将给定缓冲区作为 UDP 数据报发送到指定的地址/端口 250 | * @param buffer 要写入的缓冲区 251 | * @param bufferLen 要写入的字节数 252 | * @param foreignAddress 要发送到的地址（IP 地址或名称） 253 | * @param foreignPort 要发送到的端口号 254 | * @return 如果发送成功则返回 true 255 | * @exception SocketException 如果无法发送数据报则抛出 256 | */ 257 | void sendTo(const void *buffer, int bufferLen, const string &foreignAddress, 258 | unsigned short foreignPort) noexcept(false); 259 | 260 | /** 261 | * 从此套接字读取最多 bufferLen 字节数据。数据将被放置在给定的缓冲区中 262 | * @param buffer 接收数据的缓冲区 263 | * @param bufferLen 最多接收的字节数 264 | * @param sourceAddress 数据报源的地址 265 | * @param sourcePort 数据源的端口 266 | * @return 接收的字节数和错误为 -1 267 | * @exception SocketException 如果无法接收数据报则抛出 268 | */ 269 | int recvFrom(void *buffer, int bufferLen, string &sourceAddress, unsigned short &sourcePort) noexcept(false); 270 | 271 | /** 272 | * 设置多播 TTL 273 | * @param multicastTTL 多播 TTL 274 | * @exception SocketException 如果无法设置 TTL 则抛出 275 | */ 276 | void setMulticastTTL(unsigned char multicastTTL) noexcept(false); 277 | 278 | /** 279 | * 加入指定的多播组 280 | * @param multicastGroup 要加入的多播组地址 281 | * @exception SocketException 如果无法加入组则抛出 282 | */ 283 | void joinGroup(const string &multicastGroup) noexcept(false); 284 | 285 | /** 286 | * 离开指定的多播组 287 | * @param multicastGroup 要离开的多播组地址 288 | * @exception SocketException 如果无法离开组则抛出 289 | */ 290 | void leaveGroup(const string &multicastGroup) noexcept(false); 291 | 292 | private: 293 | void setBroadcast(); 294 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/serial_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "serial.h" 7 | #include "crc.h" 8 | 9 | class SerialManager { 10 | static constexpr size_t IMU_DATA_NUM = 52; 11 | static constexpr size_t BUF_LEN = 65540; 12 | 13 | public: 14 | explicit SerialManager(const std::string &port); 15 | 16 | ~SerialManager(); 17 | 18 | [[nodiscard]] bool is_available() const { return serial_.isOpen(); } 19 | void Start(); 20 | void Stop(); 21 | 22 | private: 23 | 24 | void Receive(); 25 | 26 | void TimeStampSynchronization(); 27 | Crc crc_; 28 | std::string port_; 29 | serial::Serial serial_; 30 | std::thread rx_thread_, tx_thread_; 31 | uint64_t time_t1_{0}; 32 | uint64_t time_t2_{0}; 33 | bool updated_t1_t2_{false}; 34 | 35 | uint64_t frame_count_{0}; 36 | 37 | bool started_{false}; 38 | }; 39 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/udp_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "data_manager.h" 5 | #include "crc.h" 6 | 7 | #include "practical_socket.h" 8 | 9 | class UdpManager { 10 | static constexpr size_t IMU_DATA_NUM = 52; 11 | static constexpr size_t BUF_LEN = 65540; 12 | 13 | public: 14 | explicit UdpManager(std::string target_ip, uint16_t port,bool use_sync_time = true); 15 | 16 | ~UdpManager(); 17 | void Start(); 18 | void Stop(); 19 | private: 20 | void Receive(); 21 | void TimeStampSynchronization(); 22 | 23 | std::thread rx_thread_, tx_thread_; 24 | std::atomic udp_connected_{false}; 25 | unsigned short port_{}; 26 | std::string target_ip_; 27 | UDPSocket sock_; 28 | Crc crc_; 29 | 30 | uint64_t time_t1_{0}; 31 | uint64_t time_t2_{0}; 32 | bool updated_t1_t2_{false}; 33 | 34 | uint64_t frame_count_{0}; 35 | 36 | bool started_{false}; 37 | bool use_sync_time_{true}; 38 | }; -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/unix.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file serial/impl/unix.h 3 | * \author William Woodall 4 | * \author John Harrison 5 | * \version 0.1 6 | * 7 | * \section LICENSE 8 | * 9 | * The MIT License 10 | * 11 | * Copyright (c) 2012 William Woodall, John Harrison 12 | * 13 | * Permission is hereby granted, free of charge, to any person obtaining a 14 | * copy of this software and associated documentation files (the "Software"), 15 | * to deal in the Software without restriction, including without limitation 16 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 17 | * and/or sell copies of the Software, and to permit persons to whom the 18 | * Software is furnished to do so, subject to the following conditions: 19 | * 20 | * The above copyright notice and this permission notice shall be included in 21 | * all copies or substantial portions of the Software. 22 | * 23 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 28 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | * DEALINGS IN THE SOFTWARE. 30 | * 31 | * \section DESCRIPTION 32 | * 33 | * This provides a unix based pimpl for the Serial class. This implementation is 34 | * based off termios.h and uses select for multiplexing the IO ports. 35 | * 36 | */ 37 | 38 | #if !defined(_WIN32) 39 | 40 | #ifndef SERIAL_IMPL_UNIX_H 41 | #define SERIAL_IMPL_UNIX_H 42 | 43 | #include "serial.h" 44 | 45 | #include 46 | 47 | namespace serial { 48 | 49 | using std::invalid_argument; 50 | using std::size_t; 51 | using std::string; 52 | 53 | using serial::IOException; 54 | using serial::SerialException; 55 | 56 | class MillisecondTimer { 57 | public: 58 | MillisecondTimer(const uint32_t millis); 59 | int64_t remaining(); 60 | 61 | private: 62 | static timespec timespec_now(); 63 | timespec expiry; 64 | }; 65 | 66 | class serial::Serial::SerialImpl { 67 | public: 68 | SerialImpl(const string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity, stopbits_t stopbits, 69 | flowcontrol_t flowcontrol); 70 | 71 | virtual ~SerialImpl(); 72 | 73 | void open(); 74 | 75 | void close(); 76 | 77 | bool isOpen() const; 78 | 79 | size_t available(); 80 | 81 | bool waitReadable(uint32_t timeout); 82 | 83 | void waitByteTimes(size_t count); 84 | 85 | size_t read(uint8_t *buf, size_t size = 1); 86 | 87 | size_t write(const uint8_t *data, size_t length); 88 | 89 | void flush(); 90 | 91 | void flushInput(); 92 | 93 | void flushOutput(); 94 | 95 | void sendBreak(int duration); 96 | 97 | void setBreak(bool level); 98 | 99 | void setRTS(bool level); 100 | 101 | void setDTR(bool level); 102 | 103 | bool waitForChange(); 104 | 105 | bool getCTS(); 106 | 107 | bool getDSR(); 108 | 109 | bool getRI(); 110 | 111 | bool getCD(); 112 | 113 | void setPort(const string &port); 114 | 115 | string getPort() const; 116 | 117 | void setTimeout(Timeout &timeout); 118 | 119 | Timeout getTimeout() const; 120 | 121 | void setBaudrate(unsigned long baudrate); 122 | 123 | unsigned long getBaudrate() const; 124 | 125 | void setBytesize(bytesize_t bytesize); 126 | 127 | bytesize_t getBytesize() const; 128 | 129 | void setParity(parity_t parity); 130 | 131 | parity_t getParity() const; 132 | 133 | void setStopbits(stopbits_t stopbits); 134 | 135 | stopbits_t getStopbits() const; 136 | 137 | void setFlowcontrol(flowcontrol_t flowcontrol); 138 | 139 | flowcontrol_t getFlowcontrol() const; 140 | 141 | void readLock(); 142 | 143 | void readUnlock(); 144 | 145 | void writeLock(); 146 | 147 | void writeUnlock(); 148 | 149 | protected: 150 | void reconfigurePort(); 151 | 152 | private: 153 | string port_; // Path to the file descriptor 154 | int fd_; // The current file descriptor 155 | 156 | bool is_open_; 157 | bool xonxoff_; 158 | bool rtscts_; 159 | 160 | Timeout timeout_; // Timeout for read operations 161 | unsigned long baudrate_; // Baudrate 162 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 163 | 164 | parity_t parity_; // Parity 165 | bytesize_t bytesize_; // Size of the bytes 166 | stopbits_t stopbits_; // Stop Bits 167 | flowcontrol_t flowcontrol_; // Flow Control 168 | 169 | // Mutex used to lock the read functions 170 | pthread_mutex_t read_mutex; 171 | // Mutex used to lock the write functions 172 | pthread_mutex_t write_mutex; 173 | }; 174 | 175 | } // namespace serial 176 | 177 | #endif // SERIAL_IMPL_UNIX_H 178 | 179 | #endif // !defined(_WIN32) 180 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/v8stdint.h: -------------------------------------------------------------------------------- 1 | // This header is from the v8 google project: 2 | // http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h 3 | 4 | // Copyright 2012 the V8 project authors. All rights reserved. 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above 12 | // copyright notice, this list of conditions and the following 13 | // disclaimer in the documentation and/or other materials provided 14 | // with the distribution. 15 | // * Neither the name of Google Inc. nor the names of its 16 | // contributors may be used to endorse or promote products derived 17 | // from this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | // Load definitions of standard types. 32 | 33 | #ifndef V8STDINT_H_ 34 | #define V8STDINT_H_ 35 | 36 | #include 37 | #include 38 | 39 | #if defined(_WIN32) && !defined(__MINGW32__) 40 | 41 | typedef signed char int8_t; 42 | typedef unsigned char uint8_t; 43 | typedef short int16_t; // NOLINT 44 | typedef unsigned short uint16_t; // NOLINT 45 | typedef int int32_t; 46 | typedef unsigned int uint32_t; 47 | typedef __int64 int64_t; 48 | typedef unsigned __int64 uint64_t; 49 | // intptr_t and friends are defined in crtdefs.h through stdio.h. 50 | 51 | #else 52 | 53 | #include 54 | 55 | #endif 56 | 57 | #endif // V8STDINT_H_ 58 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/include/win.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file serial/impl/windows.h 3 | * \author William Woodall 4 | * \author John Harrison 5 | * \version 0.1 6 | * 7 | * \section LICENSE 8 | * 9 | * The MIT License 10 | * 11 | * Copyright (c) 2012 William Woodall, John Harrison 12 | * 13 | * Permission is hereby granted, free of charge, to any person obtaining a 14 | * copy of this software and associated documentation files (the "Software"), 15 | * to deal in the Software without restriction, including without limitation 16 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 17 | * and/or sell copies of the Software, and to permit persons to whom the 18 | * Software is furnished to do so, subject to the following conditions: 19 | * 20 | * The above copyright notice and this permission notice shall be included in 21 | * all copies or substantial portions of the Software. 22 | * 23 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 28 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | * DEALINGS IN THE SOFTWARE. 30 | * 31 | * \section DESCRIPTION 32 | * 33 | * This provides a windows implementation of the Serial class interface. 34 | * 35 | */ 36 | 37 | #if defined(_WIN32) 38 | 39 | #ifndef SERIAL_IMPL_WINDOWS_H 40 | #define SERIAL_IMPL_WINDOWS_H 41 | 42 | #include "serial/serial.h" 43 | 44 | #include "windows.h" 45 | 46 | namespace serial { 47 | 48 | using std::invalid_argument; 49 | using std::string; 50 | using std::wstring; 51 | 52 | using serial::IOException; 53 | using serial::SerialException; 54 | 55 | class serial::Serial::SerialImpl { 56 | public: 57 | SerialImpl(const string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity, stopbits_t stopbits, 58 | flowcontrol_t flowcontrol); 59 | 60 | virtual ~SerialImpl(); 61 | 62 | void open(); 63 | 64 | void close(); 65 | 66 | bool isOpen() const; 67 | 68 | size_t available(); 69 | 70 | bool waitReadable(uint32_t timeout); 71 | 72 | void waitByteTimes(size_t count); 73 | 74 | size_t read(uint8_t *buf, size_t size = 1); 75 | 76 | size_t write(const uint8_t *data, size_t length); 77 | 78 | void flush(); 79 | 80 | void flushInput(); 81 | 82 | void flushOutput(); 83 | 84 | void sendBreak(int duration); 85 | 86 | void setBreak(bool level); 87 | 88 | void setRTS(bool level); 89 | 90 | void setDTR(bool level); 91 | 92 | bool waitForChange(); 93 | 94 | bool getCTS(); 95 | 96 | bool getDSR(); 97 | 98 | bool getRI(); 99 | 100 | bool getCD(); 101 | 102 | void setPort(const string &port); 103 | 104 | string getPort() const; 105 | 106 | void setTimeout(Timeout &timeout); 107 | 108 | Timeout getTimeout() const; 109 | 110 | void setBaudrate(unsigned long baudrate); 111 | 112 | unsigned long getBaudrate() const; 113 | 114 | void setBytesize(bytesize_t bytesize); 115 | 116 | bytesize_t getBytesize() const; 117 | 118 | void setParity(parity_t parity); 119 | 120 | parity_t getParity() const; 121 | 122 | void setStopbits(stopbits_t stopbits); 123 | 124 | stopbits_t getStopbits() const; 125 | 126 | void setFlowcontrol(flowcontrol_t flowcontrol); 127 | 128 | flowcontrol_t getFlowcontrol() const; 129 | 130 | void readLock(); 131 | 132 | void readUnlock(); 133 | 134 | void writeLock(); 135 | 136 | void writeUnlock(); 137 | 138 | protected: 139 | void reconfigurePort(); 140 | 141 | private: 142 | wstring port_; // Path to the file descriptor 143 | HANDLE fd_; 144 | 145 | bool is_open_; 146 | 147 | Timeout timeout_; // Timeout for read operations 148 | unsigned long baudrate_; // Baudrate 149 | 150 | parity_t parity_; // Parity 151 | bytesize_t bytesize_; // Size of the bytes 152 | stopbits_t stopbits_; // Stop Bits 153 | flowcontrol_t flowcontrol_; // Flow Control 154 | 155 | // Mutex used to lock the read functions 156 | HANDLE read_mutex; 157 | // Mutex used to lock the write functions 158 | HANDLE write_mutex; 159 | }; 160 | 161 | } // namespace serial 162 | 163 | #endif // SERIAL_IMPL_WINDOWS_H 164 | 165 | #endif // if defined(_WIN32) 166 | -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/CommonParameters.ini: -------------------------------------------------------------------------------- 1 | ;该配置文件列出了部分可配置的参数，其他可配置参数请参考软件安装路径下Development\Documentations\工业相机SDK可配置化参数表.xlsx 2 | 3 | ;不分设备类型的通用参数 4 | [COMMON] 5 | ;设置SDK内部图像缓存节点个数，大于等于1，若不设置SDK内部默认为1 6 | ImageNodeNum=1 7 | 8 | ;网口相机相关参数 9 | [GIGE] 10 | ;设置GVCP命令超时时间，默认500ms，范围：0-10000ms 11 | GvcpTimeout=500 12 | 13 | ;U口相机相关参数 14 | [U3V] 15 | ;设置U3V的传输包大小，Byte，默认为1M，rang：>=0x400 16 | TransferSize=1048576 17 | 18 | ;CameraLink相机相关参数 19 | [CAML] 20 | 21 | ;图像处理相关的参数 22 | [IMG_PROCESSING] 23 | ;设置插值算法类型，0-快速 1-均衡 2-最优（默认为最优） 24 | BayerCvtQuality=2 25 | ;设置插值算法处理线程个数，0-自适应 其他-具体线程个数(1,2,3,...)（默认线程个数为4） 26 | BayerCvtThreadNum=4 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/MvProducerGEV.cti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/MvProducerGEV.cti -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/MvProducerU3V.cti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/MvProducerU3V.cti -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libCLAllSerial_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libCLAllSerial_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libCLProtocol_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libCLProtocol_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libCLSerCOM.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libCLSerCOM.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libFormatConversion.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libFormatConversion.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libGCBase_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libGCBase_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libGenCP_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libGenCP_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libLog_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libLog_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMVGigEVisionSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMVGigEVisionSDK.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMVGigEVisionSDK.so.4.1.2.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMVGigEVisionSDK.so.4.1.2.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMVRender.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMVRender.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMediaProcess.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMediaProcess.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCamLVision.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCamLVision.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCamLVision.so.4.1.0.3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCamLVision.so.4.1.0.3 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCameraControl.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCameraControl.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCameraControl.so.4.1.2.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCameraControl.so.4.1.2.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCameraControlWrapper.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCameraControlWrapper.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvCameraControlWrapper.so.1.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvCameraControlWrapper.so.1.0.1.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvUsb3vTL.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvUsb3vTL.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libMvUsb3vTL.so.4.1.2.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libMvUsb3vTL.so.4.1.2.2 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libavutil.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libavutil.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libfusion.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libfusion.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libinfinite_sense_core.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libinfinite_sense_core.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/liblog4cpp_gcc447_v3_0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/liblog4cpp_gcc447_v3_0.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_core.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_core.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_core.so.4.8.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_core.so.4.8.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_core.so.408: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_core.so.408 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_highgui.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_highgui.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_highgui.so.4.8.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_highgui.so.4.8.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_highgui.so.408: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_highgui.so.408 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so.4.8.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so.4.8.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so.408: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgcodecs.so.408 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so.4.8.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so.4.8.0 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so.408: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libopencv_imgproc.so.408 -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libserial.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libserial.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libswscale.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libswscale.so -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libudp.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libudp.a -------------------------------------------------------------------------------- /sdk/infinite_sense_core/x86/lib/libusb-1.0.so.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/Multi-Sensor-Time-Synchronisation-System/2f9497aaa0af6aee017e0866b4408def5471b9d3/sdk/infinite_sense_core/x86/lib/libusb-1.0.so.0 --------------------------------------------------------------------------------