诺耕NAV992组合惯导ROS程序

src/ros/open_source_code/NAV992_GNSS_INS/Beijing Nuogeng ROS Driver Notes.txt

@@ -0,0 +1,146 @@
+/*********************************************/
+/*            Beijing Nuogeng                      */
+/*                ROS Driver                           */
+/*    Packet to Published Message           */
+/*                                                           */
+/*    Copyright 2017, Beijing Nuogeng    */
+/*                                                          */
+/*********************************************/
+
+
+/*********************************************/
+/*              Introduction                           */
+/*********************************************/
+This is an example to create a ROS driver that reads and decodes the GSOF packets (in this case packet #40) and publishes the information as ROS topics / messages. 
+
+It should work only on NAV992 GNSS/INS devices.
+
+This example has been developed and tested using Ubuntu Linux v16.04 LTS and ROS Lunar. Installation instructions for ROS can be found here: http://wiki.ros.org/lunar/Installation/Ubuntu.
+
+If you require any assistance using this code, please email support@nuogeng.com.cn
+
+
+/*********************************************/
+/*            Build Instructions                     */
+/*********************************************/
+ROS driver build instructions from: http://www.clearpathrobotics.com/assets/guides/ros/Creating%20publisher.html#creating-a-workspace-package
+
+1. Open a new Terminal window and type: "mkdir ~/ros[enter]".
+2. Type: "mkdir ~/ros/src[enter]".
+3. Type: "cd ~/ros/src[enter]".
+4. Type: "catkin_init_workspace[enter]".
+5. Type: "catkin_create_pkg nav992_driver roscpp std_msgs[enter]".
+6. Copy the supplied "src" directory and the "package.xml" file into the "~/ros/src/nav992_driver" directory. It should be OK to overwrite the existing "package.xml" file if you are following these instructions explicitly. If installing into your own pre-existing catkin workspace it may be necessary to manually merge the contents of these files.
+7. Modify the "package.xml" file in ~/ros/src/nav992_driver directory as required.
+8. Copy the supplied "CMakeLists.txt"file into the "~/ros/src/nav992_driver" directory, or modify the exisitng CMakeLists.txt file in the "~/ros/src/nav992_driver" directory by adding two lines at the end (you may need to modify the paths to suit your installation):
+   add_executable(nav992_driver src/nav992_driver.cpp src/gsof_records.c src/gsof_packet_protocol.c src/struct_endian.c src/struct_pack_unpack.c src/rs232/rs232.c)
+   target_link_libraries(nav992_driver ${catkin_LIBRARIES})
+9. Type: "cd ~/ros[enter]".
+10. Type: "catkin_make[enter]".
+
+
+/*********************************************/
+/*           Device Configuration            */
+/*********************************************/
+To use this example code, your nav992 device should be already configured to output gsof records #0x10/0x31/0x32.
+
+
+/*********************************************/
+/*             Run Instructions              */
+/*********************************************/
+1. Open a new Terminal window and type: "sudo adduser user_name dialout[enter]". This should only need to be done once.
+2. Plug in the USB to RS232 adapter cable that is connected to the nav992 device.
+3. In the Terminal window type: "dmesg | grep tty [enter]" to confirm the name of the com port (should be something like "ttyUSB0").
+4. Type: "roscore[enter]" to start ROS Master.
+5. Open a new Terminal window and type: "source ~/ros/devel/setup.bash[enter]".
+6. If your com port is named "ttyUSB0" and your baud rate is set to 115200 bps, type: "rosrun nav992_driver nav992_driver [enter]". If your com port or baud rate are different then type: "rosrun nav992_driver nav992_driver /dev/your_com_port_name your_baud_rate [enter]". 
+7. Open a new Terminal window and type: "rosnode list[enter]" to list the available nodes. You should see these listed:
+   /nav992_driver_node
+   /rosout
+8. Type: "rostopic list[enter]" to list the published topics. You should see these listed:
+   /nav992_driver/FilterStatus
+   /nav992_driver/Imu
+   /nav992_driver/NavSatFix
+   /nav992_driver/SystemStatus
+   /nav992_driver/Twist
+9. Type: "rostopic echo /nav992_driver/Imu[enter]"to see the "Imu" messages being published.
+10. Open a new Terminal window and type: "rostopic echo /nav992_driver/NavSatFix[enter]"to see the "NavSatFix" messages being published.
+11. Open a new Terminal window and type: "rostopic echo /nav992_driver/Twist[enter]"to see the "Twist" messages being published.
+12. Open a new Terminal window and type: "rostopic echo /nav992_driver/FilterStatus[enter]"to see the "Filter Status" messages being published.
+13. Open a new Terminal window and type: "rostopic echo /nav992_driver/SystemStatus[enter]"to see the "System Status" messages being published.
+14. You need to subscribe to these topics in your code to get access to the data in the messages.
+
+
+/*********************************************/
+/*             Published Topics              */
+/*********************************************/
+nav992_driver/NavSatFix
+nav992_driver/Twist
+nav992_driver/Imu
+nav992_driver/SystemStatus
+nav992_driver/FilterStatus
+
+
+/*********************************************/
+/* Published Messages: nav992_driver/NavSatFix   */
+/*********************************************/
+sensor_msgs / NavSatFix / Header / Stamp / Sec		
+sensor_msgs / NavSatFix / Header / Stamp / Nsec 	
+sensor_msgs / NavSatFix / Header / Frame_ID	 		// Fixed to 0
+sensor_msgs / NavSatFix / Status / Status			
+sensor_msgs / NavSatFix / Status / Service			// Fixed to 1 (GPS)
+sensor_msgs / NavSatFix / Latitude					
+sensor_msgs / NavSatFix / Longitude					
+sensor_msgs / NavSatFix / Altitude					
+sensor_msgs / NavSatFix / Position_Covariance		
+sensor_msgs / NavSatFix / Position_Covariance_Type	// Fixed to 2 (diagonal known)
+
+
+/*********************************************/
+/*   Published Messages: nav992_driver/Twist     */
+/*********************************************/
+geometry_msgs / Twist / Linear / X					
+geometry_msgs / Twist / Linear / Y					
+geometry_msgs / Twist / Linear / Z					
+geometry_msgs / Twist / Angular / X					
+geometry_msgs / Twist / Angular / Y					
+geometry_msgs / Twist / Angular / Z					
+
+
+/*********************************************/
+/*    Published Messages: nav992_driver/Imu      */
+/*********************************************/
+sensor_msgs / Imu / Orientation / X					
+sensor_msgs / Imu / Orientation / Y					
+sensor_msgs / Imu / Orientation / Z					
+sensor_msgs / Imu / Orientation / W					
+sensor_msgs / Imu / Orientation_Covariance			
+sensor_msgs / Imu / Angular_Velocity / X			
+sensor_msgs / Imu / Angular_Velocity / Y
+sensor_msgs / Imu / Angular_Velocity / Z
+sensor_msgs / Imu / Linear_Acceleration / X
+sensor_msgs / Imu / Linear_Acceleration / Y
+sensor_msgs / Imu / Linear_Acceleration / Z
+
+
+/**********************************************/
+/* Published Messages: nav992_driver/SystemStatus */
+/**********************************************/
+diagnostic_msgs / Diagnostic_Status / Name			// Fixed to "System Status"
+diagnostic_msgs / Diagnostic_Status / Level			
+diagnostic_msgs / Diagnostic_Status / Message		
+
+
+/**********************************************/
+/* Published Messages: nav992_driver/FilterStatus */
+/**********************************************/
+diagnostic_msgs / Diagnostic_Status / Name			// Fixed to "Filter Status"
+diagnostic_msgs / Diagnostic_Status / Level			
+diagnostic_msgs / Diagnostic_Status / Message
+
+
+/**********************************************/
+/*          Messages Not Published            */
+/**********************************************/
+sensor_msgs / Imu / Angular_Velocity_Covariance 
+sensor_msgs / Imu / Linear_Acceleration_Covariance

src/ros/open_source_code/NAV992_GNSS_INS/CMakeLists.txt

@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(nav992_driver)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include 
+#  LIBRARIES an_driver 
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories( 
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/nav992_driver.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/nav992_driver_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/nav992_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_an_driver.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+#########################
+## NAV992 GNSS INS            ##
+#########################
+add_executable(nav992_driver src/nav992_driver.cpp src/gsof_records.c src/gsof_packet_protocol.c src/struct_endian.c src/struct_pack_unpack.c src/rs232/rs232.c)
+target_link_libraries(nav992_driver ${catkin_LIBRARIES})

src/ros/open_source_code/NAV992_GNSS_INS/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 an-scott
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/ros/open_source_code/NAV992_GNSS_INS/README.md

@@ -0,0 +1,15 @@
+# nav992_driver
+Driver for the range of INS systems from Beijing Nuogeng
+
+Packet to Published Message Example
+
+Copyright 2017, Beijing Nuogeng
+
+This is an example to create a ROS driver that reads and decodes the GSOF packets (in this case packet #40) and publishes the information as ROS topics / messages. 
+
+This example has been developed and tested using Ubuntu Linux v16.04 LTS and ROS Lunar. Installation instructions for ROS can be found here: http://wiki.ros.org/lunar/Installation/Ubuntu.
+
+If you require any assistance using this code, please email support@nuogeng.com.cn.
+
+Installation, build, device configuration, and execution instructions can be found in the file "Beijing Nuogeng ROS Driver Notes.txt". 
+

src/ros/open_source_code/NAV992_GNSS_INS/nav992_example/nav992_example.sln

@@ -0,0 +1,28 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 14
+VisualStudioVersion = 14.0.25420.1
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "nav992_example", "nav992_example.vcxproj", "{69B82D95-0713-4575-93EF-512CE5731971}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|x64 = Debug|x64
+		Debug|x86 = Debug|x86
+		Release|x64 = Release|x64
+		Release|x86 = Release|x86
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{69B82D95-0713-4575-93EF-512CE5731971}.Debug|x64.ActiveCfg = Debug|x64
+		{69B82D95-0713-4575-93EF-512CE5731971}.Debug|x64.Build.0 = Debug|x64
+		{69B82D95-0713-4575-93EF-512CE5731971}.Debug|x86.ActiveCfg = Debug|Win32
+		{69B82D95-0713-4575-93EF-512CE5731971}.Debug|x86.Build.0 = Debug|Win32
+		{69B82D95-0713-4575-93EF-512CE5731971}.Release|x64.ActiveCfg = Release|x64
+		{69B82D95-0713-4575-93EF-512CE5731971}.Release|x64.Build.0 = Release|x64
+		{69B82D95-0713-4575-93EF-512CE5731971}.Release|x86.ActiveCfg = Release|Win32
+		{69B82D95-0713-4575-93EF-512CE5731971}.Release|x86.Build.0 = Release|Win32
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal

src/ros/open_source_code/NAV992_GNSS_INS/nav992_example/nav992_example.vcxproj

@@ -0,0 +1,166 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <ProjectGuid>{69B82D95-0713-4575-93EF-512CE5731971}</ProjectGuid>
+    <Keyword>Win32Proj</Keyword>
+    <RootNamespace>nav992_example</RootNamespace>
+    <WindowsTargetPlatformVersion>8.1</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v140</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v140</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v140</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v140</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <PrecompiledHeader>
+      </PrecompiledHeader>
+      <WarningLevel>Level3</WarningLevel>
+      <Optimization>Disabled</Optimization>
+      <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <SDLCheck>true</SDLCheck>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <PrecompiledHeader>
+      </PrecompiledHeader>
+      <WarningLevel>Level3</WarningLevel>
+      <Optimization>Disabled</Optimization>
+      <PreprocessorDefinitions>_DEBUG;_CONSOLE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <SDLCheck>true</SDLCheck>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <PrecompiledHeader>
+      </PrecompiledHeader>
+      <Optimization>MaxSpeed</Optimization>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <SDLCheck>true</SDLCheck>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <PrecompiledHeader>
+      </PrecompiledHeader>
+      <Optimization>MaxSpeed</Optimization>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <SDLCheck>true</SDLCheck>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemGroup>
+    <ClCompile Include="..\src\nav992_driver.cpp" />
+    <ClCompile Include="..\src\gsof_packet_protocol.c" />
+    <ClCompile Include="..\src\gsof_records.c" />
+    <ClCompile Include="..\src\rs232\rs232.c" />
+    <ClCompile Include="..\src\struct_endian.c" />
+    <ClCompile Include="..\src\struct_pack_unpack.c" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="..\src\gsof_packet_protocol.h" />
+    <ClInclude Include="..\src\gsof_records.h" />
+    <ClInclude Include="..\src\rs232\rs232.h" />
+    <ClInclude Include="..\src\struct_endian.h" />
+    <ClInclude Include="..\src\struct_pack_unpack.h" />
+  </ItemGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>

src/ros/open_source_code/NAV992_GNSS_INS/nav992_example/nav992_example.vcxproj.filters

@@ -0,0 +1,60 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup>
+    <Filter Include="源文件">
+      <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
+      <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
+    </Filter>
+    <Filter Include="头文件">
+      <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
+      <Extensions>h;hh;hpp;hxx;hm;inl;inc;xsd</Extensions>
+    </Filter>
+    <Filter Include="资源文件">
+      <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
+      <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
+    </Filter>
+    <Filter Include="头文件\rs232">
+      <UniqueIdentifier>{efd81f66-e0c2-4248-ba0e-5c52e978f420}</UniqueIdentifier>
+    </Filter>
+    <Filter Include="源文件\rs232">
+      <UniqueIdentifier>{82e50101-8618-4414-a9af-6e3f219b834a}</UniqueIdentifier>
+    </Filter>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="..\src\rs232\rs232.c">
+      <Filter>源文件\rs232</Filter>
+    </ClCompile>
+    <ClCompile Include="..\src\struct_endian.c">
+      <Filter>源文件</Filter>
+    </ClCompile>
+    <ClCompile Include="..\src\struct_pack_unpack.c">
+      <Filter>源文件</Filter>
+    </ClCompile>
+    <ClCompile Include="..\src\gsof_packet_protocol.c">
+      <Filter>源文件</Filter>
+    </ClCompile>
+    <ClCompile Include="..\src\nav992_driver.cpp">
+      <Filter>源文件</Filter>
+    </ClCompile>
+    <ClCompile Include="..\src\gsof_records.c">
+      <Filter>源文件</Filter>
+    </ClCompile>
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="..\src\rs232\rs232.h">
+      <Filter>头文件\rs232</Filter>
+    </ClInclude>
+    <ClInclude Include="..\src\gsof_packet_protocol.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
+    <ClInclude Include="..\src\gsof_records.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
+    <ClInclude Include="..\src\struct_pack_unpack.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
+    <ClInclude Include="..\src\struct_endian.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
+  </ItemGroup>
+</Project>

src/ros/open_source_code/NAV992_GNSS_INS/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package>
+  <name>nav992_driver</name>
+  <version>1.0.0</version>
+  <description>The NAV992 driver package for ROS</description>
+
+  <maintainer email="support@nuogeng.com.cn">support@nuogeng.com.cn</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+	
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+	
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+</package>

src/ros/open_source_code/NAV992_GNSS_INS/src/gsof_packet_protocol.c

@@ -0,0 +1,151 @@
+/****************************************************************/
+/*                                                              */
+/*          GSOF Packet Protocol Library                        */
+/*          C Language Dynamic, Version 1.0                     */
+/*   Copyright 2021, Beijing Nuogeng Technology Ltd             */
+/*                                                              */
+/****************************************************************/
+/*
+ * Copyright (C) 2021 Beijing Nuogeng Technology Ltd
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+#include "gsof_packet_protocol.h"
+
+/*
+ * Function to decode gsof_records from raw data
+ * Returns 0 with valid gsof_record
+ */
+int gsof_record_decode(gsof_decoder_t* gsof_decoder, gsof_record_t* gsof_record)
+{
+	// read one or more gsof_record
+	if (gsof_decoder->record_buf_length > 0)
+	{
+		gsof_record->record_type = gsof_decoder->record_check_pos[0];
+		gsof_record->length = gsof_decoder->record_check_pos[1];
+		gsof_record->data = gsof_decoder->record_check_pos + 2;
+
+		gsof_decoder->record_buf_length -= gsof_record->length + 2;
+		gsof_decoder->record_check_pos += gsof_record->length + 2;
+		return 0;
+	}
+
+	// decode gsof_package data, it may have one or more gsof_record
+	uint16_t checksum = 0;
+	gsof_packet_t* gsof_pkt = &gsof_decoder->gsof_packet;
+	while (gsof_decoder->decode_iterator + GSOF_PACKET_HEADER_SIZE <= gsof_decoder->recv_buf_length)
+	{
+		uint8_t* recv_buf = gsof_decoder->recv_buffer + gsof_decoder->decode_iterator;
+		if (recv_buf[0] != 0x02) {
+			++gsof_decoder->decode_iterator;
+			continue;
+		}
+		gsof_pkt->status = recv_buf[1];
+		gsof_pkt->package_type = recv_buf[2];
+		gsof_pkt->length = recv_buf[3];
+		gsof_pkt->transmission_number = recv_buf[4];
+		gsof_pkt->page_index = recv_buf[5];
+		gsof_pkt->max_page_index = recv_buf[6];
+
+		// check packet size
+		if (gsof_pkt->length + 6 + gsof_decoder->decode_iterator > gsof_decoder->recv_buf_length)
+			break; // need more data
+		
+		gsof_decoder->decode_iterator += GSOF_PACKET_HEADER_SIZE;
+		// check valid STX ETX range
+		if (recv_buf[gsof_pkt->length + 5] != 0x03) {
+			gsof_decoder->decode_iterator -= (GSOF_PACKET_HEADER_SIZE - 1);
+			continue; // find next valid STX-ETX range
+		}
+
+		// skip non gsof packet type
+		if (gsof_pkt->package_type != 0x40) {
+			gsof_decoder->decode_iterator += (gsof_pkt->length - 3 + 2);
+			continue; // find next gosf packet type
+		}
+		
+		// calculated check sum
+		checksum = 0;
+		uint8_t* sum_begin = recv_buf + 1;
+		int sum_length = 3 + gsof_pkt->length;
+		for (int i = 0; i < sum_length; ++i) {
+			checksum += sum_begin[i];
+		}
+
+		gsof_pkt->check_sum = recv_buf[gsof_pkt->length + 4];
+		// two check sum should be the same
+		if (checksum % 256 == gsof_pkt->check_sum)
+		{
+			memcpy(gsof_decoder->record_buffer + gsof_decoder->record_buf_length,
+				recv_buf + GSOF_PACKET_HEADER_SIZE,
+				gsof_pkt->length - 3);
+
+			gsof_decoder->record_buf_length += gsof_pkt->length - 3;
+			gsof_decoder->record_check_pos = gsof_decoder->record_buffer;
+			gsof_decoder->decode_iterator += (gsof_pkt->length - 3 + 2);
+
+			if (gsof_pkt->page_index == gsof_pkt->max_page_index)
+			{
+				// read one or more gsof_record
+				if (gsof_decoder->record_buf_length > 0)
+				{
+					gsof_record->record_type = gsof_decoder->record_check_pos[0];
+					gsof_record->length = gsof_decoder->record_check_pos[1];
+					gsof_record->data = gsof_decoder->record_check_pos + 2;
+
+					gsof_decoder->record_buf_length -= gsof_record->length + 2;
+					gsof_decoder->record_check_pos += gsof_record->length + 2;
+					return 0; // return one valid gsof_record
+				}
+			}
+			else // have multi page, copy this page data to gsof_record buffer for future decode
+				continue; // continue to check recv_buffer
+		}
+		else // check sum error, skip this package
+		{
+			gsof_decoder->decode_iterator += (gsof_pkt->length - 3 + 2);
+			gsof_decoder->checksum_errors++;
+			continue; // continue to check recv_buffer
+		}
+	}
+
+	// to check whether have some data in the buffer
+	if (gsof_decoder->decode_iterator < gsof_decoder->recv_buf_length)
+	{
+		if (gsof_decoder->decode_iterator > 0)
+		{ // copy unchecked data to recv_buffer begin position
+			memmove(gsof_decoder->recv_buffer,
+				gsof_decoder->recv_buffer + gsof_decoder->decode_iterator,
+				(gsof_decoder->recv_buf_length - gsof_decoder->decode_iterator) * sizeof(uint8_t));
+
+			gsof_decoder->recv_buf_length -= gsof_decoder->decode_iterator;
+		}
+	}
+	else
+		gsof_decoder->recv_buf_length = 0;
+	// reset decode_iterator to recv_buffer begin position
+	gsof_decoder->decode_iterator = 0;
+
+	return 1;
+}

src/ros/open_source_code/NAV992_GNSS_INS/src/gsof_packet_protocol.h

@@ -0,0 +1,85 @@
+/****************************************************************/
+/*                                                              */
+/*          GSOF Packet Protocol Library                        */
+/*          C Language Dynamic, Version 1.0                     */
+/*   Copyright 2021, Beijing Nuogeng Technology Ltd             */
+/*                                                              */
+/****************************************************************/
+/*
+* Copyright (C) 2021 Beijing Nuogeng Technology Ltd
+*
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define GSOF_PACKET_HEADER_SIZE 7
+#define RECV_BUFFER_SIZE 4096
+#define GSOF_RECORD_BUFFER_SIZE 1024
+
+// point to address which incoming data can be append here
+#define gsof_decoder_pointer(gsof_decoder) &(gsof_decoder)->recv_buffer[(gsof_decoder)->recv_buf_length]
+// size have not filled data
+#define gsof_decoder_size(gsof_decoder) (sizeof((gsof_decoder)->recv_buffer) - (gsof_decoder)->recv_buf_length)
+// update filled data size
+#define gsof_decoder_increment(gsof_decoder, bytes_received) (gsof_decoder)->recv_buf_length += bytes_received
+
+
+typedef struct
+{
+	uint8_t record_type;
+	uint8_t length;
+	uint8_t* data;
+} gsof_record_t;
+
+typedef struct
+{
+	//uint8_t STX; //sync header=0x02;
+	uint8_t status;
+	uint8_t package_type; //0x40=gsof type
+	uint8_t length;
+	uint8_t transmission_number;
+	uint8_t page_index;
+	uint8_t max_page_index;
+	/*data section，length-3 bytes*/
+	// below is data end, two bytes
+	uint8_t check_sum; // (status + package_type + length + data bytes) modulo 256
+	//uint8_t ETX; // sync trailer=0x03;
+} gsof_packet_t;
+
+typedef struct
+{
+	uint8_t recv_buffer[RECV_BUFFER_SIZE];
+	int16_t recv_buf_length;
+	int16_t decode_iterator;
+	uint8_t record_buffer[GSOF_RECORD_BUFFER_SIZE];
+	int16_t record_buf_length;
+	uint8_t* record_check_pos;
+	uint32_t checksum_errors;
+	gsof_packet_t gsof_packet;
+} gsof_decoder_t;
+
+int gsof_record_decode(gsof_decoder_t* gsof_decoder, gsof_record_t* gsof_record);
+
+#ifdef __cplusplus
+}
+#endif

src/ros/open_source_code/NAV992_GNSS_INS/src/gsof_records.c

@@ -0,0 +1,101 @@
+/****************************************************************/
+/*                                                              */
+/*          GSOF Packet Protocol Library                        */
+/*          C Language Dynamic, Version 1.0                     */
+/*   Copyright 2021, Beijing Nuogeng Technology Ltd             */
+/*                                                              */
+/****************************************************************/
+/*
+ * Copyright (C) 2021 Beijing Nuogeng Technology Ltd
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+ 
+ #include <stdint.h>
+ #include <string.h>
+ #include "gsof_packet_protocol.h"
+ #include "gsof_records.h"
+ #include "struct_pack_unpack.h"
+
+#define vas_bswap16_(s) ((((s) & 0xff) << 8) | (((s) >> 8) & 0xff))  
+
+#define vas_bswap32_(l) (((l) >> 24) | \
+(((l) & 0x00ff0000) >> 8) | \
+(((l) & 0x0000ff00) << 8) | \
+((l) << 24))
+
+#define vas_bswap64_(ll) (((ll) >> 56) | \
+(((ll) & 0x00ff000000000000) >> 40)  | \
+(((ll) & 0x0000ff0000000000) >> 24)  | \
+(((ll) & 0x000000ff00000000) >> 8)   | \
+(((ll) & 0x00000000ff000000) << 8)   | \
+(((ll) & 0x0000000000ff0000) << 24)  | \
+(((ll) & 0x000000000000ff00) << 40)  | \
+(((ll) << 56)))
+
+
+int decode_utc_record(utc_record_t *msg, gsof_record_t *gsof_record)
+{
+	if (gsof_record->record_type == record_type_utc && gsof_record->length == 0x09)
+	{
+		struct_unpack(gsof_record->data, ">I2HB",
+			&msg->gps_time_ms,
+			&msg->gps_week, &msg->utc_offset,
+			&msg->flags.val);
+
+		return 0;
+	}
+	else return 1;
+}
+
+int decode_ins_full_navigation_record(ins_full_navigation_record_t *msg, gsof_record_t *gsof_record)
+{
+	if (gsof_record->record_type == record_type_ins_full_navigation && gsof_record->length == 0x68)
+	{
+		struct_unpack(gsof_record->data, ">HI2B3d4f4d6f",
+			&msg->gps_week,
+			&msg->gps_time_ms,
+			&msg->align_status, &msg->quality_indication,
+			&msg->latitude, &msg->longitude, &msg->altitude,
+			&msg->north_velocity, &msg->east_velocity, &msg->down_velocity, &msg->total_speed,
+			&msg->roll, &msg->pitch, &msg->heading, &msg->track_angle,
+			&msg->angle_rate_x, &msg->angle_rate_y, &msg->angle_rate_z,
+			&msg->longitudinal_acceleration, &msg->traverse_acceleration, &msg->down_acceleration);
+
+		return 0;
+	}
+	else return 1;
+}
+
+int decode_ins_rms_info_record(ins_rms_info_record_t *msg, gsof_record_t *gsof_record)
+{
+	if (gsof_record->record_type == record_type_ins_rms_info && gsof_record->length == 0x2C)
+	{
+		struct_unpack(gsof_record->data, ">HI2B9f",
+			&msg->gps_week,
+			&msg->gps_time_ms,
+			&msg->align_status, &msg->quality_indication,
+			&msg->north_position_rms, &msg->east_position_rms, &msg->down_position_rms,
+			&msg->north_velocity_rms, &msg->east_velocity_rms, &msg->down_velocity_rms,
+			&msg->roll_rms, &msg->pitch_rms, &msg->heading_rms);
+
+		return 0;
+	}
+	else return 1;
+}

src/ros/open_source_code/NAV992_GNSS_INS/src/gsof_records.h

@@ -0,0 +1,143 @@
+/****************************************************************/
+/*                                                              */
+/*          GSOF Packet Protocol Library                        */
+/*          C Language Dynamic, Version 1.0                     */
+/*   Copyright 2021, Beijing Nuogeng Technology Ltd             */
+/*                                                              */
+/****************************************************************/
+/*
+ * Copyright (C) 2021 Beijing Nuogeng Technology Ltd
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+typedef enum
+{
+	record_type_utc = 0x10,
+	record_type_ins_full_navigation = 0x31,
+	record_type_ins_rms_info = 0x32,
+} record_type_e;
+
+typedef struct
+{
+	uint8_t record_type; // =0x10
+	uint8_t record_length; // = 0x09
+	uint32_t gps_time_ms; // GPS time, in milliseconds, of GPS week
+	uint16_t gps_week; // GPS week count since January 1980
+	uint16_t utc_offset; // GPS to UTC offset
+	union
+	{
+		uint8_t r_uint8;
+		struct
+		{
+			uint8_t time_valid : 1; // 1 = week and millisecond of week valid
+			uint8_t offset_valid : 1; // 1 = UTC offset validity
+			uint8_t recv1 : 6;
+		} val;
+	} flags;
+
+	//stream_size = 11(record_length+2);
+} utc_record_t;
+
+enum QualityIndication
+{
+	Quality_FixInvalid = 0,
+	Quality_Autonomous,
+	Quality_Differential,
+	Quality_PPSMode,
+	Quality_RTKFixed,
+	Quality_RTKFloat,
+	Quality_DeadReckoning,
+	Quality_ManualInputMode,
+	Quality_SimulatorMode
+};
+
+enum AlignStatus
+{
+	Align_GpsOnly = 0,
+	Align_CoarseLeveling,
+	Align_DegradedSolution,
+	Align_Aligned,
+	Align_FullNavigationMode
+};
+
+//=========================== ins_full_navigation_record ===========================//
+typedef struct
+{
+	uint8_t record_type; // =0x31
+	uint8_t record_length; // = 0x68
+	uint16_t gps_week; // GPS week count since January 1980
+	uint32_t gps_time_ms; // GPS time, in milliseconds, of GPS week
+	uint8_t align_status;
+	uint8_t quality_indication;
+	double latitude; // deg
+	double longitude; // deg
+	double altitude; // meter
+	float north_velocity; // m/s
+	float east_velocity; // m/s
+	float down_velocity; // m/s
+	float total_speed; // m/s
+	double roll; // deg
+	double pitch; // deg
+	double heading; // deg
+	double track_angle; // deg
+	float angle_rate_x; // deg/s
+	float angle_rate_y; // deg/s
+	float angle_rate_z; // deg/s
+	float longitudinal_acceleration; // m/s2
+	float traverse_acceleration; // m/s2
+	float down_acceleration; // m/s2
+
+	//stream_size = 106(record_length+2);
+} ins_full_navigation_record_t;
+
+typedef struct
+{
+	uint8_t record_type; // =0x32
+	uint8_t record_length; // = 0x2C
+	uint16_t gps_week; // GPS week count since January 1980
+	uint32_t gps_time_ms; // GPS time, in milliseconds, of GPS week
+	uint8_t align_status;
+	uint8_t quality_indication;
+	float north_position_rms; // m
+	float east_position_rms; // m
+	float down_position_rms; // m
+	float north_velocity_rms; // m/s
+	float east_velocity_rms; // m/s
+	float down_velocity_rms; // m/s
+	float roll_rms; // degree
+	float pitch_rms; // degree
+	float heading_rms; // degree
+
+	//stream_size = 46(record_length+2);
+} ins_rms_info_record_t;
+
+
+int decode_utc_record(utc_record_t *utc_record, gsof_record_t *gsof_record);
+int decode_ins_full_navigation_record(ins_full_navigation_record_t *ins_full_navigation_record, gsof_record_t *gsof_record);
+int decode_ins_rms_info_record(ins_rms_info_record_t *ins_rms_info_record, gsof_record_t *gsof_record);
+
+#ifdef __cplusplus
+}
+#endif

src/ros/open_source_code/NAV992_GNSS_INS/src/nav992_driver.cpp

@@ -0,0 +1,434 @@
+/****************************************************************/
+/*                                                              */
+/*          GSOF Packet Protocol Library                        */
+/*        ROS Driver, Packet to Published Message Example       */
+/*          Copyright 2021, Beijing Nuogeng Technology Ltd      */
+/*                                                              */
+/****************************************************************/
+/*
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifdef HAVE_ROS
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <sensor_msgs/TimeReference.h>
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Twist.h>
+#include <diagnostic_msgs/DiagnosticArray.h>
+#endif // HAVE_ROS
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string>
+#include <thread>
+#include <chrono>
+
+#include "rs232/rs232.h"
+#include "gsof_packet_protocol.h"
+#include "gsof_records.h"
+
+// enable this if use ROS
+//#define HAVE_ROS
+
+#define RADIANS_TO_DEGREES (180.0/M_PI)
+#define DEGREES_TO_RADIANS (M_PI/180.0)
+ 
+ // The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
+// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
+// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
+#define GPS_TIME_START_TIME_MSEC (315964800000)
+
+#define MINUTES_IN_WEEK (10080)       //!< Number of minutes in a week.
+
+
+int64_t gps_time_to_utc(utc_record_t* utc_record)
+{
+	int64_t ms_gps_utc = 0;
+	if (utc_record->flags.val.time_valid && utc_record->flags.val.offset_valid)
+	{
+		ms_gps_utc = int64_t(utc_record->gps_week) * MINUTES_IN_WEEK * 60000;
+		ms_gps_utc += utc_record->gps_time_ms;
+		ms_gps_utc += GPS_TIME_START_TIME_MSEC + int64_t(utc_record->utc_offset) * 1000;
+	}
+	return ms_gps_utc;
+}
+
+
+int main(int argc, char *argv[])
+{
+
+#ifdef HAVE_ROS
+	// Set up ROS node //
+	ros::init(argc, argv, "nav992_device_node");
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+#endif // HAVE_ROS
+
+	printf("\nYour NAV992 ROS driver is currently running\nPress Ctrl-C to interrupt\n");
+
+	// Set up the COM port
+	std::string com_port = "COM7";
+	int baud_rate = 115200;
+	std::string imu_frame_id;
+	std::string nav_sat_frame_id;
+	std::string topic_prefix;
+
+	if (argc >= 3) {
+		com_port = std::string(argv[1]);
+		baud_rate = atoi(argv[2]);
+	}
+
+#ifdef HAVE_ROS
+	else {
+		pnh.param("port", com_port, std::string("/dev/ttyUSB0"));
+		pnh.param("baud_rate", baud_rate, 115200);
+	}
+
+	pnh.param("imu_frame_id", imu_frame_id, std::string("imu"));
+	pnh.param("nav_sat_frame_id", nav_sat_frame_id, std::string("gps"));
+	pnh.param("topic_prefix", topic_prefix, std::string("nav992_device"));
+#endif // HAVE_ROS
+
+
+#ifdef HAVE_ROS
+	// Initialise Publishers and Topics //
+	ros::Publisher nav_sat_fix_pub=nh.advertise<sensor_msgs::NavSatFix>(topic_prefix + "/NavSatFix",10);
+	ros::Publisher twist_pub=nh.advertise<geometry_msgs::Twist>(topic_prefix + "/Twist",10);
+	ros::Publisher imu_pub=nh.advertise<sensor_msgs::Imu>(topic_prefix + "/Imu",10);
+	ros::Publisher system_status_pub=nh.advertise<diagnostic_msgs::DiagnosticStatus>(topic_prefix + "/SystemStatus",10);
+	ros::Publisher filter_status_pub=nh.advertise<diagnostic_msgs::DiagnosticStatus>(topic_prefix + "/FilterStatus",10);
+
+	// Initialise messages
+	sensor_msgs::NavSatFix nav_sat_fix_msg;
+	nav_sat_fix_msg.header.stamp.sec=0;
+	nav_sat_fix_msg.header.stamp.nsec=0;
+	nav_sat_fix_msg.header.frame_id='0';
+	nav_sat_fix_msg.status.status=0;
+	nav_sat_fix_msg.status.service=1; // fixed to GPS
+	nav_sat_fix_msg.latitude=0.0;
+	nav_sat_fix_msg.longitude=0.0;
+	nav_sat_fix_msg.altitude=0.0;
+	nav_sat_fix_msg.position_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+	nav_sat_fix_msg.position_covariance_type=2; // fixed to variance on the diagonal
+
+	geometry_msgs::Twist twist_msg;
+	twist_msg.linear.x=0.0;
+	twist_msg.linear.y=0.0;
+	twist_msg.linear.z=0.0;
+	twist_msg.angular.x=0.0;
+	twist_msg.angular.y=0.0;
+	twist_msg.angular.z=0.0;
+
+	sensor_msgs::Imu imu_msg;
+	imu_msg.header.stamp.sec=0;
+	imu_msg.header.stamp.nsec=0;
+	imu_msg.header.frame_id='0';
+	imu_msg.orientation.x=0.0;
+	imu_msg.orientation.y=0.0;
+	imu_msg.orientation.z=0.0;
+	imu_msg.orientation.w=0.0;
+	imu_msg.orientation_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+	imu_msg.angular_velocity.x=0.0;
+	imu_msg.angular_velocity.y=0.0;
+	imu_msg.angular_velocity.z=0.0;
+	imu_msg.angular_velocity_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed
+	imu_msg.linear_acceleration.x=0.0;
+	imu_msg.linear_acceleration.y=0.0;
+	imu_msg.linear_acceleration.z=0.0;
+	imu_msg.linear_acceleration_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed
+
+	diagnostic_msgs::DiagnosticStatus system_status_msg;
+	system_status_msg.level = 0; // default OK state
+	system_status_msg.name = "System Status";
+	system_status_msg.message = "";
+
+	diagnostic_msgs::DiagnosticStatus filter_status_msg;
+	filter_status_msg.level = 0; // default OK state
+	filter_status_msg.name = "Filter Status";
+	filter_status_msg.message = "";
+#endif // HAVE_ROS
+
+	// get data from com port //
+	gsof_decoder_t* gsof_decoder = (gsof_decoder_t*)malloc(sizeof(gsof_decoder_t));
+	memset(gsof_decoder, 0, sizeof(gsof_decoder_t));
+	gsof_record_t* gsof_record = (gsof_record_t*)malloc(sizeof(gsof_record_t));
+	memset(gsof_record, 0, sizeof(gsof_record_t));
+	utc_record_t* utc_record = (utc_record_t*)malloc(sizeof(utc_record_t));
+	memset(utc_record, 0, sizeof(utc_record_t));
+	ins_full_navigation_record_t* ins_full_navigation_record = (ins_full_navigation_record_t*)malloc(sizeof(ins_full_navigation_record_t));
+	memset(ins_full_navigation_record, 0, sizeof(ins_full_navigation_record_t));
+	ins_rms_info_record_t* ins_rms_info_record = (ins_rms_info_record_t*)malloc(sizeof(ins_rms_info_record_t));
+	memset(ins_rms_info_record, 0, sizeof(ins_rms_info_record_t));
+	int bytes_received;
+	
+	if (OpenComport(const_cast<char*>(com_port.c_str()), baud_rate))
+	{
+		printf("Could not open serial port: %s \n",com_port.c_str());
+		exit(EXIT_FAILURE);
+	}
+
+	printf("Open com port: %s, baud rate: %d\n", com_port.c_str(), baud_rate);
+	// Loop continuously, polling for packets
+#ifdef HAVE_ROS
+	printf("Run with ROS\n");
+	while (ros::ok())
+	{
+		std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 10ms=100Hz
+
+		ros::spinOnce();
+		if ((bytes_received = PollComport(gsof_decoder_pointer(gsof_decoder), gsof_decoder_size(gsof_decoder))) > 0)
+		{
+			printf("Received bytes: %d\n", bytes_received);
+			// increment the decode buffer length by the number of bytes received //
+			gsof_decoder_increment(gsof_decoder, bytes_received);
+
+			// decode all the packets in the buffer //
+			while (0 == gsof_record_decode(gsof_decoder, gsof_record))
+			{
+				printf("GSOF packet decoded.\n");
+				switch (gsof_record->record_type)
+				{
+				case record_type_utc:
+				{
+					if (decode_utc_record(utc_record, gsof_record) == 0)
+					{
+						printf("utc time record decoded.\n");
+						int64_t ms_gps_utc = gps_time_to_utc(utc_record);
+						if (ms_gps_utc != 0)
+						{
+							// NavSatFix
+							nav_sat_fix_msg.header.stamp.sec = static_cast<uint32_t>(ms_gps_utc / 1000);
+							nav_sat_fix_msg.header.stamp.nsec = static_cast<uint32_t>((ms_gps_utc - nav_sat_fix_msg.header.stamp.sec) * 1000000);
+							// IMU
+							imu_msg.header.stamp.sec = nav_sat_fix_msg.header.stamp.sec;
+							imu_msg.header.stamp.nsec = nav_sat_fix_msg.header.stamp.nsec;
+						}
+					}
+					break;
+				}
+				case record_type_ins_full_navigation:
+				{
+					if (decode_ins_full_navigation_record(ins_full_navigation_record, gsof_record) == 0)
+					{
+						printf("ins_full_navigation record decoded.\n");
+						// NavSatFix
+						nav_sat_fix_msg.header.frame_id = nav_sat_frame_id;
+						if (ins_full_navigation_record->quality_indication == Quality_Autonomous)
+						{
+							nav_sat_fix_msg.status.status = 0; // STATUS_FIX
+						}
+						else if (ins_full_navigation_record->quality_indication == Quality_PPSMode)
+						{
+							nav_sat_fix_msg.status.status = 1; // STATUS_SBAS_FIX
+						}
+						else if ((ins_full_navigation_record->quality_indication == Quality_Differential) ||
+							(ins_full_navigation_record->quality_indication == Quality_RTKFixed) ||
+							(ins_full_navigation_record->quality_indication == Quality_RTKFloat))
+						{
+							nav_sat_fix_msg.status.status = 2; // STATUS_GBAS_FIX
+						}
+						else
+						{
+							nav_sat_fix_msg.status.status = -1; // STATUS_NO_FIX
+						}
+						nav_sat_fix_msg.latitude = ins_full_navigation_record->latitude;
+						nav_sat_fix_msg.longitude = ins_full_navigation_record->longitude;
+						nav_sat_fix_msg.altitude = ins_full_navigation_record->altitude;
+
+						// Twist
+						twist_msg.linear.x = ins_full_navigation_record->north_velocity;
+						twist_msg.linear.y = ins_full_navigation_record->east_velocity;
+						twist_msg.linear.z = ins_full_navigation_record->down_velocity;
+						twist_msg.angular.x = ins_full_navigation_record->angle_rate_x * DEGREES_TO_RADIANS;
+						twist_msg.angular.y = ins_full_navigation_record->angle_rate_y * DEGREES_TO_RADIANS;
+						twist_msg.angular.z = ins_full_navigation_record->angle_rate_z * DEGREES_TO_RADIANS;
+
+						// IMU
+						imu_msg.header.frame_id = imu_frame_id;
+						// Convert roll, pitch, yaw from radians to quaternion format //
+						float phi = ins_full_navigation_record->roll * DEGREES_TO_RADIANS / 2.0f;
+						float theta = ins_full_navigation_record->pitch * DEGREES_TO_RADIANS / 2.0f;
+						float psi = ins_full_navigation_record->heading * DEGREES_TO_RADIANS / 2.0f;
+						float sin_phi = sinf(phi);
+						float cos_phi = cosf(phi);
+						float sin_theta = sinf(theta);
+						float cos_theta = cosf(theta);
+						float sin_psi = sinf(psi);
+						float cos_psi = cosf(psi);
+						imu_msg.orientation.x = -cos_phi * sin_theta * sin_psi + sin_phi * cos_theta * cos_psi;
+						imu_msg.orientation.y = cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi;
+						imu_msg.orientation.z = cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi;
+						imu_msg.orientation.w = cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi;
+
+						imu_msg.angular_velocity.x = ins_full_navigation_record->angle_rate_x * DEGREES_TO_RADIANS; // These the same as the TWIST msg values
+						imu_msg.angular_velocity.y = ins_full_navigation_record->angle_rate_y * DEGREES_TO_RADIANS;
+						imu_msg.angular_velocity.z = ins_full_navigation_record->angle_rate_z * DEGREES_TO_RADIANS;
+						imu_msg.linear_acceleration.x = ins_full_navigation_record->longitudinal_acceleration;
+						imu_msg.linear_acceleration.y = ins_full_navigation_record->traverse_acceleration;
+						imu_msg.linear_acceleration.z = ins_full_navigation_record->down_acceleration;
+
+						// System Status
+						system_status_msg.message = "";
+						system_status_msg.level = 0; // default OK state
+
+						// Filter Status
+						filter_status_msg.message = "";
+						filter_status_msg.level = 0; // default OK state
+						if (ins_full_navigation_record->align_status == Align_Aligned ||
+							ins_full_navigation_record->align_status == Align_FullNavigationMode) {
+							filter_status_msg.message = filter_status_msg.message + "0. Aligned. ";
+						}
+						else {
+							filter_status_msg.level = 1; // WARN state
+							filter_status_msg.message = filter_status_msg.message + "1. Not aligned. ";
+						}
+
+						//{
+						//	namespace sc = std::chrono;
+						//	static sc::duration<double> start_time = sc::system_clock::now().time_since_epoch();
+						//	sc::duration<double> now_time = sc::system_clock::now().time_since_epoch();
+						//	double fps = 0;
+						//	double diff = (now_time - start_time).count();
+						//	if (diff > 0)
+						//	{
+						//		fps = 1 / diff;
+						//		start_time = now_time;
+						//	}
+						//}
+
+						//printf("INS Full fps:%.1fHz, gps_ms:%d\n", fps, ins_full_navigation_record->gps_time_ms);
+					}
+					break;
+				}
+				case record_type_ins_rms_info:
+				{
+					if (decode_ins_rms_info_record(ins_rms_info_record, gsof_record) == 0)
+					{
+						printf("ins_rms_info record decoded.\n");
+						// NavSatFix
+						nav_sat_fix_msg.position_covariance = {
+							pow(ins_rms_info_record->east_position_rms,2), 0.0, 0.0,
+							0.0, pow(ins_rms_info_record->north_position_rms,2), 0.0,
+							0.0, 0.0, pow(ins_rms_info_record->down_position_rms,2)
+						};
+
+						// IMU
+						imu_msg.orientation_covariance[0] = ins_rms_info_record->roll_rms * DEGREES_TO_RADIANS;
+						imu_msg.orientation_covariance[4] = ins_rms_info_record->pitch_rms * DEGREES_TO_RADIANS;
+						imu_msg.orientation_covariance[8] = ins_rms_info_record->heading_rms * DEGREES_TO_RADIANS;
+
+						//printf("INS RMS, gps_ms:%d\n", ins_rms_info_record->gps_time_ms);
+					}
+					break;
+				}
+				default:
+					break;
+				}
+				
+				// Publish messages //
+				if (utc_record->flags.val.time_valid && utc_record->flags.val.offset_valid)
+				{
+					printf("Publish messages.\n");
+					nav_sat_fix_pub.publish(nav_sat_fix_msg);
+					twist_pub.publish(twist_msg);
+					imu_pub.publish(imu_msg);
+					system_status_pub.publish(system_status_msg);
+					filter_status_pub.publish(filter_status_msg);
+				}
+				else {
+					printf("UTC time invalid, messages not published.\n");
+				}
+			}
+		}
+	}
+
+#else // HAVE_ROS
+	while (1)
+	{
+		std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 10ms=100Hz
+
+		if ((bytes_received = PollComport(gsof_decoder_pointer(gsof_decoder), gsof_decoder_size(gsof_decoder))) > 0)
+		{
+			// increment the decode buffer length by the number of bytes received //
+			gsof_decoder_increment(gsof_decoder, bytes_received);
+
+			printf("Received bytes:%d\n", bytes_received);
+			// decode all the packets in the buffer //
+			while (0 == gsof_record_decode(gsof_decoder, gsof_record))
+			{
+				switch (gsof_record->record_type)
+				{
+				case record_type_utc:
+				{
+					if (decode_utc_record(utc_record, gsof_record) == 0)
+					{
+						int64_t ms_gps_to_utc = gps_time_to_utc(utc_record);
+						printf("UTC offset:%d, gps_ms:%d\n", utc_record->utc_offset, utc_record->gps_time_ms);
+					}
+					break;
+				}
+				case record_type_ins_full_navigation:
+				{
+					if (decode_ins_full_navigation_record(ins_full_navigation_record, gsof_record) == 0)
+					{
+						namespace sc = std::chrono;
+						static sc::duration<double> start_time = sc::system_clock::now().time_since_epoch();
+						sc::duration<double> now_time = sc::system_clock::now().time_since_epoch();
+						double fps = 0;
+						double diff = (now_time - start_time).count();
+						if (diff > 0)
+						{
+							fps = 1 / diff;
+							start_time = now_time;
+						}
+
+						printf("INS Full fps:%.1fHz, gps_ms:%d\n", fps, ins_full_navigation_record->gps_time_ms);
+					}
+					break;
+				}
+				case record_type_ins_rms_info:
+				{
+					if (decode_ins_rms_info_record(ins_rms_info_record, gsof_record) == 0)
+					{
+						printf("INS RMS, gps_ms:%d\n", ins_rms_info_record->gps_time_ms);
+					}
+					break;
+				}
+				default:
+					break;
+				}
+			}
+		}
+	}
+
+#endif
+
+	// realese resource
+	free(gsof_decoder);
+	free(gsof_record);
+	free(utc_record);
+	free(ins_full_navigation_record);
+	free(ins_rms_info_record);
+
+} // end of main()

src/ros/open_source_code/NAV992_GNSS_INS/src/rs232/gpl.txt

@@ -0,0 +1,282 @@
+
+        GNU GENERAL PUBLIC LICENSE
+           Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+          Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users.  This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it.  (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.)  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+  To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have.  You must make sure that they, too, receive or can get the
+source code.  And you must show them these terms so they know their
+rights.
+
+  We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+  Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software.  If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+  Finally, any free program is threatened constantly by software
+patents.  We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary.  To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+        GNU GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License.  The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language.  (Hereinafter, translation is included without limitation in
+the term "modification".)  Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+  1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+  2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) You must cause the modified files to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in
+    whole or in part contains or is derived from the Program or any
+    part thereof, to be licensed as a whole at no charge to all third
+    parties under the terms of this License.
+
+    c) If the modified program normally reads commands interactively
+    when run, you must cause it, when started running for such
+    interactive use in the most ordinary way, to print or display an
+    announcement including an appropriate copyright notice and a
+    notice that there is no warranty (or else, saying that you provide
+    a warranty) and that users may redistribute the program under
+    these conditions, and telling the user how to view a copy of this
+    License.  (Exception: if the Program itself is interactive but
+    does not normally print such an announcement, your work based on
+    the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable
+    source code, which must be distributed under the terms of Sections
+    1 and 2 above on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three
+    years, to give any third party, for a charge no more than your
+    cost of physically performing source distribution, a complete
+    machine-readable copy of the corresponding source code, to be
+    distributed under the terms of Sections 1 and 2 above on a medium
+    customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer
+    to distribute corresponding source code.  (This alternative is
+    allowed only for noncommercial distribution and only if you
+    received the program in object code or executable form with such
+    an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it.  For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable.  However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License.  Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+  5. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Program or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+  6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+  7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded.  In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+  9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation.  If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+  10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission.  For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this.  Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+          NO WARRANTY
+
+  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+         END OF TERMS AND CONDITIONS
+

src/ros/open_source_code/NAV992_GNSS_INS/src/rs232/rs232.c

@@ -0,0 +1,369 @@
+/*
+***************************************************************************
+*
+* Author: Teunis van Beelen
+*
+* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen
+*
+* teuniz@gmail.com
+*
+***************************************************************************
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program; if not, write to the Free Software Foundation, Inc.,
+* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*
+***************************************************************************
+*
+* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
+*
+***************************************************************************
+*/
+
+
+
+#include "rs232.h"
+
+
+
+#ifdef __linux__   /* Linux */
+
+
+int Cport,
+    error;
+
+struct termios new_port_settings,
+       old_port_settings;
+
+int OpenComport(char *comport, int baudrate)
+{
+  int baudr;
+
+  switch(baudrate)
+  {
+    case      50 : baudr = B50;
+                   break;
+    case      75 : baudr = B75;
+                   break;
+    case     110 : baudr = B110;
+                   break;
+    case     134 : baudr = B134;
+                   break;
+    case     150 : baudr = B150;
+                   break;
+    case     200 : baudr = B200;
+                   break;
+    case     300 : baudr = B300;
+                   break;
+    case     600 : baudr = B600;
+                   break;
+    case    1200 : baudr = B1200;
+                   break;
+    case    1800 : baudr = B1800;
+                   break;
+    case    2400 : baudr = B2400;
+                   break;
+    case    4800 : baudr = B4800;
+                   break;
+    case    9600 : baudr = B9600;
+                   break;
+    case   19200 : baudr = B19200;
+                   break;
+    case   38400 : baudr = B38400;
+                   break;
+    case   57600 : baudr = B57600;
+                   break;
+    case  115200 : baudr = B115200;
+                   break;
+    case  230400 : baudr = B230400;
+                   break;
+    case  460800 : baudr = B460800;
+                   break;
+    case  500000 : baudr = B500000;
+                   break;
+    case  576000 : baudr = B576000;
+                   break;
+    case  921600 : baudr = B921600;
+                   break;
+    case 1000000 : baudr = B1000000;
+                   break;
+    default      : printf("invalid baudrate\n");
+                   return(1);
+                   break;
+  }
+
+  Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY);
+  if(Cport==-1)
+  {
+    perror("unable to open comport ");
+    return(1);
+  }
+
+  error = tcgetattr(Cport, &old_port_settings);
+  if(error==-1)
+  {
+    close(Cport);
+    perror("unable to read portsettings ");
+    return(1);
+  }
+  memset(&new_port_settings, 0, sizeof(new_port_settings));  /* clear the new struct */
+
+  new_port_settings.c_cflag = baudr | CS8 | CLOCAL | CREAD;
+  new_port_settings.c_iflag = IGNPAR;
+  new_port_settings.c_oflag = 0;
+  new_port_settings.c_lflag = 0;
+  new_port_settings.c_cc[VMIN] = 0;      /* block untill n bytes are received */
+  new_port_settings.c_cc[VTIME] = 0;     /* block untill a timer expires (n * 100 mSec.) */
+  error = tcsetattr(Cport, TCSANOW, &new_port_settings);
+  if(error==-1)
+  {
+    close(Cport);
+    perror("unable to adjust portsettings ");
+    return(1);
+  }
+
+  return(0);
+}
+
+
+int PollComport(unsigned char *buf, int size)
+{
+  int n;
+
+#ifndef __STRICT_ANSI__                       /* __STRICT_ANSI__ is defined when the -ansi option is used for gcc */
+  if(size>SSIZE_MAX)  size = (int)SSIZE_MAX;  /* SSIZE_MAX is defined in limits.h */
+#else
+  if(size>4096)  size = 4096;
+#endif
+
+  n = read(Cport, buf, size);
+
+  return(n);
+}
+
+
+int SendByte(unsigned char byte)
+{
+  int n;
+
+  n = write(Cport, &byte, 1);
+  if(n<0)  return(1);
+
+  return(0);
+}
+
+
+int SendBuf(unsigned char *buf, int size)
+{
+  return(write(Cport, buf, size));
+}
+
+
+void CloseComport()
+{
+  close(Cport);
+  tcsetattr(Cport, TCSANOW, &old_port_settings);
+}
+
+/*
+Constant  Description
+TIOCM_LE  DSR (data set ready/line enable)
+TIOCM_DTR DTR (data terminal ready)
+TIOCM_RTS RTS (request to send)
+TIOCM_ST  Secondary TXD (transmit)
+TIOCM_SR  Secondary RXD (receive)
+TIOCM_CTS CTS (clear to send)
+TIOCM_CAR DCD (data carrier detect)
+TIOCM_CD  Synonym for TIOCM_CAR
+TIOCM_RNG RNG (ring)
+TIOCM_RI  Synonym for TIOCM_RNG
+TIOCM_DSR DSR (data set ready)
+*/
+
+int IsCTSEnabled(int comport_number)
+{
+  int status;
+
+  status = ioctl(Cport, TIOCMGET, &status);
+
+  if(status&TIOCM_CTS) return(1);
+  else return(0);
+}
+
+
+#else         /* windows */
+
+
+HANDLE Cport;
+
+char baudr[64];
+
+
+int OpenComport(char *comport, int baudrate)
+{
+  DCB port_settings;
+  COMMTIMEOUTS Cptimeouts;
+  
+  char comport_path[32];
+  sprintf(comport_path, "\\\\.\\%s", comport);
+
+  switch(baudrate)
+  {
+    case     110 : strcpy(baudr, "baud=110 data=8 parity=N stop=1");
+                   break;
+    case     300 : strcpy(baudr, "baud=300 data=8 parity=N stop=1");
+                   break;
+    case     600 : strcpy(baudr, "baud=600 data=8 parity=N stop=1");
+                   break;
+    case    1200 : strcpy(baudr, "baud=1200 data=8 parity=N stop=1");
+                   break;
+    case    2400 : strcpy(baudr, "baud=2400 data=8 parity=N stop=1");
+                   break;
+    case    4800 : strcpy(baudr, "baud=4800 data=8 parity=N stop=1");
+                   break;
+    case    9600 : strcpy(baudr, "baud=9600 data=8 parity=N stop=1");
+                   break;
+    case   19200 : strcpy(baudr, "baud=19200 data=8 parity=N stop=1");
+                   break;
+    case   38400 : strcpy(baudr, "baud=38400 data=8 parity=N stop=1");
+                   break;
+    case   57600 : strcpy(baudr, "baud=57600 data=8 parity=N stop=1");
+                   break;
+    case  115200 : strcpy(baudr, "baud=115200 data=8 parity=N stop=1");
+                   break;
+    case  128000 : strcpy(baudr, "baud=128000 data=8 parity=N stop=1");
+                   break;
+	case  230400 : strcpy(baudr, "baud=230400 data=8 parity=N stop=1");
+                   break;
+    case  256000 : strcpy(baudr, "baud=256000 data=8 parity=N stop=1");
+                   break;
+	case  460800 : strcpy(baudr, "baud=460800 data=8 parity=N stop=1");
+                   break;
+	case  500000 : strcpy(baudr, "baud=500000 data=8 parity=N stop=1");
+                   break;
+	case  921600 : strcpy(baudr, "baud=921600 data=8 parity=N stop=1");
+                   break;
+	case 1000000 : strcpy(baudr, "baud=1000000 data=8 parity=N stop=1");
+                   break;
+    default      : printf("invalid baudrate\n");
+                   return(1);
+                   break;
+  }
+
+  Cport = CreateFileA(comport_path,
+                      GENERIC_READ|GENERIC_WRITE,
+                      0,                          /* no share  */
+                      NULL,                       /* no security */
+                      OPEN_EXISTING,
+                      0,                          /* no threads */
+                      NULL);                      /* no templates */
+
+  if(Cport==INVALID_HANDLE_VALUE)
+  {
+    printf("unable to open comport\n");
+    return(1);
+  }
+
+  
+  memset(&port_settings, 0, sizeof(port_settings));  /* clear the new struct  */
+  port_settings.DCBlength = sizeof(port_settings);
+
+  if(!BuildCommDCBA(baudr, &port_settings))
+  {
+    printf("unable to set comport dcb settings\n");
+    CloseHandle(Cport);
+    return(1);
+  }
+
+  if(!SetCommState(Cport, &port_settings))
+  {
+    printf("unable to set comport cfg settings\n");
+    CloseHandle(Cport);
+    return(1);
+  }
+
+  Cptimeouts.ReadIntervalTimeout         = MAXDWORD;
+  Cptimeouts.ReadTotalTimeoutMultiplier  = 0;
+  Cptimeouts.ReadTotalTimeoutConstant    = 0;
+  Cptimeouts.WriteTotalTimeoutMultiplier = 0;
+  Cptimeouts.WriteTotalTimeoutConstant   = 0;
+
+  if(!SetCommTimeouts(Cport, &Cptimeouts))
+  {
+    printf("unable to set comport time-out settings\n");
+    CloseHandle(Cport);
+    return(1);
+  }
+
+  return(0);
+}
+
+
+int PollComport(unsigned char *buf, int size)
+{
+  int n;
+
+  if(size>4096)  size = 4096;
+
+/* added the void pointer cast, otherwise gcc will complain about */
+/* "warning: dereferencing type-punned pointer will break strict aliasing rules" */
+
+  ReadFile(Cport, buf, size, (LPDWORD)((void *)&n), NULL);
+
+  return(n);
+}
+
+
+int SendByte(unsigned char byte)
+{
+  int n;
+
+  WriteFile(Cport, &byte, 1, (LPDWORD)((void *)&n), NULL);
+
+  if(n<0)  return(1);
+
+  return(0);
+}
+
+
+int SendBuf(unsigned char *buf, int size)
+{
+  int n;
+
+  if(WriteFile(Cport, buf, size, (LPDWORD)((void *)&n), NULL))
+  {
+    return(n);
+  }
+
+  return(-1);
+}
+
+
+void CloseComport()
+{
+  CloseHandle(Cport);
+}
+
+
+int IsCTSEnabled()
+{
+  int status;
+
+  GetCommModemStatus(Cport, (LPDWORD)((void *)&status));
+
+  if(status&MS_CTS_ON) return(1);
+  else return(0);
+}
+
+
+#endif
+

src/ros/open_source_code/NAV992_GNSS_INS/src/rs232/rs232.h

@@ -0,0 +1,76 @@
+/*
+***************************************************************************
+*
+* Author: Teunis van Beelen
+*
+* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen
+*
+* teuniz@gmail.com
+*
+***************************************************************************
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program; if not, write to the Free Software Foundation, Inc.,
+* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*
+***************************************************************************
+*
+* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
+*
+***************************************************************************
+*/
+
+
+
+#ifndef rs232_INCLUDED
+#define rs232_INCLUDED
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <string.h>
+
+
+
+#ifdef __linux__
+
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <limits.h>
+
+#else
+
+#include <windows.h>
+
+#endif
+
+int OpenComport(char *, int);
+int PollComport(unsigned char *, int);
+int SendByte(unsigned char);
+int SendBuf(unsigned char *, int);
+void CloseComport();
+int IsCTSEnabled();
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif
+
+

src/ros/open_source_code/NAV992_GNSS_INS/src/struct_endian.c

@@ -0,0 +1,26 @@
+/******************************************************************************
+* Copyright 2017. All Rights Reserved.
+*
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+******************************************************************************/
+
+/**
+* @file
+*/
+
+#include "struct_endian.h"
+
+int struct_get_endian(void)
+{
+	int i = 0x00000001;
+	if (((char *)&i)[0]) {
+		return STRUCT_ENDIAN_LITTLE;
+	} else {
+		return STRUCT_ENDIAN_BIG;
+	}
+}

src/ros/open_source_code/NAV992_GNSS_INS/src/struct_endian.h

@@ -0,0 +1,10 @@
+#ifndef VAS_COMMON_STRUCT_ENDIAN_H_
+#define VAS_COMMON_STRUCT_ENDIAN_H_
+
+#define STRUCT_ENDIAN_NOT_SET   0
+#define STRUCT_ENDIAN_BIG       1
+#define STRUCT_ENDIAN_LITTLE    2
+
+extern int struct_get_endian(void);
+
+#endif /* !VAS_COMMON_STRUCT_ENDIAN_H_ */

src/ros/open_source_code/NAV992_GNSS_INS/src/struct_pack_unpack.c

@@ -0,0 +1,813 @@
+/******************************************************************************
+* Copyright 2017. All Rights Reserved.
+*
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+******************************************************************************/
+
+/**
+* @file
+*/
+
+#ifdef _MSC_VER
+#pragma warning(disable:4508 4101 4700 4244 4003)
+#endif // _MSC_VER
+
+#include "struct_pack_unpack.h"
+#include "struct_endian.h"
+
+#include <stdarg.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+#include <math.h>
+
+#define IEEE754_32_NAN     0x7FC00000
+#define IEEE754_32_INF     0x7F800000
+#define IEEE754_32_NEG_INF 0xFF800000
+
+#define IEEE754_64_NAN     0x7FF8000000000000
+#define IEEE754_64_INF     0x7FF0000000000000
+#define IEEE754_64_NEG_INF 0xFFF0000000000000
+
+// refer to
+// http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html#serialization
+// - Beej's Guide to Network Programming
+//
+// macros for packing floats and doubles:
+#define PACK_IEEE754_32(f) (pack_ieee754((f), 32, 8))
+#define PACK_IEEE754_64(f) (pack_ieee754((f), 64, 11))
+#define UNPACK_IEEE754_32(i) (unpack_ieee754((i), 32, 8))
+#define UNPACK_IEEE754_64(i) (unpack_ieee754((i), 64, 11))
+
+#define INIT_REPETITION(_x) int _struct_rep = 0
+
+#define BEGIN_REPETITION(_x) do { _struct_rep--
+
+#define END_REPETITION(_x) } while(_struct_rep > 0)
+
+#define INC_REPETITION(_x) _struct_rep = _struct_rep * 10 + (*p - '0')
+
+#define CLEAR_REPETITION(_x) _struct_rep = 0
+
+static int myendian = STRUCT_ENDIAN_NOT_SET;
+
+static void struct_init(void)
+{
+    myendian = struct_get_endian();
+}
+
+static uint64_t pack_ieee754(long double f,
+        unsigned int bits, unsigned int expbits)
+{
+    long double fnorm;
+    int shift;
+    long long sign;
+    long long exp;
+    long long significand;
+    unsigned int significandbits = bits - expbits - 1; // -1 for sign bit
+
+    int isinf_ret = isinf(f);
+    if (isinf_ret != 0) {
+      // isinf(f) returns 1 if f is positive infinity,
+      //                  -1 if f is negative infinity.
+      if (bits == 32) {
+        return (isinf_ret == 1) ? IEEE754_32_INF : IEEE754_32_NEG_INF;
+      } else {
+        return (isinf_ret == 1) ? IEEE754_64_INF : IEEE754_64_NEG_INF;
+      }
+    }
+
+    if (isnan(f)) {
+      if (bits == 32) {
+        return IEEE754_32_NAN;
+      } else {
+        return IEEE754_64_NAN;
+      }
+    }
+
+    if (f == 0.0) {
+        return 0; // get this special case out of the way
+    }
+
+    // check sign and begin normalization
+    if (f < 0) {
+        sign = 1;
+        fnorm = -f;
+    } else {
+        sign = 0;
+        fnorm = f;
+    }
+
+    // get the normalized form of f and track the exponent
+    shift = 0;
+    while (fnorm >= 2.0) {
+        fnorm /= 2.0;
+        shift++;
+    }
+    while (fnorm < 1.0) {
+        fnorm *= 2.0;
+        shift--;
+    }
+    fnorm = fnorm - 1.0;
+
+    // calculate the binary form (non-float) of the significand data
+    significand = (long long)(fnorm * ((1LL << significandbits) + 0.5F));
+
+    // get the biased exponent
+    // shift + bias
+    exp = shift + ((1LL << (expbits - 1)) - 1);
+
+    // return the final answer
+    return (sign << (bits - 1U)) | (exp << (bits - expbits - 1U)) | significand;
+}
+
+static long double unpack_ieee754(uint64_t i,
+        unsigned int bits, unsigned int expbits)
+{
+    long double result;
+    long long shift;
+    unsigned int bias;
+    unsigned int significandbits = bits - expbits - 1; // -1 for sign bit
+
+    if (i == 0) {
+        return 0.0;
+    }
+
+    if (bits == 32) {
+      if (i == IEEE754_32_NAN) {
+        return NAN;
+      }
+
+      if (i == IEEE754_32_INF) {
+        return INFINITY;
+      }
+
+      if (i == IEEE754_32_NEG_INF) {
+        return -INFINITY;
+      }
+    } else {
+      if (i == IEEE754_64_NAN) {
+        return NAN;
+      }
+
+      if (i == IEEE754_64_INF) {
+        return INFINITY;
+      }
+
+      if (i == IEEE754_64_NEG_INF) {
+        return -INFINITY;
+      }
+    }
+
+    // pull the significand
+    result = (i & ((1LL << significandbits) - 1)); // mask
+    result /= (1LL << significandbits); // convert back to float
+    result += 1.0F; // add the one back on
+
+    // deal with the exponent
+    bias = (1 << (expbits - 1)) - 1;
+    shift = ((i >> significandbits) & ((1LL << expbits) - 1)) - bias;
+    while (shift > 0) {
+        result *= 2.0;
+        shift--;
+    }
+    while (shift < 0) {
+        result /= 2.0;
+        shift++;
+    }
+
+    // sign it
+    result *= ((i >> (bits - 1)) & 1) ? -1.0 : 1.0;
+
+    return result;
+}
+
+static void pack_int16_t(unsigned char **bp, uint16_t val, int endian)
+{
+    if (endian == myendian) {
+        *((*bp)++) = val;
+        *((*bp)++) = val >> 8;
+    } else {
+        *((*bp)++) = val >> 8;
+        *((*bp)++) = val;
+    }
+}
+
+static void pack_int32_t(unsigned char **bp, uint32_t val, int endian)
+{
+    if (endian == myendian) {
+        *((*bp)++) = val;
+        *((*bp)++) = val >> 8;
+        *((*bp)++) = val >> 16;
+        *((*bp)++) = val >> 24;
+    } else {
+        *((*bp)++) = val >> 24;
+        *((*bp)++) = val >> 16;
+        *((*bp)++) = val >> 8;
+        *((*bp)++) = val;
+    }
+}
+
+static void pack_int64_t(unsigned char **bp, uint64_t val, int endian)
+{
+    if (endian == myendian) {
+        *((*bp)++) = val;
+        *((*bp)++) = val >> 8;
+        *((*bp)++) = val >> 16;
+        *((*bp)++) = val >> 24;
+        *((*bp)++) = val >> 32;
+        *((*bp)++) = val >> 40;
+        *((*bp)++) = val >> 48;
+        *((*bp)++) = val >> 56;
+    } else {
+        *((*bp)++) = val >> 56;
+        *((*bp)++) = val >> 48;
+        *((*bp)++) = val >> 40;
+        *((*bp)++) = val >> 32;
+        *((*bp)++) = val >> 24;
+        *((*bp)++) = val >> 16;
+        *((*bp)++) = val >> 8;
+        *((*bp)++) = val;
+    }
+}
+
+static void pack_float(unsigned char **bp, float val, int endian)
+{
+    uint64_t ieee754_encoded_val = PACK_IEEE754_32(val);
+    pack_int32_t(bp, ieee754_encoded_val, endian);
+}
+
+static void pack_double(unsigned char **bp, double val, int endian)
+{
+    uint64_t ieee754_encoded_val = PACK_IEEE754_64(val);
+    pack_int64_t(bp, ieee754_encoded_val, endian);
+}
+
+static void unpack_int16_t(const unsigned char **bp, int16_t *dst, int endian)
+{
+    uint16_t val;
+    if (endian == myendian) {
+        val = *((*bp)++);
+        val |= (uint16_t)(*((*bp)++)) << 8;
+    } else {
+        val = (uint16_t)(*((*bp)++)) << 8;
+        val |= *((*bp)++);
+    }
+    if (val <= 0x7fffU) {
+        *dst = val;
+    } else {
+        *dst = -1 - (int16_t)(0xffffU - val);
+    }
+}
+
+static void unpack_uint16_t(const unsigned char **bp, uint16_t *dst, int endian)
+{
+    if (endian == myendian) {
+        *dst = *((*bp)++);
+        *dst |= (uint16_t)(*((*bp)++)) << 8;
+    } else {
+        *dst = (uint16_t)(*((*bp)++)) << 8;
+        *dst |= *((*bp)++);
+    }
+}
+
+static void unpack_int32_t(const unsigned char **bp, int32_t *dst, int endian)
+{
+    uint32_t val;
+    if (endian == myendian) {
+        val = *((*bp)++);
+        val |= (uint32_t)(*((*bp)++)) << 8;
+        val |= (uint32_t)(*((*bp)++)) << 16;
+        val |= (uint32_t)(*((*bp)++)) << 24;
+    } else {
+        val = *((*bp)++) << 24;
+        val |= (uint32_t)(*((*bp)++)) << 16;
+        val |= (uint32_t)(*((*bp)++)) << 8;
+        val |= (uint32_t)(*((*bp)++));
+    }
+    if (val <= 0x7fffffffU) {
+        *dst = val;
+    } else {
+        *dst = -1 - (int32_t)(0xffffffffU - val);
+    }
+}
+
+static void unpack_uint32_t(const unsigned char **bp, uint32_t *dst, int endian)
+{
+    if (endian == myendian) {
+        *dst = *((*bp)++);
+        *dst |= (uint32_t)(*((*bp)++)) << 8;
+        *dst |= (uint32_t)(*((*bp)++)) << 16;
+        *dst |= (uint32_t)(*((*bp)++)) << 24;
+    } else {
+        *dst = *((*bp)++) << 24;
+        *dst |= (uint32_t)(*((*bp)++)) << 16;
+        *dst |= (uint32_t)(*((*bp)++)) << 8;
+        *dst |= (uint32_t)(*((*bp)++));
+    }
+}
+
+static void unpack_int64_t(const unsigned char **bp, int64_t *dst, int endian)
+{
+    uint64_t val;
+    if (endian == myendian) {
+        val = *((*bp)++);
+        val |= (uint64_t)(*((*bp)++)) << 8;
+        val |= (uint64_t)(*((*bp)++)) << 16;
+        val |= (uint64_t)(*((*bp)++)) << 24;
+        val |= (uint64_t)(*((*bp)++)) << 32;
+        val |= (uint64_t)(*((*bp)++)) << 40;
+        val |= (uint64_t)(*((*bp)++)) << 48;
+        val |= (uint64_t)(*((*bp)++)) << 56;
+    } else {
+        val = (uint64_t)(*((*bp)++)) << 56;
+        val |= (uint64_t)(*((*bp)++)) << 48;
+        val |= (uint64_t)(*((*bp)++)) << 40;
+        val |= (uint64_t)(*((*bp)++)) << 32;
+        val |= (uint64_t)(*((*bp)++)) << 24;
+        val |= (uint64_t)(*((*bp)++)) << 16;
+        val |= (uint64_t)(*((*bp)++)) << 8;
+        val |= *((*bp)++);
+    }
+    if (val <= 0x7fffffffffffffffULL) {
+        *dst = val;
+    } else {
+        *dst = -1 - (int64_t)(0xffffffffffffffffULL - val);
+    }
+}
+
+static void unpack_uint64_t(const unsigned char **bp, uint64_t *dst, int endian)
+{
+    if (endian == myendian) {
+        *dst = *((*bp)++);
+        *dst |= (uint64_t)(*((*bp)++)) << 8;
+        *dst |= (uint64_t)(*((*bp)++)) << 16;
+        *dst |= (uint64_t)(*((*bp)++)) << 24;
+        *dst |= (uint64_t)(*((*bp)++)) << 32;
+        *dst |= (uint64_t)(*((*bp)++)) << 40;
+        *dst |= (uint64_t)(*((*bp)++)) << 48;
+        *dst |= (uint64_t)(*((*bp)++)) << 56;
+    } else {
+        *dst = (uint64_t)(*((*bp)++)) << 56;
+        *dst |= (uint64_t)(*((*bp)++)) << 48;
+        *dst |= (uint64_t)(*((*bp)++)) << 40;
+        *dst |= (uint64_t)(*((*bp)++)) << 32;
+        *dst |= (uint64_t)(*((*bp)++)) << 24;
+        *dst |= (uint64_t)(*((*bp)++)) << 16;
+        *dst |= (uint64_t)(*((*bp)++)) << 8;
+        *dst |= *((*bp)++);
+    }
+}
+
+static void unpack_float(const unsigned char **bp, float *dst, int endian)
+{
+    uint32_t ieee754_encoded_val = 0;
+    unpack_uint32_t(bp, &ieee754_encoded_val, endian);
+    *dst = UNPACK_IEEE754_32(ieee754_encoded_val);
+}
+
+static void unpack_double(const unsigned char **bp, double *dst, int endian)
+{
+    uint64_t ieee754_encoded_val = 0;
+    unpack_uint64_t(bp, &ieee754_encoded_val, endian);
+    *dst = UNPACK_IEEE754_64(ieee754_encoded_val);
+}
+
+static int pack_va_list(unsigned char *buf, int offset, const char *fmt,
+                          va_list args)
+{
+    INIT_REPETITION();
+    const char *p;
+    unsigned char *bp;
+    int *ep = &myendian;
+    int endian;
+
+    char b;
+    unsigned char B;
+    int16_t h;
+    uint16_t H;
+    int32_t l;
+    uint32_t L;
+    int64_t q;
+    uint64_t Q;
+    float f;
+    double d;
+    char *s;
+
+    if (STRUCT_ENDIAN_NOT_SET == myendian) {
+        struct_init();
+    }
+
+    /*
+     * 'char' and 'short' values, they must be extracted as 'int's,
+     * because C promotes 'char' and 'short' arguments to 'int' when they are
+     * represented by an ellipsis ... parameter.
+     */
+
+    bp = buf + offset;
+    for (p = fmt; *p != '\0'; p++) {
+        switch (*p) {
+        case '=': /* native */
+            ep = &myendian;
+            break;
+        case '<': /* little-endian */
+            endian = STRUCT_ENDIAN_LITTLE;
+            ep = &endian;
+            break;
+        case '>': /* big-endian */
+            endian = STRUCT_ENDIAN_BIG;
+            ep = &endian;
+            break;
+        case '!': /* network (= big-endian) */
+            endian = STRUCT_ENDIAN_BIG;
+            ep = &endian;
+            break;
+        case 'b':
+            BEGIN_REPETITION();
+                b = va_arg(args, int);
+                *bp++ = b;
+            END_REPETITION();
+            break;
+        case 'B':
+            BEGIN_REPETITION();
+                B = va_arg(args, unsigned int);
+                *bp++ = B;
+            END_REPETITION();
+            break;
+        case 'h':
+            BEGIN_REPETITION();
+                h = va_arg(args, int);
+                pack_int16_t(&bp, h, *ep);
+            END_REPETITION();
+            break;
+        case 'H':
+            BEGIN_REPETITION();
+                H = va_arg(args, int);
+                pack_int16_t(&bp, H, *ep);
+            END_REPETITION();
+            break;
+        case 'i': /* fall through */
+        case 'l':
+            BEGIN_REPETITION();
+                l = va_arg(args, int32_t);
+                pack_int32_t(&bp, l, *ep);
+            END_REPETITION();
+            break;
+        case 'I': /* fall through */
+        case 'L':
+            BEGIN_REPETITION();
+                L = va_arg(args, uint32_t);
+                pack_int32_t(&bp, L, *ep);
+            END_REPETITION();
+            break;
+        case 'q':
+            BEGIN_REPETITION();
+                q = va_arg(args, int64_t);
+                pack_int64_t(&bp, q, *ep);
+            END_REPETITION();
+            break;
+        case 'Q':
+            BEGIN_REPETITION();
+                Q = va_arg(args, uint64_t);
+                pack_int64_t(&bp, Q, *ep);
+            END_REPETITION();
+            break;
+        case 'f':
+            BEGIN_REPETITION();
+                f = va_arg(args, double);
+                pack_float(&bp, f, *ep);
+            END_REPETITION();
+            break;
+        case 'd':
+            BEGIN_REPETITION();
+                d = va_arg(args, double);
+                pack_double(&bp, d, *ep);
+            END_REPETITION();
+            break;
+        case 's': /* fall through */
+        case 'p':
+            {
+                int i = 0;
+                s = va_arg(args, char*);
+                BEGIN_REPETITION();
+                    *bp++ = s[i++];
+                END_REPETITION();
+            }
+            break;
+        case 'x':
+            BEGIN_REPETITION();
+                *bp++ = 0;
+            END_REPETITION();
+            break;
+        default:
+            if (isdigit((int)*p)) {
+                INC_REPETITION();
+            } else {
+                return -1;
+            }
+        }
+
+        if (!isdigit((int)*p)) {
+            CLEAR_REPETITION();
+        }
+    }
+    return (bp - buf);
+}
+
+static int unpack_va_list(
+    const unsigned char *buf,
+    int offset,
+    const char *fmt,
+    va_list args)
+{
+    INIT_REPETITION();
+    const char *p;
+    const unsigned char *bp;
+    int *ep = &myendian;
+    int endian;
+
+    char *b;
+    unsigned char *B;
+    int16_t *h;
+    uint16_t *H;
+    int32_t *l;
+    uint32_t *L;
+    int64_t *q;
+    uint64_t *Q;
+    float *f;
+    double *d;
+    char *s;
+
+    if (STRUCT_ENDIAN_NOT_SET == myendian) {
+        struct_init();
+    }
+
+    bp = buf + offset;
+    for (p = fmt; *p != '\0'; p++) {
+        switch (*p) {
+        case '=': /* native */
+            ep = &myendian;
+            break;
+        case '<': /* little-endian */
+            endian = STRUCT_ENDIAN_LITTLE;
+            ep = &endian;
+            break;
+        case '>': /* big-endian */
+            endian = STRUCT_ENDIAN_BIG;
+            ep = &endian;
+            break;
+        case '!': /* network (= big-endian) */
+            endian = STRUCT_ENDIAN_BIG;
+            ep = &endian;
+            break;
+        case 'b':
+            BEGIN_REPETITION();
+                b = va_arg(args, char*);
+                *b = *bp++;
+            END_REPETITION();
+            break;
+        case 'B':
+            BEGIN_REPETITION();
+                B = va_arg(args, unsigned char*);
+                *B = *bp++;
+            END_REPETITION();
+            break;
+        case 'h':
+            BEGIN_REPETITION();
+                h = va_arg(args, int16_t*);
+                unpack_int16_t(&bp, h, *ep);
+            END_REPETITION();
+            break;
+        case 'H':
+            BEGIN_REPETITION();
+                H = va_arg(args, uint16_t*);
+                unpack_uint16_t(&bp, H, *ep);
+            END_REPETITION();
+            break;
+        case 'i': /* fall through */
+        case 'l':
+            BEGIN_REPETITION();
+                l = va_arg(args, int32_t*);
+                unpack_int32_t(&bp, l, *ep);
+            END_REPETITION();
+            break;
+        case 'I': /* fall through */
+        case 'L':
+            BEGIN_REPETITION();
+                L = va_arg(args, uint32_t*);
+                unpack_uint32_t(&bp, L, *ep);
+            END_REPETITION();
+            break;
+        case 'q':
+            BEGIN_REPETITION();
+                q = va_arg(args, int64_t*);
+                unpack_int64_t(&bp, q, *ep);
+            END_REPETITION();
+            break;
+        case 'Q':
+            BEGIN_REPETITION();
+                Q = va_arg(args, uint64_t*);
+                unpack_uint64_t(&bp, Q, *ep);
+            END_REPETITION();
+            break;
+        case 'f':
+            BEGIN_REPETITION();
+                f = va_arg(args, float*);
+                unpack_float(&bp, f, *ep);
+            END_REPETITION();
+            break;
+        case 'd':
+            BEGIN_REPETITION();
+                d = va_arg(args, double*);
+                unpack_double(&bp, d, *ep);
+            END_REPETITION();
+            break;
+        case 's': /* fall through */
+        case 'p':
+            {
+                int i = 0;
+                s = va_arg(args, char*);
+                BEGIN_REPETITION();
+                    s[i++] = *bp++;
+                END_REPETITION();
+            }
+            break;
+        case 'x':
+            BEGIN_REPETITION();
+                bp++;
+            END_REPETITION();
+            break;
+        default:
+            if (isdigit((int)*p)) {
+                INC_REPETITION();
+            } else {
+                return -1;
+            }
+        }
+
+        if (!isdigit((int)*p)) {
+            CLEAR_REPETITION();
+        }
+    }
+    return (bp - buf);
+}
+
+/*
+ * EXPORT
+ *
+ * preifx: struct_
+ *
+ */
+int struct_pack(void *buf, const char *fmt, ...)
+{
+    va_list args;
+    int packed_len = 0;
+
+    va_start(args, fmt);
+    packed_len = pack_va_list(
+            (unsigned char*)buf, 0, fmt, args);
+    va_end(args);
+
+    return packed_len;
+}
+
+int struct_pack_into(int offset, void *buf, const char *fmt, ...)
+{
+    va_list args;
+    int packed_len = 0;
+
+    va_start(args, fmt);
+    packed_len = pack_va_list(
+            (unsigned char*)buf, offset, fmt, args);
+    va_end(args);
+
+    return packed_len;
+}
+
+int struct_unpack(const void *buf, const char *fmt, ...)
+{
+    va_list args;
+    int unpacked_len = 0;
+
+    va_start(args, fmt);
+    unpacked_len = unpack_va_list(
+            (const unsigned char*)buf, 0, fmt, args);
+    va_end(args);
+
+    return unpacked_len;
+}
+
+int struct_unpack_from(int offset, const void *buf, const char *fmt, ...)
+{
+    va_list args;
+    int unpacked_len = 0;
+
+    va_start(args, fmt);
+    unpacked_len = unpack_va_list(
+            (const unsigned char*)buf, offset, fmt, args);
+    va_end(args);
+
+    return unpacked_len;
+}
+
+int struct_calcsize(const char *fmt)
+{
+    INIT_REPETITION();
+    int ret = 0;
+    const char *p;
+
+    if (STRUCT_ENDIAN_NOT_SET == myendian) {
+        struct_init();
+    }
+
+    for (p = fmt; *p != '\0'; p++) {
+        switch (*p) {
+        case '=': /* fall through */
+        case '<': /* fall through */
+        case '>': /* fall through */
+        case '!': /* ignore endian characters */
+            break;
+        case 'b':
+            BEGIN_REPETITION();
+            ret += sizeof(int8_t);
+            END_REPETITION();
+            break;
+        case 'B':
+            BEGIN_REPETITION();
+            ret += sizeof(uint8_t);
+            END_REPETITION();
+            break;
+        case 'h':
+            BEGIN_REPETITION();
+            ret += sizeof(int16_t);
+            END_REPETITION();
+            break;
+        case 'H':
+            BEGIN_REPETITION();
+            ret += sizeof(uint16_t);
+            END_REPETITION();
+            break;
+        case 'i': /* fall through */
+        case 'l':
+            BEGIN_REPETITION();
+            ret += sizeof(int32_t);
+            END_REPETITION();
+            break;
+        case 'I': /* fall through */
+        case 'L':
+            BEGIN_REPETITION();
+            ret += sizeof(uint32_t);
+            END_REPETITION();
+            break;
+        case 'q':
+            BEGIN_REPETITION();
+            ret += sizeof(int64_t);
+            END_REPETITION();
+            break;
+        case 'Q':
+            BEGIN_REPETITION();
+            ret += sizeof(uint64_t);
+            END_REPETITION();
+            break;
+        case 'f':
+            BEGIN_REPETITION();
+            ret += sizeof(int32_t); // see pack_float()
+            END_REPETITION();
+            break;
+        case 'd':
+            BEGIN_REPETITION();
+            ret += sizeof(int64_t); // see pack_double()
+            END_REPETITION();
+            break;
+        case 's': /* fall through */
+        case 'p':
+            BEGIN_REPETITION();
+            ret += sizeof(int8_t);
+            END_REPETITION();
+            break;
+        case 'x':
+            BEGIN_REPETITION();
+            ret += sizeof(int8_t);
+            END_REPETITION();
+            break;
+        default:
+            if (isdigit((int)*p)) {
+                INC_REPETITION();
+            } else {
+                return -1;
+            }
+        }
+
+        if (!isdigit((int)*p)) {
+            CLEAR_REPETITION();
+        }
+    }
+    return ret;
+}

src/ros/open_source_code/NAV992_GNSS_INS/src/struct_pack_unpack.h

@@ -0,0 +1,151 @@
+/******************************************************************************
+* Copyright 2017. All Rights Reserved.
+*
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+******************************************************************************/
+
+/**
+* @file
+*/
+
+#ifndef VAS_COMMON_STRUCT_PACK_UNPACK_H_
+#define VAS_COMMON_STRUCT_PACK_UNPACK_H_
+
+/*
+ * struct.h
+ *
+ *  Created on: 2011. 5. 2.
+ *      Author: wonseok choi (svperbeast@gmail.com)
+ *
+ * Interpret strings as packed binary data
+ *
+ * Table 1. Byte order
+ *  ----------------------------------
+ *  Character | Byte order
+ *  ----------+-----------------------
+ *   =        | native
+ *  ----------+-----------------------
+ *   <        | little-endian
+ *  ----------+-----------------------
+ *   >        | big-endian
+ *  ----------+-----------------------
+ *   !        | network (= big-endian)
+ *  ----------------------------------
+ *
+ * Table 2. Format characters
+ *  -------------------------------------------
+ *  Format | C/C++ Type         | Standard size
+ *  -------+--------------------+--------------
+ *   b     | char               | 1
+ *  -------+--------------------+--------------
+ *   B     | unsigned char      | 1
+ *  -------+--------------------+--------------
+ *   h     | short              | 2
+ *  -------+--------------------+--------------
+ *   H     | unsigned short     | 2
+ *  -------+--------------------+--------------
+ *   i     | int                | 4
+ *  -------+--------------------+--------------
+ *   I     | unsigned int       | 4
+ *  -------+--------------------+--------------
+ *   l     | long               | 4
+ *  -------+--------------------+--------------
+ *   L     | unsigned long      | 4
+ *  -------+--------------------+--------------
+ *   q     | long long          | 8
+ *  -------+--------------------+--------------
+ *   Q     | unsigned long long | 8
+ *  -------+--------------------+--------------
+ *   f     | float              | 4
+ *  -------+--------------------+--------------
+ *   d     | double             | 8
+ *  -------+--------------------+--------------
+ *   s     | char[]             |
+ *  -------+--------------------+--------------
+ *   p     | char[]             |
+ *  -------+--------------------+--------------
+ *   x     | pad bytes          |
+ *  -------------------------------------------
+ *
+ * A format character may be preceded by an integral repeat count.
+ * For example, the format string '4h' means exactly the same as 'hhhh'.
+ *
+ * For the 's' format character, the count is interpreted as the size of the
+ * string, not a repeat count like for the other format characters.
+ * For example, '10s' means a single 10-byte string.
+ *
+ * Example 1. pack/unpack int type value.
+ *
+ * char buf[BUFSIZ] = {0, };
+ * int val = 0x12345678;
+ * int oval;
+ *
+ * struct_pack(buf, "i", val);
+ * struct_unpack(buf, "i", &oval);
+ *
+ * Example 2. pack/unpack a string.
+ *
+ * char buf[BUFSIZ] = {0, };
+ * char str[32] = {'\0', };
+ * char fmt[32] = {'\0', };
+ * char ostr[32] = {'\0', };
+ *
+ * strcpy(str, "test");
+ * sprintf(fmt, "%ds", strlen(str));
+ *
+ * struct_pack(buf, fmt, str);
+ * struct_unpack(buf, fmt, ostr);
+ *
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief pack data
+ * @return the number of bytes encoded on success, -1 on failure.
+ */
+extern int struct_pack(void *buf, const char *fmt, ...);
+
+/**
+ * @brief pack data with offset
+ * @return the number of bytes encoded on success, -1 on failure.
+ */
+extern int struct_pack_into(int offset, void *buf, const char *fmt, ...);
+
+/**
+ * @brief unpack data
+ * @return the number of bytes decoded on success, -1 on failure.
+ */
+extern int struct_unpack(const void *buf, const char *fmt, ...);
+
+/**
+ * @brief unpack data with offset
+ * @return the number of bytes decoded on success, -1 on failure.
+ */
+extern int struct_unpack_from(
+    int offset,
+    const void *buf,
+    const char *fmt,
+    ...);
+
+/**
+ * @brief calculate the size of a format string
+ * @return the number of bytes needed by the format string on success,
+ * -1 on failure.
+ *
+ * make sure that the return value is > 0, before using it.
+ */
+extern int struct_calcsize(const char *fmt);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !STRUCT_INCLUDED */