diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..c08fd54 Binary files /dev/null and b/.DS_Store differ diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4620532 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.pythonPath": "/Users/bjarnejohannsen/miniforge3/envs/python38/bin/python" +} \ No newline at end of file diff --git a/ntrip_ros/CMakeLists.txt b/ntrip_ros/CMakeLists.txt deleted file mode 100644 index 8610c79..0000000 --- a/ntrip_ros/CMakeLists.txt +++ /dev/null @@ -1,189 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ntrip_ros) - -## 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 -# rtcm_msgs -# nmea_msgs - rospy - 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 -# nmea_msgs# rtcm_msgs# 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 ntrip_ros -# CATKIN_DEPENDS nmea_msgs rospy rtcm_msgs std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(ntrip_ros -# src/${PROJECT_NAME}/ntrip_ros.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(ntrip_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(ntrip_ros_node src/ntrip_ros_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(ntrip_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(ntrip_ros_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_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 ntrip_ros ntrip_ros_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_ntrip_ros.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) diff --git a/ntrip_ros/README.md b/ntrip_ros/README.md deleted file mode 100644 index d02549c..0000000 --- a/ntrip_ros/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# ntrip_ros -NTRIP client, imports RTCM streams to ROS topic - -This was forked from github.com/tilk/ntrip_ros - -The CORS correction server that I am using does not have the /n/r characters. So I parsed out individual messages and published each one on the /rtcm ROS topic. -It would crash with IncompleteRead error. I added patch at top of file. -But the connection had closed and it would crash again. I ended up detecting zero length data and closing and reopening the data stream. -It continues on without a glitch. - -You can generate the require $GPGGA message at this site. https://www.nmeagen.org/ Set a point near where you want to run and click "Generate NMEA file". Cut and paste the $GPGGA message into the launch file. - -I intend to use it with https://github.com/ros-agriculture/ublox_f9p - -It may also require this package: https://github.com/tilk/rtcm_msgs - -A similar NTRIP client (may be better than mine) is here: https://github.com/dayjaby/ntrip_ros diff --git a/ntrip_ros/launch/ntrip_ros.launch b/ntrip_ros/launch/ntrip_ros.launch deleted file mode 100755 index 142611b..0000000 --- a/ntrip_ros/launch/ntrip_ros.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/ntrip_ros/package.xml b/ntrip_ros/package.xml deleted file mode 100644 index 31a60e3..0000000 --- a/ntrip_ros/package.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - ntrip_ros - 0.0.1 - The ntrip_ros package - - - - - tilk - - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - nmea_msgs - rospy - rtcm_msgs - std_msgs - - - - - - - - diff --git a/ntrip_ros/scripts/ntripclient.py b/ntrip_ros/scripts/ntripclient.py deleted file mode 100755 index 9beb6ce..0000000 --- a/ntrip_ros/scripts/ntripclient.py +++ /dev/null @@ -1,147 +0,0 @@ -#!/usr/bin/python3 - -import rospy -from datetime import datetime - -#from nmea_msgs.msg import Sentence -from rtcm_msgs.msg import Message - -from base64 import b64encode -from threading import Thread - -from http.client import HTTPConnection -from http.client import IncompleteRead - -''' This is to fix the IncompleteRead error - http://bobrochel.blogspot.com/2010/11/bad-servers-chunked-encoding-and.html''' -import http.client - -import requests -from requests.auth import HTTPBasicAuth - - - -def NiceToICY(self): - class InterceptedHTTPResponse(): - pass - import io - line = self.fp.readline().replace(b"ICY 200 OK\r\n", b"HTTP/1.0 200 OK\r\n") - InterceptedSelf = InterceptedHTTPResponse() - InterceptedSelf.fp = io.BufferedReader(io.BytesIO(line)) - InterceptedSelf.debuglevel = self.debuglevel - InterceptedSelf._close_conn = self._close_conn - return ORIGINAL_HTTP_CLIENT_READ_STATUS(InterceptedSelf) - -ORIGINAL_HTTP_CLIENT_READ_STATUS = http.client.HTTPResponse._read_status -http.client.HTTPResponse._read_status = NiceToICY - - -def patch_http_response_read(func): - def inner(*args): - try: - return func(*args) - except http.client.IncompleteRead as e: - return e.partial - return inner -http.client.HTTPResponse.read = patch_http_response_read(http.client.HTTPResponse.read) - -class ntripconnect(Thread): - def __init__(self, ntc): - super(ntripconnect, self).__init__() - self.ntc = ntc - self.stop = False - - def run(self): - headers = { - 'Ntrip-Version': 'Ntrip/2.0', - 'User-Agent': 'NTRIP ntrip_ros', - 'Connection': 'close', - 'Authorization': 'Basic ' + b64encode((self.ntc.ntrip_user + ':' + self.ntc.ntrip_pass).encode()).decode("ascii") - } - connection = HTTPConnection(self.ntc.ntrip_server) - connection.set_debuglevel(1) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - rmsg = Message() - restart_count = 0 - while not self.stop: - ''' - data = response.read(100) - pos = data.find('\r\n') - if pos != -1: - rmsg.message = buf + data[:pos] - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - buf = data[pos+2:] - self.ntc.pub.publish(rmsg) - else: buf += data - ''' - - ''' This now separates individual RTCM messages and publishes each one on the same topic ''' - data = response.read(1) - if len(data) != 0: - if data[0] == 211: - buf = [] - buf.append(data[0]) - data = response.read(2) - buf.append(data[0]) - buf.append(data[1]) - cnt = data[0] * 256 + data[1] - data = response.read(2) - buf.append(data[0]) - buf.append(data[1]) - typ = (data[0] * 256 + data[1]) / 16 - print(str(datetime.now()), cnt, typ) - cnt = cnt + 1 - for x in range(cnt): - data = response.read(1) - buf.append(data[0]) - rmsg.message = buf - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - self.ntc.pub.publish(rmsg) - buf = [] - else: print (data) - else: - ''' If zero length data, close connection and reopen it ''' - restart_count = restart_count + 1 - print("Zero length ", restart_count) - connection.close() - connection = HTTPConnection(self.ntc.ntrip_server) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - - connection.close() - -class ntripclient: - def __init__(self): - rospy.init_node('ntripclient', anonymous=True) - - self.rtcm_topic = rospy.get_param('~rtcm_topic', 'rtcm') - self.nmea_topic = rospy.get_param('~nmea_topic', 'nmea') - - self.ntrip_server = rospy.get_param('~ntrip_server') - self.ntrip_user = rospy.get_param('~ntrip_user') - self.ntrip_pass = rospy.get_param('~ntrip_pass') - self.ntrip_stream = rospy.get_param('~ntrip_stream') - self.nmea_gga = rospy.get_param('~nmea_gga') - - self.pub = rospy.Publisher(self.rtcm_topic, Message, queue_size=10) - - self.connection = None - self.connection = ntripconnect(self) - self.connection.start() - - def run(self): - rospy.spin() - if self.connection is not None: - self.connection.stop = True - -if __name__ == '__main__': - c = ntripclient() - c.run() - diff --git a/ntrip_ros/scripts/ntripclient_old.py b/ntrip_ros/scripts/ntripclient_old.py deleted file mode 100755 index c6ff098..0000000 --- a/ntrip_ros/scripts/ntripclient_old.py +++ /dev/null @@ -1,122 +0,0 @@ -#!/usr/bin/python - -import rospy -from datetime import datetime - -#from nmea_msgs.msg import Sentence -from rtcm_msgs.msg import Message - -from base64 import b64encode -from threading import Thread - -from httplib import HTTPConnection -from httplib import IncompleteRead - -''' This is to fix the IncompleteRead error - http://bobrochel.blogspot.com/2010/11/bad-servers-chunked-encoding-and.html''' -import httplib -def patch_http_response_read(func): - def inner(*args): - try: - return func(*args) - except httplib.IncompleteRead, e: - return e.partial - return inner -httplib.HTTPResponse.read = patch_http_response_read(httplib.HTTPResponse.read) - -class ntripconnect(Thread): - def __init__(self, ntc): - super(ntripconnect, self).__init__() - self.ntc = ntc - self.stop = False - - def run(self): - - headers = { - 'Ntrip-Version': 'Ntrip/2.0', - 'User-Agent': 'NTRIP ntrip_ros', - 'Connection': 'close', - 'Authorization': 'Basic ' + b64encode(self.ntc.ntrip_user + ':' + str(self.ntc.ntrip_pass)) - } - connection = HTTPConnection(self.ntc.ntrip_server) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - rmsg = Message() - restart_count = 0 - while not self.stop: - ''' - data = response.read(100) - pos = data.find('\r\n') - if pos != -1: - rmsg.message = buf + data[:pos] - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - buf = data[pos+2:] - self.ntc.pub.publish(rmsg) - else: buf += data - ''' - - ''' This now separates individual RTCM messages and publishes each one on the same topic ''' - data = response.read(1) - if len(data) != 0: - if ord(data[0]) == 211: - buf += data - data = response.read(2) - buf += data - cnt = ord(data[0]) * 256 + ord(data[1]) - data = response.read(2) - buf += data - typ = (ord(data[0]) * 256 + ord(data[1])) / 16 - print (str(datetime.now()), cnt, typ) - cnt = cnt + 1 - for x in range(cnt): - data = response.read(1) - buf += data - rmsg.message = buf - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - self.ntc.pub.publish(rmsg) - buf = "" - else: print (data) - else: - ''' If zero length data, close connection and reopen it ''' - restart_count = restart_count + 1 - print("Zero length ", restart_count) - connection.close() - connection = HTTPConnection(self.ntc.ntrip_server) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - - connection.close() - -class ntripclient: - def __init__(self): - rospy.init_node('ntripclient', anonymous=True) - - self.rtcm_topic = rospy.get_param('~rtcm_topic', 'rtcm') - self.nmea_topic = rospy.get_param('~nmea_topic', 'nmea') - - self.ntrip_server = rospy.get_param('~ntrip_server') - self.ntrip_user = rospy.get_param('~ntrip_user') - self.ntrip_pass = rospy.get_param('~ntrip_pass') - self.ntrip_stream = rospy.get_param('~ntrip_stream') - self.nmea_gga = rospy.get_param('~nmea_gga') - - self.pub = rospy.Publisher(self.rtcm_topic, Message, queue_size=10) - - self.connection = None - self.connection = ntripconnect(self) - self.connection.start() - - def run(self): - rospy.spin() - if self.connection is not None: - self.connection.stop = True - -if __name__ == '__main__': - c = ntripclient() - c.run() diff --git a/ntrip_ros/scripts/ntripclient_py2.py b/ntrip_ros/scripts/ntripclient_py2.py deleted file mode 100644 index 652fea6..0000000 --- a/ntrip_ros/scripts/ntripclient_py2.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/python - -import rospy -from datetime import datetime - -#from nmea_msgs.msg import Sentence -from rtcm_msgs.msg import Message - -from base64 import b64encode -from threading import Thread - -from httplib import HTTPConnection -from httplib import IncompleteRead - -''' This is to fix the IncompleteRead error - http://bobrochel.blogspot.com/2010/11/bad-servers-chunked-encoding-and.html''' -import httplib -def patch_http_response_read(func): - def inner(*args): - try: - return func(*args) - except httplib.IncompleteRead, e: - return e.partial - return inner -httplib.HTTPResponse.read = patch_http_response_read(httplib.HTTPResponse.read) - -class ntripconnect(Thread): - def __init__(self, ntc): - super(ntripconnect, self).__init__() - self.ntc = ntc - self.stop = False - - def run(self): - - headers = { - 'Ntrip-Version': 'Ntrip/2.0', - 'User-Agent': 'NTRIP ntrip_ros', - 'Connection': 'close', - 'Authorization': 'Basic ' + b64encode(self.ntc.ntrip_user + ':' + str(self.ntc.ntrip_pass)) - } - connection = HTTPConnection(self.ntc.ntrip_server) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - rmsg = Message() - restart_count = 0 - while not self.stop: - ''' - data = response.read(100) - pos = data.find('\r\n') - if pos != -1: - rmsg.message = buf + data[:pos] - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - buf = data[pos+2:] - self.ntc.pub.publish(rmsg) - else: buf += data - ''' - - ''' This now separates individual RTCM messages and publishes each one on the same topic ''' - data = response.read(1) - if len(data) != 0: - if ord(data[0]) == 211: - buf += data - data = response.read(2) - buf += data - cnt = ord(data[0]) * 256 + ord(data[1]) - data = response.read(2) - buf += data - typ = (ord(data[0]) * 256 + ord(data[1])) / 16 - print (str(datetime.now()), cnt, typ) - cnt = cnt + 1 - for x in range(cnt): - data = response.read(1) - buf += data - rmsg.message = buf - rmsg.header.seq += 1 - rmsg.header.stamp = rospy.get_rostime() - self.ntc.pub.publish(rmsg) - buf = "" - else: print (data) - else: - ''' If zero length data, close connection and reopen it ''' - restart_count = restart_count + 1 - print("Zero length ", restart_count) - connection.close() - connection = HTTPConnection(self.ntc.ntrip_server) - connection.request('GET', '/'+self.ntc.ntrip_stream, self.ntc.nmea_gga, headers) - response = connection.getresponse() - if response.status != 200: raise Exception("blah") - buf = "" - - connection.close() - -class ntripclient: - def __init__(self): - rospy.init_node('ntripclient', anonymous=True) - - self.rtcm_topic = rospy.get_param('~rtcm_topic', 'rtcm') - self.nmea_topic = rospy.get_param('~nmea_topic', 'nmea') - - self.ntrip_server = rospy.get_param('~ntrip_server') - self.ntrip_user = rospy.get_param('~ntrip_user') - self.ntrip_pass = rospy.get_param('~ntrip_pass') - self.ntrip_stream = rospy.get_param('~ntrip_stream') - self.nmea_gga = rospy.get_param('~nmea_gga') - - self.pub = rospy.Publisher(self.rtcm_topic, Message, queue_size=10) - - self.connection = None - self.connection = ntripconnect(self) - self.connection.start() - - def run(self): - rospy.spin() - if self.connection is not None: - self.connection.stop = True - -if __name__ == '__main__': - c = ntripclient() - c.run() - diff --git a/odrive_ros/CMakeLists.txt b/odrive_ros/CMakeLists.txt deleted file mode 100644 index 8ed502f..0000000 --- a/odrive_ros/CMakeLists.txt +++ /dev/null @@ -1,201 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(odrive_ros) - -## 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 - rospy - std_msgs - geometry_msgs - diagnostic_msgs - diagnostic_updater -) - -## 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 exec_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 exec_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 # Or other packages containing 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 exec_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 your 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 odrive_ros -# CATKIN_DEPENDS rospy -# 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}/odrive_ros.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/odrive_ros_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/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -install(PROGRAMS - nodes/odrive_basic_node.py - 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_odrive_ros.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) diff --git a/odrive_ros/freds_config.json b/odrive_ros/freds_config.json deleted file mode 100644 index 6375b61..0000000 --- a/odrive_ros/freds_config.json +++ /dev/null @@ -1 +0,0 @@ -{"config": {"enable_uart": true, "uart_baudrate": 115200, "enable_i2c_instead_of_can": false, "enable_ascii_protocol_on_usb": true, "max_regen_current": 0.0, "brake_resistance": 2.0, "dc_bus_undervoltage_trip_level": 8.0, "dc_bus_overvoltage_trip_level": 59.92000198364258, "enable_dc_bus_overvoltage_ramp": false, "dc_bus_overvoltage_ramp_start": 59.92000198364258, "dc_bus_overvoltage_ramp_end": 59.92000198364258, "dc_max_positive_current": Infinity, "dc_max_negative_current": -9.999999974752427e-07}, "axis0": {"config": {"startup_motor_calibration": false, "startup_encoder_index_search": false, "startup_encoder_offset_calibration": false, "startup_closed_loop_control": false, "startup_sensorless_control": false, "startup_homing": false, "enable_step_dir": false, "step_dir_always_on": false, "turns_per_step": 0.0009765625, "watchdog_timeout": 0.0, "enable_watchdog": false, "step_gpio_pin": 1, "dir_gpio_pin": 2, "can_node_id": 0, "can_node_id_extended": false, "can_heartbeat_rate_ms": 100}, "fet_thermistor": {"config": {"temp_limit_lower": 100.0, "temp_limit_upper": 120.0, "enabled": true}}, "motor_thermistor": {"config": {"gpio_pin": 4, "poly_coefficient_0": 0.0, "poly_coefficient_1": 0.0, "poly_coefficient_2": 0.0, "poly_coefficient_3": 0.0, "temp_limit_lower": 100.0, "temp_limit_upper": 120.0, "enabled": false}}, "motor": {"config": {"pre_calibrated": true, "pole_pairs": 15, "calibration_current": 10.0, "resistance_calib_max_voltage": 4.0, "phase_inductance": 0.0003865232574753463, "phase_resistance": 0.19353704154491425, "torque_constant": 0.5168750286102295, "direction": -1, "motor_type": 0, "current_lim": 10.0, "current_lim_margin": 8.0, "torque_lim": Infinity, "inverter_temp_limit_lower": 100.0, "inverter_temp_limit_upper": 120.0, "requested_current_range": 25.0, "current_control_bandwidth": 100.0, "acim_slip_velocity": 14.706000328063965, "acim_gain_min_flux": 10.0, "acim_autoflux_min_Id": 10.0, "acim_autoflux_enable": false, "acim_autoflux_attack_gain": 10.0, "acim_autoflux_decay_gain": 1.0}}, "controller": {"config": {"gain_scheduling_width": 10.0, "enable_vel_limit": true, "enable_current_mode_vel_limit": true, "enable_gain_scheduling": false, "enable_overspeed_error": true, "control_mode": 2, "input_mode": 1, "pos_gain": 1.0, "vel_gain": 0.9303750395774841, "vel_integrator_gain": 4.6518754959106445, "vel_limit": 4.199999809265137, "vel_limit_tolerance": 1.2000000476837158, "vel_ramp_rate": 1.0, "torque_ramp_rate": 0.009999999776482582, "circular_setpoints": false, "circular_setpoint_range": 1.0, "homing_speed": 0.25, "inertia": 0.0, "axis_to_mirror": 255, "mirror_ratio": 1.0, "load_encoder_axis": 0, "input_filter_bandwidth": 2.0}}, "encoder": {"config": {"mode": 1, "use_index": false, "find_idx_on_lockin_only": false, "abs_spi_cs_gpio_pin": 1, "zero_count_on_find_idx": true, "cpr": 90, "offset": -22, "pre_calibrated": true, "offset_float": -0.49387502670288086, "enable_phase_interpolation": true, "bandwidth": 100.0, "calib_range": 0.019999999552965164, "calib_scan_distance": 50.26548385620117, "calib_scan_omega": 12.566370964050293, "idx_search_unidirectional": false, "ignore_illegal_hall_state": false, "sincos_gpio_pin_sin": 3, "sincos_gpio_pin_cos": 4}}, "sensorless_estimator": {"config": {"observer_gain": 1000.0, "pll_bandwidth": 1000.0, "pm_flux_linkage": 0.0015800000401213765}}, "trap_traj": {"config": {"vel_limit": 2.0, "accel_limit": 0.5, "decel_limit": 0.5}}, "min_endstop": {"config": {"gpio_num": 0, "enabled": false, "offset": 0.0, "is_active_high": false, "pullup": true, "debounce_ms": 50}}, "max_endstop": {"config": {"gpio_num": 0, "enabled": false, "offset": 0.0, "is_active_high": false, "pullup": true, "debounce_ms": 50}}}, "axis1": {"config": {"startup_motor_calibration": false, "startup_encoder_index_search": false, "startup_encoder_offset_calibration": false, "startup_closed_loop_control": false, "startup_sensorless_control": false, "startup_homing": false, "enable_step_dir": false, "step_dir_always_on": false, "turns_per_step": 0.0009765625, "watchdog_timeout": 0.0, "enable_watchdog": false, "step_gpio_pin": 7, "dir_gpio_pin": 8, "can_node_id": 1, "can_node_id_extended": false, "can_heartbeat_rate_ms": 100}, "fet_thermistor": {"config": {"temp_limit_lower": 100.0, "temp_limit_upper": 120.0, "enabled": true}}, "motor_thermistor": {"config": {"gpio_pin": 4, "poly_coefficient_0": 0.0, "poly_coefficient_1": 0.0, "poly_coefficient_2": 0.0, "poly_coefficient_3": 0.0, "temp_limit_lower": 100.0, "temp_limit_upper": 120.0, "enabled": false}}, "motor": {"config": {"pre_calibrated": true, "pole_pairs": 15, "calibration_current": 10.0, "resistance_calib_max_voltage": 4.0, "phase_inductance": 0.00035849385312758386, "phase_resistance": 0.1860181838274002, "torque_constant": 0.5168750286102295, "direction": -1, "motor_type": 0, "current_lim": 10.0, "current_lim_margin": 8.0, "torque_lim": Infinity, "inverter_temp_limit_lower": 100.0, "inverter_temp_limit_upper": 120.0, "requested_current_range": 25.0, "current_control_bandwidth": 100.0, "acim_slip_velocity": 14.706000328063965, "acim_gain_min_flux": 10.0, "acim_autoflux_min_Id": 10.0, "acim_autoflux_enable": false, "acim_autoflux_attack_gain": 10.0, "acim_autoflux_decay_gain": 1.0}}, "controller": {"config": {"gain_scheduling_width": 10.0, "enable_vel_limit": true, "enable_current_mode_vel_limit": true, "enable_gain_scheduling": false, "enable_overspeed_error": true, "control_mode": 2, "input_mode": 1, "pos_gain": 1.0, "vel_gain": 0.9303750395774841, "vel_integrator_gain": 4.6518754959106445, "vel_limit": 4.199999809265137, "vel_limit_tolerance": 1.2000000476837158, "vel_ramp_rate": 1.0, "torque_ramp_rate": 0.009999999776482582, "circular_setpoints": false, "circular_setpoint_range": 1.0, "homing_speed": 0.25, "inertia": 0.0, "axis_to_mirror": 255, "mirror_ratio": 1.0, "load_encoder_axis": 1, "input_filter_bandwidth": 2.0}}, "encoder": {"config": {"mode": 1, "use_index": false, "find_idx_on_lockin_only": false, "abs_spi_cs_gpio_pin": 1, "zero_count_on_find_idx": true, "cpr": 90, "offset": -23, "pre_calibrated": true, "offset_float": 0.427484393119812, "enable_phase_interpolation": true, "bandwidth": 100.0, "calib_range": 0.019999999552965164, "calib_scan_distance": 50.26548385620117, "calib_scan_omega": 12.566370964050293, "idx_search_unidirectional": false, "ignore_illegal_hall_state": false, "sincos_gpio_pin_sin": 3, "sincos_gpio_pin_cos": 4}}, "sensorless_estimator": {"config": {"observer_gain": 1000.0, "pll_bandwidth": 1000.0, "pm_flux_linkage": 0.0015800000401213765}}, "trap_traj": {"config": {"vel_limit": 2.0, "accel_limit": 0.5, "decel_limit": 0.5}}, "min_endstop": {"config": {"gpio_num": 0, "enabled": false, "offset": 0.0, "is_active_high": false, "pullup": true, "debounce_ms": 50}}, "max_endstop": {"config": {"gpio_num": 0, "enabled": false, "offset": 0.0, "is_active_high": false, "pullup": true, "debounce_ms": 50}}}, "can": {"config": {"baud_rate": 250000, "protocol": 0}}} \ No newline at end of file diff --git a/odrive_ros/launch/odrive.launch b/odrive_ros/launch/odrive.launch deleted file mode 100644 index 1c4c1d3..0000000 --- a/odrive_ros/launch/odrive.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/odrive_ros/nodes/__pycache__/odrive_interface.cpython-37.pyc b/odrive_ros/nodes/__pycache__/odrive_interface.cpython-37.pyc deleted file mode 100644 index 3a4522b..0000000 Binary files a/odrive_ros/nodes/__pycache__/odrive_interface.cpython-37.pyc and /dev/null differ diff --git a/odrive_ros/nodes/__pycache__/odrive_simulator.cpython-37.pyc b/odrive_ros/nodes/__pycache__/odrive_simulator.cpython-37.pyc deleted file mode 100644 index 65d1da3..0000000 Binary files a/odrive_ros/nodes/__pycache__/odrive_simulator.cpython-37.pyc and /dev/null differ diff --git a/odrive_ros/nodes/odrive_basic_node.py b/odrive_ros/nodes/odrive_basic_node.py deleted file mode 100755 index 0373737..0000000 --- a/odrive_ros/nodes/odrive_basic_node.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python3 -#import roslib; roslib.load_manifest('BINCADDY') -import rospy -#import roslib -import tf.transformations -import tf_conversions -import tf2_ros - -import std_msgs.msg -from std_msgs.msg import Float64, Int32, Int8 -from geometry_msgs.msg import Twist, TwistWithCovarianceStamped, TransformStamped -from nav_msgs.msg import Odometry -from sensor_msgs.msg import JointState -import std_srvs.srv - -#roslib.load_manifest('diagnostic_updater') -import diagnostic_updater, diagnostic_msgs.msg - -import time -import math -import traceback -import queue -import numpy as np - -from odrive_interface import ODriveInterfaceAPI, ODriveFailure -from odrive_interface import ChannelBrokenException, ChannelDamagedException -from odrive_simulator import ODriveInterfaceSimulator - - -class ODriveNode(object): - - - def __init__(self): - - self.wheel_track = 0.4 - self.m_s_to_value = 1.0 - self.tyre_circumference = 0.638 - self.angular_vel_limit = 10 - self.vel_subscriber = rospy.Subscriber("/cmd_vel",Twist, self.cmd_vel_callback, queue_size=2) - self.emergency_obstacle_subscriber = rospy.Subscriber("/emergency_message",Int8, self.emergency_stop_callback, queue_size=2) - self.status_pub = rospy.Publisher('/odrive_basic_node/status', std_msgs.msg.String, queue_size=2) - self.status_pub_voltage = rospy.Publisher('/odrive_basic_node/bus_voltage', std_msgs.msg.String, queue_size=2) - self.encoder_pub = rospy.Publisher('/odrive_basic_node/twist_estimation',TwistWithCovarianceStamped, queue_size=10) - self.status = "disconnected" - self.interface = ODriveInterfaceAPI() - - - def connect(self): - self.status_pub.publish("Connecting...") - result = self.interface.connect() - result = self.interface.connect() - if result == True: - self.status = "connected" - else: - self.status = "not connected" - self.status_pub.publish(("Connecting... result: {}".format(self.status))) - - - - def engage(self): - #self.interface.calibrate() - result = self.interface.engage() - self.status_pub.publish("Engaging... result: {}".format(result)) - - - def release(self): - self.interface.drive(0,0) - self.interface.release() - self.status_pub.publish("Released") - - - def compute_estimated_twist(self, left_angular_vel, right_angular_vel): - message = TwistWithCovarianceStamped() - left_linear_vel = -1.0*left_angular_vel*self.tyre_circumference - right_linear_vel = right_angular_vel*self.tyre_circumference - angular_vel = (right_linear_vel-left_linear_vel)/self.wheel_track - linear_vel = (left_linear_vel+right_linear_vel)/2.0 - message.twist.twist.linear.x = linear_vel - message.twist.twist.angular.z = angular_vel - message.twist.covariance[0] = 0.1 - message.twist.covariance[7] = 0.1 - message.twist.covariance[14] = 0.1 - message.twist.covariance[21] = 0.1 - message.twist.covariance[28] = 0.1 - message.twist.covariance[35] = 0.1 - message.header.stamp = rospy.Time.now() - # IN CASE WE NEED THE FRAME ID: - message.header.frame_id = 'base_link' - return message - - - def convert(self, forward, ccw): - angular_to_linear = ccw * (self.wheel_track/2.0) - left_angular_vel = (forward - angular_to_linear/2.0) * self.m_s_to_value/self.tyre_circumference - right_angular_vel = (forward + angular_to_linear/2.0) * self.m_s_to_value/self.tyre_circumference - left_angular_vel = np.sign(left_angular_vel) * min(abs(left_angular_vel),self.angular_vel_limit) - right_angular_vel = np.sign(right_angular_vel) * min(abs(right_angular_vel),self.angular_vel_limit) - return left_angular_vel,right_angular_vel - - - def cmd_vel_callback(self, msg): - left_angular_vel, right_angular_vel = self.convert(msg.linear.x, msg.angular.z) - self.interface.drive(right_angular_vel,left_angular_vel) - self.status_pub.publish("Driving... velocities: {},{}".format(left_angular_vel,right_angular_vel)) - - def emergency_stop_callback(self): - self.release() - - - def encoder_publisher_loop(self): - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - try: - left_vel_estimate = self.interface.left_vel_estimate() - right_vel_estimate = self.interface.right_vel_estimate() - # print("encoder estimations: {},{}".format(left_vel_estimate,right_vel_estimate)) - left_pos = str(self.interface.left_pos()) - right_pos = str(self.interface.right_pos()) - # left_current = str(self.interface.left_current()) - # right_current = str(self.interface.right_current()) - message = self.compute_estimated_twist(left_vel_estimate, right_vel_estimate) - self.encoder_pub.publish(message) - bus_voltage_message = str(self.interface.bus_voltage()) - self.status_pub_voltage.publish(bus_voltage_message) - rate.sleep() - except rospy.ROSInterruptException: - break - - - -def stopEnginesAndRelease(): - global odrive_node - odrive_node.interface.drive(0,0) - odrive_node.release() - -def start_odrive(odrive_node): - rospy.init_node('odrive') - odrive_node.connect() - odrive_node.engage() - odrive_node.encoder_publisher_loop() - # odrive_node.main_loop() - rospy.on_shutdown(stopEnginesAndRelease) - rospy.spin() - -if __name__ == '__main__': - try: - odrive_node = ODriveNode() - start_odrive(odrive_node) - except rospy.ROSInterruptException: - pass diff --git a/odrive_ros/nodes/odrive_interface.py b/odrive_ros/nodes/odrive_interface.py deleted file mode 100755 index 9806a43..0000000 --- a/odrive_ros/nodes/odrive_interface.py +++ /dev/null @@ -1,344 +0,0 @@ -import serial -from serial.serialutil import SerialException -import sys -import time -import logging -import traceback - -import odrive -from odrive.enums import * - -import fibre -from fibre import ChannelBrokenException, ChannelDamagedException - -default_logger = logging.getLogger(__name__) -default_logger.setLevel(logging.DEBUG) - -# create console handler and set level to debug -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) - -default_logger.addHandler(ch) - - -class ODriveFailure(Exception): - pass - -class ODriveInterfaceAPI(object): - driver = None - encoder_cpr = 90 - right_axis = None - left_axis = None - connected = False - _preroll_started = False - _preroll_completed = False - #engaged = False - - def __init__(self, logger=None, active_odrive=None): - self.logger = logger if logger else default_logger - - if active_odrive: # pass in the odrv0 object from odrivetool shell to use it directly. - self.driver = active_odrive - self.axes = (self.driver.axis0, self.driver.axis1) - self.right_axis = self.driver.axis0 - self.left_axis = self.driver.axis1 - self.logger.info("Loaded pre-existing ODrive interface. Check index search status.") - self.encoder_cpr = self.driver.axis0.encoder.config.cpr - self.connected = True - self._preroll_started = False - self._preroll_completed = True - self.AXIS_STATE_CLOSED_LOOP_CONTROL = 8 - self.AXIS_STATE_SENSORLESS_CONTROL = 5 - self.CTRL_MODE_VELOCITY_CONTROL = 2 - self.ENCODER_MODE_HALL = 1 - self.AXIS_STATE_IDLE = 1 - - def __del__(self): - self.disconnect() - - def update_time(self, curr_time): - # provided so simulator can update position - pass - - def connect(self, port=None, right_axis=0, timeout=30): - if self.driver: - self.logger.info("Already connected. Disconnecting and reconnecting.") - try: - self.driver = odrive.find_any(timeout=timeout, logger=self.logger) - self.axes = (self.driver.axis0, self.driver.axis1) - except: - self.logger.error("No ODrive found. Is device powered?") - return False - - # save some parameters for easy access - self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1 - self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0 - - # check for no errors - for axis in [self.right_axis, self.left_axis]: - if axis.error != 0: - error_str = "Had error on startup, rebooting. Axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) - self.logger.error(error_str) - self.reboot() - return False - - self.encoder_cpr = self.driver.axis0.encoder.config.cpr - - self.connected = True - self.logger.info("Connected to ODrive. " + self.get_version_string()) - - self._preroll_started = False - self._preroll_completed = False - - return True - - def disconnect(self): - self.connected = False - self.right_axis = None - self.left_axis = None - - #self.engaged = False - - if not self.driver: - self.logger.error("Not connected.") - return False - - try: - self.release() - except: - self.logger.error("Error in timer: " + traceback.format_exc()) - return False - finally: - self.driver = None - return True - - def get_version_string(self): - if not self.driver or not self.connected: - return "Not connected." - return "ODrive %s, hw v%d.%d-%d, fw v%d.%d.%d%s, sdk v%s" % ( - str(self.driver.serial_number), - self.driver.hw_version_major, self.driver.hw_version_minor, self.driver.hw_version_variant, - self.driver.fw_version_major, self.driver.fw_version_minor, self.driver.fw_version_revision, - "-dev" if self.driver.fw_version_unreleased else "", - odrive.version.get_version_str()) - - - def reboot(self): - if not self.driver: - self.logger.error("Not connected.") - return False - try: - self.driver.reboot() - except KeyError: - self.logger.error("Rebooted ODrive.") - except: - self.logger.error("Failed to reboot: " + traceback.format_exc()) - finally: - self.driver = None - return True - - def calibrate(self): - if not self.driver: - self.logger.error("Not connected.") - return False - - self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) - - for i, axis in enumerate(self.axes): - self.logger.info("Calibrating axis %d..." % i) - axis.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE - time.sleep(1) - while axis.current_state != self.AXIS_STATE_IDLE: - time.sleep(0.1) - if axis.error != 0: - self.logger.error("Failed calibration with axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) - return False - - return True - - def preroll(self, wait=True): - if not self.driver: - self.logger.error("Not connected.") - return False - - if self._preroll_started: # must be prerolling or already prerolled - return False - self._preroll_started = True - self._preroll_completed = False - - #self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) - - for i, axis in enumerate(self.axes): - self.logger.info("Index search preroll axis %d..." % i) - axis.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH - - if wait: - for i, axis in enumerate(self.axes): - while axis.current_state != self.AXIS_STATE_IDLE: - time.sleep(0.1) - self._preroll_started = False - for i, axis in enumerate(self.axes): - if axis.error != 0: - self.logger.error("Failed preroll with left_axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) - return False - self._preroll_completed = True - self.logger.info("Index search preroll complete.") - return True - else: - return False - - # def prerolling(self): - # return self.axes[0].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH or self.axes[1].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH - # - # def prerolled(self): # - # return self._prerolled and not self.prerolling() - - def ensure_prerolled(self): - # preroll success - if self._preroll_completed: - return True - # started, not completed - elif self._preroll_started: - #self.logger.info("Checking for preroll complete.") - if self.axes[0].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH and self.axes[1].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH: - # completed, check for errors before marking complete - for i, axis in enumerate(self.axes): - if axis.error != 0: - self._preroll_started = False - error_str = "Failed index search preroll with axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) - #self.reboot() - self.logger.error(error_str) - raise Exception(error_str) - # no errors, success - self._preroll_started = False - self._preroll_completed = True - self.logger.info("Index search preroll complete. Ready to drive.") - return True - else: - # still prerolling - return False - else: # start preroll - #self.logger.info("Preroll started.") - self.preroll(wait=False) - return False - - def has_prerolled(self): - return self._preroll_completed - - def engaged(self): - if self.driver and hasattr(self, 'axes'): - return self.axes[0].current_state == self.AXIS_STATE_CLOSED_LOOP_CONTROL and self.axes[1].current_state == self.AXIS_STATE_CLOSED_LOOP_CONTROL - else: - return False - - def idle(self): - if self.driver and hasattr(self, 'axes'): - return self.axes[0].current_state == self.AXIS_STATE_IDLE and self.axes[1].current_state == self.AXIS_STATE_IDLE - else: - return False - - def engage(self): - if not self.driver: - self.logger.error("Not connected.") - return False - - #self.logger.debug("Setting drive mode.") - for axis in self.axes: - axis.controller.input_vel = 0 - print(type(axis.requested_state)) - axis.encoder.config.mode = self.ENCODER_MODE_HALL - # axis.encoder.config.ignore_illegal_hall_state = True - axis.requested_state = self.AXIS_STATE_CLOSED_LOOP_CONTROL - # axis.requested_state = self.AXIS_STATE_SENSORLESS_CONTROL - axis.controller.config.control_mode = self.CTRL_MODE_VELOCITY_CONTROL - - - #self.engaged = True - return True - - def say_hello(self): - print("hello") - - def check_encoder_mode_available(self): - print(type(self.left_axis.encoder.config.mode)) - - def release(self): - if not self.driver: - self.logger.error("Not connected.") - return False - #self.logger.debug("Releasing.") - for axis in self.axes: - axis.requested_state = self.AXIS_STATE_IDLE - - #self.engaged = False - return True - - def drive(self, left_motor_val, right_motor_val): - if not self.driver: - self.logger.error("Not connected.") - return - #try: - self.axes[0].controller.input_vel = left_motor_val - self.axes[1].controller.input_vel = -right_motor_val - #except (fibre.protocol.ChannelBrokenException, AttributeError) as e: - # raise ODriveFailure(str(e)) - - def feed_watchdog(self): - self.left_axis.watchdog_feed() - self.right_axis.watchdog_feed() - - def get_errors(self, clear=True): - # TODO: add error parsing, see: https://github.com/madcowswe/ODrive/blob/master/tools/odrive/utils.py#L34 - if not self.driver: - return None - - axis_error = self.axes[0].error or self.axes[1].error - - if axis_error: - error_string = "Errors(hex): L: a%x m%x e%x c%x, R: a%x m%x e%x c%x" % ( - self.left_axis.error, self.left_axis.motor.error, self.left_axis.encoder.error, self.left_axis.controller.error, - self.right_axis.error, self.right_axis.motor.error, self.right_axis.encoder.error, self.right_axis.controller.error, - ) - - if clear: - for axis in self.axes: - axis.error = 0 - axis.motor.error = 0 - axis.encoder.error = 0 - axis.controller.error = 0 - - if axis_error: - return error_string - - def left_vel_estimate(self): return self.left_axis.encoder.vel_estimate if self.left_axis else 0 # units: encoder counts/s - def right_vel_estimate(self): return self.right_axis.encoder.vel_estimate if self.right_axis else 0 # neg is forward for right - def left_pos(self): return self.left_axis.encoder.pos_cpr if self.left_axis else 0 # units: encoder counts - def right_pos(self): return self.right_axis.encoder.pos_cpr if self.right_axis else 0 # sign! - - # TODO check these match the right motors, but it doesn't matter for now - def left_temperature(self): return self.left_axis.motor.get_inverter_temp() if self.left_axis else 0. - def right_temperature(self): return self.right_axis.motor.get_inverter_temp() if self.right_axis else 0. - - def left_current(self): return self.left_axis.motor.current_control.Ibus if self.left_axis and self.left_axis.current_state > 1 else 0. - def right_current(self): return self.right_axis.motor.current_control.Ibus if self.right_axis and self.right_axis.current_state > 1 else 0. - - # from axis.hpp: https://github.com/madcowswe/ODrive/blob/767a2762f9b294b687d761029ef39e742bdf4539/Firmware/MotorControl/axis.hpp#L26 - MOTOR_STATES = [ - "UNDEFINED", # motor speed conversion - wheel_track = None - tyre_circumference = None - encoder_counts_per_rev = None - m_s_to_value = 1.0 - axis_for_right = 0 - encoder_cpr = 4096 - - # Startup parameters - connect_on_startup = False - calibrate_on_startup = False - engage_on_startup = False - - publish_joint_angles = True - # Simulation mode - # When enabled, output simulated odometry and joint angles (TODO: do joint angles anyway from ?) - sim_mode = False - - def __init__(self): - self.sim_mode = get_param('simulation_mode', False) - self.publish_joint_angles = get_param('publish_joint_angles', True) # if self.sim_mode else False - self.publish_temperatures = get_param('publish_temperatures', True) - - self.axis_for_right = float(get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1 - self.wheel_track = float(get_param('~wheel_track', 0.285)) # m, distance between wheel centres - self.tyre_circumference = float(get_param('~tyre_circumference', 0.341)) # used to translate velocity commands in m/s into motor rpm - - self.connect_on_startup = get_param('~connect_on_startup', False) - #self.calibrate_on_startup = get_param('~calibrate_on_startup', False) - #self.engage_on_startup = get_param('~engage_on_startup', False) - - self.has_preroll = get_param('~use_preroll', True) - - self.publish_current = get_param('~publish_current', True) - self.publish_raw_odom =get_param('~publish_raw_odom', True) - - self.publish_odom = get_param('~publish_odom', True) - self.publish_tf = get_param('~publish_odom_tf', False) - self.odom_topic = get_param('~odom_topic', "odom") - self.odom_frame = get_param('~odom_frame', "odom") - self.base_frame = get_param('~base_frame', "base_link") - self.odom_calc_hz = get_param('~odom_calc_hz', 10) - - rospy.on_shutdown(self.terminate) - - rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver) - rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver) - - rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor) - rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor) - rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor) - - # odometry update, disable during preroll, whenever wheels off ground - self.odometry_update_enabled = True - rospy.Service('enable_odometry_updates', std_srvs.srv.SetBool, self.enable_odometry_update_service) - - self.status_pub = rospy.Publisher('status', std_msgs.msg.String, latch=True, queue_size=2) - self.status = "disconnected" - self.status_pub.publish(self.status) - - self.command_queue = queue.Queue(maxsize=5) - self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) - - self.publish_diagnostics = True - if self.publish_diagnostics: - self.diagnostic_updater = diagnostic_updater.Updater() - self.diagnostic_updater.setHardwareID("Not connected, unknown") - self.diagnostic_updater.add("ODrive Diagnostics", self.pub_diagnostics) - - if self.publish_temperatures: - self.temperature_publisher_left = rospy.Publisher('left/temperature', Float64, queue_size=2) - self.temperature_publisher_right = rospy.Publisher('right/temperature', Float64, queue_size=2) - - self.i2t_error_latch = False - if self.publish_current: - #self.current_loop_count = 0 - #self.left_current_accumulator = 0.0 - #self.right_current_accumulator = 0.0 - self.current_publisher_left = rospy.Publisher('left/current', Float64, queue_size=2) - self.current_publisher_right = rospy.Publisher('right/current', Float64, queue_size=2) - self.i2t_publisher_left = rospy.Publisher('left/i2t', Float64, queue_size=2) - self.i2t_publisher_right = rospy.Publisher('right/i2t', Float64, queue_size=2) - - rospy.logdebug("ODrive will publish motor currents.") - - self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222) - self.i2t_warning_threshold = get_param('~i2t_warning_threshold', 333) - self.i2t_error_threshold = get_param('~i2t_error_threshold', 666) - - self.last_cmd_vel_time = rospy.Time.now() - - if self.publish_raw_odom: - self.raw_odom_publisher_encoder_left = rospy.Publisher('left/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_encoder_right = rospy.Publisher('right/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_vel_left = rospy.Publisher('left/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_vel_right = rospy.Publisher('right/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None - - if self.publish_odom: - rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry) - self.old_pos_l = 0 - self.old_pos_r = 0 - - self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, tcp_nodelay=True, queue_size=2) - # setup message - self.odom_msg = Odometry() - #print(dir(self.odom_msg)) - self.odom_msg.header.frame_id = self.odom_frame - self.odom_msg.child_frame_id = self.base_frame - self.odom_msg.pose.pose.position.x = 0.0 - self.odom_msg.pose.pose.position.y = 0.0 - self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope - self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical - self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical - self.odom_msg.pose.pose.orientation.z = 0.0 - self.odom_msg.pose.pose.orientation.w = 1.0 - self.odom_msg.twist.twist.linear.x = 0.0 - self.odom_msg.twist.twist.linear.y = 0.0 # no sideways - self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward - self.odom_msg.twist.twist.angular.x = 0.0 # or roll - self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw - self.odom_msg.twist.twist.angular.z = 0.0 - - # store current location to be updated. - self.x = 0.0 - self.y = 0.0 - self.theta = 0.0 - - # setup transform - self.tf_publisher = tf2_ros.TransformBroadcaster() - self.tf_msg = TransformStamped() - self.tf_msg.header.frame_id = self.odom_frame - self.tf_msg.child_frame_id = self.base_frame - self.tf_msg.transform.translation.x = 0.0 - self.tf_msg.transform.translation.y = 0.0 - self.tf_msg.transform.translation.z = 0.0 - self.tf_msg.transform.rotation.x = 0.0 - self.tf_msg.transform.rotation.y = 0.0 - self.tf_msg.transform.rotation.w = 0.0 - self.tf_msg.transform.rotation.z = 1.0 - - if self.publish_joint_angles: - self.joint_state_publisher = rospy.Publisher('/odrive/joint_states', JointState, queue_size=2) - - jsm = JointState() - self.joint_state_msg = jsm - #jsm.name.resize(2) - #jsm.position.resize(2) - jsm.name = ['joint_left_wheel','joint_right_wheel'] - jsm.position = [0.0, 0.0] - - - def main_loop(self): - # Main control, handle startup and error handling - # while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs - main_rate = rospy.Rate(1) # hz - # Start timer to run high-rate comms - self.fast_timer = rospy.Timer(rospy.Duration(1/float(self.odom_calc_hz)), self.fast_timer) - - self.fast_timer_comms_active = False - - while not rospy.is_shutdown(): - try: - main_rate.sleep() - except rospy.ROSInterruptException: # shutdown / stop ODrive?? - break; - - # fast timer running, so do nothing and wait for any errors - if self.fast_timer_comms_active: - continue - - # check for errors - if self.driver: - try: - # driver connected, but fast_comms not active -> must be an error? - # TODO: try resetting errors and recalibrating, not just a full disconnection - error_string = self.driver.get_errors(clear=True) - if error_string: - rospy.logerr("Had errors, disconnecting and retrying connection.") - rospy.logerr(error_string) - self.driver.disconnect() - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - else: - # must have called connect service from another node - self.fast_timer_comms_active = True - except (ChannelBrokenException, ChannelDamagedException, AttributeError): - rospy.logerr("ODrive USB connection failure in main_loop.") - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - except: - rospy.logerr("Unknown errors accessing ODrive:" + traceback.format_exc()) - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - - if not self.driver: - if not self.connect_on_startup: - #rospy.loginfo("ODrive node started, but not connected.") - continue - - if not self.connect_driver(None)[0]: - rospy.logerr("Failed to connect.") # TODO: can we check for timeout here? - continue - - if self.publish_diagnostics: - self.diagnostic_updater.setHardwareID(self.driver.get_version_string()) - - else: - pass # loop around and try again - - def fast_timer(self, timer_event): - time_now = rospy.Time.now() - # in case of failure, assume some values are zero - self.vel_l = 0 - self.vel_r = 0 - self.new_pos_l = 0 - self.new_pos_r = 0 - self.current_l = 0 - self.current_r = 0 - self.temp_v_l = 0. - self.temp_v_r = 0. - self.motor_state_l = "not connected" # undefined - self.motor_state_r = "not connected" - self.bus_voltage = 0. - - # Handle reading from Odrive and sending odometry - if self.fast_timer_comms_active: - try: - # check errors - error_string = self.driver.get_errors() - if error_string: - self.fast_timer_comms_active = False - else: - # reset watchdog - self.driver.feed_watchdog() - - # read all required values from ODrive for odometry - self.motor_state_l = self.driver.left_state() - self.motor_state_r = self.driver.right_state() - - self.encoder_cpr = self.driver.encoder_cpr - self.m_s_to_value = self.encoder_cpr/self.tyre_circumference # calculated - - self.driver.update_time(time_now.to_sec()) - self.vel_l = self.driver.left_vel_estimate() # units: encoder counts/s - self.vel_r = -self.driver.right_vel_estimate() # neg is forward for right - self.new_pos_l = self.driver.left_pos() # units: encoder counts - self.new_pos_r = -self.driver.right_pos() # sign! - - # for temperatures - self.temp_v_l = self.driver.left_temperature() - self.temp_v_r = self.driver.right_temperature() - # for current - self.current_l = self.driver.left_current() - self.current_r = self.driver.right_current() - # voltage - self.bus_voltage = self.driver.bus_voltage() - - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in fast_timer." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - except: - rospy.logerr("Fast timer ODrive failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - # odometry is published regardless of ODrive connection or failure (but assumed zero for those) - # as required by SLAM - if self.publish_odom: - self.pub_odometry(time_now) - if self.publish_temperatures: - self.pub_temperatures() - if self.publish_current: - self.pub_current() - if self.publish_joint_angles: - self.pub_joint_angles(time_now) - if self.publish_diagnostics: - self.diagnostic_updater.update() - - try: - # check and stop motor if no vel command has been received in > 1s - #if self.fast_timer_comms_active: - if self.driver: - if (time_now - self.last_cmd_vel_time).to_sec() > 0.5 and self.last_speed > 0: - self.driver.drive(0,0) - self.last_speed = 0 - self.last_cmd_vel_time = time_now - # release motor after 10s stopped - if (time_now - self.last_cmd_vel_time).to_sec() > 10.0 and self.driver.engaged(): - self.driver.release() # and release - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in cmd_vel timeout." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.driver = None - except: - rospy.logerr("cmd_vel timeout unknown failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - - # handle sending drive commands. - # from here, any errors return to get out - if self.fast_timer_comms_active and not self.command_queue.empty(): - # check to see if we're initialised and engaged motor - try: - if not self.driver.has_prerolled(): #ensure_prerolled(): - rospy.logwarn_throttle(5.0, "ODrive has not been prerolled, ignoring drive command.") - motor_command = self.command_queue.get_nowait() - return - except: - rospy.logerr("Fast timer exception on preroll." + traceback.format_exc()) - self.fast_timer_comms_active = False - try: - motor_command = self.command_queue.get_nowait() - except queue.Empty: - rospy.logerr("Queue was empty??" + traceback.format_exc()) - return - - if motor_command[0] == 'drive': - try: - if self.publish_current and self.i2t_error_latch: - # have exceeded i2t bounds - return - - if not self.driver.engaged(): - self.driver.engage() - self.status = "engaged" - - left_linear_val, right_linear_val = motor_command[1] - self.driver.drive(left_linear_val, right_linear_val) - self.last_speed = max(abs(left_linear_val), abs(right_linear_val)) - self.last_cmd_vel_time = time_now - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in drive_cmd." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.driver = None - except: - rospy.logerr("motor drive unknown failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - elif motor_command[0] == 'release': - pass - # ? - else: - pass - - - def terminate(self): - self.fast_timer.shutdown() - if self.driver: - self.driver.release() - - # ROS services - def connect_driver(self, request): - if self.driver: - return (False, "Already connected.") - - ODriveClass = ODriveInterfaceAPI if not self.sim_mode else ODriveInterfaceSimulator - - self.driver = ODriveInterfaceAPI(logger=ROSLogger()) - rospy.loginfo("Connecting to ODrive...") - if not self.driver.connect(right_axis=self.axis_for_right): - self.driver = None - #rospy.logerr("Failed to connect.") - return (False, "Failed to connect.") - - #rospy.loginfo("ODrive connected.") - - # okay, connected, - self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference - - if self.publish_odom: - self.old_pos_l = self.driver.left_axis.encoder.pos_cpr - self.old_pos_r = self.driver.right_axis.encoder.pos_cpr - - self.fast_timer_comms_active = True - - self.status = "connected" - self.status_pub.publish(self.status) - return (True, "ODrive connected successfully") - - def disconnect_driver(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - try: - if not self.driver.disconnect(): - return (False, "Failed disconnection, but try reconnecting.") - except: - rospy.logerr('Error while disconnecting: {}'.format(traceback.format_exc())) - finally: - self.status = "disconnected" - self.status_pub.publish(self.status_pub) - self.driver = None - return (True, "Disconnection success.") - - def calibrate_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - - if self.has_preroll: - self.odometry_update_enabled = False # disable odometry updates while we preroll - if not self.driver.preroll(wait=True): - self.status = "preroll_fail" - self.status_pub.publish(self.status) - return (False, "Failed preroll.") - - self.status_pub.publish("ready") - rospy.sleep(1) - self.odometry_update_enabled = True - else: - if not self.driver.calibrate(): - return (False, "Failed calibration.") - - return (True, "Calibration success.") - - def engage_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - if not self.driver.has_prerolled(): - return (False, "Not prerolled.") - if not self.driver.engage(): - return (False, "Failed to engage motor.") - return (True, "Engage motor success.") - - def release_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - if not self.driver.release(): - return (False, "Failed to release motor.") - return (True, "Release motor success.") - - def enable_odometry_update_service(self, request): - enable = request.data - - if enable: - self.odometry_update_enabled = True - return(True, "Odometry enabled.") - else: - self.odometry_update_enabled = False - return(True, "Odometry disabled.") - - def reset_odometry(self, request): - self.x = 0.0 - self.y = 0.0 - self.theta = 0.0 - - return(True, "Odometry reset.") - - # Helpers and callbacks - - def convert(self, forward, ccw): - angular_to_linear = ccw * (self.wheel_track/2.0) - left_linear_val = int((forward - angular_to_linear) * self.m_s_to_value) - right_linear_val = int((forward + angular_to_linear) * self.m_s_to_value) - - return left_linear_val, right_linear_val - - def cmd_vel_callback(self, msg): - #rospy.loginfo("Received a /cmd_vel message!") - #rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z)) - #rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z)) - - # rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0 - - # Do velocity processing here: - # Use the kinematics of your robot to map linear and angular velocities into motor commands - - # 3600 ERPM = 360 RPM ~= 6 km/hr - - #angular_to_linear = msg.angular.z * (wheel_track/2.0) - #left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm - #right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm - left_linear_val, right_linear_val = self.convert(msg.linear.x, msg.angular.z) - - # if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm - - # Then set your wheel speeds (using wheel_left and wheel_right as examples) - #self.left_motor_pub.publish(left_linear_rpm) - #self.right_motor_pub.publish(right_linear_rpm) - #wheel_left.set_speed(v_l) - #wheel_right.set_speed(v_r) - - #rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z)) - try: - drive_command = ('drive', (left_linear_val, right_linear_val)) - self.command_queue.put_nowait(drive_command) - except queue.Full: - pass - - self.last_cmd_vel_time = rospy.Time.now() - - def pub_diagnostics(self, stat): - stat.add("Status", self.status) - stat.add("Motor state L", self.motor_state_l) - stat.add("Motor state R", self.motor_state_r) - stat.add("FET temp L (C)", round(self.temp_v_l,1)) - stat.add("FET temp R (C)", round(self.temp_v_r,1)) - stat.add("Motor temp L (C)", "unimplemented") - stat.add("Motor temp R (C)", "unimplemented") - stat.add("Motor current L (A)", round(self.current_l,1)) - stat.add("Motor current R (A)", round(self.current_r,1)) - stat.add("Voltage (V)", round(self.bus_voltage,2)) - stat.add("Motor i2t L", round(self.left_energy_acc,1)) - stat.add("Motor i2t R", round(self.right_energy_acc,1)) - - # https://github.com/ros/common_msgs/blob/jade-devel/diagnostic_msgs/msg/DiagnosticStatus.msg - if self.status == "disconnected": - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Not connected") - else: - if self.i2t_error_latch: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "i2t overheated, drive ignored until cool") - elif self.left_energy_acc > self.i2t_warning_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Left motor over i2t warning threshold") - elif self.left_energy_acc > self.i2t_error_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Left motor over i2t error threshold") - elif self.right_energy_acc > self.i2t_warning_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Right motor over i2t warning threshold") - elif self.right_energy_acc > self.i2t_error_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Right motor over i2t error threshold") - # Everything is okay: - else: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Running") - - - def pub_temperatures(self): - # https://discourse.odriverobotics.com/t/odrive-mosfet-temperature-rise-measurements-using-the-onboard-thermistor/972 - # https://discourse.odriverobotics.com/t/thermistors-on-the-odrive/813/7 - # https://www.digikey.com/product-detail/en/murata-electronics-north-america/NCP15XH103F03RC/490-4801-1-ND/1644682 - #p3 = 363.0 - #p2 = -459.2 - #p1 = 308.3 - #p0 = -28.1 - # - #vl = self.temp_v_l - #vr = self.temp_v_r - - #temperature_l = p3*vl**3 + p2*vl**2 + p1*vl + p0 - #temperature_r = p3*vr**3 + p2*vr**2 + p1*vr + p0 - - #print(temperature_l, temperature_r) - - self.temperature_publisher_left.publish(self.temp_v_l) - self.temperature_publisher_right.publish(self.temp_v_r) - - # Current publishing and i2t calculation - i2t_current_nominal = 2.0 - i2t_update_rate = 0.01 - - def pub_current(self): - self.current_publisher_left.publish(float(self.current_l)) - self.current_publisher_right.publish(float(self.current_r)) - - now = time.time() - - if not hasattr(self, 'last_pub_current_time'): - self.last_pub_current_time = now - self.left_energy_acc = 0 - self.right_energy_acc = 0 - return - - # calculate and publish i2t - dt = now - self.last_pub_current_time - - power = max(0, self.current_l**2 - self.i2t_current_nominal**2) - energy = power * dt - self.left_energy_acc *= 1 - self.i2t_update_rate * dt - self.left_energy_acc += energy - - power = max(0, self.current_r**2 - self.i2t_current_nominal**2) - energy = power * dt - self.right_energy_acc *= 1 - self.i2t_update_rate * dt - self.right_energy_acc += energy - - self.last_pub_current_time = now - - self.i2t_publisher_left.publish(float(self.left_energy_acc)) - self.i2t_publisher_right.publish(float(self.right_energy_acc)) - - # stop odrive if overheated - if self.left_energy_acc > self.i2t_error_threshold or self.right_energy_acc > self.i2t_error_threshold: - if not self.i2t_error_latch: - self.driver.release() - self.status = "overheated" - self.i2t_error_latch = True - rospy.logerr("ODrive has exceeded i2t error threshold, ignoring drive commands. Waiting to cool down.") - elif self.i2t_error_latch: - if self.left_energy_acc < self.i2t_resume_threshold and self.right_energy_acc < self.i2t_resume_threshold: - # have cooled enough now - self.status = "ready" - self.i2t_error_latch = False - rospy.logerr("ODrive has cooled below i2t resume threshold, ignoring drive commands. Waiting to cool down.") - - - # current_quantizer = 5 - # - # self.left_current_accumulator += self.current_l - # self.right_current_accumulator += self.current_r - # - # self.current_loop_count += 1 - # if self.current_loop_count >= current_quantizer: - # self.current_publisher_left.publish(float(self.left_current_accumulator) / current_quantizer) - # self.current_publisher_right.publish(float(self.right_current_accumulator) / current_quantizer) - # - # self.current_loop_count = 0 - # self.left_current_accumulator = 0.0 - # self.right_current_accumulator = 0.0 - - def pub_odometry(self, time_now): - now = time_now - self.odom_msg.header.stamp = now - self.tf_msg.header.stamp = now - - wheel_track = self.wheel_track # check these. Values in m - tyre_circumference = self.tyre_circumference - # self.m_s_to_value = encoder_cpr/tyre_circumference set earlier - - # if odometry updates disabled, just return the old position and zero twist. - if not self.odometry_update_enabled: - self.odom_msg.twist.twist.linear.x = 0. - self.odom_msg.twist.twist.angular.z = 0. - - # but update the old encoder positions, so when we restart updates - # it will start by giving zero change from the old position. - self.old_pos_l = self.new_pos_l - self.old_pos_r = self.new_pos_r - - self.odom_publisher.publish(self.odom_msg) - if self.publish_tf: - self.tf_publisher.sendTransform(self.tf_msg) - - return - - # Twist/velocity: calculated from motor values only - s = tyre_circumference * (self.vel_l+self.vel_r) / (2.0*self.encoder_cpr) - w = tyre_circumference * (self.vel_r-self.vel_l) / (wheel_track * self.encoder_cpr) # angle: vel_r*tyre_radius - vel_l*tyre_radius - self.odom_msg.twist.twist.linear.x = s - self.odom_msg.twist.twist.angular.z = w - - #rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % ( - # vel_l, -vel_r, - # vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z, - # self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr)) - - # Position - delta_pos_l = self.new_pos_l - self.old_pos_l - delta_pos_r = self.new_pos_r - self.old_pos_r - - self.old_pos_l = self.new_pos_l - self.old_pos_r = self.new_pos_r - - # Check for overflow. Assume we can't move more than half a circumference in a single timestep. - half_cpr = self.encoder_cpr/2.0 - if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr - elif delta_pos_l < -half_cpr: delta_pos_l = delta_pos_l + self.encoder_cpr - if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr - elif delta_pos_r < -half_cpr: delta_pos_r = delta_pos_r + self.encoder_cpr - - # counts to metres - delta_pos_l_m = delta_pos_l / self.m_s_to_value - delta_pos_r_m = delta_pos_r / self.m_s_to_value - - # Distance travelled - d = (delta_pos_l_m+delta_pos_r_m)/2.0 # delta_ps - th = (delta_pos_r_m-delta_pos_l_m)/wheel_track # works for small angles - - xd = math.cos(th)*d - yd = -math.sin(th)*d - - # elapsed time = event.last_real, event.current_real - #elapsed = (event.current_real-event.last_real).to_sec() - # calc_vel: d/elapsed, th/elapsed - - # Pose: updated from previous pose + position delta - self.x += math.cos(self.theta)*xd - math.sin(self.theta)*yd - self.y += math.sin(self.theta)*xd + math.cos(self.theta)*yd - self.theta = (self.theta + th) % (2*math.pi) - - #rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % ( - # delta_pos_l_m, delta_pos_r_m, - # d, th, xd, yd, - # self.x, self.y, self.theta - # )) - - # fill odom message and publish - - self.odom_msg.pose.pose.position.x = self.x - self.odom_msg.pose.pose.position.y = self.y - q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, self.theta) - self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2 - self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2 - - #rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % ( - # self.theta, - # math.sin(self.theta)/2, math.cos(self.theta)/2, - # q[2],q[3],q[0],q[1] - # )) - - #self.odom_msg.pose.covariance - # x y z - # x y z - - self.tf_msg.transform.translation.x = self.x - self.tf_msg.transform.translation.y = self.y - #self.tf_msg.transform.rotation.x - #self.tf_msg.transform.rotation.x - self.tf_msg.transform.rotation.z = q[2] - self.tf_msg.transform.rotation.w = q[3] - - if self.publish_raw_odom: - self.raw_odom_publisher_encoder_left.publish(self.new_pos_l) - self.raw_odom_publisher_encoder_right.publish(self.new_pos_r) - self.raw_odom_publisher_vel_left.publish(self.vel_l) - self.raw_odom_publisher_vel_right.publish(self.vel_r) - - # ... and publish! - self.odom_publisher.publish(self.odom_msg) - if self.publish_tf: - self.tf_publisher.sendTransform(self.tf_msg) - - def pub_joint_angles(self, time_now): - jsm = self.joint_state_msg - jsm.header.stamp = time_now - if self.driver: - jsm.position[0] = 2*math.pi * self.new_pos_l / self.encoder_cpr - jsm.position[1] = 2*math.pi * self.new_pos_r / self.encoder_cpr - - self.joint_state_publisher.publish(jsm) - -def start_odrive(): - rospy.init_node('odrive') - odrive_node = ODriveNode() - odrive_node.main_loop() - #rospy.spin() - -if __name__ == '__main__': - try: - start_odrive() - except rospy.ROSInterruptException: - pass - diff --git a/odrive_ros/nodes/odrive_simulator.py b/odrive_ros/nodes/odrive_simulator.py deleted file mode 100644 index e916cca..0000000 --- a/odrive_ros/nodes/odrive_simulator.py +++ /dev/null @@ -1,108 +0,0 @@ -import sys -import time -import logging -import traceback - -default_logger = logging.getLogger(__name__) -default_logger.setLevel(logging.DEBUG) - -# create console handler and set level to debug -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) - -default_logger.addHandler(ch) - -class ODriveInterfaceSimulator(object): - encoder_cpr = 4096 - connected = False - engaged = False - - right_axis_vel = 0 # units: encoder counts/s - left_axis_vel = 0 - right_axis_pos = 0 # go from 0 up to encoder_cpr-1 - left_axis_pos = 0 - last_time_update = None - - - def __init__(self, logger=None): - self.logger = logger if logger else default_logger - - def update_time(self, curr_time): - # provided so simulator can update position - if last_time_update is None: - last_time_update = curr_time - return - - dt = curr_time - last_time_update - left_axis_pos = floor(left_axis_pos + left_axis_vel *dt) % self.encoder_cpr - right_axis_pos = floor(right_axis_pos + right_axis_vel*dt) % self.encoder_cpr - last_time_update = curr_time - - def connect(self, port=None, right_axis=0, timeout=30): - if self.connected: - self.logger.info("Already connected. Simulating disc/reconnect.") - - self.encoder_cpr = 4096 - self.logger.info("Connected to simulated ODrive.") - return True - - def disconnect(self): - self.connected = False - return True - - def calibrate(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.logger.info("Calibrated.") - return True - - def preroll(self, wait=True, reverse=False): - if not self.connected: - self.logger.error("Not connected.") - return False - - return True - - def ensure_prerolled(self): - return True - - def engaged(self): - return self.engaged - - def engage(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.engaged = True - return True - - def release(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.engaged = False - return True - - def drive(self, left_motor_val, right_motor_val): - if not self.connnected: - self.logger.error("Not connected.") - return - self.left_axis.controller.vel_setpoint = left_motor_val - self.right_axis.controller.vel_setpoint = -right_motor_val - - return True - - def get_errors(self, clear=True): - if not self.driver: - return None - return "Simulated ODrive, no errors." - - def left_vel_estimate(self): return self.left_axis_vel - def right_vel_estimate(self): return self.right_axis_vel - def left_pos(self): return self.left_axis_pos - def right_pos(self): return self.right_axis_pos - - def left_current(self): return 0 - def right_current(self): return 0 - \ No newline at end of file diff --git a/odrive_ros/package.xml b/odrive_ros/package.xml deleted file mode 100644 index fb30f05..0000000 --- a/odrive_ros/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - odrive_ros - 0.8.1 - ODrive motor controller driver for ROS - Modified BSD - - Josh Marshall, Tecevo - https://github.com/neomanic/odrive_ros - - catkin - rospy - rospy - - rospy - std_msgs - geometry_msgs - nav_msgs - std_srvs - diagnostic_msgs - diagnostic_updater - - ODrive - - - - - diff --git a/odrive_ros/setup.py b/odrive_ros/setup.py deleted file mode 100644 index ce5f625..0000000 --- a/odrive_ros/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup( - packages=['odrive_ros'], - package_dir={'': 'src'}, -) - -setup(**setup_args) \ No newline at end of file diff --git a/odrive_ros/src/odrive_ros/__init__.py b/odrive_ros/src/odrive_ros/__init__.py deleted file mode 100644 index 817eefa..0000000 --- a/odrive_ros/src/odrive_ros/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from odrive_node import ODriveNode, start_odrive \ No newline at end of file diff --git a/odrive_ros/src/odrive_ros/odrive_interface.py b/odrive_ros/src/odrive_ros/odrive_interface.py deleted file mode 100644 index 01f7468..0000000 --- a/odrive_ros/src/odrive_ros/odrive_interface.py +++ /dev/null @@ -1,328 +0,0 @@ -import serial -from serial.serialutil import SerialException - -import sys -import time -import logging -import traceback - -import odrive -from odrive.enums import * - -import fibre -from fibre import ChannelBrokenException, ChannelDamagedException - -default_logger = logging.getLogger(__name__) -default_logger.setLevel(logging.DEBUG) - -# create console handler and set level to debug -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) - -default_logger.addHandler(ch) - -class ODriveFailure(Exception): - pass - -class ODriveInterfaceAPI(object): - driver = None - encoder_cpr = 4096 - right_axis = None - left_axis = None - connected = False - _preroll_started = False - _preroll_completed = False - #engaged = False - - def __init__(self, logger=None, active_odrive=None): - self.logger = logger if logger else default_logger - - if active_odrive: # pass in the odrv0 object from odrivetool shell to use it directly. - self.driver = active_odrive - self.axes = (self.driver.axis0, self.driver.axis1) - self.right_axis = self.driver.axis0 - self.left_axis = self.driver.axis1 - self.logger.info("Loaded pre-existing ODrive interface. Check index search status.") - self.encoder_cpr = self.driver.axis0.encoder.config.cpr - self.connected = True - self._preroll_started = False - self._preroll_completed = True - - def __del__(self): - self.disconnect() - - def update_time(self, curr_time): - # provided so simulator can update position - pass - - def connect(self, port=None, right_axis=0, timeout=30): - if self.driver: - self.logger.info("Already connected. Disconnecting and reconnecting.") - try: - self.driver = odrive.find_any(timeout=timeout, logger=self.logger) - self.axes = (self.driver.axis0, self.driver.axis1) - except: - self.logger.error("No ODrive found. Is device powered?") - return False - - # save some parameters for easy access - self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1 - self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0 - - # check for no errors - for axis in [self.right_axis, self.left_axis]: - if axis.error != 0: - error_str = "Had error on startup, rebooting. Axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) - self.logger.error(error_str) - self.reboot() - return False - - self.encoder_cpr = self.driver.axis0.encoder.config.cpr - - self.connected = True - self.logger.info("Connected to ODrive. " + self.get_version_string()) - - self._preroll_started = False - self._preroll_completed = False - - return True - - def disconnect(self): - self.connected = False - self.right_axis = None - self.left_axis = None - - #self.engaged = False - - if not self.driver: - self.logger.error("Not connected.") - return False - - try: - self.release() - except: - self.logger.error("Error in timer: " + traceback.format_exc()) - return False - finally: - self.driver = None - return True - - def get_version_string(self): - if not self.driver or not self.connected: - return "Not connected." - return "ODrive %s, hw v%d.%d-%d, fw v%d.%d.%d%s, sdk v%s" % ( - str(self.driver.serial_number), - self.driver.hw_version_major, self.driver.hw_version_minor, self.driver.hw_version_variant, - self.driver.fw_version_major, self.driver.fw_version_minor, self.driver.fw_version_revision, - "-dev" if self.driver.fw_version_unreleased else "", - odrive.version.get_version_str()) - - - def reboot(self): - if not self.driver: - self.logger.error("Not connected.") - return False - try: - self.driver.reboot() - except KeyError: - self.logger.error("Rebooted ODrive.") - except: - self.logger.error("Failed to reboot: " + traceback.format_exc()) - finally: - self.driver = None - return True - - def calibrate(self): - if not self.driver: - self.logger.error("Not connected.") - return False - - self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) - - for i, axis in enumerate(self.axes): - self.logger.info("Calibrating axis %d..." % i) - axis.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE - time.sleep(1) - while axis.current_state != AXIS_STATE_IDLE: - time.sleep(0.1) - if axis.error != 0: - self.logger.error("Failed calibration with axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) - return False - - return True - - def preroll(self, wait=True): - if not self.driver: - self.logger.error("Not connected.") - return False - - if self._preroll_started: # must be prerolling or already prerolled - return False - self._preroll_started = True - self._preroll_completed = False - - #self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) - - for i, axis in enumerate(self.axes): - self.logger.info("Index search preroll axis %d..." % i) - axis.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH - - if wait: - for i, axis in enumerate(self.axes): - while axis.current_state != AXIS_STATE_IDLE: - time.sleep(0.1) - self._preroll_started = False - for i, axis in enumerate(self.axes): - if axis.error != 0: - self.logger.error("Failed preroll with left_axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) - return False - self._preroll_completed = True - self.logger.info("Index search preroll complete.") - return True - else: - return False - - # def prerolling(self): - # return self.axes[0].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH or self.axes[1].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH - # - # def prerolled(self): # - # return self._prerolled and not self.prerolling() - - def ensure_prerolled(self): - # preroll success - if self._preroll_completed: - return True - # started, not completed - elif self._preroll_started: - #self.logger.info("Checking for preroll complete.") - if self.axes[0].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH and self.axes[1].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH: - # completed, check for errors before marking complete - for i, axis in enumerate(self.axes): - if axis.error != 0: - self._preroll_started = False - error_str = "Failed index search preroll with axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) - #self.reboot() - self.logger.error(error_str) - raise Exception(error_str) - # no errors, success - self._preroll_started = False - self._preroll_completed = True - self.logger.info("Index search preroll complete. Ready to drive.") - return True - else: - # still prerolling - return False - else: # start preroll - #self.logger.info("Preroll started.") - self.preroll(wait=False) - return False - - def has_prerolled(self): - return self._preroll_completed - - def engaged(self): - if self.driver and hasattr(self, 'axes'): - return self.axes[0].current_state == AXIS_STATE_CLOSED_LOOP_CONTROL and self.axes[1].current_state == AXIS_STATE_CLOSED_LOOP_CONTROL - else: - return False - - def idle(self): - if self.driver and hasattr(self, 'axes'): - return self.axes[0].current_state == AXIS_STATE_IDLE and self.axes[1].current_state == AXIS_STATE_IDLE - else: - return False - - def engage(self): - if not self.driver: - self.logger.error("Not connected.") - return False - - #self.logger.debug("Setting drive mode.") - for axis in self.axes: - axis.controller.vel_setpoint = 0 - axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL - axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL - - #self.engaged = True - return True - - def release(self): - if not self.driver: - self.logger.error("Not connected.") - return False - #self.logger.debug("Releasing.") - for axis in self.axes: - axis.requested_state = AXIS_STATE_IDLE - - #self.engaged = False - return True - - def drive(self, left_motor_val, right_motor_val): - if not self.driver: - self.logger.error("Not connected.") - return - #try: - self.left_axis.controller.vel_setpoint = left_motor_val - self.right_axis.controller.vel_setpoint = -right_motor_val - #except (fibre.protocol.ChannelBrokenException, AttributeError) as e: - # raise ODriveFailure(str(e)) - - def feed_watchdog(self): - self.left_axis.watchdog_feed() - self.right_axis.watchdog_feed() - - def get_errors(self, clear=True): - # TODO: add error parsing, see: https://github.com/madcowswe/ODrive/blob/master/tools/odrive/utils.py#L34 - if not self.driver: - return None - - axis_error = self.axes[0].error or self.axes[1].error - - if axis_error: - error_string = "Errors(hex): L: a%x m%x e%x c%x, R: a%x m%x e%x c%x" % ( - self.left_axis.error, self.left_axis.motor.error, self.left_axis.encoder.error, self.left_axis.controller.error, - self.right_axis.error, self.right_axis.motor.error, self.right_axis.encoder.error, self.right_axis.controller.error, - ) - - if clear: - for axis in self.axes: - axis.error = 0 - axis.motor.error = 0 - axis.encoder.error = 0 - axis.controller.error = 0 - - if axis_error: - return error_string - - def left_vel_estimate(self): return self.left_axis.encoder.vel_estimate if self.left_axis else 0 # units: encoder counts/s - def right_vel_estimate(self): return self.right_axis.encoder.vel_estimate if self.right_axis else 0 # neg is forward for right - def left_pos(self): return self.left_axis.encoder.pos_cpr if self.left_axis else 0 # units: encoder counts - def right_pos(self): return self.right_axis.encoder.pos_cpr if self.right_axis else 0 # sign! - - # TODO check these match the right motors, but it doesn't matter for now - def left_temperature(self): return self.left_axis.motor.get_inverter_temp() if self.left_axis else 0. - def right_temperature(self): return self.right_axis.motor.get_inverter_temp() if self.right_axis else 0. - - def left_current(self): return self.left_axis.motor.current_control.Ibus if self.left_axis and self.left_axis.current_state > 1 else 0. - def right_current(self): return self.right_axis.motor.current_control.Ibus if self.right_axis and self.right_axis.current_state > 1 else 0. - - # from axis.hpp: https://github.com/madcowswe/ODrive/blob/767a2762f9b294b687d761029ef39e742bdf4539/Firmware/MotorControl/axis.hpp#L26 - MOTOR_STATES = [ - "UNDEFINED", # motor speed conversion - wheel_track = None - tyre_circumference = None - encoder_counts_per_rev = None - m_s_to_value = 1.0 - axis_for_right = 0 - encoder_cpr = 4096 - - # Startup parameters - connect_on_startup = False - calibrate_on_startup = False - engage_on_startup = False - - publish_joint_angles = True - # Simulation mode - # When enabled, output simulated odometry and joint angles (TODO: do joint angles anyway from ?) - sim_mode = False - - def __init__(self): - self.sim_mode = get_param('simulation_mode', False) - self.publish_joint_angles = get_param('publish_joint_angles', True) # if self.sim_mode else False - self.publish_temperatures = get_param('publish_temperatures', True) - - self.axis_for_right = float(get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1 - self.wheel_track = float(get_param('~wheel_track', 0.285)) # m, distance between wheel centres - self.tyre_circumference = float(get_param('~tyre_circumference', 0.341)) # used to translate velocity commands in m/s into motor rpm - - self.connect_on_startup = get_param('~connect_on_startup', False) - #self.calibrate_on_startup = get_param('~calibrate_on_startup', False) - #self.engage_on_startup = get_param('~engage_on_startup', False) - - self.has_preroll = get_param('~use_preroll', True) - - self.publish_current = get_param('~publish_current', True) - self.publish_raw_odom =get_param('~publish_raw_odom', True) - - self.publish_odom = get_param('~publish_odom', True) - self.publish_tf = get_param('~publish_odom_tf', False) - self.odom_topic = get_param('~odom_topic', "odom") - self.odom_frame = get_param('~odom_frame', "odom") - self.base_frame = get_param('~base_frame', "base_link") - self.odom_calc_hz = get_param('~odom_calc_hz', 10) - - rospy.on_shutdown(self.terminate) - - rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver) - rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver) - - rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor) - rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor) - rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor) - - # odometry update, disable during preroll, whenever wheels off ground - self.odometry_update_enabled = True - rospy.Service('enable_odometry_updates', std_srvs.srv.SetBool, self.enable_odometry_update_service) - - self.status_pub = rospy.Publisher('status', std_msgs.msg.String, latch=True, queue_size=2) - self.status = "disconnected" - self.status_pub.publish(self.status) - - self.command_queue = Queue.Queue(maxsize=5) - self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) - - self.publish_diagnostics = True - if self.publish_diagnostics: - self.diagnostic_updater = diagnostic_updater.Updater() - self.diagnostic_updater.setHardwareID("Not connected, unknown") - self.diagnostic_updater.add("ODrive Diagnostics", self.pub_diagnostics) - - if self.publish_temperatures: - self.temperature_publisher_left = rospy.Publisher('left/temperature', Float64, queue_size=2) - self.temperature_publisher_right = rospy.Publisher('right/temperature', Float64, queue_size=2) - - self.i2t_error_latch = False - if self.publish_current: - #self.current_loop_count = 0 - #self.left_current_accumulator = 0.0 - #self.right_current_accumulator = 0.0 - self.current_publisher_left = rospy.Publisher('left/current', Float64, queue_size=2) - self.current_publisher_right = rospy.Publisher('right/current', Float64, queue_size=2) - self.i2t_publisher_left = rospy.Publisher('left/i2t', Float64, queue_size=2) - self.i2t_publisher_right = rospy.Publisher('right/i2t', Float64, queue_size=2) - - rospy.logdebug("ODrive will publish motor currents.") - - self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222) - self.i2t_warning_threshold = get_param('~i2t_warning_threshold', 333) - self.i2t_error_threshold = get_param('~i2t_error_threshold', 666) - - self.last_cmd_vel_time = rospy.Time.now() - - if self.publish_raw_odom: - self.raw_odom_publisher_encoder_left = rospy.Publisher('left/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_encoder_right = rospy.Publisher('right/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_vel_left = rospy.Publisher('left/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None - self.raw_odom_publisher_vel_right = rospy.Publisher('right/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None - - if self.publish_odom: - rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry) - self.old_pos_l = 0 - self.old_pos_r = 0 - - self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, tcp_nodelay=True, queue_size=2) - # setup message - self.odom_msg = Odometry() - #print(dir(self.odom_msg)) - self.odom_msg.header.frame_id = self.odom_frame - self.odom_msg.child_frame_id = self.base_frame - self.odom_msg.pose.pose.position.x = 0.0 - self.odom_msg.pose.pose.position.y = 0.0 - self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope - self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical - self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical - self.odom_msg.pose.pose.orientation.z = 0.0 - self.odom_msg.pose.pose.orientation.w = 1.0 - self.odom_msg.twist.twist.linear.x = 0.0 - self.odom_msg.twist.twist.linear.y = 0.0 # no sideways - self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward - self.odom_msg.twist.twist.angular.x = 0.0 # or roll - self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw - self.odom_msg.twist.twist.angular.z = 0.0 - - # store current location to be updated. - self.x = 0.0 - self.y = 0.0 - self.theta = 0.0 - - # setup transform - self.tf_publisher = tf2_ros.TransformBroadcaster() - self.tf_msg = TransformStamped() - self.tf_msg.header.frame_id = self.odom_frame - self.tf_msg.child_frame_id = self.base_frame - self.tf_msg.transform.translation.x = 0.0 - self.tf_msg.transform.translation.y = 0.0 - self.tf_msg.transform.translation.z = 0.0 - self.tf_msg.transform.rotation.x = 0.0 - self.tf_msg.transform.rotation.y = 0.0 - self.tf_msg.transform.rotation.w = 0.0 - self.tf_msg.transform.rotation.z = 1.0 - - if self.publish_joint_angles: - self.joint_state_publisher = rospy.Publisher('/odrive/joint_states', JointState, queue_size=2) - - jsm = JointState() - self.joint_state_msg = jsm - #jsm.name.resize(2) - #jsm.position.resize(2) - jsm.name = ['joint_left_wheel','joint_right_wheel'] - jsm.position = [0.0, 0.0] - - - def main_loop(self): - # Main control, handle startup and error handling - # while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs - main_rate = rospy.Rate(1) # hz - # Start timer to run high-rate comms - self.fast_timer = rospy.Timer(rospy.Duration(1/float(self.odom_calc_hz)), self.fast_timer) - - self.fast_timer_comms_active = False - - while not rospy.is_shutdown(): - try: - main_rate.sleep() - except rospy.ROSInterruptException: # shutdown / stop ODrive?? - break; - - # fast timer running, so do nothing and wait for any errors - if self.fast_timer_comms_active: - continue - - # check for errors - if self.driver: - try: - # driver connected, but fast_comms not active -> must be an error? - # TODO: try resetting errors and recalibrating, not just a full disconnection - error_string = self.driver.get_errors(clear=True) - if error_string: - rospy.logerr("Had errors, disconnecting and retrying connection.") - rospy.logerr(error_string) - self.driver.disconnect() - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - else: - # must have called connect service from another node - self.fast_timer_comms_active = True - except (ChannelBrokenException, ChannelDamagedException, AttributeError): - rospy.logerr("ODrive USB connection failure in main_loop.") - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - except: - rospy.logerr("Unknown errors accessing ODrive:" + traceback.format_exc()) - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - - if not self.driver: - if not self.connect_on_startup: - #rospy.loginfo("ODrive node started, but not connected.") - continue - - if not self.connect_driver(None)[0]: - rospy.logerr("Failed to connect.") # TODO: can we check for timeout here? - continue - - if self.publish_diagnostics: - self.diagnostic_updater.setHardwareID(self.driver.get_version_string()) - - else: - pass # loop around and try again - - def fast_timer(self, timer_event): - time_now = rospy.Time.now() - # in case of failure, assume some values are zero - self.vel_l = 0 - self.vel_r = 0 - self.new_pos_l = 0 - self.new_pos_r = 0 - self.current_l = 0 - self.current_r = 0 - self.temp_v_l = 0. - self.temp_v_r = 0. - self.motor_state_l = "not connected" # undefined - self.motor_state_r = "not connected" - self.bus_voltage = 0. - - # Handle reading from Odrive and sending odometry - if self.fast_timer_comms_active: - try: - # check errors - error_string = self.driver.get_errors() - if error_string: - self.fast_timer_comms_active = False - else: - # reset watchdog - self.driver.feed_watchdog() - - # read all required values from ODrive for odometry - self.motor_state_l = self.driver.left_state() - self.motor_state_r = self.driver.right_state() - - self.encoder_cpr = self.driver.encoder_cpr - self.m_s_to_value = self.encoder_cpr/self.tyre_circumference # calculated - - self.driver.update_time(time_now.to_sec()) - self.vel_l = self.driver.left_vel_estimate() # units: encoder counts/s - self.vel_r = -self.driver.right_vel_estimate() # neg is forward for right - self.new_pos_l = self.driver.left_pos() # units: encoder counts - self.new_pos_r = -self.driver.right_pos() # sign! - - # for temperatures - self.temp_v_l = self.driver.left_temperature() - self.temp_v_r = self.driver.right_temperature() - # for current - self.current_l = self.driver.left_current() - self.current_r = self.driver.right_current() - # voltage - self.bus_voltage = self.driver.bus_voltage() - - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in fast_timer." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.status = "disconnected" - self.status_pub.publish(self.status) - self.driver = None - except: - rospy.logerr("Fast timer ODrive failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - # odometry is published regardless of ODrive connection or failure (but assumed zero for those) - # as required by SLAM - if self.publish_odom: - self.pub_odometry(time_now) - if self.publish_temperatures: - self.pub_temperatures() - if self.publish_current: - self.pub_current() - if self.publish_joint_angles: - self.pub_joint_angles(time_now) - if self.publish_diagnostics: - self.diagnostic_updater.update() - - try: - # check and stop motor if no vel command has been received in > 1s - #if self.fast_timer_comms_active: - if self.driver: - if (time_now - self.last_cmd_vel_time).to_sec() > 0.5 and self.last_speed > 0: - self.driver.drive(0,0) - self.last_speed = 0 - self.last_cmd_vel_time = time_now - # release motor after 10s stopped - if (time_now - self.last_cmd_vel_time).to_sec() > 10.0 and self.driver.engaged(): - self.driver.release() # and release - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in cmd_vel timeout." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.driver = None - except: - rospy.logerr("cmd_vel timeout unknown failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - - # handle sending drive commands. - # from here, any errors return to get out - if self.fast_timer_comms_active and not self.command_queue.empty(): - # check to see if we're initialised and engaged motor - try: - if not self.driver.has_prerolled(): #ensure_prerolled(): - rospy.logwarn_throttle(5.0, "ODrive has not been prerolled, ignoring drive command.") - motor_command = self.command_queue.get_nowait() - return - except: - rospy.logerr("Fast timer exception on preroll." + traceback.format_exc()) - self.fast_timer_comms_active = False - try: - motor_command = self.command_queue.get_nowait() - except Queue.Empty: - rospy.logerr("Queue was empty??" + traceback.format_exc()) - return - - if motor_command[0] == 'drive': - try: - if self.publish_current and self.i2t_error_latch: - # have exceeded i2t bounds - return - - if not self.driver.engaged(): - self.driver.engage() - self.status = "engaged" - - left_linear_val, right_linear_val = motor_command[1] - self.driver.drive(left_linear_val, right_linear_val) - self.last_speed = max(abs(left_linear_val), abs(right_linear_val)) - self.last_cmd_vel_time = time_now - except (ChannelBrokenException, ChannelDamagedException): - rospy.logerr("ODrive USB connection failure in drive_cmd." + traceback.format_exc(1)) - self.fast_timer_comms_active = False - self.driver = None - except: - rospy.logerr("motor drive unknown failure:" + traceback.format_exc()) - self.fast_timer_comms_active = False - - elif motor_command[0] == 'release': - pass - # ? - else: - pass - - - def terminate(self): - self.fast_timer.shutdown() - if self.driver: - self.driver.release() - - # ROS services - def connect_driver(self, request): - if self.driver: - return (False, "Already connected.") - - ODriveClass = ODriveInterfaceAPI if not self.sim_mode else ODriveInterfaceSimulator - - self.driver = ODriveInterfaceAPI(logger=ROSLogger()) - rospy.loginfo("Connecting to ODrive...") - if not self.driver.connect(right_axis=self.axis_for_right): - self.driver = None - #rospy.logerr("Failed to connect.") - return (False, "Failed to connect.") - - #rospy.loginfo("ODrive connected.") - - # okay, connected, - self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference - - if self.publish_odom: - self.old_pos_l = self.driver.left_axis.encoder.pos_cpr - self.old_pos_r = self.driver.right_axis.encoder.pos_cpr - - self.fast_timer_comms_active = True - - self.status = "connected" - self.status_pub.publish(self.status) - return (True, "ODrive connected successfully") - - def disconnect_driver(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - try: - if not self.driver.disconnect(): - return (False, "Failed disconnection, but try reconnecting.") - except: - rospy.logerr('Error while disconnecting: {}'.format(traceback.format_exc())) - finally: - self.status = "disconnected" - self.status_pub.publish(self.status_pub) - self.driver = None - return (True, "Disconnection success.") - - def calibrate_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - - if self.has_preroll: - self.odometry_update_enabled = False # disable odometry updates while we preroll - if not self.driver.preroll(wait=True): - self.status = "preroll_fail" - self.status_pub.publish(self.status) - return (False, "Failed preroll.") - - self.status_pub.publish("ready") - rospy.sleep(1) - self.odometry_update_enabled = True - else: - if not self.driver.calibrate(): - return (False, "Failed calibration.") - - return (True, "Calibration success.") - - def engage_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - if not self.driver.has_prerolled(): - return (False, "Not prerolled.") - if not self.driver.engage(): - return (False, "Failed to engage motor.") - return (True, "Engage motor success.") - - def release_motor(self, request): - if not self.driver: - rospy.logerr("Not connected.") - return (False, "Not connected.") - if not self.driver.release(): - return (False, "Failed to release motor.") - return (True, "Release motor success.") - - def enable_odometry_update_service(self, request): - enable = request.data - - if enable: - self.odometry_update_enabled = True - return(True, "Odometry enabled.") - else: - self.odometry_update_enabled = False - return(True, "Odometry disabled.") - - def reset_odometry(self, request): - self.x = 0.0 - self.y = 0.0 - self.theta = 0.0 - - return(True, "Odometry reset.") - - # Helpers and callbacks - - def convert(self, forward, ccw): - angular_to_linear = ccw * (self.wheel_track/2.0) - left_linear_val = int((forward - angular_to_linear) * self.m_s_to_value) - right_linear_val = int((forward + angular_to_linear) * self.m_s_to_value) - - return left_linear_val, right_linear_val - - def cmd_vel_callback(self, msg): - #rospy.loginfo("Received a /cmd_vel message!") - #rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z)) - #rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z)) - - # rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0 - - # Do velocity processing here: - # Use the kinematics of your robot to map linear and angular velocities into motor commands - - # 3600 ERPM = 360 RPM ~= 6 km/hr - - #angular_to_linear = msg.angular.z * (wheel_track/2.0) - #left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm - #right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm - left_linear_val, right_linear_val = self.convert(msg.linear.x, msg.angular.z) - - # if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm - - # Then set your wheel speeds (using wheel_left and wheel_right as examples) - #self.left_motor_pub.publish(left_linear_rpm) - #self.right_motor_pub.publish(right_linear_rpm) - #wheel_left.set_speed(v_l) - #wheel_right.set_speed(v_r) - - #rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z)) - try: - drive_command = ('drive', (left_linear_val, right_linear_val)) - self.command_queue.put_nowait(drive_command) - except Queue.Full: - pass - - self.last_cmd_vel_time = rospy.Time.now() - - def pub_diagnostics(self, stat): - stat.add("Status", self.status) - stat.add("Motor state L", self.motor_state_l) - stat.add("Motor state R", self.motor_state_r) - stat.add("FET temp L (C)", round(self.temp_v_l,1)) - stat.add("FET temp R (C)", round(self.temp_v_r,1)) - stat.add("Motor temp L (C)", "unimplemented") - stat.add("Motor temp R (C)", "unimplemented") - stat.add("Motor current L (A)", round(self.current_l,1)) - stat.add("Motor current R (A)", round(self.current_r,1)) - stat.add("Voltage (V)", round(self.bus_voltage,2)) - stat.add("Motor i2t L", round(self.left_energy_acc,1)) - stat.add("Motor i2t R", round(self.right_energy_acc,1)) - - # https://github.com/ros/common_msgs/blob/jade-devel/diagnostic_msgs/msg/DiagnosticStatus.msg - if self.status == "disconnected": - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Not connected") - else: - if self.i2t_error_latch: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "i2t overheated, drive ignored until cool") - elif self.left_energy_acc > self.i2t_warning_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Left motor over i2t warning threshold") - elif self.left_energy_acc > self.i2t_error_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Left motor over i2t error threshold") - elif self.right_energy_acc > self.i2t_warning_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Right motor over i2t warning threshold") - elif self.right_energy_acc > self.i2t_error_threshold: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Right motor over i2t error threshold") - # Everything is okay: - else: - stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Running") - - - def pub_temperatures(self): - # https://discourse.odriverobotics.com/t/odrive-mosfet-temperature-rise-measurements-using-the-onboard-thermistor/972 - # https://discourse.odriverobotics.com/t/thermistors-on-the-odrive/813/7 - # https://www.digikey.com/product-detail/en/murata-electronics-north-america/NCP15XH103F03RC/490-4801-1-ND/1644682 - #p3 = 363.0 - #p2 = -459.2 - #p1 = 308.3 - #p0 = -28.1 - # - #vl = self.temp_v_l - #vr = self.temp_v_r - - #temperature_l = p3*vl**3 + p2*vl**2 + p1*vl + p0 - #temperature_r = p3*vr**3 + p2*vr**2 + p1*vr + p0 - - #print(temperature_l, temperature_r) - - self.temperature_publisher_left.publish(self.temp_v_l) - self.temperature_publisher_right.publish(self.temp_v_r) - - # Current publishing and i2t calculation - i2t_current_nominal = 2.0 - i2t_update_rate = 0.01 - - def pub_current(self): - self.current_publisher_left.publish(float(self.current_l)) - self.current_publisher_right.publish(float(self.current_r)) - - now = time.time() - - if not hasattr(self, 'last_pub_current_time'): - self.last_pub_current_time = now - self.left_energy_acc = 0 - self.right_energy_acc = 0 - return - - # calculate and publish i2t - dt = now - self.last_pub_current_time - - power = max(0, self.current_l**2 - self.i2t_current_nominal**2) - energy = power * dt - self.left_energy_acc *= 1 - self.i2t_update_rate * dt - self.left_energy_acc += energy - - power = max(0, self.current_r**2 - self.i2t_current_nominal**2) - energy = power * dt - self.right_energy_acc *= 1 - self.i2t_update_rate * dt - self.right_energy_acc += energy - - self.last_pub_current_time = now - - self.i2t_publisher_left.publish(float(self.left_energy_acc)) - self.i2t_publisher_right.publish(float(self.right_energy_acc)) - - # stop odrive if overheated - if self.left_energy_acc > self.i2t_error_threshold or self.right_energy_acc > self.i2t_error_threshold: - if not self.i2t_error_latch: - self.driver.release() - self.status = "overheated" - self.i2t_error_latch = True - rospy.logerr("ODrive has exceeded i2t error threshold, ignoring drive commands. Waiting to cool down.") - elif self.i2t_error_latch: - if self.left_energy_acc < self.i2t_resume_threshold and self.right_energy_acc < self.i2t_resume_threshold: - # have cooled enough now - self.status = "ready" - self.i2t_error_latch = False - rospy.logerr("ODrive has cooled below i2t resume threshold, ignoring drive commands. Waiting to cool down.") - - - # current_quantizer = 5 - # - # self.left_current_accumulator += self.current_l - # self.right_current_accumulator += self.current_r - # - # self.current_loop_count += 1 - # if self.current_loop_count >= current_quantizer: - # self.current_publisher_left.publish(float(self.left_current_accumulator) / current_quantizer) - # self.current_publisher_right.publish(float(self.right_current_accumulator) / current_quantizer) - # - # self.current_loop_count = 0 - # self.left_current_accumulator = 0.0 - # self.right_current_accumulator = 0.0 - - def pub_odometry(self, time_now): - now = time_now - self.odom_msg.header.stamp = now - self.tf_msg.header.stamp = now - - wheel_track = self.wheel_track # check these. Values in m - tyre_circumference = self.tyre_circumference - # self.m_s_to_value = encoder_cpr/tyre_circumference set earlier - - # if odometry updates disabled, just return the old position and zero twist. - if not self.odometry_update_enabled: - self.odom_msg.twist.twist.linear.x = 0. - self.odom_msg.twist.twist.angular.z = 0. - - # but update the old encoder positions, so when we restart updates - # it will start by giving zero change from the old position. - self.old_pos_l = self.new_pos_l - self.old_pos_r = self.new_pos_r - - self.odom_publisher.publish(self.odom_msg) - if self.publish_tf: - self.tf_publisher.sendTransform(self.tf_msg) - - return - - # Twist/velocity: calculated from motor values only - s = tyre_circumference * (self.vel_l+self.vel_r) / (2.0*self.encoder_cpr) - w = tyre_circumference * (self.vel_r-self.vel_l) / (wheel_track * self.encoder_cpr) # angle: vel_r*tyre_radius - vel_l*tyre_radius - self.odom_msg.twist.twist.linear.x = s - self.odom_msg.twist.twist.angular.z = w - - #rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % ( - # vel_l, -vel_r, - # vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z, - # self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr)) - - # Position - delta_pos_l = self.new_pos_l - self.old_pos_l - delta_pos_r = self.new_pos_r - self.old_pos_r - - self.old_pos_l = self.new_pos_l - self.old_pos_r = self.new_pos_r - - # Check for overflow. Assume we can't move more than half a circumference in a single timestep. - half_cpr = self.encoder_cpr/2.0 - if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr - elif delta_pos_l < -half_cpr: delta_pos_l = delta_pos_l + self.encoder_cpr - if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr - elif delta_pos_r < -half_cpr: delta_pos_r = delta_pos_r + self.encoder_cpr - - # counts to metres - delta_pos_l_m = delta_pos_l / self.m_s_to_value - delta_pos_r_m = delta_pos_r / self.m_s_to_value - - # Distance travelled - d = (delta_pos_l_m+delta_pos_r_m)/2.0 # delta_ps - th = (delta_pos_r_m-delta_pos_l_m)/wheel_track # works for small angles - - xd = math.cos(th)*d - yd = -math.sin(th)*d - - # elapsed time = event.last_real, event.current_real - #elapsed = (event.current_real-event.last_real).to_sec() - # calc_vel: d/elapsed, th/elapsed - - # Pose: updated from previous pose + position delta - self.x += math.cos(self.theta)*xd - math.sin(self.theta)*yd - self.y += math.sin(self.theta)*xd + math.cos(self.theta)*yd - self.theta = (self.theta + th) % (2*math.pi) - - #rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % ( - # delta_pos_l_m, delta_pos_r_m, - # d, th, xd, yd, - # self.x, self.y, self.theta - # )) - - # fill odom message and publish - - self.odom_msg.pose.pose.position.x = self.x - self.odom_msg.pose.pose.position.y = self.y - q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, self.theta) - self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2 - self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2 - - #rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % ( - # self.theta, - # math.sin(self.theta)/2, math.cos(self.theta)/2, - # q[2],q[3],q[0],q[1] - # )) - - #self.odom_msg.pose.covariance - # x y z - # x y z - - self.tf_msg.transform.translation.x = self.x - self.tf_msg.transform.translation.y = self.y - #self.tf_msg.transform.rotation.x - #self.tf_msg.transform.rotation.x - self.tf_msg.transform.rotation.z = q[2] - self.tf_msg.transform.rotation.w = q[3] - - if self.publish_raw_odom: - self.raw_odom_publisher_encoder_left.publish(self.new_pos_l) - self.raw_odom_publisher_encoder_right.publish(self.new_pos_r) - self.raw_odom_publisher_vel_left.publish(self.vel_l) - self.raw_odom_publisher_vel_right.publish(self.vel_r) - - # ... and publish! - self.odom_publisher.publish(self.odom_msg) - if self.publish_tf: - self.tf_publisher.sendTransform(self.tf_msg) - - def pub_joint_angles(self, time_now): - jsm = self.joint_state_msg - jsm.header.stamp = time_now - if self.driver: - jsm.position[0] = 2*math.pi * self.new_pos_l / self.encoder_cpr - jsm.position[1] = 2*math.pi * self.new_pos_r / self.encoder_cpr - - self.joint_state_publisher.publish(jsm) - -def start_odrive(): - rospy.init_node('odrive') - odrive_node = ODriveNode() - odrive_node.main_loop() - #rospy.spin() - -if __name__ == '__main__': - try: - start_odrive() - except rospy.ROSInterruptException: - pass - diff --git a/odrive_ros/src/odrive_ros/odrive_simulator.py b/odrive_ros/src/odrive_ros/odrive_simulator.py deleted file mode 100644 index e916cca..0000000 --- a/odrive_ros/src/odrive_ros/odrive_simulator.py +++ /dev/null @@ -1,108 +0,0 @@ -import sys -import time -import logging -import traceback - -default_logger = logging.getLogger(__name__) -default_logger.setLevel(logging.DEBUG) - -# create console handler and set level to debug -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) - -default_logger.addHandler(ch) - -class ODriveInterfaceSimulator(object): - encoder_cpr = 4096 - connected = False - engaged = False - - right_axis_vel = 0 # units: encoder counts/s - left_axis_vel = 0 - right_axis_pos = 0 # go from 0 up to encoder_cpr-1 - left_axis_pos = 0 - last_time_update = None - - - def __init__(self, logger=None): - self.logger = logger if logger else default_logger - - def update_time(self, curr_time): - # provided so simulator can update position - if last_time_update is None: - last_time_update = curr_time - return - - dt = curr_time - last_time_update - left_axis_pos = floor(left_axis_pos + left_axis_vel *dt) % self.encoder_cpr - right_axis_pos = floor(right_axis_pos + right_axis_vel*dt) % self.encoder_cpr - last_time_update = curr_time - - def connect(self, port=None, right_axis=0, timeout=30): - if self.connected: - self.logger.info("Already connected. Simulating disc/reconnect.") - - self.encoder_cpr = 4096 - self.logger.info("Connected to simulated ODrive.") - return True - - def disconnect(self): - self.connected = False - return True - - def calibrate(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.logger.info("Calibrated.") - return True - - def preroll(self, wait=True, reverse=False): - if not self.connected: - self.logger.error("Not connected.") - return False - - return True - - def ensure_prerolled(self): - return True - - def engaged(self): - return self.engaged - - def engage(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.engaged = True - return True - - def release(self): - if not self.connected: - self.logger.error("Not connected.") - return False - self.engaged = False - return True - - def drive(self, left_motor_val, right_motor_val): - if not self.connnected: - self.logger.error("Not connected.") - return - self.left_axis.controller.vel_setpoint = left_motor_val - self.right_axis.controller.vel_setpoint = -right_motor_val - - return True - - def get_errors(self, clear=True): - if not self.driver: - return None - return "Simulated ODrive, no errors." - - def left_vel_estimate(self): return self.left_axis_vel - def right_vel_estimate(self): return self.right_axis_vel - def left_pos(self): return self.left_axis_pos - def right_pos(self): return self.right_axis_pos - - def left_current(self): return 0 - def right_current(self): return 0 - \ No newline at end of file diff --git a/robot_localization/README.md b/robot_localization/README.md index 5a717f2..5e8b98b 100644 --- a/robot_localization/README.md +++ b/robot_localization/README.md @@ -1,9 +1,11 @@ robot_localization ================== -robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. +robot_localization is a package of nonlinear state estimation nodes. -Please see documentation here: http://docs.ros.org/melodic/api/robot_localization/html/index.html +The following example shows the ROS service call of the navsat_transform node. It can be used to transform WGS84 coordinates from and to local refference coordinates. + +This example call "/fromLL" tansforms a point on AAU (Aalborg) campus east to the local coordinate system (meters). ``` rosservice call /fromLL "ll_point: @@ -16,7 +18,7 @@ map_point: z: 0.0 ``` - +The following call does the oposite. ``` rosservice call /toLL "map_point: diff --git a/ros-mpu9250-ahrs b/ros-mpu9250-ahrs deleted file mode 160000 index 6b6c85a..0000000 --- a/ros-mpu9250-ahrs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6b6c85a0e822b3991bcdef120a9c5cc16745e280 diff --git a/sbg_ros_driver/.gitattributes b/sbg_ros_driver/.gitattributes deleted file mode 100644 index e9b5795..0000000 --- a/sbg_ros_driver/.gitattributes +++ /dev/null @@ -1,3 +0,0 @@ -# Define the end of line behavior for Git, to avoid errors compatibility with Windows files. -* text eol=lf -*.png -text \ No newline at end of file diff --git a/sbg_ros_driver/.gitignore b/sbg_ros_driver/.gitignore deleted file mode 100644 index fa8d82a..0000000 --- a/sbg_ros_driver/.gitignore +++ /dev/null @@ -1,43 +0,0 @@ -# Jetbrains IDEs -cmake-build-debug -.idea - -# Build folders -Build -build - -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -# Visual Studio code -.vscode \ No newline at end of file diff --git a/sbg_ros_driver/CHANGELOG.rst b/sbg_ros_driver/CHANGELOG.rst deleted file mode 100644 index 71e4de5..0000000 --- a/sbg_ros_driver/CHANGELOG.rst +++ /dev/null @@ -1,149 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package sbg_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.3 (2020-04-01) ------------------- -* Improve matrix handling -* Fix body velocity computation (Issue `#31 `_) - -2.0.1 (2020-01-24) ------------------- -* Improve vector handling -* Fix include file (Issue `#26 `_) - -2.0.0 (2020-01-06) ------------------- -* Fix integer type -* Update sbgECom messages (AirData, ImuShort) -* Update sbgECom library to 1.11.920-stable -* Improve numeric type -* Improve configuration applier -* Improve error handling -* Code improvement -* Improve device configuration -* Update changelog -* Update and improve README.md -* Update magnetic services -* Improve message timestamping -* Add some ROS standard sensor messages (Issue `#17 `_) -* Comply file structure to ROS best pratices -* Add a processing time to improve message handling -* Add udev rules to documentation (Issue `#21 `_) -* Improve magnetometers calibration -* Update maintainer of the package (Issue `#20 `_) -* Enable/Disable the configuration of the device (Issue `#19 `_) -* Define unified class and launch files for all SBG devices -* Define classes for device configuration -* Merge pull request `#18 `_ from SBG-Systems/messagePublisherRework -* Integrate new message publisher to the Ellipse class (Issue `#15 `_) -* Define a class to publish messages -* Define class to wrap SBG logs to Ros messages -* Merge pull request `#16 `_ from SBG-Systems/v4.3 -* [src] Update SDK version + add LogE support -* Merge pull request `#13 `_ from nicolaje/remove-non-ascii-char -* [conf] Removed non-ASCII characters, (Issue `#8 `_) -* [msg] Remove non ascii characters - -1.1.7 (2018-07-19) ------------------- -* [src] Change SbgEkfEuler comments -* [src] Move .h to include folder + test new method for time saving in calib - -1.1.6 (2018-03-18) ------------------- -* [config, src] Update default port for gps aiding (Ellipse-E) + add save & reboot for mag calibration -* [build] Add include for debian jessie arm64 build issue - -1.1.5 (2018-03-12 23:49) ------------------------- -* [src] Update mag calibration - -1.1.4 (2018-03-12 23:10) ------------------------- -* [catkin] Update install launch & config -* [src] Update library + Correction bugs - -1.1.3 (2018-03-12 11:46) ------------------------- -* Update dependencies to std_srvs - -1.1.2 (2018-03-12 09:54) ------------------------- -* [ChangeLog] Remove -* [ChangeLog] Update -* [Changelog] Test -* [test] Changelog -* [Changelog] Update -* [CMake] Correction of message dependency - -1.1.1 (2018-03-11) ------------------- -* [xml] Update version number -* [src] Correction of small bugs + add publisher only on activated log -* [merge] Finalize merge from devel branch (master divergence issue) -* [lib] Update the library sbgECom version after merging from devel -* [Merge] -* Merge branch 'master' of https://github.com/ENSTABretagneRobotics/sbg_ros_driver -* [src] Update doc -* [src] Update magnetic calibration node -* Revert "1.0.7" - This reverts commit 8f57f9e578937ac23383e39ebf616d1039384b09. -* Update README -* Merge pull request `#2 `_ from rpng/master - Upgrade sbg_ros_driver -* Moved the logging function into the class -* Added - Start of heading code -* Refactor and added new publishers -* Increased rates -* refactoring - use a class for callbacks - changed callbacks around a bit, now shows raw data rather than ekf logs -* use gps log message for NavSatFix message -* add extra debug messages -* use private namespace -* modified launch file - moved to launch folder and added optional arguments -* updated sbgECom library - -1.1.0 (2018-03-10) ------------------- -* [src] Update Events -* [src] Add params -* [src] Update (add configuration of the ellipse) -* [src] Update messages -* [src] Start creating sbg messages - -1.0.7 (2017-04-01) ------------------- -* [src][minor] Correct launch file - -1.0.6 (2017-03-31) ------------------- -* CHANGELOG -* [src] Add launch example -* [src] Change imu data & add gyroscopes - -1.0.5 (2016-11-17 00:04) ------------------------- - -1.0.4 (2016-11-17 00:02) ------------------------- - -1.0.3 (2016-11-16 23:59) ------------------------- -* [src][minor] Correction of Project name in CmakeList - -1.0.2 (2016-11-16 22:58) ------------------------- -* [doc] minor -* [doc] Update Package - -1.0.1 (2016-11-16 22:30) ------------------------- -* [doc] Update package version to 1.0.0 -* [doc] Add Changelog -* [src] Update of deprecated function -* [src] Update (correcting cmake sub project) -* [src] Correct cmake subdirectory issue -* Initial commit diff --git a/sbg_ros_driver/CMakeLists.txt b/sbg_ros_driver/CMakeLists.txt deleted file mode 100644 index 90fbbe2..0000000 --- a/sbg_ros_driver/CMakeLists.txt +++ /dev/null @@ -1,150 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sbg_driver) - -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - std_msgs - std_srvs - geometry_msgs - message_generation -) - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate messages in the 'msg' folder -add_message_files(FILES - SbgGpsVelStatus.msg - SbgGpsPosStatus.msg - SbgMagStatus.msg - SbgShipMotionStatus.msg - SbgUtcTimeStatus.msg - - SbgAirData.msg - SbgAirDataStatus.msg - SbgEkfEuler.msg - SbgGpsPos.msg - SbgMag.msg - SbgStatusCom.msg - SbgEkfNav.msg - SbgGpsRaw.msg - SbgOdoVel.msg - SbgStatusGeneral.msg - SbgEkfQuat.msg - SbgGpsVel.msg - SbgStatus.msg - SbgEvent.msg - SbgImuStatus.msg - SbgImuData.msg - SbgImuShort.msg - SbgShipMotion.msg - SbgUtcTime.msg - SbgGpsHdt.msg - SbgMagCalib.msg - SbgStatusAiding.msg - SbgEkfStatus.msg -) - -## Generate added messages and services with any dependencies listed here -generate_messages(DEPENDENCIES - std_msgs - geometry_msgs -) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS src - #LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - message_runtime - roscpp - sensor_msgs - std_msgs - std_srvs - geometry_msgs -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -add_subdirectory(external/sbgECom) - -include_directories( - ${catkin_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/include/sbg_driver/ - ${PROJECT_SOURCE_DIR}/external/sbgECom/common - ${PROJECT_SOURCE_DIR}/external/sbgECom/src -) - -## 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(sbg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Define common resources. -set (SBG_COMMON_RESOURCES - src/config_applier.cpp - src/message_publisher.cpp - src/message_wrapper.cpp - src/config_store.cpp - src/sbg_device.cpp -) - -## Declare a C++ executable -add_executable(sbg_device ${SBG_COMMON_RESOURCES} src/main.cpp) -add_dependencies(sbg_device ${sbg_driver_EXPORTED_TARGETS}) -target_compile_options(sbg_device PRIVATE -Wall -Wextra) - -add_executable(sbg_device_mag ${SBG_COMMON_RESOURCES} src/main_mag.cpp) -add_dependencies(sbg_device_mag ${sbg_driver_EXPORTED_TARGETS}) -target_compile_options(sbg_device_mag PRIVATE -Wall -Wextra) - -## Specify libraries to link a library or executable target against -target_link_libraries(sbg_device ${catkin_LIBRARIES} sbgECom) -target_link_libraries(sbg_device_mag ${catkin_LIBRARIES} sbgECom) - -set_property(TARGET sbg_device PROPERTY CXX_STANDARD 11) -set_property(TARGET sbg_device_mag PROPERTY CXX_STANDARD 11) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executables and/or libraries for installation -install(TARGETS sbg_device sbg_device_mag -# 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 - PATTERN ".git" EXCLUDE -) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".git" EXCLUDE) - -install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config - PATTERN ".git" EXCLUDE) - -############# -## Testing ## -############# \ No newline at end of file diff --git a/sbg_ros_driver/LICENSE b/sbg_ros_driver/LICENSE deleted file mode 100644 index 9fe2223..0000000 --- a/sbg_ros_driver/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ - The MIT license - - Copyright (C) 2007-2020, SBG Systems SAS. All rights reserved. - - 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. diff --git a/sbg_ros_driver/README.md b/sbg_ros_driver/README.md deleted file mode 100644 index 6aced0c..0000000 --- a/sbg_ros_driver/README.md +++ /dev/null @@ -1,291 +0,0 @@ -# sbg_driver - -[![Build Status](https://img.shields.io/jenkins/build?jobUrl=http%3A%2F%2Fbuild.ros.org%2Fjob%2FMdev__sbg_driver__ubuntu_bionic_amd64%2F&label=Ubuntu%20bionic)](http://build.ros.org/job/Mdev__sbg_driver__ubuntu_bionic_amd64/) [![Build Status](https://img.shields.io/jenkins/build?jobUrl=http%3A%2F%2Fbuild.ros.org%2Fjob%2FKdev__sbg_driver__ubuntu_xenial_amd64%2F&label=Ubuntu%20xenial)](http://build.ros.org/job/Kdev__sbg_driver__ubuntu_xenial_amd64/) - -## Overview -ROS package for SBG Systems IMU.
-The driver allows the user to configure the IMU (if possible, according to the device), to receive messages from the Sbg message protocol, publish ROS standard messages , and to calibrate the magnetometers. - -Initial work has been done by [ENSTA Bretagne](https://github.com/ENSTABretagneRobotics). - -**Author : [SBG Systems](https://www.sbg-systems.com/)
-Maintainer : SBG Systems, support@sbg-systems.com** - -## Installation -### Installation from Packages -User can install the sbg_ros_driver through the standard ROS installation system. -* Melodic ```sudo apt-get install ros-melodic-sbg-driver``` -* Kinectic ```sudo apt-get install ros-kinetic-sbg-driver``` -* Lunar ```sudo apt-get install ros-lunar-sbg-driver``` - -### Building from sources -#### Dependencies -* [Robot Operating System (ROS)](http://wiki.ros.org/) -* SBG communication protocol sbgECom, v1.11.920-stable (full compatible with firmwares from 1.7.x). - -#### Building -1. Clone the repository (use a Release version) -2. Build using the normal ROS catkin build system - -``` -cd catkin_ws/src -git clone https://github.com/SBG-Systems/sbg_ros_driver.git -cd ../ -catkin_make -``` - -## Usage -To run the default Ros node with the default configuration - -``` -roslaunch sbg_driver sbg_device.launch -``` - -To run the magnetic calibration node - -``` -roslaunch sbg_driver sbg_device_mag_calibration.launch -``` - -## Config files -### Default config files -Every configuration file is defined according to the same structure.
- -* **sbg_device_uart_default.yaml**
-This config file is the default one for Uart connection with the device.
-It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node).
-It defines a few outputs for the device : - * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz - * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz - * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. - -* **sbg_device_udp_default.yaml**
-This config file is the default one for an Udp connection with the device.
-It does not configure the device through the ROS node, so it has to be previously configured (manually or with the ROS node).
-It defines a few outputs for the device : - * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz - * ROS standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz - * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz. - -### Example config files -* **ellipse_A_default.yaml**
-Default config file for an Ellipse-A. - -* **ellipse_E_default.yaml**
-Default config file for an Ellipse-E with an external antenna and external Gnss. - -* **ellipse_N_default.yaml**
-Default config file for an Ellipse-N with an external antenna and internal Gnss. - -## Launch files -### Default launch files -* **sbg_device.launch**
-Launch the sbg_device node to handle the receivde data, and load the `sbg_device_uart_default.yaml` configuration. - -* **sbg_device_mag_calibration.launch**
-Launch the sbg_device_mag node to calibrate the magnetometers, and load the `ellipse_E_default.yaml` configuration. - -## Nodes -### sbg_device -The sbg_device node handles the communication with the connected device, and publishes the SBG output to the Ros environment. - -#### Published Topics -##### SBG specific topics -* **`/sbg/status`** [sbg_driver/SbgStatus](http://docs.ros.org/api/sbg_driver/html/msg/SbgStatus.html) - - Provides informations about the general status (Communication, Aiding, etc..). - -* **`/sbg/utc_time`** [sbg_driver/SbgUtcTime](http://docs.ros.org/api/sbg_driver/html/msg/SbgUtcTime.html) - - Provides UTC time reference. - -* **`/sbg/imu_data`** [sbg_driver/SbgImuData](http://docs.ros.org/api/sbg_driver/html/msg/SbgImuData.html) - - IMU status, and sensors values. - -* **`/sbg/ekf_euler`** [sbg_driver/SbgEkfEuler](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfEuler.html) - - Computed orientation using Euler angles. - -* **`/sbg/ekf_quat`** [sbg_driver/SbgEkfQuat](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfQuat.html) - - Computed orientation using Quaternion. - -* **`/sbg/ekf_nav`** [sbg_driver/SbgEkfNav](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfNav.html) - - Computed navigation data. - -* **`/sbg/mag`** [sbg_driver/SbgMag](http://docs.ros.org/api/sbg_driver/html/msg/SbgMag.html) - - Magnetic data. - -* **`/sbg/mag_calib`** [sbg_driver/SbgMagCalib](http://docs.ros.org/api/sbg_driver/html/msg/SbgMagCalib.html) - - Magnetometer calibration data. - -* **`/sbg/ship_motion`** [sbg_driver/SbgShipMotion](http://docs.ros.org/api/sbg_driver/html/msg/SbgShipMotion.html) - - Heave, surge and sway data. - -* **`/sbg/gps_vel`** [sbg_driver/SbgGpsVel](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsVel.html) - - GPS velocities from GPS receiver. - -* **`/sbg/gps_pos`** [sbg_driver/SbgGpsPos](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsPos.html) - - GPS positions from GPS receiver. - -* **`/sbg/gps_hdt`** [sbg_driver/SbgGpsHdt](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsHdt.html) - - GPS true heading from dual antenna system. - -* **`/sbg/gps_raw`** [sbg_driver/SbgGpsRaw](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsRaw.html) - - GPS raw data for post processing. - -* **`/sbg/odo_vel`** [sbg_driver/SbgOdoVel](http://docs.ros.org/api/sbg_driver/html/msg/SbgOdoVel.html) - - Odometer velocity. - -* **`/sbg/event[ABCDE]`** [sbg_driver/SbgEvent](http://docs.ros.org/api/sbg_driver/html/msg/SbgEvent.html) - - Event on sync in the corresponding pin. - -* **`/sbg/pressure`** [sbg_driver/SbgPressure](http://docs.ros.org/api/sbg_driver/html/msg/SbgPressure.html) - - Pressure data. - -##### ROS standard topics -In order to define ROS standard topics, it requires sometimes several SBG messages, to be merged. -For each ROS standard, you have to activate the needed SBG outputs. - -* **`/imu/data`** [sensor_msgs/Imu](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html) - - IMU data. - Requires `/sbg/imu_data` and `/sbg/ekf_quat`. - -* **`/imu/temp`** [sensor_msgs/Temperature](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Temperature.html) - - IMU temperature data. - Requires `/sbg/imu_data`. - -* **`/imu/velocity`** [geometry_msgs/TwistStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/TwistStamped.html) - - IMU velocity data. - Requires `/sbg/imu_data` and `/sbg/ekf_nav` and either `/sbg/ekf_euler` or `/sbg/ekf_quat`. - -* **`/imu/mag`** [sensor_msgs/MagneticField](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/MagneticField.html) - - IMU magnetic field. - Requires `/sbg/mag`. - -* **`/imu/pres`** [sensor_msgs/FluidPressure](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/FluidPressure.html) - - IMU pressure data. - Requires `/sbg/pressure`. - -* **`/imu/pos_ecef`** [geometry_msgs/PointStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PointStamped.html) - - Earth-Centered Earth-Fixed position. - Requires `/sbg/ekf_nav`. - -* **`/imu/utc_ref`** [sensor_msgs/TimeReference](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html) - - UTC time reference. - Requires `/sbg/utc_time`. - -* **`/imu/nav_sat_fix`** [sensor_msgs/NavSatFix](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/NavSatFix.html) - - Navigation satellite fix for any Global Navigation Satellite System. - Requires `/sbg/gps_pos`. - - -### sbg_device_mag -The sbg_device_mag node handles the magnetic calibration for suitable devices. - -#### Services -* **`/sbg/mag_calibration`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html) - - Service to start/stop the magnetic calibration. - -* **`/sbg/mag_calibration_save`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html) - - Service to save the magnetic calibration to the connected device. - -## HowTo -### Configure the SBG device -The SBG Ros driver allows the user to configure the device before starting the data handling.
-To do so, set the corresponding parameter in the used config file. - -``` -# Configuration of the device with ROS. -confWithRos: true -``` - -Then, modify the desired parameters in the config file, using the SBG Firmware Manual, to see which features are configurable, and which parameter values are available. - -### Calibrate the magnetometers -Ellipse-A/E/N use magnemoter to provide heading. A calibration is then required to compensate soft and hard iron distortions due to the environmenent (motors, batteries, ...). The magnetic calibration procedure should be held in a non magnetic area (outside of buildings). - -``` -roslaunch sbg_driver sbg_device_mag_calibration.launch -rosservice call /sbg/mag_calibration -``` - -> success: True
-> message: "Magnetometer calibration process started." - -Proceed rotations of the IMU (every orientation if possible). - -``` -rosservice call /sbg/mag_calibration -``` - -> success: True
-> message: "Magnetometer calibration is finished. See the output console to get calibration informations." - -If the magnetic calibration is satisfaying (Quality, Confidence), it could be uploaded/saved to the device. - -``` -rosservice call /sbg/mag_calibration_save -``` - -> success: True
-> message: "Magnetometer calibration has been uploaded to the device." - -### Enable communication with the SBG device -To be able to communicate with the device, be sure that your user is part of the dialout group.
-Once added, restart your machine to save and apply the changes. - -``` -sudo adduser $USER dialout -``` - -### Create udev rules -Udev rules can be defined for communication port, in order to avoid modifying the port in configuration if it has changed. -[Udev documentation](https://wiki.debian.org/udev) - -A symlink can be configured and defined to uniquely identify the connected device.
-Once it is done, configuration file could be updated `portName: "/dev/sbg"`. - -See the docs folder, to see an example of rules with the corresponding screenshot using the udev functions. - -### Synchronize the ROS messages from an external time source -#### No external source -When no external time source is available, the header time of messages is the system Epoch (Unix) processing time of the SBG callback, given by the ```ros::Time::now()```. - -#### External Gnss receiver and/or External antenna -When the SBG device is connected with an external Gnss receiver, if the device receives a full valid SBG Utc log, i.e : -* A stable input clock to be synchronized with the internal clock -* A valid Utc time data (with or without the leap second) -* The clock has converged to the PPS - -then, the time header will be computed from the last received Utc log and the device timestamp given by the internal clock. - -## Contributing -### Bugs and issues -Please report bugs and/or issues using the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros_driver/issues) - -### Features requests or additions -In order to contribute to the code, please use Pull requests to the `devel` branch.
-If you have some feature requests, use the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros_driver/issues) as well. diff --git a/sbg_ros_driver/config/example/ellipse_A_default.yaml b/sbg_ros_driver/config/example/ellipse_A_default.yaml deleted file mode 100644 index 43e848b..0000000 --- a/sbg_ros_driver/config/example/ellipse_A_default.yaml +++ /dev/null @@ -1,276 +0,0 @@ -# Configuration file for SBG Ellipse -# YAML - -# Ellipse-A - Magnetic-based -# Ellipse-E - External GNSS -# Ellipse-N - GNSS-based -# Ellipse-D - Dual-antenna GNSS - -# Configuration of the device with ROS. -confWithRos: true - -uartConf: - # Port Name - portName: "/dev/ttyUSB0" - - # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 - - # Port Id - # 0 PORT_A: Main communication interface. Full duplex. - # 1 PORT_B: Auxiliary input interface for RTCM - # 2 PORT_C: Auxiliary communication interface. Full duplex. - # 3 PORT_D: Auxiliary input interface - # 4 PORT_E: Auxiliary input/output interface - portID: 0 - -# Sensor Parameters -sensorParameters: - # Initial latitude (°) - initLat: 48.419727 - # Initial longitude (°) - initLong: -4.472119 - # Initial altitude (above WGS84 ellipsoid) (m) - initAlt: 100.0 - # Year at startup - year: 2018 - # month in year at startup - month: 03 - # day in month at startup - day: 10 - - # Montion profile ID - # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply - # 2 AUTOMOTIVE Dedicated to car applications - # 3 MARINE Used in marine and underwater applications - # 4 AIRPLANE For fixed wings aircraft - # 5 HELICOPTER For rotary wing aircraft - motionProfile: 1 - -# IMU_ALIGNMENT_LEVER_ARM -imuAlignementLeverArm: - # IMU X axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionX: 0 - # IMU Y axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionY: 3 - # Residual roll error after axis alignment rad - misRoll: 0 - # Residual pitch error after axis alignment rad - misPitch: 0 - # Residual yaw error after axis alignment rad - misYaw: 0 - # X Primary lever arm in IMU X axis (once IMU alignment is applied) m - leverArmX: 0 - # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m - leverArmY: 0 - # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m - leverArmZ: 0 - -# AIDING_ASSIGNMENT -# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version. -# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only -# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users. -aidingAssignment: - # GNSS module port assignment: - # 255 Module is disabled - # 1 Module connected on PORT_B - # 2 Module connected on PORT_C - # 3 Module connected on PORT_D - # 5 Module is connected internally - gnss1ModulePortAssignment: 255 - # GNSS module sync assignment: - # 0 Module is disabled - # 1 Synchronization is done using SYNC_IN_A pin - # 2 Synchronization is done using SYNC_IN_B pin - # 3 Synchronization is done using SYNC_IN_C pin - # 4 Synchronization is done using SYNC_IN_D pin - # 5 Synchronization is internal - # 6 Synchronization is done using SYNC_OUT_A pin - # 7 Synchronization is done using SYNC_OUT_B pin - gnss1ModuleSyncAssignment: 0 - # RTCM input port assignment for Ellipse-N DGPS - rtcmPortAssignment: 255 - # Odometer module pin assignment - # 0 Odometer is disabled - # 1 Odometer connected only to ODO_A (unidirectional). - # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer. - odometerPinAssignment: 0 - - - -magnetometer: - # Magnetometer model ID - # 201 Should be used in most applications - # 202 Should be used in disturbed magnetic environment - magnetometerModel: 201 - # Magnetometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - magnetometerRejectMode: 1 - - # Theses parameters are only used for a calibration run - calibration: - # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. - # This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - # To work correctly, the device should be rotated through at least a full circle. - # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy - mode: 2 - - # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. - # Motion during calibration is then limited to slow rotations. - # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. - # It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration. - # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high. - bandwidth: 2 - - -# GNSS configuration -# Note: Secondary level arms should only be considered in case of dual antenna GNSS receiver. It can be left to 0 otherwise. -gnss: - # Gnss Model Id - # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS - # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU - # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. - # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. - # 107 Used on Ellipse-D by default - gnss_model_id: 102 - - #GNSS primary antenna lever arm in IMU X axis (m) - primaryLeverArmX: 0 - #GNSS primary antenna lever arm in IMU Y axis (m) - primaryLeverArmY: 0 - #GNSS primary antenna lever arm in IMU Z axis (m) - primaryLeverArmZ: 0 - #GNSS primary antenna precise. Set to true if the primary lever arm has been accurately entered and doesn't need online re-estimation. - primaryLeverPrecise: true - - #GNSS secondary antenna lever arm in IMU X axis (m) - secondaryLeverArmX: 0 - #GNSS secondary antenna lever arm in IMU Y axis (m) - secondaryLeverArmY: 0 - #GNSS secondary antenna lever arm in IMU Z axis (m) - secondaryLeverArmZ: 0 - - # Secondary antenna operating mode. - # 1 The GNSS will be used in single antenna mode only and the secondary lever arm is not used. - # 2 [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. - # 3 The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. - # 4 The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. - secondaryLeverMode: 1 - - # Rejection mode for position - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - posRejectMode: 1 - # Rejection mode for velocity (see posRejectMode values) - velRejectMode: 1 - # Rejection mode for true heading (see posRejectMode values) - hdtRejectMode: 1 - -# Odometer configuration -odom: - # Odometer's gain Pulses/m - gain: 4800 - # User gain average error (%) - gain_error: 0.1 - # Odometer's direction - # 0: positive - # 1: negative - direction: 0 - - # Odometer lever arm in IMU X axis (m) - leverArmX: 0 - # Odometer lever arm in IMU Y axis (m) - leverArmY: 0 - # Odometer lever arm in IMU Z axis (m) - leverArmZ: 0 - - # Odometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - rejectMode: 1 - -# ToDo: event & CAN configuration - -############################### Output configuration ############################### -# 0 Output is disabled -# 1 Output is generated at 200Hz -# 2 Output is generated at 100Hz -# 4 Output is generated at 50Hz -# 8 Output is generated at 25Hz -# 10 Output is generated at 20Hz -# 20 Output is generated at 10Hz -# 40 Output is generated at 5Hz -# 200 Output is generated at 1Hz -# 10000 Pulse Per Second. Same mode as above. -# 10001 Output sent when a new data is available. -# 10002 Output is generated when a new virtual odometer event occurs -# 10003 Output is generated on a Sync In A event -# 10004 Output is generated on a Sync In B event -# 10005 Output is generated on a Sync In C event -# 10006 Output is generated on a Sync In D event -output: - # Time Reference - # 0 No external time reference is used. Internal clock is used instead. - # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin. - # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment) - timeReference: 0 - ros_standard: false - - # Status general, clock, com aiding, solution, heave - log_status: 8 - # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 - # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 8 - # Includes the 4 quaternions values - log_ekf_quat: 0 - # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 - # Heave, surge and sway and accelerations on each axis for up to 4 points - log_ship_motion: 8 - # Provides UTC time reference - log_utc_time: 8 - # Magnetic data with associated accelerometer on each axis - log_mag: 8 - # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 - # GPS velocities from primary or secondary GPS receiver - log_gps1_vel: 0 - # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 0 - # GPS true heading from dual antenna system - log_gps1_hdt: 0 - # GPS 1 raw data for post processing. - log_gps1_raw: 0 - # Provides odometer velocity - log_odo_vel: 0 - # Event A/B/C/D Event markers sent when events are detected on a sync in pin - log_event_a: 0 - log_event_b: 0 - log_event_c: 0 - log_event_d: 0 - # Air data - log_air_data: 0 - # Short IMU data - log_imu_short: 0 - - # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run) - frequency: 0 - diff --git a/sbg_ros_driver/config/example/ellipse_E_default.yaml b/sbg_ros_driver/config/example/ellipse_E_default.yaml deleted file mode 100644 index dd4113d..0000000 --- a/sbg_ros_driver/config/example/ellipse_E_default.yaml +++ /dev/null @@ -1,276 +0,0 @@ -# Configuration file for SBG Ellipse -# YAML - -# Ellipse-A - Magnetic-based -# Ellipse-E - External GNSS -# Ellipse-N - GNSS-based -# Ellipse-D - Dual-antenna GNSS - -# Configuration of the device with ROS. -confWithRos: true - -uartConf: - # Port Name - portName: "/dev/ttyUSB0" - - # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 - - # Port Id - # 0 PORT_A: Main communication interface. Full duplex. - # 1 PORT_B: Auxiliary input interface for RTCM - # 2 PORT_C: Auxiliary communication interface. Full duplex. - # 3 PORT_D: Auxiliary input interface - # 4 PORT_E: Auxiliary input/output interface - portID: 0 - -# Sensor Parameters -sensorParameters: - # Initial latitude (°) - initLat: 48.419727 - # Initial longitude (°) - initLong: -4.472119 - # Initial altitude (above WGS84 ellipsoid) (m) - initAlt: 100.0 - # Year at startup - year: 2018 - # month in year at startup - month: 03 - # day in month at startup - day: 10 - - # Montion profile ID - # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply - # 2 AUTOMOTIVE Dedicated to car applications - # 3 MARINE Used in marine and underwater applications - # 4 AIRPLANE For fixed wings aircraft - # 5 HELICOPTER For rotary wing aircraft - motionProfile: 1 - -# IMU_ALIGNMENT_LEVER_ARM -imuAlignementLeverArm: - # IMU X axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionX: 0 - # IMU Y axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionY: 3 - # Residual roll error after axis alignment rad - misRoll: 0 - # Residual pitch error after axis alignment rad - misPitch: 0 - # Residual yaw error after axis alignment rad - misYaw: 0 - # X Primary lever arm in IMU X axis (once IMU alignment is applied) m - leverArmX: 0 - # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m - leverArmY: 0 - # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m - leverArmZ: 0 - -# AIDING_ASSIGNMENT -# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version. -# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only -# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users. -aidingAssignment: - # GNSS module port assignment: - # 255 Module is disabled - # 1 Module connected on PORT_B - # 2 Module connected on PORT_C - # 3 Module connected on PORT_D - # 5 Module is connected internally - gnss1ModulePortAssignment: 2 - # GNSS module sync assignment: - # 0 Module is disabled - # 1 Synchronization is done using SYNC_IN_A pin - # 2 Synchronization is done using SYNC_IN_B pin - # 3 Synchronization is done using SYNC_IN_C pin - # 4 Synchronization is done using SYNC_IN_D pin - # 5 Synchronization is internal - # 6 Synchronization is done using SYNC_OUT_A pin - # 7 Synchronization is done using SYNC_OUT_B pin - gnss1ModuleSyncAssignment: 0 - # RTCM input port assignment for Ellipse-N DGPS (see gnss1ModulePortAssignment for values) - rtcmPortAssignment: 255 - # Odometer module pin assignment - # 0 Odometer is disabled - # 1 Odometer connected only to ODO_A (unidirectional). - # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer. - odometerPinAssignment: 0 - - - -magnetometer: - # Magnetometer model ID - # 201 Should be used in most applications - # 202 Should be used in disturbed magnetic environment - magnetometerModel: 201 - # Magnetometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - magnetometerRejectMode: 1 - - # Theses parameters are only used for a calibration run - calibration: - # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. - # This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - # To work correctly, the device should be rotated through at least a full circle. - # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy - mode: 2 - - # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. - # Motion during calibration is then limited to slow rotations. - # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. - # It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration. - # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high. - bandwidth: 2 - - -# GNSS configuration -# Note: Secondary level arms should only be considered in case of dual antenna GNSS receiver. It can be left to 0 otherwise. -gnss: - # Gnss Model Id - # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS - # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU - # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. - # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. - # 107 Used on Ellipse-D by default - gnss_model_id: 102 - - #GNSS primary antenna lever arm in IMU X axis (m) - primaryLeverArmX: 0 - #GNSS primary antenna lever arm in IMU Y axis (m) - primaryLeverArmY: 0 - #GNSS primary antenna lever arm in IMU Z axis (m) - primaryLeverArmZ: 0 - #GNSS primary antenna precise. Set to true if the primary lever arm has been accurately entered and doesn't need online re-estimation. - primaryLeverPrecise: true - - #GNSS secondary antenna lever arm in IMU X axis (m) - secondaryLeverArmX: 0 - #GNSS secondary antenna lever arm in IMU Y axis (m) - secondaryLeverArmY: 0 - #GNSS secondary antenna lever arm in IMU Z axis (m) - secondaryLeverArmZ: 0 - - # Secondary antenna operating mode. - # 1 The GNSS will be used in single antenna mode only and the secondary lever arm is not used. - # 2 [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. - # 3 The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. - # 4 The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. - secondaryLeverMode: 1 - - # Rejection mode for position - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - posRejectMode: 1 - # Rejection mode for velocity (see posRejectMode values) - velRejectMode: 1 - # Rejection mode for true heading (see posRejectMode values) - hdtRejectMode: 1 - -# Odometer configuration -odom: - # Odometer's gain Pulses/m - gain: 4800 - # User gain average error (%) - gain_error: 0.1 - # Odometer's direction - # 0: positive - # 1: negative - direction: 0 - - # Odometer lever arm in IMU X axis (m) - leverArmX: 0 - # Odometer lever arm in IMU Y axis (m) - leverArmY: 0 - # Odometer lever arm in IMU Z axis (m) - leverArmZ: 0 - - # Odometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - rejectMode: 1 - -# ToDo: event & CAN configuration - -############################### Output configuration ############################### -# 0 Output is disabled -# 1 Output is generated at 200Hz -# 2 Output is generated at 100Hz -# 4 Output is generated at 50Hz -# 8 Output is generated at 25Hz -# 10 Output is generated at 20Hz -# 20 Output is generated at 10Hz -# 40 Output is generated at 5Hz -# 200 Output is generated at 1Hz -# 10000 Pulse Per Second. Same mode as above. -# 10001 Output sent when a new data is available. -# 10002 Output is generated when a new virtual odometer event occurs -# 10003 Output is generated on a Sync In A event -# 10004 Output is generated on a Sync In B event -# 10005 Output is generated on a Sync In C event -# 10006 Output is generated on a Sync In D event -output: - # Time Reference - # 0 No external time reference is used. Internal clock is used instead. - # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin. - # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment) - timeReference: 0 - ros_standard: false - - # Status general, clock, com aiding, solution, heave - log_status: 8 - # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 - # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 8 - # Includes the 4 quaternions values - log_ekf_quat: 0 - # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 8 - # Heave, surge and sway and accelerations on each axis for up to 4 points - log_ship_motion: 8 - # Provides UTC time reference - log_utc_time: 8 - # Magnetic data with associated accelerometer on each axis - log_mag: 8 - # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 - # GPS velocities from primary or secondary GPS receiver - log_gps1_vel: 10001 - # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 10001 - # GPS true heading from dual antenna system - log_gps1_hdt: 10001 - # GPS 1 raw data for post processing. - log_gps1_raw: 10001 - # Provides odometer velocity - log_odo_vel: 0 - # Event A/B/C/D Event markers sent when events are detected on a sync in pin - log_event_a: 0 - log_event_b: 0 - log_event_c: 0 - log_event_d: 0 - # Air data - log_air_data: 8 - # Short IMU data - log_imu_short: 0 - - # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run) - frequency: 0 - diff --git a/sbg_ros_driver/config/example/ellipse_N_default.yaml b/sbg_ros_driver/config/example/ellipse_N_default.yaml deleted file mode 100644 index 9be3bca..0000000 --- a/sbg_ros_driver/config/example/ellipse_N_default.yaml +++ /dev/null @@ -1,273 +0,0 @@ -# Configuration file for SBG Ellipse -# YAML - -# Ellipse-A - Magnetic-based -# Ellipse-E - External GNSS -# Ellipse-N - GNSS-based -# Ellipse-D - Dual-antenna GNSS - -# Configuration of the device with ROS. -confWithRos: true - -uartConf: - # Port Name - portName: "/dev/ttyUSB0" - - # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 - - # Port Id - # 0 PORT_A: Main communication interface. Full duplex. - # 1 PORT_B: Auxiliary input interface for RTCM - # 2 PORT_C: Auxiliary communication interface. Full duplex. - # 3 PORT_D: Auxiliary input interface - # 4 PORT_E: Auxiliary input/output interface - portID: 0 - -# Sensor Parameters -sensorParameters: - # Initial latitude (°) - initLat: 48.419727 - # Initial longitude (°) - initLong: -4.472119 - # Initial altitude (above WGS84 ellipsoid) (m) - initAlt: 100.0 - # Year at startup - year: 2018 - # month in year at startup - month: 03 - # day in month at startup - day: 10 - - # Montion profile ID - # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply - # 2 AUTOMOTIVE Dedicated to car applications - # 3 MARINE Used in marine and underwater applications - # 4 AIRPLANE For fixed wings aircraft - # 5 HELICOPTER For rotary wing aircraft - motionProfile: 1 - -# IMU_ALIGNMENT_LEVER_ARM -imuAlignementLeverArm: - # IMU X axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionX: 0 - # IMU Y axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionY: 3 - # Residual roll error after axis alignment rad - misRoll: 0 - # Residual pitch error after axis alignment rad - misPitch: 0 - # Residual yaw error after axis alignment rad - misYaw: 0 - # X Primary lever arm in IMU X axis (once IMU alignment is applied) m - leverArmX: 0 - # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m - leverArmY: 0 - # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m - leverArmZ: 0 - -# AIDING_ASSIGNMENT -# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version. -# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only -# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users. -aidingAssignment: - # GNSS module port assignment: - # 255 Module is disabled - # 1 Module connected on PORT_B - # 2 Module connected on PORT_C - # 3 Module connected on PORT_D - # 5 Module is connected internally - gnss1ModulePortAssignment: 255 - # GNSS module sync assignment: - # 0 Module is disabled - # 1 Synchronization is done using SYNC_IN_A pin - # 2 Synchronization is done using SYNC_IN_B pin - # 3 Synchronization is done using SYNC_IN_C pin - # 4 Synchronization is done using SYNC_IN_D pin - # 5 Synchronization is internal - # 6 Synchronization is done using SYNC_OUT_A pin - # 7 Synchronization is done using SYNC_OUT_B pin - gnss1ModuleSyncAssignment: 0 - # RTCM input port assignment for Ellipse-N DGPS - rtcmPortAssignment: 255 - # Odometer module pin assignment - # 0 Odometer is disabled - # 1 Odometer connected only to ODO_A (unidirectional). - # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer. - odometerPinAssignment: 0 - -magnetometer: - # Magnetometer model ID - # 201 Should be used in most applications - # 202 Should be used in disturbed magnetic environment - magnetometerModel: 201 - # Magnetometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - magnetometerRejectMode: 1 - - # Theses parameters are only used for a calibration run - calibration: - # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. - # This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - # To work correctly, the device should be rotated through at least a full circle. - # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy - mode: 2 - - # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. - # Motion during calibration is then limited to slow rotations. - # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. - # It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration. - # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high. - bandwidth: 2 - - -# GNSS configuration -# Note: Secondary level arms should only be considered in case of dual antenna GNSS receiver. It can be left to 0 otherwise. -gnss: - # Gnss Model Id - # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS - # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU - # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. - # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. - # 107 Used on Ellipse-D by default - gnss_model_id: 101 - - #GNSS primary antenna lever arm in IMU X axis (m) - primaryLeverArmX: 0 - #GNSS primary antenna lever arm in IMU Y axis (m) - primaryLeverArmY: 0 - #GNSS primary antenna lever arm in IMU Z axis (m) - primaryLeverArmZ: 0 - #GNSS primary antenna precise. Set to true if the primary lever arm has been accurately entered and doesn't need online re-estimation. - primaryLeverPrecise: true - - #GNSS secondary antenna lever arm in IMU X axis (m) - secondaryLeverArmX: 0 - #GNSS secondary antenna lever arm in IMU Y axis (m) - secondaryLeverArmY: 0 - #GNSS secondary antenna lever arm in IMU Z axis (m) - secondaryLeverArmZ: 0 - - # Secondary antenna operating mode. - # 1 The GNSS will be used in single antenna mode only and the secondary lever arm is not used. - # 2 [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. - # 3 The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. - # 4 The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. - secondaryLeverMode: 1 - - # Rejection mode for position - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - posRejectMode: 1 - # Rejection mode for velocity (see posRejectMode values) - velRejectMode: 1 - # Rejection mode for true heading (see posRejectMode values) - hdtRejectMode: 1 - -# Odometer configuration -odom: - # Odometer's gain Pulses/m - gain: 4800 - # User gain average error (%) - gain_error: 0.1 - # Odometer's direction - # 0: positive - # 1: negative - direction: 0 - - # Odometer lever arm in IMU X axis (m) - leverArmX: 0 - # Odometer lever arm in IMU Y axis (m) - leverArmY: 0 - # Odometer lever arm in IMU Z axis (m) - leverArmZ: 0 - - # Odometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - rejectMode: 1 - -# ToDo: event & CAN configuration - -############################### Output configuration ############################### -# 0 Output is disabled -# 1 Output is generated at 200Hz -# 2 Output is generated at 100Hz -# 4 Output is generated at 50Hz -# 8 Output is generated at 25Hz -# 10 Output is generated at 20Hz -# 20 Output is generated at 10Hz -# 40 Output is generated at 5Hz -# 200 Output is generated at 1Hz -# 10000 Pulse Per Second. Same mode as above. -# 10001 Output sent when a new data is available. -# 10002 Output is generated when a new virtual odometer event occurs -# 10003 Output is generated on a Sync In A event -# 10004 Output is generated on a Sync In B event -# 10005 Output is generated on a Sync In C event -# 10006 Output is generated on a Sync In D event -output: - # Time Reference - # 0 No external time reference is used. Internal clock is used instead. - # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin. - # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment) - timeReference: 0 - ros_standard: true - - # Status general, clock, com aiding, solution, heave - log_status: 8 - # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 - # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 0 - # Includes the 4 quaternions values - log_ekf_quat: 8 - # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 - # Heave, surge and sway and accelerations on each axis for up to 4 points - log_ship_motion: 0 - # Provides UTC time reference - log_utc_time: 8 - # Magnetic data with associated accelerometer on each axis - log_mag: 0 - # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 - # GPS velocities from primary or secondary GPS receiver - log_gps1_vel: 10001 - # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 10001 - # GPS true heading from dual antenna system - log_gps1_hdt: 10001 - # GPS 1 raw data for post processing. - log_gps1_raw: 10001 - # Provides odometer velocity - log_odo_vel: 0 - # Event A/B/C/D Event markers sent when events are detected on a sync in pin - log_event_a: 0 - log_event_b: 0 - log_event_c: 0 - log_event_d: 0 - # Air data - log_air_data: 0 - # Short IMU data - log_imu_short: 8 - - # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run) - frequency: 0 \ No newline at end of file diff --git a/sbg_ros_driver/config/sbg_device_uart_default.yaml b/sbg_ros_driver/config/sbg_device_uart_default.yaml deleted file mode 100644 index 3dc3c1f..0000000 --- a/sbg_ros_driver/config/sbg_device_uart_default.yaml +++ /dev/null @@ -1,268 +0,0 @@ -# Configuration file for SBG device through an Uart interface. - -# Configuration of the device with ROS. -confWithRos: false - -# Uart configuration -uartConf: - # Port Name - portName: "/dev/sbg" - - # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 - - # Port Id - # 0 PORT_A: Main communication interface. Full duplex. - # 1 PORT_B: Auxiliary input interface for RTCM - # 2 PORT_C: Auxiliary communication interface. Full duplex. - # 3 PORT_D: Auxiliary input interface - # 4 PORT_E: Auxiliary input/output interface - portID: 0 - -# Sensor Parameters -sensorParameters: - # Initial latitude (°) - initLat: 48.419727 - # Initial longitude (°) - initLong: -4.472119 - # Initial altitude (above WGS84 ellipsoid) (m) - initAlt: 100.0 - # Year at startup - year: 2018 - # month in year at startup - month: 03 - # day in month at startup - day: 10 - - # Montion profile ID - # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply - # 2 AUTOMOTIVE Dedicated to car applications - # 3 MARINE Used in marine and underwater applications - # 4 AIRPLANE For fixed wings aircraft - # 5 HELICOPTER For rotary wing aircraft - motionProfile: 1 - -# IMU_ALIGNMENT_LEVER_ARM -imuAlignementLeverArm: - # IMU X axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionX: 0 - # IMU Y axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionY: 3 - # Residual roll error after axis alignment rad - misRoll: 0 - # Residual pitch error after axis alignment rad - misPitch: 0 - # Residual yaw error after axis alignment rad - misYaw: 0 - # X Primary lever arm in IMU X axis (once IMU alignment is applied) m - leverArmX: 0 - # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m - leverArmY: 0 - # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m - leverArmZ: 0 - -# AIDING_ASSIGNMENT -# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version. -# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only -# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users. -aidingAssignment: - # GNSS module port assignment: - # 255 Module is disabled - # 1 Module connected on PORT_B - # 2 Module connected on PORT_C - # 3 Module connected on PORT_D - # 5 Module is connected internally - gnss1ModulePortAssignment: 255 - # GNSS module sync assignment: - # 0 Module is disabled - # 1 Synchronization is done using SYNC_IN_A pin - # 2 Synchronization is done using SYNC_IN_B pin - # 3 Synchronization is done using SYNC_IN_C pin - # 4 Synchronization is done using SYNC_IN_D pin - # 5 Synchronization is internal - # 6 Synchronization is done using SYNC_OUT_A pin - # 7 Synchronization is done using SYNC_OUT_B pin - gnss1ModuleSyncAssignment: 0 - # RTCM input port assignment for Ellipse-N DGPS - rtcmPortAssignment: 255 - # Odometer module pin assignment - # 0 Odometer is disabled - # 1 Odometer connected only to ODO_A (unidirectional). - # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer. - odometerPinAssignment: 0 - -magnetometer: - # Magnetometer model ID - # 201 Should be used in most applications - # 202 Should be used in disturbed magnetic environment - magnetometerModel: 201 - # Magnetometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - magnetometerRejectMode: 1 - - # Theses parameters are only used for a calibration run - calibration: - # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. - # This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - # To work correctly, the device should be rotated through at least a full circle. - # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy - mode: 2 - - # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. - # Motion during calibration is then limited to slow rotations. - # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. - # It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration. - # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high. - bandwidth: 2 - - -# GNSS configuration -# Note: Secondary level arms should only be considered in case of dual antenna GNSS receiver. It can be left to 0 otherwise. -gnss: - # Gnss Model Id - # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS - # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU - # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. - # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. - # 107 Used on Ellipse-D by default - gnss_model_id: 101 - - #GNSS primary antenna lever arm in IMU X axis (m) - primaryLeverArmX: 0 - #GNSS primary antenna lever arm in IMU Y axis (m) - primaryLeverArmY: 0 - #GNSS primary antenna lever arm in IMU Z axis (m) - primaryLeverArmZ: 0 - #GNSS primary antenna precise. Set to true if the primary lever arm has been accurately entered and doesn't need online re-estimation. - primaryLeverPrecise: true - - #GNSS secondary antenna lever arm in IMU X axis (m) - secondaryLeverArmX: 0 - #GNSS secondary antenna lever arm in IMU Y axis (m) - secondaryLeverArmY: 0 - #GNSS secondary antenna lever arm in IMU Z axis (m) - secondaryLeverArmZ: 0 - - # Secondary antenna operating mode. - # 1 The GNSS will be used in single antenna mode only and the secondary lever arm is not used. - # 2 [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. - # 3 The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. - # 4 The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. - secondaryLeverMode: 1 - - # Rejection mode for position - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - posRejectMode: 1 - # Rejection mode for velocity (see posRejectMode values) - velRejectMode: 1 - # Rejection mode for true heading (see posRejectMode values) - hdtRejectMode: 1 - -# Odometer configuration -odom: - # Odometer's gain Pulses/m - gain: 4800 - # User gain average error (%) - gain_error: 0.1 - # Odometer's direction - # 0: positive - # 1: negative - direction: 0 - - # Odometer lever arm in IMU X axis (m) - leverArmX: 0 - # Odometer lever arm in IMU Y axis (m) - leverArmY: 0 - # Odometer lever arm in IMU Z axis (m) - leverArmZ: 0 - - # Odometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - rejectMode: 1 - -# ToDo: event & CAN configuration - -############################### Output configuration ############################### -# 0 Output is disabled -# 1 Output is generated at 200Hz -# 2 Output is generated at 100Hz -# 4 Output is generated at 50Hz -# 8 Output is generated at 25Hz -# 10 Output is generated at 20Hz -# 20 Output is generated at 10Hz -# 40 Output is generated at 5Hz -# 200 Output is generated at 1Hz -# 10000 Pulse Per Second. Same mode as above. -# 10001 Output sent when a new data is available. -# 10002 Output is generated when a new virtual odometer event occurs -# 10003 Output is generated on a Sync In A event -# 10004 Output is generated on a Sync In B event -# 10005 Output is generated on a Sync In C event -# 10006 Output is generated on a Sync In D event -output: - # Time Reference - # 0 No external time reference is used. Internal clock is used instead. - # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin. - # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment) - timeReference: 0 - ros_standard: true - - # Status general, clock, com aiding, solution, heave - log_status: 200 - # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 - # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 0 - # Includes the 4 quaternions values - log_ekf_quat: 8 - # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 - # Heave, surge and sway and accelerations on each axis for up to 4 points - log_ship_motion: 0 - # Provides UTC time reference - log_utc_time: 200 - # Magnetic data with associated accelerometer on each axis - log_mag: 0 - # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 - # GPS velocities from primary or secondary GPS receiver - log_gps1_vel: 0 - # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 0 - # GPS true heading from dual antenna system - log_gps1_hdt: 0 - # GPS 1 raw data for post processing. - log_gps1_raw: 0 - # Provides odometer velocity - log_odo_vel: 0 - # Event A/B/C/D Event markers sent when events are detected on a sync in pin - log_event_a: 0 - log_event_b: 0 - log_event_c: 0 - log_event_d: 0 - # Air data - log_air_data: 0 - # Short IMU data - log_imu_short: 8 - - # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run) - frequency: 0 \ No newline at end of file diff --git a/sbg_ros_driver/config/sbg_device_udp_default.yaml b/sbg_ros_driver/config/sbg_device_udp_default.yaml deleted file mode 100644 index 6a6e18e..0000000 --- a/sbg_ros_driver/config/sbg_device_udp_default.yaml +++ /dev/null @@ -1,260 +0,0 @@ -# Configuration file for SBG device through an Udp interface. - -# Configuration of the device with ROS. -confWithRos: false - -# Udp configuration -ipConf: - ipAddress: "0.0.0.0" # Ip address of the device. - out_port: 1234 # Output port of the device. - in_port: 5678 # Inport of the device. - -# Sensor Parameters -sensorParameters: - # Initial latitude (°) - initLat: 48.419727 - # Initial longitude (°) - initLong: -4.472119 - # Initial altitude (above WGS84 ellipsoid) (m) - initAlt: 100.0 - # Year at startup - year: 2018 - # month in year at startup - month: 03 - # day in month at startup - day: 10 - - # Montion profile ID - # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply - # 2 AUTOMOTIVE Dedicated to car applications - # 3 MARINE Used in marine and underwater applications - # 4 AIRPLANE For fixed wings aircraft - # 5 HELICOPTER For rotary wing aircraft - motionProfile: 1 - -# IMU_ALIGNMENT_LEVER_ARM -imuAlignementLeverArm: - # IMU X axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionX: 0 - # IMU Y axis direction in vehicle frame - # 0 ALIGNMENT_FORWARD IMU Axis is turned in vehicle's forward direction - # 1 ALIGNMENT_BACKWARD IMU Axis is turned in vehicle's backward direction - # 2 ALIGNMENT_LEFT IMU Axis is turned in vehicle's left direction - # 3 ALIGNMENT_RIGHT IMU Axis is turned in vehicle's right direction - # 4 ALIGNMENT_UP IMU Axis is turned in vehicle's up direction - # 5 ALIGNMENT_DOWN IMU Axis is turned in vehicle's down direction - axisDirectionY: 3 - # Residual roll error after axis alignment rad - misRoll: 0 - # Residual pitch error after axis alignment rad - misPitch: 0 - # Residual yaw error after axis alignment rad - misYaw: 0 - # X Primary lever arm in IMU X axis (once IMU alignment is applied) m - leverArmX: 0 - # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m - leverArmY: 0 - # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m - leverArmZ: 0 - -# AIDING_ASSIGNMENT -# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version. -# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only -# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users. -aidingAssignment: - # GNSS module port assignment: - # 255 Module is disabled - # 1 Module connected on PORT_B - # 2 Module connected on PORT_C - # 3 Module connected on PORT_D - # 5 Module is connected internally - gnss1ModulePortAssignment: 255 - # GNSS module sync assignment: - # 0 Module is disabled - # 1 Synchronization is done using SYNC_IN_A pin - # 2 Synchronization is done using SYNC_IN_B pin - # 3 Synchronization is done using SYNC_IN_C pin - # 4 Synchronization is done using SYNC_IN_D pin - # 5 Synchronization is internal - # 6 Synchronization is done using SYNC_OUT_A pin - # 7 Synchronization is done using SYNC_OUT_B pin - gnss1ModuleSyncAssignment: 0 - # RTCM input port assignment for Ellipse-N DGPS - rtcmPortAssignment: 255 - # Odometer module pin assignment - # 0 Odometer is disabled - # 1 Odometer connected only to ODO_A (unidirectional). - # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer. - odometerPinAssignment: 0 - - - -magnetometer: - # Magnetometer model ID - # 201 Should be used in most applications - # 202 Should be used in disturbed magnetic environment - magnetometerModel: 201 - # Magnetometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - magnetometerRejectMode: 1 - - # Theses parameters are only used for a calibration run - calibration: - # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. - # This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - # To work correctly, the device should be rotated through at least a full circle. - # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy - mode: 2 - - # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. - # Motion during calibration is then limited to slow rotations. - # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. - # It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration. - # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high. - bandwidth: 2 - - -# GNSS configuration -# Note: Secondary level arms should only be considered in case of dual antenna GNSS receiver. It can be left to 0 otherwise. -gnss: - # Gnss Model Id - # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS - # 102 Default mode for Ellipse-E connection to external GNSS - # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU - # 104 Used on Ellipse-E to setup a connection to ublox in read only mode. - # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode. - # 107 Used on Ellipse-D by default - gnss_model_id: 102 - - #GNSS primary antenna lever arm in IMU X axis (m) - primaryLeverArmX: 0 - #GNSS primary antenna lever arm in IMU Y axis (m) - primaryLeverArmY: 0 - #GNSS primary antenna lever arm in IMU Z axis (m) - primaryLeverArmZ: 0 - #GNSS primary antenna precise. Set to true if the primary lever arm has been accurately entered and doesn't need online re-estimation. - primaryLeverPrecise: true - - #GNSS secondary antenna lever arm in IMU X axis (m) - secondaryLeverArmX: 0 - #GNSS secondary antenna lever arm in IMU Y axis (m) - secondaryLeverArmY: 0 - #GNSS secondary antenna lever arm in IMU Z axis (m) - secondaryLeverArmZ: 0 - - # Secondary antenna operating mode. - # 1 The GNSS will be used in single antenna mode only and the secondary lever arm is not used. - # 2 [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. - # 3 The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. - # 4 The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. - secondaryLeverMode: 1 - - # Rejection mode for position - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - posRejectMode: 1 - # Rejection mode for velocity (see posRejectMode values) - velRejectMode: 1 - # Rejection mode for true heading (see posRejectMode values) - hdtRejectMode: 1 - -# Odometer configuration -odom: - # Odometer's gain Pulses/m - gain: 4800 - # User gain average error (%) - gain_error: 0.1 - # Odometer's direction - # 0: positive - # 1: negative - direction: 0 - - # Odometer lever arm in IMU X axis (m) - leverArmX: 0 - # Odometer lever arm in IMU Y axis (m) - leverArmY: 0 - # Odometer lever arm in IMU Z axis (m) - leverArmZ: 0 - - # Odometer rejection mode - # 0 Measurement is not taken into account - # 1 Measurement is rejected if inconsistent with current estimate (depending on error model) - # 2 Measurement is always accepted - rejectMode: 1 - -# ToDo: event & CAN configuration - -############################### Output configuration ############################### -# 0 Output is disabled -# 1 Output is generated at 200Hz -# 2 Output is generated at 100Hz -# 4 Output is generated at 50Hz -# 8 Output is generated at 25Hz -# 10 Output is generated at 20Hz -# 20 Output is generated at 10Hz -# 40 Output is generated at 5Hz -# 200 Output is generated at 1Hz -# 10000 Pulse Per Second. Same mode as above. -# 10001 Output sent when a new data is available. -# 10002 Output is generated when a new virtual odometer event occurs -# 10003 Output is generated on a Sync In A event -# 10004 Output is generated on a Sync In B event -# 10005 Output is generated on a Sync In C event -# 10006 Output is generated on a Sync In D event -output: - # Time Reference - # 0 No external time reference is used. Internal clock is used instead. - # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin. - # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment) - timeReference: 0 - ros_standard: true - - # Status general, clock, com aiding, solution, heave - log_status: 200 - # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 - # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 0 - # Includes the 4 quaternions values - log_ekf_quat: 8 - # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 - # Heave, surge and sway and accelerations on each axis for up to 4 points - log_ship_motion: 0 - # Provides UTC time reference - log_utc_time: 200 - # Magnetic data with associated accelerometer on each axis - log_mag: 0 - # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 - # GPS velocities from primary or secondary GPS receiver - log_gps1_vel: 0 - # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 0 - # GPS true heading from dual antenna system - log_gps1_hdt: 0 - # GPS 1 raw data for post processing. - log_gps1_raw: 0 - # Provides odometer velocity - log_odo_vel: 0 - # Event A/B/C/D Event markers sent when events are detected on a sync in pin - log_event_a: 0 - log_event_b: 0 - log_event_c: 0 - log_event_d: 0 - # Air data - log_air_data: 0 - # Short IMU data - log_imu_short: 0 - - # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run) - frequency: 0 \ No newline at end of file diff --git a/sbg_ros_driver/docs/99-sbg_example.rules b/sbg_ros_driver/docs/99-sbg_example.rules deleted file mode 100644 index e2f5ef1..0000000 --- a/sbg_ros_driver/docs/99-sbg_example.rules +++ /dev/null @@ -1 +0,0 @@ -SUBSYSTEMS=="usb", ATTRS{product}=="USB-RS232 Cable", ATTRS{serial}=="FTUCFXCN", SYMLINK+="sbg", GROUP="dialout", MODE="0660" \ No newline at end of file diff --git a/sbg_ros_driver/docs/changeLog_sbgECom_v1.11.920-stable.pdf b/sbg_ros_driver/docs/changeLog_sbgECom_v1.11.920-stable.pdf deleted file mode 100644 index 8d90969..0000000 Binary files a/sbg_ros_driver/docs/changeLog_sbgECom_v1.11.920-stable.pdf and /dev/null differ diff --git a/sbg_ros_driver/docs/udev_example.png b/sbg_ros_driver/docs/udev_example.png deleted file mode 100644 index 5aad769..0000000 Binary files a/sbg_ros_driver/docs/udev_example.png and /dev/null differ diff --git a/sbg_ros_driver/external/sbgECom/CMakeLists.txt b/sbg_ros_driver/external/sbgECom/CMakeLists.txt deleted file mode 100644 index 0eb27a2..0000000 --- a/sbg_ros_driver/external/sbgECom/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.0) - -project(sbgECom) - -include_directories( - ${PROJECT_SOURCE_DIR}/src/ - ${PROJECT_SOURCE_DIR}/common/ -) - -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/) -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/) - -file(GLOB_RECURSE SRC ${PROJECT_SOURCE_DIR}/src/*.c) -file(GLOB_RECURSE COMMON_SRC ${PROJECT_SOURCE_DIR}/common/*.c) - -# -# Exclude platform specific files -# -if (NOT MSVC) - list(REMOVE_ITEM COMMON_SRC "${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialWin.c") -else () - list(REMOVE_ITEM COMMON_SRC "${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialUnix.c") -endif() - -add_library(sbgECom STATIC ${SRC} ${COMMON_SRC}) - -if (MSVC) - target_link_libraries(sbgECom Ws2_32) -endif() - diff --git a/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.c b/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.c deleted file mode 100644 index 3b5c5f6..0000000 --- a/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.c +++ /dev/null @@ -1,271 +0,0 @@ -#include "sbgCrc.h" - -//----------------------------------------------------------------------// -//- Static global CRC tables -// -//----------------------------------------------------------------------// - -/*!< CRC table used to compute a 16 bit CRC with the polynom 0x8408. */ -static const uint16_t crc16LookupTable[256] = { - 0x0000,0x1189,0x2312,0x329B,0x4624,0x57AD,0x6536,0x74BF,0x8C48,0x9DC1,0xAF5A,0xBED3,0xCA6C,0xDBE5,0xE97E,0xF8F7, - 0x1081,0x0108,0x3393,0x221A,0x56A5,0x472C,0x75B7,0x643E,0x9CC9,0x8D40,0xBFDB,0xAE52,0xDAED,0xCB64,0xF9FF,0xE876, - 0x2102,0x308B,0x0210,0x1399,0x6726,0x76AF,0x4434,0x55BD,0xAD4A,0xBCC3,0x8E58,0x9FD1,0xEB6E,0xFAE7,0xC87C,0xD9F5, - 0x3183,0x200A,0x1291,0x0318,0x77A7,0x662E,0x54B5,0x453C,0xBDCB,0xAC42,0x9ED9,0x8F50,0xFBEF,0xEA66,0xD8FD,0xC974, - 0x4204,0x538D,0x6116,0x709F,0x0420,0x15A9,0x2732,0x36BB,0xCE4C,0xDFC5,0xED5E,0xFCD7,0x8868,0x99E1,0xAB7A,0xBAF3, - 0x5285,0x430C,0x7197,0x601E,0x14A1,0x0528,0x37B3,0x263A,0xDECD,0xCF44,0xFDDF,0xEC56,0x98E9,0x8960,0xBBFB,0xAA72, - 0x6306,0x728F,0x4014,0x519D,0x2522,0x34AB,0x0630,0x17B9,0xEF4E,0xFEC7,0xCC5C,0xDDD5,0xA96A,0xB8E3,0x8A78,0x9BF1, - 0x7387,0x620E,0x5095,0x411C,0x35A3,0x242A,0x16B1,0x0738,0xFFCF,0xEE46,0xDCDD,0xCD54,0xB9EB,0xA862,0x9AF9,0x8B70, - 0x8408,0x9581,0xA71A,0xB693,0xC22C,0xD3A5,0xE13E,0xF0B7,0x0840,0x19C9,0x2B52,0x3ADB,0x4E64,0x5FED,0x6D76,0x7CFF, - 0x9489,0x8500,0xB79B,0xA612,0xD2AD,0xC324,0xF1BF,0xE036,0x18C1,0x0948,0x3BD3,0x2A5A,0x5EE5,0x4F6C,0x7DF7,0x6C7E, - 0xA50A,0xB483,0x8618,0x9791,0xE32E,0xF2A7,0xC03C,0xD1B5,0x2942,0x38CB,0x0A50,0x1BD9,0x6F66,0x7EEF,0x4C74,0x5DFD, - 0xB58B,0xA402,0x9699,0x8710,0xF3AF,0xE226,0xD0BD,0xC134,0x39C3,0x284A,0x1AD1,0x0B58,0x7FE7,0x6E6E,0x5CF5,0x4D7C, - 0xC60C,0xD785,0xE51E,0xF497,0x8028,0x91A1,0xA33A,0xB2B3,0x4A44,0x5BCD,0x6956,0x78DF,0x0C60,0x1DE9,0x2F72,0x3EFB, - 0xD68D,0xC704,0xF59F,0xE416,0x90A9,0x8120,0xB3BB,0xA232,0x5AC5,0x4B4C,0x79D7,0x685E,0x1CE1,0x0D68,0x3FF3,0x2E7A, - 0xE70E,0xF687,0xC41C,0xD595,0xA12A,0xB0A3,0x8238,0x93B1,0x6B46,0x7ACF,0x4854,0x59DD,0x2D62,0x3CEB,0x0E70,0x1FF9, - 0xF78F,0xE606,0xD49D,0xC514,0xB1AB,0xA022,0x92B9,0x8330,0x7BC7,0x6A4E,0x58D5,0x495C,0x3DE3,0x2C6A,0x1EF1,0x0F78}; - -/*!< CRC table used to compute an Ethernet 32 bit CRC using the normal polynom 0x04C11DB7. */ -static const uint32_t crc32EthernetTable[256] = -{ - 0x00000000, - 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, - 0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, - 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, - 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, - 0x5bd4b01b, 0x569796c2, 0x52568b75, 0x6a1936c8, 0x6ed82b7f, - 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, - 0x745e66cd, 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, - 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, 0xbe2b5b58, - 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, - 0xa4ad16ea, 0xa06c0b5d, 0xd4326d90, 0xd0f37027, 0xddb056fe, - 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, - 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, - 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, 0x34867077, 0x30476dc0, - 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, - 0x2ac12072, 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, - 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, 0x7897ab07, - 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, - 0x6211e6b5, 0x66d0fb02, 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, - 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, - 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, - 0xbb60adfc, 0xb6238b25, 0xb2e29692, 0x8aad2b2f, 0x8e6c3698, - 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, - 0x94ea7b2a, 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, - 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, 0xc6bcf05f, - 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, - 0xdc3abded, 0xd8fba05a, 0x690ce0ee, 0x6dcdfd59, 0x608edb80, - 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, - 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, - 0x58c1663d, 0x558240e4, 0x51435d53, 0x251d3b9e, 0x21dc2629, - 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, - 0x3b5a6b9b, 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, - 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, 0xf12f560e, - 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, - 0xeba91bbc, 0xef68060b, 0xd727bbb6, 0xd3e6a601, 0xdea580d8, - 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, - 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, - 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, 0x9b3660c6, 0x9ff77d71, - 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, - 0x857130c3, 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, - 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, 0x7b827d21, - 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, - 0x61043093, 0x65c52d24, 0x119b4be9, 0x155a565e, 0x18197087, - 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, - 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, - 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, 0xc5a92679, 0xc1683bce, - 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, - 0xdbee767c, 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, - 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, 0x89b8fd09, - 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, - 0x933eb0bb, 0x97ffad0c, 0xafb010b1, 0xab710d06, 0xa6322bdf, - 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 -}; - -//----------------------------------------------------------------------// -//- 32 bits Ethernet CRC -// -//----------------------------------------------------------------------// - -/*! - * Initialize the 32 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc32 object. - */ -SBG_COMMON_LIB_API void sbgCrc32Initialize(SbgCrc32 *pInstance) -{ - // - // Test input argument - // - assert(pInstance); - - *pInstance = 0xFFFFFFFF; -} - -/*! - * Compute a 32 bit CRC using an Ethernet polynome. - * Warning: the buffer size should be at least 4 bytes long. - * \param[in] pInstance Read only pointer on a valid Crc32 object. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. - */ -SBG_COMMON_LIB_API void sbgCrc32Update(SbgCrc32 *pInstance, const void *pData, size_t dataSize) -{ - const uint8_t *pBuffer = (const uint8_t*)pData; - uint32_t byte; - size_t i; - size_t dataSizeCorrected; - size_t numBytesLeft; - size_t index; - - // - // Test input arguments - // - assert(pInstance); - assert(pData); - - // - // Compute the data size that corresponds to complete uinht32 and how many bytes remains - // - dataSizeCorrected = dataSize & (~0x00000003); - numBytesLeft = dataSize & 0x03; - - // For each byte, update the CRC - // - for (i = 0; i < dataSizeCorrected; i++) - { - // - // We have to get index in reversed order per 4 bytes - // - index = i ^ 0x03; - - // - // Get the current byte value - // - byte = pBuffer[index]; - - // - // Update the CRC value - // - *pInstance = (*pInstance << 8) ^ crc32EthernetTable[((*pInstance >> 24) ^ byte) & 0xFF]; - } - - // - // Test how many bytes remains - // - for (i = 0; i < numBytesLeft; i++) - { - // - // We have to get index in reversed order per 4 bytes - // - index = (dataSizeCorrected-1) + (numBytesLeft - i); - - // - // Get the current byte value - // - byte = pBuffer[index]; - - // - // Update the CRC value - // - *pInstance = (*pInstance << 8) ^ crc32EthernetTable[((*pInstance >> 24) ^ byte) & 0xFF]; - } -} - -/*! - * Compute a 32 Bit CRC using an Ethernet polynome. - * Warning: the buffer size should be at least 4 bytes long. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. - * \return The computed CRC. - */ -SBG_COMMON_LIB_API uint32_t sbgCrc32Compute(const void *pData, size_t dataSize) -{ - SbgCrc32 crcInst; - - // - // Initialize the CRC system - // - sbgCrc32Initialize(&crcInst); - - // - // Compute the CRC - // - sbgCrc32Update(&crcInst, pData, dataSize); - - // - // Return it - // - return sbgCrc32Get(&crcInst); -} - -//----------------------------------------------------------------------// -//- CRC-16 operations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the 16 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc16 object. - */ -SBG_COMMON_LIB_API void sbgCrc16Initialize(SbgCrc16 *pInstance) -{ - // - // Test input argument - // - assert(pInstance); - - *pInstance = 0; -} - -/*! - * Compute a 16 bit CRC using an the polynome 0x8408. - * \param[in] pInstance Read only pointer on a valid Crc16 object. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer. - */ -SBG_COMMON_LIB_API void sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize) -{ - const uint8_t *pBuffer = (const uint8_t*)pData; - uint8_t index; - size_t i; - - // - // Test input arguments - // - assert(pInstance); - assert(pData); - - // - // For each byte in our buffer - // - for (i = 0; i < dataSize; i++) - { - // - // Update the current CRC - // - index = (pBuffer[i] ^ *pInstance) & 0xFF; - *pInstance = crc16LookupTable[index] ^ (*pInstance >> 8); - } -} - -/*! - * Compute a 32 Bit CRC using an the polynome 0x8408. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer. - * \return The computed CRC. - */ -SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize) -{ - SbgCrc16 crcInst; - - // - // Initialize the CRC system - // - sbgCrc16Initialize(&crcInst); - - // - // Compute the CRC - // - sbgCrc16Update(&crcInst, pData, dataSize); - - // - // Return it - // - return sbgCrc16Get(&crcInst); -} diff --git a/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.h b/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.h deleted file mode 100644 index 2013fa7..0000000 --- a/sbg_ros_driver/external/sbgECom/common/crc/sbgCrc.h +++ /dev/null @@ -1,124 +0,0 @@ -/*! - * \file sbgCrc.h - * \author SBG Systems (Raphael Siryani) - * \date 15 January 2013 - * - * \brief This file provides CRC-32 and CRC-16 methods. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_CRC_H -#define SBG_CRC_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include - -//----------------------------------------------------------------------// -//- Types definitions -// -//----------------------------------------------------------------------// - -/*!< Type used to compute a 32 bit Ethernet CRC. */ -typedef uint32_t SbgCrc32; - -/*!< Type used to compute a 16 bit CRC. */ -typedef uint16_t SbgCrc16; - -//----------------------------------------------------------------------// -//- 32 bits Ethernet CRC -// -//----------------------------------------------------------------------// - -/*! - * Initialize the 32 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc32 object. - */ -SBG_COMMON_LIB_API void sbgCrc32Initialize(SbgCrc32 *pInstance); - -/*! - * Compute a 32 bit CRC using an Ethernet polynome. - * Warning: the buffer size should be at least 4 bytes long. - * \param[in] pInstance Read only pointer on a valid Crc32 object. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. - */ -SBG_COMMON_LIB_API void sbgCrc32Update(SbgCrc32 *pInstance, const void *pData, size_t dataSize); - -/*! - * Returns the computed 32 bit CRC value. - * \param[in] pInstance Read only pointer on a valid Crc32 object. - * \return The computed CRC. - */ -SBG_INLINE uint32_t sbgCrc32Get(const SbgCrc32 *pInstance) -{ - return *pInstance; -} - -/*! - * Compute a 32 Bit CRC using an Ethernet polynome. - * Warning: the buffer size should be at least 4 bytes long. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. - * \return The computed CRC. - */ -SBG_COMMON_LIB_API uint32_t sbgCrc32Compute(const void *pData, size_t dataSize); - -//----------------------------------------------------------------------// -//- CRC-16 operations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the 16 bit CRC computation system. - * \param[in] pInstance Pointer on an allocated but non initialized Crc16 object. - */ -SBG_COMMON_LIB_API void sbgCrc16Initialize(SbgCrc16 *pInstance); - -/*! - * Compute a 16 bit CRC using an the polynome 0x8408. - * \param[in] pInstance Read only pointer on a valid Crc16 object. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer. - */ -SBG_COMMON_LIB_API void sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize); - -/*! - * Returns the computed 32 bit CRC value. - * \param[in] pInstance Read only pointer on a valid Crc16 object. - * \return The computed CRC. - */ -SBG_INLINE uint16_t sbgCrc16Get(const SbgCrc16 *pInstance) -{ - return *pInstance; -} - -/*! - * Compute a 32 Bit CRC using an the polynome 0x8408. - * \param[in] pData Read only pointer on the data buffer to compute CRC on. - * \param[in] dataSize Data size in bytes of the buffer. - * \return The computed CRC. - */ -SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_CRC_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.c b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.c deleted file mode 100644 index bc8fd23..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.c +++ /dev/null @@ -1,81 +0,0 @@ -/* sbgCommonLib headers */ -#include - -/* Local headers */ -#include "sbgInterface.h" - -//----------------------------------------------------------------------// -//- Private definitions -// -//----------------------------------------------------------------------// - -static const char *gInterfaceType[] = -{ - [SBG_IF_TYPE_UNKNOW] = "unknown", - [SBG_IF_TYPE_SERIAL] = "serial", - [SBG_IF_TYPE_ETH_UDP] = "eth UDP", - [SBG_IF_TYPE_ETH_TCP_IP] = "eth TCP", - [SBG_IF_TYPE_FILE] = "file" -}; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -void sbgInterfaceZeroInit(SbgInterface *pHandle) -{ - assert(pHandle); - - // - // Make sure the whole struct is zero init - // - memset(pHandle, 0x00, sizeof(*pHandle)); - - // - // Initialize all fields to default values - // - pHandle->handle = NULL; - pHandle->type = SBG_IF_TYPE_UNKNOW; - pHandle->name[0] = '\0'; - - pHandle->pWriteFunc = NULL; - pHandle->pReadFunc = NULL; - pHandle->pFlushFunc = NULL; - pHandle->pDelayFunc = NULL; -} - -const char *sbgInterfaceTypeGetAsString(const SbgInterface *pInterface) -{ - assert(pInterface); - - if (pInterface->type < SBG_ARRAY_SIZE(gInterfaceType)) - { - return gInterfaceType[pInterface->type]; - } - else - { - SBG_LOG_ERROR(SBG_INVALID_PARAMETER, "Unknown interface type: %u", pInterface->type); - return gInterfaceType[SBG_IF_TYPE_UNKNOW]; - } -} - -void sbgInterfaceNameSet(SbgInterface *pInterface, const char *pName) -{ - size_t nameLen; - - assert(pInterface); - assert(pName); - - // - // Only keep the end of the name that can fit in the interface name buffer - // - nameLen = strlen(pName); - - if (nameLen < SBG_ARRAY_SIZE(pInterface->name)) - { - strcpy(pInterface->name, pName); - } - else - { - strcpy(pInterface->name, pName+(nameLen-(SBG_ARRAY_SIZE(pInterface->name)-1))); - } -} diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.h b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.h deleted file mode 100644 index 92139b9..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterface.h +++ /dev/null @@ -1,317 +0,0 @@ -/*! - * \file sbgInterface.h - * \author SBG Systems (Raphael Siryani) - * \date 10 December 2012 - * - * \brief This file implements the base interface for all Serial and Ethernet ports. - * - * An interface is used to provide a common API for both serial and ethernet ports. - * An interface can be opened/closed and some data can be written or read from it. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_INTERFACE_H -#define SBG_INTERFACE_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -/* sbgCommonLib headers */ -#include - -//----------------------------------------------------------------------// -//- Constant definitions -// -//----------------------------------------------------------------------// - -#define SBG_IF_NAME_MAX_SIZE (48) /*!< Maximum size in bytes for the interface name string */ - -/*! - * Type values reserved for standard interface types. - */ -#define SBG_IF_TYPE_UNKNOW (0) /*!< The interface type is not defined. */ -#define SBG_IF_TYPE_SERIAL (1) /*!< The interface is a serial com port. */ -#define SBG_IF_TYPE_ETH_UDP (2) /*!< The interface is an UDP one. */ -#define SBG_IF_TYPE_ETH_TCP_IP (3) /*!< The interface is an TCP/IP one. */ -#define SBG_IF_TYPE_FILE (4) /*!< The interface is a file. */ -#define SBG_IF_TYPE_LAST_RESERVED (999) /*!< Last reserved value for standard types. */ - -//----------------------------------------------------------------------// -//- Predefinitions -// -//----------------------------------------------------------------------// - -/*! - * Interface structure pre-definition. - */ -typedef struct _SbgInterface SbgInterface; - -/*! - * Handle that stores the internal interface handle (ie Serial or Ethernet) - */ -typedef void* SbgInterfaceHandle; - -//----------------------------------------------------------------------// -//- Callbacks definitions -// -//----------------------------------------------------------------------// - -/*! - * Method to implement to write a buffer to an interface. - * - * This method should return an error only if all bytes were not written successfully. - * If you try to write zero byte, the method shouldn't return any error. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write (can be zero). - * \return SBG_NO_ERROR if exactly bytesToWrite have been written successfully. - */ -typedef SbgErrorCode (*SbgInterfaceWriteFunc)(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite); - -/*! - * Method to implement to read data from an interface. - * - * This method returns an error only if there is a 'low level' error on the interface. - * If no byte is read at all or less bytes than bytesToRead, this method returns SBG_NO_ERROR. - * You have to check pReadBytes field to know the number of bytes actually read. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Returns the number of bytes actually read (can be zero and up to bytesToRead). - * \param[in] bytesToRead Maximum number of bytes to try to read on the interface. - * \return SBG_NO_ERROR if zero or some bytes have been read successfully. - */ -typedef SbgErrorCode (*SbgInterfaceReadFunc)(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); - -/*! - * Make an interface flush all pending input or output data. - * \param[in] pHandle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if successful. - */ -typedef SbgErrorCode (*SbgInterfaceFlushFunc)(SbgInterface *pHandle); - -/*! - * Compute and return the delay needed by the interface to transmit / receive X number of bytes. - * - * This method isn't applicable to all interfaces and as such should return a default delay of zero. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] numBytes The number of bytes to transmit / receive to evaluate the needed delay. - * \return The expected delay in us needed to transmit / receive the specified number of bytes or 0 if not applicable. - */ -typedef uint32_t (*SbgInterfaceGetDelayFunc)(SbgInterface *pHandle, size_t numBytes); - -//----------------------------------------------------------------------// -//- Structures definitions -// -//----------------------------------------------------------------------// - -/*! - * Interface definition that stores methods used to communicate on the interface. - * - * The interface class is designed to allow custom user implementations. The type member stores - * a type identifier allowing the identification of the underlying type, including custom - * implementations. Standard interfaces provided by this library use types from 1 up to - * and including SBG_IF_TYPE_LAST_RESERVED. Greater values are intended to identify custom - * types that are normally specific to the project using this library. The value 0 identifies - * an unknown interface type, usually indicating that the interface was not correctly initialized. - */ -struct _SbgInterface -{ - SbgInterfaceHandle handle; /*!< Internal interface handle used to access the media. */ - uint32_t type; /*!< Opaque interface type. */ - char name[SBG_IF_NAME_MAX_SIZE]; /*!< The interface name as passed during the creation */ - - SbgInterfaceWriteFunc pWriteFunc; /*!< Mandatory method used to write some data to this interface. */ - SbgInterfaceReadFunc pReadFunc; /*!< Mandatory method used to read some data to this interface. */ - SbgInterfaceFlushFunc pFlushFunc; /*!< Optional method used to make this interface flush all pending data. */ - SbgInterfaceGetDelayFunc pDelayFunc; /*!< Optional method used to compute an expected delay to transmit/receive X bytes */ -}; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Initialize an interface instance to zero. - * - * \param[in] pHandle Handle on an allocated interface to initialize to zero. - */ -SBG_COMMON_LIB_API void sbgInterfaceZeroInit(SbgInterface *pHandle); - -/*! - * Write some data to an interface. - * - * This method should return an error only if all bytes were not written successfully. - * If you try to write zero byte, the method shouldn't return any error. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write (can be zero). - * \return SBG_NO_ERROR if exactly bytesToWrite have been written successfully. - */ -SBG_INLINE SbgErrorCode sbgInterfaceWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - assert(pHandle); - assert(pBuffer); - assert(pHandle->pWriteFunc); - - // - // Call the correct write method according to the interface - // - return pHandle->pWriteFunc(pHandle, pBuffer, bytesToWrite); -} - -/*! - * Try to read some data from an interface. - * - * This method returns an error only if there is a 'low level' error on the interface. - * If no byte is read at all or less bytes than bytesToRead, this method returns SBG_NO_ERROR. - * You have to check pReadBytes field to know the number of bytes actually read. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Returns the number of bytes actually read (can be zero and up to bytesToRead). - * \param[in] bytesToRead Maximum number of bytes to try to read on the interface. - * \return SBG_NO_ERROR if zero or some bytes have been read successfully. - */ -SBG_INLINE SbgErrorCode sbgInterfaceRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - assert(pHandle); - assert(pBuffer); - assert(pReadBytes); - assert(pHandle->pReadFunc); - - // - // Call the correct read method according to the interface - // - return pHandle->pReadFunc(pHandle, pBuffer, pReadBytes, bytesToRead); -} - -/*! - * Make an interface flush all pending input or output data. - * - * This method isn't applicable to all interfaces and as such if no implementation is provided it does nothing. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if successful. - */ -SBG_INLINE SbgErrorCode sbgInterfaceFlush(SbgInterface *pHandle) -{ - SbgErrorCode errorCode; - - assert(pHandle); - - // - // The Flush method is optional so check if it has been defined. - // - if (pHandle->pFlushFunc) - { - // - // Call the correct flush method according to the interface - // - errorCode = pHandle->pFlushFunc(pHandle); - } - else - { - errorCode = SBG_NO_ERROR; - } - - return errorCode; -} - -/*! - * Compute and return the delay needed by the interface to transmit / receive X number of bytes. - * - * This method isn't applicable to all interfaces and as such if no implementation is provided a default delay of zero. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] numBytes The number of bytes to transmit / receive to evaluate the needed delay. - * \return The expected delay in us needed to transmit / receive the specified number of bytes or 0 if not applicable. - */ -SBG_INLINE uint32_t sbgInterfaceGetDelay(SbgInterface *pHandle, size_t numBytes) -{ - assert(pHandle); - - // - // The get delay method is optional so check if it has been defined. - // - if (pHandle->pDelayFunc) - { - // - // Call the correct flush method according to the interface - // - return pHandle->pDelayFunc(pHandle, numBytes); - } - - return 0; -} - -/*! - * Returns the interface type. - * - * \param[in] pInterface Interface instance - * \return The interface type. - */ -SBG_INLINE uint32_t sbgInterfaceTypeGet(const SbgInterface *pInterface) -{ - assert(pInterface); - - return pInterface->type; -} - -/*! - * Returns the interface as string. - * - * \param[in] pInterface Interface instance - * \return The interface type. - */ -SBG_COMMON_LIB_API const char *sbgInterfaceTypeGetAsString(const SbgInterface *pInterface); - -/*! - * Returns the interface name string. - * - * \param[in] pInterface Interface instance - * \return The interface name as a NULL terminated C string. - */ -SBG_INLINE const char *sbgInterfaceNameGet(const SbgInterface *pInterface) -{ - assert(pInterface); - - return pInterface->name; -} - -/*! - * Define the interface name as a NULL terminated C string. - * - * This method make sure that the provided string will always fit within - * the allocated name buffer. - * - * If the interface name you would like to set is too long, only the end - * of the string will be kept. - * - * \param[in] pInterface Interface instance - * \param[in] pName The interface name to set as a NULL terminated C string - */ -SBG_COMMON_LIB_API void sbgInterfaceNameSet(SbgInterface *pInterface, const char *pName); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_INTERFACE_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.c b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.c deleted file mode 100644 index fc6947b..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.c +++ /dev/null @@ -1,378 +0,0 @@ -#include "sbgInterfaceFile.h" - -//----------------------------------------------------------------------// -//- Internal methods declarations -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Open a file as an interface for read only operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] filePath File path to open. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileOpen(SbgInterface *pHandle, const char *filePath) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - FILE *pInputFile; - - assert(pHandle); - assert(filePath); - - // - // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions - // - sbgInterfaceZeroInit(pHandle); - - // - // Try to open the file - // - pInputFile = fopen(filePath, "rb"); - - // - // Test if the input file has been opened - // - if (pInputFile) - { - // - // Define base interface members - // - pHandle->handle = pInputFile; - pHandle->type = SBG_IF_TYPE_FILE; - - // - // Define the interface name - // - sbgInterfaceNameSet(pHandle, filePath); - - // - // Define all overloaded members - // - pHandle->pReadFunc = sbgInterfaceFileRead; - pHandle->pWriteFunc = sbgInterfaceFileWriteFake; - pHandle->pFlushFunc = sbgInterfaceFileFlush; - } - else - { - // - // Unable to open the input file - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; -} - -/*! - * Open a file as an interface for write only operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] filePath File path to open. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteOpen(SbgInterface *pHandle, const char *filePath) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - FILE *pInputFile; - - // - // Test input arguments - // - assert(pHandle); - assert(filePath); - - // - // Try to open the file - // - pInputFile = fopen(filePath, "wb"); - - // - // Test if the input file has been opened - // - if (pInputFile) - { - // - // Define the interface members - // - pHandle->handle = pInputFile; - pHandle->type = SBG_IF_TYPE_FILE; - pHandle->pReadFunc = sbgInterfaceFileReadFake; - pHandle->pWriteFunc = sbgInterfaceFileWrite; - pHandle->pFlushFunc = sbgInterfaceFileFlush; - } - else - { - // - // Unable to open the input file - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; -} - -/*! - * Destroy an interface initialized using sbgInterfaceFileOpen. - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileClose(SbgInterface *pHandle) -{ - FILE *pInputFile; - - // - // Test input arguments - // - assert(pHandle); - - // - // Get the internal FILE handle - // - pInputFile = (FILE*)(pHandle->handle); - - // - // Close the input file only if opened - // - if (pInputFile) - { - fclose(pInputFile); - pHandle->handle = NULL; - } - - return SBG_NO_ERROR; -} - -/*! - * Returns the file size in bytes. - * \param[in] pInterface Valid handle on an initialized interface. - * \return The file size in bytes. - */ -SBG_COMMON_LIB_API size_t sbgInterfaceFileGetSize(SbgInterface *pHandle) -{ - FILE *pInputFile; - long cursorPos; - long fileSize; - - // - // Test input arguments - // - assert(pHandle); - - // - // Initialize the file size to 0 in case of error - // - fileSize = 0; - - // - // Get the internal FILE handle - // - pInputFile = (FILE*)(pHandle->handle); - - // - // Test if the file handle is valid - // - if (pInputFile) - { - // - // Compute the file size - // - cursorPos = ftell(pInputFile); - fseek(pInputFile, 0, SEEK_END); - fileSize = ftell(pInputFile); - fseek(pInputFile, cursorPos, SEEK_SET); - } - - return (size_t)fileSize; -} - -/*! - * Returns the current cursor position in the file in bytes. - * \param[in] pInterface Valid handle on an initialized interface. - * \return The current cursor position in bytes. - */ -SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pHandle) -{ - FILE *pInputFile; - - // - // Test input argument - // - assert(pHandle); - - // - // Get the internal FILE handle - // - pInputFile = (FILE*)(pHandle->handle); - - // - // Test if the file handle is valid - // - if (pInputFile) - { - // - // Return the current cursor position - // - return (size_t)ftell(pInputFile); - } - - return 0; -} - - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - FILE *pOutputFile; - size_t bytesWritten; - - // - // Test input argument - // - assert(pHandle); - assert(pBuffer); - - // - // Get the internal FILE handle - // - pOutputFile = (FILE*)(pHandle->handle); - - // - // Write as much bytes as we can - // - bytesWritten = fwrite(pBuffer, sizeof(uint8_t), bytesToWrite, pOutputFile); - - // - // Check if we could successfuly write our bytes - // - if (bytesWritten == bytesToWrite) - { - - return SBG_NO_ERROR; - } - else - { - return SBG_WRITE_ERROR; - } -} - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - FILE *pInputFile; - - // - // Test input argument - // - assert(pHandle); - assert(pBuffer); - assert(pReadBytes); - - // - // Get the internal FILE handle - // - pInputFile = (FILE*)(pHandle->handle); - - // - // Read some bytes from the file - // - *pReadBytes = fread(pBuffer, sizeof(uint8_t), bytesToRead, pInputFile); - - return SBG_NO_ERROR; -} - -/*! - * Make an interface flush all pending input or output data. - * \param[in] pHandle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if successful. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileFlush(SbgInterface *pHandle) -{ - SbgErrorCode errorCode; - FILE *pInputFile; - int ret; - - // - // Test input argument - // - assert(pHandle); - - // - // Get the internal FILE handle - // - pInputFile = (FILE*)(pHandle->handle); - - // - // Flush all pending data - // - ret = fflush(pInputFile); - - if (ret == 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - } - - return errorCode; -} - -/*! - * Fake write function for read only interfaces - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteFake(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - // - // Simply avoid warnings and return an error - // - SBG_UNUSED_PARAMETER(pHandle); - SBG_UNUSED_PARAMETER(pBuffer); - SBG_UNUSED_PARAMETER(bytesToWrite); - - return SBG_ERROR; -} - -/*! - * Fake read function for write only interfaces - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileReadFake(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - // - // Simply avoid warnings and return an error - // - SBG_UNUSED_PARAMETER(pHandle); - SBG_UNUSED_PARAMETER(pBuffer); - SBG_UNUSED_PARAMETER(pReadBytes); - SBG_UNUSED_PARAMETER(bytesToRead); - - return SBG_ERROR; -} diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.h b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.h deleted file mode 100644 index 6def401..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceFile.h +++ /dev/null @@ -1,138 +0,0 @@ -/*! - * \file sbgInterfaceFile.h - * \author SBG Systems (Raphael Siryani) - * \date 01 April 2013 - * - * \brief This file implements a file interface for read only operations. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_INTERFACE_FILE_H -#define SBG_INTERFACE_FILE_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include "sbgInterface.h" - -//----------------------------------------------------------------------// -//- Definitions -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Structures definitions -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Open a file as an interface for read only operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] filePath File path to open. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileOpen(SbgInterface *pHandle, const char *filePath); - -/*! - * Open a file as an interface for write only operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] filePath File path to open. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteOpen(SbgInterface *pHandle, const char *filePath); - -/*! - * Destroy an interface initialized using sbgInterfaceFileOpen. - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileClose(SbgInterface *pHandle); - -/*! - * Returns the file size in bytes. - * \param[in] pInterface Valid handle on an initialized interface. - * \return The file size in bytes. - */ -SBG_COMMON_LIB_API size_t sbgInterfaceFileGetSize(SbgInterface *pHandle); - -/*! - * Returns the current cursor position in the file in bytes. - * \param[in] pInterface Valid handle on an initialized interface. - * \return The current cursor position in bytes. - */ -SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pHandle); - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite); - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); - -/*! - * Make an interface flush all pending input or output data. - * \param[in] pHandle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if successful. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileFlush(SbgInterface *pHandle); - -/*! - * Fake write function for read only interfaces - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteFake(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite); - -/*! - * Fake read function for write only interfaces - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileReadFake(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_INTERFACE_FILE_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerial.h b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerial.h deleted file mode 100644 index 169e653..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerial.h +++ /dev/null @@ -1,108 +0,0 @@ -/*! - * \file sbgInterfaceSerial.h - * \author SBG Systems (Raphael Siryani) - * \date 06 February 2013 - * - * \brief This file implements a serial interface. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_INTERFACE_SERIAL_H -#define SBG_INTERFACE_SERIAL_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include "sbgInterface.h" - -//----------------------------------------------------------------------// -//- Definitions -// -//----------------------------------------------------------------------// -#define SBG_IF_SERIAL_TX_BUFFER_SIZE (4096u) /*!< Define the transmission buffer size for the serial port. */ -#define SBG_IF_SERIAL_RX_BUFFER_SIZE (4096u) /*!< Define the reception buffer size for the serial port. */ - -//----------------------------------------------------------------------// -//- Structures definitions -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize a serial interface for read and write operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] deviceName Serial interface location (COM21 , /dev/ttys0, depending on platform). - * \param[in] baudRate Serial interface baud rate in bps. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pHandle, const char *deviceName, uint32_t baudRate); - -/*! - * Destroy an interface initialized using sbgInterfaceSerialCreate. - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pHandle); - -/*! - * Change the serial interface baud rate immediatly. - * \param[in] handle Valid handle on an initialized interface. - * \param[in] baudRate The new baudrate to apply in bps. - * \return SBG_NO_ERROR if everything is OK - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialChangeBaudrate(SbgInterface *pHandle, uint32_t baudRate); - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite); - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); - -/*! - * Flush the RX and TX buffers (remove all old data) - * \param[in] handle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if everything is OK - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pHandle); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_INTERFACE_SERIAL_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialUnix.c b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialUnix.c deleted file mode 100644 index 2d6eae5..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialUnix.c +++ /dev/null @@ -1,481 +0,0 @@ -#include "sbgInterfaceSerial.h" -#include -#include -#include -#include -#include -#include -#include - -//----------------------------------------------------------------------// -//- Private methods declarations -// -//----------------------------------------------------------------------// - - -/*! - * Returns the right unix baud rate const according to a baud rate value. - * \param[in] baudRate The baud rate value (ie 115200). - * \return The Unix baud rate constant. - */ -uint32 sbgInterfaceSerialGetBaudRateConst(uint32 baudRate) -{ - uint32 baudRateConst; - - // - // Create the right baud rate value for unix platforms - // - switch (baudRate) - { - case 9600: - baudRateConst = B9600; - break; - case 19200: - baudRateConst = B19200; - break; -#ifdef B38400 - case 38400: - baudRateConst = B38400; - break; -#endif -#ifdef B57600 - case 57600: - baudRateConst = B57600; - break; -#endif -#ifdef B115200 - case 115200: - baudRateConst = B115200; - break; -#endif -#ifdef B230400 - case 230400: - baudRateConst = B230400; - break; -#endif -#ifdef B460800 - case 460800: - baudRateConst = B460800; - break; -#endif -#ifdef B921600 - case 921600: - baudRateConst = B921600; - break; -#endif - default: - baudRateConst = baudRate; - } - - return baudRateConst; -} - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize a serial interface for read and write operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] deviceName Serial interface location (COM21 , /dev/ttys0, depending on platform). - * \param[in] baudRate Serial interface baud rate in bps. - * \return SBG_NO_ERROR if the interface has been created. - */ -SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pHandle, const char *deviceName, uint32 baudRate) -{ - int *pSerialHandle; - struct termios options; - uint32 baudRateConst; - - // - // First check if we have a valid pHandle - // - if (pHandle) - { - // - // Check if the device name is valid - // - if (deviceName) - { - // - // Get our baud rate const for our Unix platform - // - baudRateConst = sbgInterfaceSerialGetBaudRateConst(baudRate); - - // - // Allocate the serial handle - // - pSerialHandle = (int*)malloc(sizeof(int)); - - // - // Init the com port - // - (*pSerialHandle) = open(deviceName, O_RDWR | O_NOCTTY | O_NDELAY); - - // - // Test that the port has been initialized - // - if ((*pSerialHandle) != -1) - { - // - // Don't block on read call if no data are available - // - if (fcntl((*pSerialHandle), F_SETFL, O_NONBLOCK) != -1) - { - // - // Retreive current options - // - if (tcgetattr((*pSerialHandle), &options) != -1) - { - // - // Define com port options - // - options.c_cflag |= (CLOCAL | CREAD); // Enable the receiver and set local mode... - options.c_cflag &= ~(PARENB|CSTOPB|CSIZE); // No parity, 1 stop bit, mask character size bits - options.c_cflag |= CS8; // Select 8 data bits - options.c_cflag &= ~CRTSCTS; // Disable Hardware flow control - - // - // Disable software flow control - // - options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); - - // - // We would like raw input - // - options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG /*| IEXTEN | ECHONL*/); - options.c_oflag &= ~OPOST; - - // - // Set our timeout to 0 - // - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 1; - - // - // Set both input and output baud - // - if ( (cfsetispeed(&options, baudRateConst) != -1) && (cfsetospeed(&options, baudRateConst) != -1) ) - { - // - // Define options - // - if (tcsetattr((*pSerialHandle), TCSANOW, &options) != -1) - { - // - // The serial port is ready so create a new serial interface - // - pHandle->handle = (void*)pSerialHandle; - pHandle->type = SBG_IF_TYPE_SERIAL; - pHandle->pReadFunc = sbgInterfaceSerialRead; - pHandle->pWriteFunc = sbgInterfaceSerialWrite; - - // - // Purge the communication - // - return sbgInterfaceSerialFlush(pHandle); - } - else - { - fprintf(stderr, "sbgInterfaceSerialCreate: tcsetattr fails.\n"); - } - } - else - { - fprintf(stderr, "sbgInterfaceSerialCreate: Unable to set speed.\n"); - } - } - else - { - fprintf(stderr, "sbgInterfaceSerialCreate: tcgetattr fails.\n"); - } - } - else - { - fprintf(stderr, "sbgInterfaceSerialCreate: fcntl fails\n"); - } - } - else - { - fprintf(stderr, "sbgInterfaceSerialCreate: Unable to open the com port: %s\n", deviceName); - } - - // - // Release the allocated serial handle - // - SBG_FREE(pSerialHandle); - - return SBG_ERROR; - } - else - { - // - // Invalid device name - // - return SBG_INVALID_PARAMETER; - } - } - else - { - return SBG_NULL_POINTER; - } -} - -/*! - * Destroy an interface initialized using sbgInterfaceSerialCreate. - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pHandle) -{ - int *pSerialHandle; - - // - // Test that we have a valid interface - // - if (pHandle) - { - // - // Get the internal serial handle - // - pSerialHandle = (int*)pHandle->handle; - - // - // Close the port com - // - close((*pSerialHandle)); - SBG_FREE(pSerialHandle); - pHandle->handle = NULL; - - return SBG_NO_ERROR; - } - else - { - return SBG_NULL_POINTER; - } -} - -/*! - * Flush the RX and TX buffers (remove all old data) - * \param[in] handle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if everything is OK - */ -SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pHandle) -{ - int hSerialHandle; - - // - // Test that we have a valid interface - // - if (pHandle) - { - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pHandle->handle); - - // - // Flush our port - // - if (tcflush(hSerialHandle, TCIOFLUSH) == 0) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } - } - else - { - return SBG_NULL_POINTER; - } -} - -/*! - * Change the serial interface baud rate immediatly. - * \param[in] handle Valid handle on an initialized interface. - * \param[in] baudRate The new baudrate to apply in bps. - * \return SBG_NO_ERROR if everything is OK - */ -SbgErrorCode sbgInterfaceSerialChangeBaudrate(SbgInterface *pHandle, uint32 baudRate) -{ - int hSerialHandle; - struct termios options; - uint32 baudRateConst; - - // - // Test that we have a valid interface - // - if (pHandle) - { - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pHandle->handle); - - // - // Get the baud rate const for our Unix platform - // - baudRateConst = sbgInterfaceSerialGetBaudRateConst(baudRate); - - // - // Retrieve current options - // - if (tcgetattr(hSerialHandle, &options) != -1) - { - // - // Set both input and output baud - // - if ( (cfsetispeed(&options, baudRateConst) == -1) || (cfsetospeed(&options, baudRateConst) == -1) ) - { - fprintf(stderr, "sbgInterfaceSerialChangeBaudrate: Unable to set speed.\n"); - return SBG_ERROR; - } - - // - // Define options - // - if (tcsetattr(hSerialHandle, TCSADRAIN, &options) != -1) - { - return SBG_NO_ERROR; - } - else - { - fprintf(stderr, "sbgInterfaceSerialChangeBaudrate: tcsetattr fails.\n"); - return SBG_ERROR; - } - } - else - { - fprintf(stderr, "sbgInterfaceSerialChangeBaudrate: tcgetattr fails.\n"); - return SBG_ERROR; - } - } - else - { - return SBG_NULL_POINTER; - } -} - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - size_t numBytesLeftToWrite = bytesToWrite; - uint8 *pCurrentBuffer = (uint8*)pBuffer; - ssize_t numBytesWritten; - int hSerialHandle; - - // - // Test input parameters - // - if ( (pHandle) && (pBuffer) ) - { - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pHandle->handle); - - // - // Write the whole buffer - // - while (numBytesLeftToWrite > 0) - { - // - // Write these bytes to the serial interface - // - numBytesWritten = write(hSerialHandle, pCurrentBuffer, numBytesLeftToWrite); - - // - // Test the there is no error - // - if (numBytesWritten == -1) - { - // - // An error has occured during the write - // - fprintf(stderr, "sbgDeviceWrite: Unable to write to our device: %s\n", strerror(errno)); - return SBG_WRITE_ERROR; - } - - // - // Update the buffer pointer and the number of bytes to write - // - numBytesLeftToWrite -= (size_t)numBytesWritten; - pCurrentBuffer += (size_t)numBytesWritten; - } - - return SBG_NO_ERROR; - } - else - { - return SBG_NULL_POINTER; - } -} - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32 used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - SbgErrorCode errorCode; - int hSerialHandle; - ssize_t numBytesRead; - - // - // Test input parameters - // - if ( (pHandle) && (pBuffer) && (pReadBytes) ) - { - // - // Get the internal serial handle - // - hSerialHandle = *((int*)pHandle->handle); - - // - // Read our buffer - // - numBytesRead = read(hSerialHandle, pBuffer, bytesToRead); - - // - // Check if we have read at least one byte - // - if (numBytesRead > 0) - { - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_READ_ERROR; - numBytesRead = 0; - } - - // - // If we can, returns the number of read bytes - // - if (pReadBytes) - { - *pReadBytes = (size_t)numBytesRead; - } - } - else - { - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialWin.c b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialWin.c deleted file mode 100644 index 9e9b40b..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceSerialWin.c +++ /dev/null @@ -1,439 +0,0 @@ -#include "sbgInterfaceSerial.h" -#include - -//----------------------------------------------------------------------// -//- Internal methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Returns the last error message for windows api calls. - * \param[out] outErrorMsg Pointer on an allocated string that can stores the windows error message. - * \return The last error number. - */ -uint32 sbgGetWindowsErrorMsg(char outErrorMsg[256]) -{ - DWORD dw = GetLastError(); - DWORD numCharWritten; - LPVOID lpMsgBuf = NULL; - - // - // Get the error message - // - numCharWritten = FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER|FORMAT_MESSAGE_FROM_SYSTEM|FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR)&lpMsgBuf, 0, NULL); - - // - // Test if a message has been correctly written - // - if (numCharWritten > 0) - { - // - // Copy the error message - // - strcpy_s(outErrorMsg, 256, lpMsgBuf); - } - else - { - outErrorMsg[0] = '\0'; - } - - // - // Release the buffer - // - LocalFree(lpMsgBuf); - - return dw; -} - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize a serial interface for read and write operations. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] deviceName Serial interface location (COM21 , /dev/ttys0, depending on platform). - * \param[in] baudRate Serial interface baud rate in bps. - * \return SBG_NO_ERROR if the interface has been created. - */ -SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pHandle, const char *deviceName, uint32 baudRate) -{ - char errorMsg[256]; - char comPortPath[32]; - COMMTIMEOUTS comTimeOut; - DCB comState; - uint32 deviceNum; - HANDLE hSerialDevice; - - assert(pHandle); - - // - // Extract device number - // - if (sscanf_s(deviceName, "COM%i", &deviceNum) == 1) - { - // - // Build the com port path - // - sprintf_s(comPortPath, 32, "\\\\.\\COM%i", deviceNum); - - // - // Init the com port - // - hSerialDevice = CreateFile(comPortPath, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - - // - // Test that the port has been initialized - // - if (hSerialDevice != INVALID_HANDLE_VALUE) - { - // - // Purge the com port - // - if (PurgeComm(hSerialDevice, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR)) - { - // - // Retreives current com state and com timeout - // - if ( (GetCommState(hSerialDevice, &comState)) && (GetCommTimeouts(hSerialDevice, &comTimeOut)) ) - { - // - // Define common attributes - // - comState.BaudRate= baudRate; - comState.Parity= NOPARITY; - comState.ByteSize= 8; - comState.StopBits= ONESTOPBIT; - - // - // Disable flow control - // - comState.fDsrSensitivity = false; - comState.fOutxCtsFlow = false; - comState.fOutxDsrFlow = false; - comState.fOutX = false; - comState.fInX = false; - - // - // Define timeout attributes (0 ms read timeout) - // - comTimeOut.ReadIntervalTimeout = MAXDWORD; - comTimeOut.ReadTotalTimeoutMultiplier = 0; - comTimeOut.ReadTotalTimeoutConstant = 0; - - comTimeOut.WriteTotalTimeoutConstant = 0; - comTimeOut.WriteTotalTimeoutMultiplier = 0; - - // - // Configure the com port - // - if ( (SetCommState(hSerialDevice, &comState)) && (SetCommTimeouts(hSerialDevice, &comTimeOut)) ) - { - // - // Wait until the com port has been configured by windows - // - sbgSleep(60); - - // - // Define the COM port buffer size - // - if (SetupComm(hSerialDevice, SBG_IF_SERIAL_RX_BUFFER_SIZE, SBG_IF_SERIAL_TX_BUFFER_SIZE)) - { - // - // The serial port is ready so create a new serial interface - // - pHandle->handle = hSerialDevice; - pHandle->type = SBG_IF_TYPE_SERIAL; - pHandle->pReadFunc = sbgInterfaceSerialRead; - pHandle->pWriteFunc = sbgInterfaceSerialWrite; - - // - // Purge the communication - // - return sbgInterfaceSerialFlush(pHandle); - } - else - { - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_ERROR, "Unable to define buffer size: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_ERROR, "Unable to set com state and/or timeout: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_ERROR, "Unable to retreive com state and/or timeout: %s", errorMsg); - } - } - else - { - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_ERROR, "Unable to purge com port %i: %s", deviceNum, errorMsg); - } - - // - // Close the port com - // An error occurred while setting up the serial port, close it. - // - CloseHandle(hSerialDevice); - } - - return SBG_ERROR; - } - else - { - // - // Invalid device name - // - return SBG_INVALID_PARAMETER; - } -} - -/*! - * Destroy an interface initialized using sbgInterfaceSerialCreate. - * \param[in] pInterface Valid handle on an initialized interface. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pHandle) -{ - HANDLE pSerialDevice; - - assert(pHandle); - - // - // Get the internal serial handle - // - pSerialDevice = (HANDLE)(pHandle->handle); - - // - // Close the port com - // - CloseHandle(pSerialDevice); - pHandle->handle = NULL; - - return SBG_NO_ERROR; -} - -/*! - * Flush the RX and TX buffers (remove all old data) - * \param[in] handle Valid handle on an initialized interface. - * \return SBG_NO_ERROR if everything is OK - */ -SbgErrorCode sbgInterfaceSerialFlush(SbgInterface *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - HANDLE pSerialDevice; - char errorMsg[256]; - uint8 dummyBuffer[256]; - size_t numBytesRead; - - assert(pHandle); - - // - // Get the internal serial handle - // - pSerialDevice = (HANDLE)(pHandle->handle); - - // - // Flush both Rx and Tx buffers - // - if (PurgeComm(pSerialDevice, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR)) - { - // - // Wait because some hardware doesn't execute the flush directly - // - sbgSleep(5); - - // - // Try to read as much data as possible to avoid flush issues with some hardware - // - do - { - errorCode = sbgInterfaceSerialRead(pHandle, dummyBuffer, &numBytesRead, sizeof(dummyBuffer)); - } while ( (errorCode == SBG_NO_ERROR) && (numBytesRead > 0) ); - - // - // Serial port successfully purged - // - return SBG_NO_ERROR; - } - else - { - // - // Flush has failed - // - sbgGetWindowsErrorMsg(errorMsg); - errorCode = SBG_ERROR; - SBG_LOG_ERROR(SBG_ERROR, "PurgeComm failed: %s", errorMsg); - } - - return errorCode; -} - -/*! - * Change the serial interface baud rate immediatly. - * \param[in] handle Valid handle on an initialized interface. - * \param[in] baudRate The new baudrate to apply in bps. - * \return SBG_NO_ERROR if everything is OK - */ -SbgErrorCode sbgInterfaceSerialChangeBaudrate(SbgInterface *pHandle, uint32 baudRate) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - HANDLE pSerialDevice; - DCB comState; - char errorMsg[256]; - - assert(pHandle); - - // - // Get the internal serial handle - // - pSerialDevice = (HANDLE)(pHandle->handle); - - // - // Try to retreive current com state - // - if (GetCommState(pSerialDevice, &comState)) - { - // - // Change the baud rate - // - comState.BaudRate = baudRate; - - // - // Configure the com state - // - if (SetCommState(pSerialDevice, &comState)) - { - // - // Wait until the com port has been configured by windows - // - sbgSleep(60); - - errorCode = SBG_NO_ERROR; - } - else - { - errorCode = SBG_ERROR; - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(errorCode, "Unable to set com state: %s", errorMsg); - - } - } - else - { - errorCode = SBG_ERROR; - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(errorCode, "Unable to retreive com state: %s", errorMsg); - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SbgErrorCode sbgInterfaceSerialWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - DWORD numBytesLeftToWrite = (DWORD)bytesToWrite; - uint8 *pCurrentBuffer = (uint8*)pBuffer; - DWORD numBytesWritten; - HANDLE pSerialDevice; - char errorMsg[256]; - - assert(pHandle); - assert(pBuffer); - - // - // Get the internal serial handle - // - pSerialDevice = (HANDLE)(pHandle->handle); - - // - // Write the whole buffer - // - while (numBytesLeftToWrite > 0) - { - // - // Write these bytes to the serial interface - // - if (!WriteFile(pSerialDevice, pCurrentBuffer, numBytesLeftToWrite, (LPDWORD)&numBytesWritten, NULL)) - { - // - // An error has occured during the write - // - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_WRITE_ERROR, "Write failed error: %s", errorMsg); - return SBG_WRITE_ERROR; - } - - // - // Update the buffer pointer and the number of bytes to write - // - numBytesLeftToWrite -= (size_t)numBytesWritten; - pCurrentBuffer += numBytesWritten; - } - - return SBG_NO_ERROR; -} - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32 used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SbgErrorCode sbgInterfaceSerialRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - HANDLE pSerialDevice; - char errorMsg[256]; - DWORD bytesRead; - - assert(pHandle); - assert(pBuffer); - assert(pReadBytes); - - // - // Get the internal serial handle - // - pSerialDevice = (HANDLE)(pHandle->handle); - - // - // Read some bytes on the serial buffer - // - if (ReadFile(pSerialDevice, pBuffer, (DWORD)bytesToRead, (LPDWORD)&bytesRead, NULL)) - { - // - // Update the number of bytes read - // - (*pReadBytes) = (size_t)bytesRead; - - return SBG_NO_ERROR; - } - else - { - *pReadBytes = (size_t)bytesRead; - - // - // Unable to read some bytes - // - sbgGetWindowsErrorMsg(errorMsg); - SBG_LOG_ERROR(SBG_READ_ERROR, "Read failed: %s", errorMsg); - return SBG_READ_ERROR; - } -} diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.c b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.c deleted file mode 100644 index f7dc63c..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.c +++ /dev/null @@ -1,588 +0,0 @@ -#include "sbgInterfaceUdp.h" - -#ifdef WIN32 - #include - #include - #include -#else - #include - #include - #include - #include - #include - #include - - - #define SOCKADDR_IN struct sockaddr_in - #define SOCKADDR struct sockaddr - #define SOCKET int - #define INVALID_SOCKET (SOCKET)(~0) - #define SOCKET_ERROR (-1) - #define NO_ERROR (0) - #define SD_BOTH (2) - - #define closesocket(socket) close(socket); - -#endif - -//----------------------------------------------------------------------// -//- Private methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the socket API. - * \return SBG_NO_ERROR if the socket API has been correctly initialized. - */ -SbgErrorCode sbgInterfaceUdpInitSockets(void) -{ -#ifdef WIN32 - WSADATA wsaData; - - // - // Initialize windows sockets version 2.2 - // - if (WSAStartup(MAKEWORD(2, 2), &wsaData) == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } -#else - // - // For Unix platform, the socket API doesn't requiere any initialization - // - return SBG_NO_ERROR; -#endif -} - -/*! - * Uninitialize the socket API. - * \return SBG_NO_ERROR if the socket API has been uninitialized. - */ -SbgErrorCode sbgInterfaceUpdateCloseSockets(void) -{ -#ifdef WIN32 - // - // Release windows sockets - // - if (WSACleanup() == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } -#else - // - // For Unix platform, the socket API doesn't requiere any initialization - // - return SBG_NO_ERROR; -#endif -} - -/*! - * Define if a socket should block or not on receive and send calls. - * \param[in] socketHandle The socket handle to change. - * \param[in] blocking Set to true for a blocking socket or false for a non blocking socket. - * \return SBG_NO_ERROR if the blocking status has been changed. - */ -SbgErrorCode sbgInterfaceUdpSetSocketBlocking(SOCKET socketHandle, bool blocking) -{ -#ifdef WIN32 - u_long blockingMode; - - // - // Define if we would like a blocking (0 or non blocking socket 1) - // - blockingMode = (blocking ?0:1); - - // - // Define the socket as non blocking - // - if (ioctlsocket(socketHandle, FIONBIO, &blockingMode) == NO_ERROR) - { - return SBG_NO_ERROR; - } - else - { - return SBG_ERROR; - } -#else - int32 flags; - - // - // Get the current socket flags and options - // - flags = fcntl(socketHandle, F_GETFL, 0); - - // - // Make sure that there is no error (return value should be positive or zero) - // - if (flags >= 0) - { - // - // Update the flags to enable or disable the blocking mode - // - flags = (blocking ? (flags&~O_NONBLOCK) : (flags|O_NONBLOCK)); - - // - // Redefine the new flags - // - if (fcntl(socketHandle, F_SETFL, flags) == 0) - { - return SBG_NO_ERROR; - } - } - - return SBG_ERROR; -#endif -} - -//----------------------------------------------------------------------// -//- Operations methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize an unconnected UDP interface for read and write operations. - * An UDP interface can send some data to an output ip address and port and read all received - * data on a input port. - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] remoteAddr IP address to send data to. - * \param[in] remotePort Ethernet port to send data to. - * \param[in] localPort Ehternet port on which the interface is listening. - * \return SBG_NO_ERROR if the interface has been created. - */ -SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32 remotePort, uint32 localPort) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterfaceUdp *pNewUdpHandle; - SOCKADDR_IN bindAddress; - SOCKET newUdpSocket; - - assert(pHandle); - - // - // Initialize the socket API - // - if (sbgInterfaceUdpInitSockets() == SBG_NO_ERROR) - { - // - // Create an UDP handle - // - pNewUdpHandle = (SbgInterfaceUdp*)malloc(sizeof(SbgInterfaceUdp)); - - // - // Test that the UDP handle has been allocated - // - if (pNewUdpHandle) - { - // - // Fill the UDP handle - // - pNewUdpHandle->remoteAddr = remoteAddr; - pNewUdpHandle->remotePort = remotePort; - pNewUdpHandle->localPort = localPort; - - // - // Allocate a socket for windows we do this because we don't know the socket type and we would like - // a base and common interface for both windows or Unix platforms. - // - pNewUdpHandle->pUdpSocket = (SOCKET*)malloc(sizeof(SOCKET)); - - // - // Make sure that the socket has been created - // - if (pNewUdpHandle->pUdpSocket) - { - // - // Create a socket to send and/or receive UDP data for IPv4 addresses - // - newUdpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - - // - // Test that the socket has been created - // - if (newUdpSocket != INVALID_SOCKET) - { - // - // Define the socket as non blocking - // - if (sbgInterfaceUdpSetSocketBlocking(newUdpSocket, false) == NO_ERROR) - { - // - // The bind should accept a connection on any ip address but only on the input port - // - bindAddress.sin_family = AF_INET; - bindAddress.sin_addr.s_addr = INADDR_ANY; - bindAddress.sin_port = htons((uint16_t)localPort); - - // - // Bind this socket to all ip addresses on the input port - // - if (bind(newUdpSocket, (SOCKADDR*) &bindAddress, sizeof(bindAddress)) != SOCKET_ERROR) - { - // - // Fill the created UDP socket - // - *((SOCKET*)pNewUdpHandle->pUdpSocket) = newUdpSocket; - - // - // The UDP interface is ready so fill the generic interface - // - pHandle->handle = pNewUdpHandle; - pHandle->type = SBG_IF_TYPE_ETH_UDP; - pHandle->pReadFunc = sbgInterfaceUdpRead; - pHandle->pWriteFunc = sbgInterfaceUdpWrite; - - // - // Return without any error - // - return SBG_NO_ERROR; - } - else - { - // - // Unable to bind the address - // - errorCode = SBG_ERROR; - } - } - else - { - // - // Unable to define the non blocking mode - // - errorCode = SBG_ERROR; - } - - // - // Close the UDP socket - // - shutdown(newUdpSocket, SD_BOTH); - closesocket(newUdpSocket); - } - else - { - // - // Unable to create the socket - // - errorCode = SBG_ERROR; - } - - // - // If needed, free the created socket - // - SBG_FREE(pNewUdpHandle->pUdpSocket); - } - else - { - // - // Unable to allocate memory for the UDP socket - // - errorCode = SBG_MALLOC_FAILED; - } - - // - // Free the UDP handle - // - SBG_FREE(pNewUdpHandle); - } - else - { - // - // Allocation failed - // - errorCode = SBG_MALLOC_FAILED; - } - - // - // Close the sockets API - // - sbgInterfaceUpdateCloseSockets(); - } - - return errorCode; -} - -/*! - * Destroy an interface initialized using sbgInterfaceUdpCreate. - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterfaceUdp *pUdpHandle; - SOCKET udpSocket; - - assert(pHandle); - assert(pHandle->type == SBG_IF_TYPE_ETH_UDP); - - // - // Get the UDP handle - // - pUdpHandle = (SbgInterfaceUdp*)pHandle->handle; - - // - // Test if we have to close the UDP socket - // - if (pUdpHandle->pUdpSocket) - { - // - // Get the UDP receive socket - // - udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket); - - // - // Close the UDP receive socket - // - shutdown(udpSocket, SD_BOTH); - closesocket(udpSocket); - - // - // Release the UDP receive socket - // - SBG_FREE(pUdpHandle->pUdpSocket); - } - - // - // Release the UDP interface - // - SBG_FREE(pUdpHandle); - - // - // Close the socket API - // - errorCode = sbgInterfaceUpdateCloseSockets(); - - return errorCode; -} - -/*! - * Define if a socket can send broadcasted packets. - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \param[in] allowBroadcast Set to true to allow this socket to send broadcasted UDP packets. - * \return SBG_NO_ERROR if the allow broadcast status has been changed. - */ -SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterfaceUdp *pUdpHandle; - SOCKET udpSocket; - uint32 broadcastMode; - - assert(pHandle); - assert(pHandle->type == SBG_IF_TYPE_ETH_UDP); - - // - // Get the UDP handle - // - pUdpHandle = (SbgInterfaceUdp*)pHandle->handle; - - // - // Test that we have a valid UDP send socket - // - if (pUdpHandle->pUdpSocket) - { - // - // Get the UDP send socket - // - udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket); - - // - // Define if we would like to enable UDP broadcast (0 No, 1 Yes) - // - broadcastMode = (allowBroadcast?true:false); - - // - // Define if the socket can send broadcasted packets - // - if (setsockopt(udpSocket, SOL_SOCKET, SO_BROADCAST, (const char*)&broadcastMode, sizeof(broadcastMode)) != NO_ERROR) - { - // - // Unable to define socket options - // - errorCode = SBG_ERROR; - } - } - else - { - // - // No valid send UDP socket found - // - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- Internal interfaces write/read implementations -// -//----------------------------------------------------------------------// - -/*! - * Try to write some data to an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterfaceUdp *pUdpHandle; - SOCKADDR_IN outAddr; - int numBytesSent; - int partialWriteSize; - SOCKET udpSocket; - - assert(pHandle); - assert(pHandle->type == SBG_IF_TYPE_ETH_UDP); - assert(pBuffer); - - // - // Get the UDP handle - // - pUdpHandle = (SbgInterfaceUdp*)pHandle->handle; - - // - // Get the UDP socket - // - udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket); - - // - // Define the receiver address and port - // - outAddr.sin_family = AF_INET; - outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; // Warning: the sbgIpAddress value is always stored in network endianness (ie big endian) - outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort); - - // - // Send packets until no more to send - // - while (bytesToWrite) - { - // - // Initialize number of bytes to write - // - partialWriteSize = (int)bytesToWrite; - - // - // Test if this packet is not over the UDP packet size limit - // - if (partialWriteSize > SBG_INTERFACE_UDP_PACKET_MAX_SIZE) - { - // - // Set it to the limit - // - partialWriteSize = SBG_INTERFACE_UDP_PACKET_MAX_SIZE; - } - - // - // Send the datagram to the receiver - // - numBytesSent = sendto(udpSocket, (const char*)pBuffer, partialWriteSize, 0, (SOCKADDR*)&outAddr, sizeof(outAddr)); - - // - // Test that all the bytes have been written - // - if (numBytesSent != partialWriteSize) - { - // - // The buffer has not been written successfully - // - break; - } - - // - // Update number of bytes to write and source buffer address - // - bytesToWrite -= (size_t)partialWriteSize; - pBuffer = (uint8*)pBuffer + partialWriteSize; - } - - // - // Test that all the bytes have been written - // - if (bytesToWrite) - { - // - // Unable to write some bytes - // - errorCode = SBG_WRITE_ERROR; - } - - return errorCode; -} - -/*! - * Try to read some data from an interface. - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32 used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgInterfaceUdp *pUdpHandle; - int retValue; - SOCKET udpSocket; - - assert(pHandle); - assert(pHandle->type == SBG_IF_TYPE_ETH_UDP); - assert(pBuffer); - assert(pReadBytes); - - // - // Get the UDP handle - // - pUdpHandle = (SbgInterfaceUdp*)pHandle->handle; - - // - // Get the UDP socket - // - udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket); - - // - // Send the datagram to the receiver - // - retValue = recvfrom(udpSocket, (char*)pBuffer, (int)bytesToRead, 0, NULL, 0); - - // - // Test that we don't have any error - // - if (retValue != SOCKET_ERROR) - { - // - // Returns the number of read bytes - // - *pReadBytes = (size_t)retValue; - } - else - { - #ifdef WIN32 - // - // Get the last error during the read - // - retValue = WSAGetLastError(); - - if (retValue== WSAEWOULDBLOCK) - { - errorCode = SBG_NOT_READY; - } - else - { - errorCode = SBG_ERROR; - } - #else - errorCode = SBG_NOT_READY; - #endif - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.h b/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.h deleted file mode 100644 index b0f7652..0000000 --- a/sbg_ros_driver/external/sbgECom/common/interfaces/sbgInterfaceUdp.h +++ /dev/null @@ -1,112 +0,0 @@ -/*! - * \file sbgInterfaceUdp.h - * \author SBG Systems - * \date 05 February 2013 - * - * \brief This file implements an UDP interface. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2013-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_INTERFACE_UDP_H -#define SBG_INTERFACE_UDP_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "sbgInterface.h" - -//----------------------------------------------------------------------// -//- Predefinitions -// -//----------------------------------------------------------------------// - -#define SBG_INTERFACE_UDP_PACKET_MAX_SIZE (1400) - -//----------------------------------------------------------------------// -//- Structures definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores all internal data used by the UDP interface. - */ -typedef struct _SbgInterfaceUdp -{ - void *pUdpSocket; /*!< The socket used to send and / or receive some UDP data. */ - sbgIpAddress remoteAddr; /*!< IP address to send data to. */ - uint32_t remotePort; /*!< Ethernet port to send data to. */ - uint32_t localPort; /*!< Ethernet port on which the interface is listening. */ -} SbgInterfaceUdp; - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * Initialize an unconnected UDP interface for read and write operations. - * - * If the remote address and port are zero, the interface waits for the first packet received on the - * local port and uses the source of that packet as its remote host. This provides a convenient way - * to create "server" UDP interfaces. - * - * \param[in] pHandle Pointer on an allocated interface instance to initialize. - * \param[in] remoteAddr IP address to send data to. - * \param[in] remotePort Ethernet port to send data to. - * \param[in] localPort Ehternet port on which the interface is listening. - * \return SBG_NO_ERROR if the interface has been created. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32_t remotePort, uint32_t localPort); - -/*! - * Destroy an interface initialized using sbgInterfaceUdpCreate. - * - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \return SBG_NO_ERROR if the interface has been closed and released. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle); - -/*! - * Define if a socket can send broadcasted packets. - * - * \param[in] pInterface Pointer on a valid UDP interface created using sbgInterfaceUdpCreate. - * \param[in] allowBroadcast Set to true to allow this socket to send broadcasted UDP packets. - * \return SBG_NO_ERROR if the allow broadcast status has been changed. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast); - -/*! - * Try to write some data to an interface. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write - * \param[in] bytesToWrite Number of bytes we would like to write. - * \return SBG_NO_ERROR if all bytes have been written successfully. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite); - -/*! - * Try to read some data from an interface. - * - * \param[in] pHandle Valid handle on an initialized interface. - * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. - * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. - * \param[in] bytesToRead Number of bytes we would like to read. - * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); - -#ifdef __cplusplus -} -#endif - -#endif /* SBG_INTERFACE_UDP_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.c b/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.c deleted file mode 100644 index f32f672..0000000 --- a/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.c +++ /dev/null @@ -1,126 +0,0 @@ -#include "sbgNetwork.h" -#include - -//----------------------------------------------------------------------// -//- IP manipulation methods -// -//----------------------------------------------------------------------// - -/*! - * Convert an ip to a string of the form A.B.C.D - * \param[in] ipAddr IP address to convert to a string. - * \param[out] pBuffer Pointer on an allocated buffer than can hold ip address as a string. - * \param[in] maxSize Maximum number of chars that can be stored in pBuffer including the NULL char. - */ -SBG_COMMON_LIB_API void sbgNetworkIpToString(sbgIpAddress ipAddr, char *pBuffer, size_t maxSize) -{ - // - // Check input arguments - // - assert(pBuffer); - assert(maxSize >= 16); - - SBG_UNUSED_PARAMETER(maxSize); - - // - // Write the IP address - // - sprintf(pBuffer, "%u.%u.%u.%u", sbgIpAddrGetA(ipAddr), sbgIpAddrGetB(ipAddr), sbgIpAddrGetC(ipAddr), sbgIpAddrGetD(ipAddr)); -} - -/*! - * Convert an ip address stored in a string of the form A.B.C.D to an sbgIpAddress object. - * \param[in] pBuffer IP address as a string of the form A.B.C.D - * \return IP address parsed from the string or 0.0.0.0 if the IP is invalid. - */ -SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer) -{ - int32_t ipAddrA; - int32_t ipAddrB; - int32_t ipAddrC; - int32_t ipAddrD; - int32_t numReadParams; - - // - // Check input arguments - // - assert(pBuffer); - - // - // Parse input arguments - // - numReadParams = sscanf(pBuffer, "%d.%d.%d.%d", &ipAddrA, &ipAddrB, &ipAddrC, &ipAddrD); - - // - // Make sure the parsed IP is normal - // - if ((numReadParams == 4) && (ipAddrA >= 0) && (ipAddrA <= 255) && (ipAddrB >= 0) && (ipAddrB <= 255) && (ipAddrC >= 0) && (ipAddrC <= 255) && (ipAddrD >= 0) && (ipAddrD <= 255)) - { - // - // Ip address correctly parsed, return it - // - return sbgIpAddr((uint8_t)ipAddrA, (uint8_t)ipAddrB, (uint8_t)ipAddrC, (uint8_t)ipAddrD); - } - else - { - // - // The IP address couldn't be parsed correctly, return a null ip address - // - return sbgIpAddr(0, 0, 0, 0); - } -} - -//----------------------------------------------------------------------// -//- IP validation methods -// -//----------------------------------------------------------------------// - -/*! -* Check if an IpV4 netmask is valid, the mask should be contiguous (1111 followed by 0) -* \param[in] netmask The netmask stored in an uint32_t (host endianness). -* \return true if the netmask is valid ie contiguous. -*/ -SBG_COMMON_LIB_API bool sbgIpNetMaskValid(sbgIpAddress netmask) -{ - uint32_t y; - uint32_t z; - - // - // First test that the netmask is not zero, if yes, it's valid - // - if (netmask != 0) - { - // - // We are doing arithmetic so we have to make sure the netmask is using the platform endianness - // The IP address is always stored in big endian so we have to swap it for little endian platforms - // -#if SBG_CONFIG_BIG_ENDIAN == 0 - netmask = sbgSwap32(netmask); -#endif - - // - // Compute the bitwise inverse - // - y = ~netmask; - - // - // Add one to the inverse so if netmask was a proper one, there will be at most 1 bit set in this. - // - z = y + 1; - - // - // Test that, simply and z with z - 1, which happens to be y. The result will be zero if all is OK, non zero otherwise. - // - if ((z&y) == 0) - { - // - // We have a valid netmask - // - return true; - } - else - { - return false; - } - } - - return true; -} diff --git a/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.h b/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.h deleted file mode 100644 index f7fac4e..0000000 --- a/sbg_ros_driver/external/sbgECom/common/network/sbgNetwork.h +++ /dev/null @@ -1,276 +0,0 @@ -/*! - * \file sbgNetwork.h - * \author SBG Systems (Raphael Siryani) - * \date 15 September 2015 - * - * \brief Useful methods for Network handling such as ip addresses. - * - * IP v4 address is stored in memory with a uint32_t. - * Each address component A.B.C.D is stored in 8 bits using the network - * endianess ie Big Endian. - * - * We thus have the following memory organisation: - * - * In Little Endian: - * |LSB| | |MSB| - * | A | B | C | D | - * - * In Big Endian: - * |MSB| | |LSB| - * | A | B | C | D | - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2016, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_NETWORK_H -#define SBG_NETWORK_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include - -//----------------------------------------------------------------------// -//- Common IPv4 definitions -// -//----------------------------------------------------------------------// -#define SBG_IPV4_UNSPECIFIED_ADDR sbgIpAddr(0, 0, 0, 0) /*!< This represents an undefined IP address. */ -#define SBG_IPV4_BROADCAST_ADDR sbgIpAddr(255, 255, 255, 255) /*!< Broadcast IP address used to address all devices within the same network. */ - -//----------------------------------------------------------------------// -//- IP setters / getters -// -//----------------------------------------------------------------------// - -/*! - * Build an IP V4 address in the form a.b.c.d - * \param[in] a First 8 bits IP address. - * \param[in] b Second 8 bits IP address. - * \param[in] c Third 8 bits IP address. - * \param[in] d Last 8 bits IP address. - * \return An initialized IP address object. - */ -SBG_INLINE sbgIpAddress sbgIpAddr(uint8_t a, uint8_t b, uint8_t c, uint8_t d) -{ -#if SBG_CONFIG_BIG_ENDIAN == 1 - return (a << 24) | (b << 16) | (c << 8) | d; -#else - return a | (b << 8) | (c << 16) | (d << 24); -#endif -} - -/*! - * Return the first A field of of an IP v4 address of the form A.B.C.D - * \param[in] ipAddr An sbgIpAddress to convert. - * \return The A field of the IP address. - */ -SBG_INLINE uint8_t sbgIpAddrGetA(sbgIpAddress ipAddr) -{ -#if SBG_CONFIG_BIG_ENDIAN == 1 - return (uint8_t)((ipAddr & 0xFF000000) >> 24); -#else - return (uint8_t)((ipAddr & 0x000000FF)); -#endif -} - -/*! -* Return the first B field of of an IP v4 address of the form A.B.C.D -* \param[in] ipAddr An sbgIpAddress to convert. -* \return The B field of the IP address. -*/ -SBG_INLINE uint8_t sbgIpAddrGetB(sbgIpAddress ipAddr) -{ -#if SBG_CONFIG_BIG_ENDIAN == 1 - return (uint8_t)((ipAddr & 0x00FF0000) >> 16); -#else - return (uint8_t)((ipAddr & 0x0000FF00) >> 8); -#endif -} - -/*! -* Return the first C field of of an IP v4 address of the form A.B.C.D -* \param[in] ipAddr An sbgIpAddress to convert. -* \return The C field of the IP address. -*/ -SBG_INLINE uint8_t sbgIpAddrGetC(sbgIpAddress ipAddr) -{ -#if SBG_CONFIG_BIG_ENDIAN == 1 - return (uint8_t)((ipAddr & 0x0000FF00) >> 8); -#else - return (uint8_t)((ipAddr & 0x00FF0000) >> 16); -#endif -} - -/*! -* Return the first D field of of an IP v4 address of the form A.B.C.D -* \param[in] ipAddr An sbgIpAddress to convert. -* \return The D field of the IP address. -*/ -SBG_INLINE uint8_t sbgIpAddrGetD(sbgIpAddress ipAddr) -{ -#if SBG_CONFIG_BIG_ENDIAN == 1 - return (uint8_t)((ipAddr & 0x000000FF)); -#else - return (uint8_t)((ipAddr & 0xFF000000) >> 24); -#endif -} - -//----------------------------------------------------------------------// -//- IP manipulation methods -// -//----------------------------------------------------------------------// - -/*! - * Convert an ip to a string of the form A.B.C.D - * \param[in] ipAddr IP address to convert to a string. - * \param[out] pBuffer Pointer on an allocated buffer than can hold ip address as a string. - * \param[in] maxSize Maximum number of chars that can be stored in pBuffer including the NULL char. - */ -SBG_COMMON_LIB_API void sbgNetworkIpToString(sbgIpAddress ipAddr, char *pBuffer, size_t maxSize); - -/*! - * Convert an ip address stored in a string of the form A.B.C.D to an sbgIpAddress object. - * \param[in] pBuffer IP address as a string of the form A.B.C.D - * \return IP address parsed from the string or 0.0.0.0 if the IP is invalid. - */ -SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer); - -//----------------------------------------------------------------------// -//- IP operations -// -//----------------------------------------------------------------------// - -/*! - * Given an ip address and the netmask, returns the network part (ip & subnetMask) - * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). - * \param[in] netmask The netmask stored in an uint32_t (host endianness). - * \return The network part of the ip address. - */ -SBG_INLINE sbgIpAddress sbgIpGetNetworkAddr(sbgIpAddress ipAddress, sbgIpAddress netmask) -{ - return (ipAddress & netmask); -} - -/*! - * Given an ip address and the netmask, returns the host part (ip & ~subnetMask) - * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). - * \param[in] netmask The netmask stored in an uint32_t (host endianness). - * \return The host part of the ip address. - */ -SBG_INLINE sbgIpAddress sbgIpGetHostAddr(sbgIpAddress ipAddress, sbgIpAddress netmask) -{ - return (ipAddress & ~netmask); -} - -//----------------------------------------------------------------------// -//- IP validation methods -// -//----------------------------------------------------------------------// - -/*! - * Returns true if the provided IP address is unspecified ie (0.0.0.0) - * \param[in] ipAddress The ip address to test - * \return true if the ip address is unspecified or false otherwise. - */ -SBG_INLINE bool sbgIpAddressIsUnspecified(sbgIpAddress ipAddress) -{ - if (ipAddress == 0) - { - return true; - } - else - { - return false; - } -} - -/*! - * Check if an IpV4 address is valid. The ip address format is A.B.C.D and A should respect 0 < A < 224 - * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). - * \return true if the ip address is valid ie contiguous. - */ -SBG_INLINE bool sbgIpAddressValid(sbgIpAddress ipAddress) -{ - // - // Check the if A part of the ip address is within 1 and 223 - // - if ((sbgIpAddrGetA(ipAddress) > 0) && (sbgIpAddrGetA(ipAddress) < 224)) - { - // - // The ip address is valid - // - return true; - } - else - { - // - // The ip address is not valid - // - return false; - } -} - -/*! - * Given an ip address and the netmask, returns true if this ip address is within the subnet. - * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). - * \param[in] netmask The netmask stored in an uint32_t (host endianness). - * \return true if this ip address is within the subnet or false otherwise. - */ -SBG_INLINE sbgIpAddress sbgIpAddrWithinSubnet(sbgIpAddress ipAddress, sbgIpAddress netmask) -{ - // - // Just check if the host part is equals to zero - // - if (sbgIpGetHostAddr(ipAddress, netmask) == 0) - { - return false; - } - else - { - return true; - } -} - -/*! - * Check if two ip addresses are in the same network given the subnet. - * \param[in] firstIpAddr The first IP address to check. - * \param[in] secondIpAddr The second IP address to check. - * \param[in] netmask The netmask of the network. - * \return true if the two ip addresses are in the same network. - */ -SBG_INLINE bool sbgIpAddrIsSameNetwork(sbgIpAddress firstIpAddr, sbgIpAddress secondIpAddr, sbgIpAddress netmask) -{ - if ((firstIpAddr & netmask) == (secondIpAddr & netmask)) - { - return true; - } - else - { - return false; - } -} - -/*! - * Check if an IpV4 netmask is valid, the mask should be contiguous (1111 followed by 0) - * \param[in] netmask The netmask stored in an uint32_t (host endianness). - * \return true if the netmask is valid ie contiguous. - */ -SBG_COMMON_LIB_API bool sbgIpNetMaskValid(sbgIpAddress netmask); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_NETWORK_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/platform/sbgPlatform.c b/sbg_ros_driver/external/sbgECom/common/platform/sbgPlatform.c deleted file mode 100644 index a48ce17..0000000 --- a/sbg_ros_driver/external/sbgECom/common/platform/sbgPlatform.c +++ /dev/null @@ -1,120 +0,0 @@ -#include -#include -#include - -//----------------------------------------------------------------------// -//- Include specific header for WIN32 and UNIX platforms -// -//----------------------------------------------------------------------// -#ifdef WIN32 - #include -#elif defined(__APPLE__) - #include -#else - #include -#endif - -//----------------------------------------------------------------------// -//- Specific timing methods to reimplement for your platform -// -//----------------------------------------------------------------------// - -/*! - * Returns the current time in ms. - * \return The current time in ms. - */ -SBG_COMMON_LIB_API uint32_t sbgGetTime(void) -{ -#ifdef WIN32 - // - // Return the current time in ms - // - return clock() / (CLOCKS_PER_SEC / 1000); -#elif defined(__APPLE__) - mach_timebase_info_data_t timeInfo; - mach_timebase_info(&timeInfo); - - // - // Return the current time in ms - // - return (mach_absolute_time() * timeInfo.numer / timeInfo.denom) / 1000000.0; -#else - struct timespec now; - clock_gettime(CLOCK_REALTIME, &now); - - // - // Return the current time in ms - // - return now.tv_sec * 1000 + now.tv_nsec / 1000000; -#endif -} - -/*! - * Sleep for the specified number of ms. - * \param[in] ms Number of millisecondes to wait. - */ -SBG_COMMON_LIB_API void sbgSleep(uint32_t ms) -{ - // - // Implementation valid for both WIN and UNIX systems - // - #ifdef WIN32 - Sleep(ms); - #else - usleep(ms*1000); - #endif -} - -//----------------------------------------------------------------------// -//- Specific logging methods to reimplement for your platform -// -//----------------------------------------------------------------------// - -/*! - * The method is called when one of the SBG_LOG_ERROR, SBG_LOG_WARNING, SBG_LOG_INFO or SBG_LOG_VERBOSE is called. - * It logs an error message with debug information and support a variable list of arguments - * \param[in] pFileName File name where the error occurred. - * \param[in] pFunctionName Function name where the error occurred. - * \param[in] line Line number where the error occurred. - * \param[in] logType Define if we have an error, a warning, an info or a verbose log. - * \param[in] errorCode The error code associated with the message. - * \param[in] pFormat The error message that will be used with the variable list of arguments. - */ -SBG_COMMON_LIB_API void sbgPlatformDebugLogMsg(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pFormat, ...) -{ - char errorMsg[SBG_CONFIG_LOG_MAX_SIZE]; - va_list args; - - // - // Initialize the list of variable arguments on the latest function argument - // - va_start(args, pFormat); - - // - // Generate the error message string - // - vsprintf(errorMsg, pFormat, args); - - // - // Close the list of variable arguments - // - va_end(args); - - // - // Log the correct message according to the log type - // - switch (logType) - { - case SBG_DEBUG_LOG_TYPE_ERROR: - fprintf(stderr, "*ERR * [%s]%s: %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_WARNING: - fprintf(stderr, "*WARN* [%s]%s: %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_INFO: - fprintf(stderr, "*INFO* %s\n\r", errorMsg); - break; - case SBG_DEBUG_LOG_TYPE_DEBUG: - fprintf(stderr, "*DBG * %s\n\r", errorMsg); - break; - default: - fprintf(stderr, "*UKNW*\t[%s]%s(%u): %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, line, errorMsg); - } -} diff --git a/sbg_ros_driver/external/sbgECom/common/sbgCommon.h b/sbg_ros_driver/external/sbgECom/common/sbgCommon.h deleted file mode 100644 index ad94a75..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgCommon.h +++ /dev/null @@ -1,132 +0,0 @@ -/*! - * \file sbgCommon.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Main header file for SBG Systems common C library. - * - * All files / projects that would like to use the SBG Systems common C library - * should include this file. - * - * Be aware that this file doesn't include SBG Systems common C library components - * but just defines all types and environments such as error logging. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2015, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_COMMON_H -#define SBG_COMMON_H - -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Include user defined configuration -// -//----------------------------------------------------------------------// -#include "sbgConfig.h" - -//----------------------------------------------------------------------// -//- Platform default configuration -// -//----------------------------------------------------------------------// - -/*! - * If set to 0, the platform support only aligned memory access. - * If set to 1, the platform support unaligned memory access. - * Default: Support only Aligned access - Disabled - */ -#ifndef SBG_CONFIG_UNALIGNED_ACCESS_AUTH - #define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) -#endif - -/*! - * If set to 0, the platform is using little endian. - * If set to 1, the platform is using big endian. - * Default: Little Endian - Disabled - */ -#ifndef SBG_CONFIG_BIG_ENDIAN - #define SBG_CONFIG_BIG_ENDIAN (0) -#endif - -//----------------------------------------------------------------------// -//- Logging default configuration -// -//----------------------------------------------------------------------// - -/*! - * If set to 1, error logs level will be thrown. - * Default: Enabled - */ -#ifndef SBG_CONFIG_ENABLE_LOG_ERROR - #define SBG_CONFIG_ENABLE_LOG_ERROR (1) -#endif - -/*! - * If set to 1, warning logs level will be thrown. - * Default: Enabled - */ -#ifndef SBG_CONFIG_ENABLE_LOG_WARNING - #define SBG_CONFIG_ENABLE_LOG_WARNING (1) -#endif - -/*! - * If set to 1, information logs level will be thrown. - * Default: Enabled - */ -#ifndef SBG_CONFIG_ENABLE_LOG_INFO - #define SBG_CONFIG_ENABLE_LOG_INFO (1) -#endif - -/*! - * If set to 1, debug logs level will be thrown. - * Default: Enabled - */ -#ifndef SBG_CONFIG_ENABLE_LOG_DEBUG - #define SBG_CONFIG_ENABLE_LOG_DEBUG (1) -#endif - -/*! - * Maximum error message size in bytes that can be generated including the NULL Char. - * Default: 1024 - */ -#ifndef SBG_CONFIG_LOG_MAX_SIZE - #define SBG_CONFIG_LOG_MAX_SIZE (size_t)(1024) -#endif - -//----------------------------------------------------------------------// -//- File system default configuration -// -//----------------------------------------------------------------------// - -/*! - * Maximum number of chars for a file name including the NULL char. - * Default: 256 - */ -#ifndef SBG_CONFIG_PATH_MAX_SIZE - #define SBG_CONFIG_PATH_MAX_SIZE (size_t)(256) -#endif - -//----------------------------------------------------------------------// -//- Include core headers -// -//----------------------------------------------------------------------// -#include "sbgTypes.h" -#include "sbgErrorCodes.h" -#include "sbgDefines.h" -#include "sbgDebug.h" -#include "sbgPlatform.h" - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_COMMON_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgCommonLibVersion.h b/sbg_ros_driver/external/sbgECom/common/sbgCommonLibVersion.h deleted file mode 100644 index 6672fd0..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgCommonLibVersion.h +++ /dev/null @@ -1,47 +0,0 @@ -/*! - * \file sbgCommonLibVersion.h - * \author SBG Systems (Rémi Burtin) - * \date 28/09/2015 - * - * \brief Header file that contains all versions related information. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_COMMON_LIB_VERSION_H__ -#define __SBG_COMMON_LIB_VERSION_H__ - -#include - -//----------------------------------------------------------------------// -//- Version definitions -// -//----------------------------------------------------------------------// - -#define SBG_COMMON_LIB_VERSION_MAJOR 1 -#define SBG_COMMON_LIB_VERSION_MINOR 1 -#define SBG_COMMON_LIB_VERSION_BUILD 174 -#define SBG_COMMON_LIB_VERSION_QUALIFIER SBG_VERSION_QUALIFIER_ALPHA - -#define SBG_COMMON_LIB_VERSION SBG_VERSION_SOFTWARE(SBG_COMMON_LIB_VERSION_MAJOR,SBG_COMMON_LIB_VERSION_MINOR,SBG_COMMON_LIB_VERSION_BUILD,SBG_COMMON_LIB_VERSION_QUALIFIER) - -/* - * Backward compatibility macro definitions. - */ -#ifndef SBG_STR - #define SBG_STR(X) #X -#endif -#ifndef SBG_ASSTR - #define SBG_ASSTR(X) SBG_STR(X) -#endif -#define SBG_COMMON_LIB_VERSION_STR SBG_ASSTR(SBG_COMMON_LIB_VERSION_MAJOR) "." SBG_ASSTR(SBG_COMMON_LIB_VERSION_MINOR) "." SBG_ASSTR(SBG_COMMON_LIB_VERSION_BUILD) "\0" - -#endif diff --git a/sbg_ros_driver/external/sbgECom/common/sbgConfig.h b/sbg_ros_driver/external/sbgECom/common/sbgConfig.h deleted file mode 100644 index a52835a..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgConfig.h +++ /dev/null @@ -1,61 +0,0 @@ -/*! - * \file sbgConfig.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Header file used to configure the framework. - * - * You can configure for example the logging system. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_CONFIG_H -#define SBG_CONFIG_H - -//----------------------------------------------------------------------// -//- Platform specific configurations -// -//----------------------------------------------------------------------// - -/*! - * Windows x86 & x64 support both aligned and unaligned access - */ -#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) - -/*! - * Windows is using little endianess - */ -#define SBG_CONFIG_BIG_ENDIAN (0) - -//----------------------------------------------------------------------// -//- Debug / logging Configurations -// -//----------------------------------------------------------------------// - -/*! - * Define the error log configuration for debug and release modes. - * You should setup your C preprocessor to define SBG_NDEBUG in release mode. - */ -#ifdef NDEBUG - #define SBG_CONFIG_ENABLE_ASSERT (0) /*!< Set to 1 to enable all assertion checks. */ - #define SBG_CONFIG_ENABLE_LOG_ERROR (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_ERROR. */ - #define SBG_CONFIG_ENABLE_LOG_WARNING (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_WARNING. */ - #define SBG_CONFIG_ENABLE_LOG_INFO (0) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_INFO. */ - #define SBG_CONFIG_ENABLE_LOG_DEBUG (0) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_DEBUG */ -#else - #define SBG_CONFIG_ENABLE_ASSERT (1) /*!< Set to 1 to enable all assertion checks. */ - #define SBG_CONFIG_ENABLE_LOG_ERROR (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_ERROR. */ - #define SBG_CONFIG_ENABLE_LOG_WARNING (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_WARNING. */ - #define SBG_CONFIG_ENABLE_LOG_INFO (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_INFO. */ - #define SBG_CONFIG_ENABLE_LOG_VERBOSE (1) /*!< Set to 1 to enable log of all errors thrown using SBG_LOG_DEBUG. */ -#endif - -#endif /* SBG_CONFIG_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgDebug.h b/sbg_ros_driver/external/sbgECom/common/sbgDebug.h deleted file mode 100644 index c493205..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgDebug.h +++ /dev/null @@ -1,138 +0,0 @@ -/*! - * \file sbgDebug.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Define and handle error logging for the SBG Systems common C library. - * - * The methods defined here should be implemented in sbgPlatform.h/sbgPlatform.c - * according to your platform and needs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2015, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_DEBUG_H -#define SBG_DEBUG_H - -// Local headers -#include "sbgDefines.h" - -#ifndef SBG_DEBUG_LOG_CATEGORY - #define SBG_DEBUG_LOG_CATEGORY ("None") -#endif - -//----------------------------------------------------------------------// -//- Errors and warning definitions -// -//----------------------------------------------------------------------// - -/*! - * Enum that identify the type of error / warning that has been thrown. - */ -typedef enum _SbgDebugLogType -{ - SBG_DEBUG_LOG_TYPE_ERROR, /*!< The message to log is an error. */ - SBG_DEBUG_LOG_TYPE_WARNING, /*!< The message to log is a warning. */ - SBG_DEBUG_LOG_TYPE_INFO, /*!< The message to log is an information. */ - SBG_DEBUG_LOG_TYPE_DEBUG /*!< The message to log is a debug information. */ -} SbgDebugLogType; - -//----------------------------------------------------------------------// -//- Errors and warning macros -// -//----------------------------------------------------------------------// - -/*! - * Log an error with its associated message. - * \param[in] errorCode The error code that has thrown this error. - * \param[in] format String litteral for the associated error message (you can use printf like string formating). - */ -#define SBG_LOG_ERROR_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_ERROR, errorCode, format, ##__VA_ARGS__) - -#if SBG_CONFIG_ENABLE_LOG_ERROR == 1 - #define SBG_LOG_ERROR SBG_LOG_ERROR_CALL -#else - #define SBG_LOG_ERROR(format, ...) ((void)sizeof(SBG_LOG_ERROR_CALL(format, ## __VA_ARGS__))) -#endif - -/*! - * Log a warning with its associated message. - * \param[in] errorCode The error code that has thrown this warning. - * \param[in] format String litteral for the associated warning message (you can use printf like string formating). - */ -#define SBG_LOG_WARNING_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_WARNING, errorCode, format, ##__VA_ARGS__) - -#if SBG_CONFIG_ENABLE_LOG_WARNING == 1 - #define SBG_LOG_WARNING SBG_LOG_WARNING_CALL -#else - #define SBG_LOG_WARNING(format, ...) ((void)sizeof(SBG_LOG_WARNING_CALL(format, ## __VA_ARGS__))) -#endif - -/*! - * Log an information message. - * \param[in] format String litteral for the information message (you can use printf like string formating). - */ -#define SBG_LOG_INFO_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_INFO, SBG_NO_ERROR, format, ##__VA_ARGS__) - -#if SBG_CONFIG_ENABLE_LOG_INFO == 1 - #define SBG_LOG_INFO SBG_LOG_INFO_CALL -#else - #define SBG_LOG_INFO(format, ...) ((void)sizeof(SBG_LOG_INFO_CALL(format, ## __VA_ARGS__))) -#endif - -/*! - * Log an information message only in debug mode - * \param[in] format String litteral for the information message (you can use printf like string formating). - */ -#define SBG_LOG_DEBUG_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_DEBUG, SBG_NO_ERROR, format, ##__VA_ARGS__) - -#if SBG_CONFIG_ENABLE_LOG_DEBUG == 1 - #define SBG_LOG_DEBUG SBG_LOG_DEBUG_CALL -#else - #define SBG_LOG_DEBUG(format, ...) ((void)sizeof(SBG_LOG_DEBUG_CALL(format, ## __VA_ARGS__))) -#endif - -//----------------------------------------------------------------------// -//- Inline functions -// -//----------------------------------------------------------------------// - -/*! - * Convert a log type into a string representation. - * - * \param[in] logType Log type. - * \return String representation for the given log type. - */ -SBG_INLINE const char *sbgDebugLogTypeToStr(SbgDebugLogType logType) -{ - const char *pString; - - switch (logType) - { - case SBG_DEBUG_LOG_TYPE_ERROR: - pString = "error"; - break; - case SBG_DEBUG_LOG_TYPE_WARNING: - pString = "warning"; - break; - case SBG_DEBUG_LOG_TYPE_INFO: - pString = "info"; - break; - case SBG_DEBUG_LOG_TYPE_DEBUG: - pString = "debug"; - break; - default: - pString = "unknown"; - break; - } - - return pString; -} - -#endif /* SBG_DEBUG_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgDefines.h b/sbg_ros_driver/external/sbgECom/common/sbgDefines.h deleted file mode 100644 index 30f619d..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgDefines.h +++ /dev/null @@ -1,386 +0,0 @@ -/*! - * \file sbgDefines.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Header file that contains all common definitions. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2015, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_DEFINES_H -#define SBG_DEFINES_H - -// Standard headers -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// -// XXX If NDEBUG is defined, most libraries define assert() as ((void)0), which may -// cause "defined but not used" warnings. Redefine assert() in a way that safely -// prevents this warning, i.e. without triggering the expression side effects. -// -#ifdef NDEBUG -#undef assert -#define assert(expression) ((void)sizeof(expression)) -#endif // NDEBUG - -/*! - * Macro used to handle export and import methods of the sbgCommon library - */ -#ifdef _MSC_VER - #if defined(SBG_COMMON_LIB_API_EXPORT) - #define SBG_COMMON_LIB_API __declspec(dllexport) - #elif defined(SBG_COMMON_LIB_API_IMPORT) - #define SBG_COMMON_LIB_API __declspec(dllimport) - #else - #define SBG_COMMON_LIB_API - #endif -#else - #define SBG_COMMON_LIB_API -#endif - -//----------------------------------------------------------------------// -//- Global definitions -// -//----------------------------------------------------------------------// -#ifndef SBG_DISABLE - #define SBG_DISABLE (0) -#endif - -#ifndef SBG_ENABLE - #define SBG_ENABLE (1) -#endif - -#ifndef FALSE - #define FALSE (0) -#endif - -#ifndef TRUE - #define TRUE (1) -#endif - -#ifndef NULL - #define NULL (0) -#endif - -#ifndef SBG_INVALID_HANDLE - #define SBG_INVALID_HANDLE (0x00000000u) -#endif - -#ifndef SBG_NOT_FOUND - #define SBG_NOT_FOUND (0xFFFFFFFFu) -#endif - -#ifndef SBG_UNDEFINED - #define SBG_UNDEFINED (0xFFFFFFFFu) -#endif - -#ifndef SBG_ARRAY_SIZE - #define SBG_ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) -#endif - -#ifndef SBG_STRLEN - #define SBG_STRLEN(s) (sizeof(s) - 1) -#endif - -#ifndef SBG_CONTAINER_OF - #define SBG_CONTAINER_OF(ptr, type, member) ((type *)((char *)(ptr) - offsetof(type, member))) -#endif - -#ifndef SBG_LIKELY - #if defined(__GNUC__) - #define SBG_LIKELY(expr) __builtin_expect((bool)(expr), true) - #else - #define SBG_LIKELY(expr) (expr) - #endif -#endif - -#ifndef SBG_UNLIKELY - #if defined(__GNUC__) - #define SBG_UNLIKELY(expr) __builtin_expect((bool)(expr), false) - #else - #define SBG_UNLIKELY(expr) (expr) - #endif -#endif - -/*! - * XXX Visual C (not C++) doesn't provide anything to implement typeof(). As a result, - * This macro is private and shouldn't be relied on. - */ -#ifndef __SBG_TYPEOF - #ifdef __cplusplus - #define __SBG_TYPEOF(x) decltype(x) - #elif defined(__GNUC__) - #define __SBG_TYPEOF(x) typeof(x) - #elif defined(__TI_COMPILER_VERSION__) - #define __SBG_TYPEOF(x) typeof(x) - #endif -#endif - -#ifndef SBG_CONST_CAST_AA - #ifdef __SBG_TYPEOF - #define SBG_CONST_CAST_AA(x) ((const __SBG_TYPEOF((x)[0][0])(*)[SBG_ARRAY_SIZE((x)[0])])(x)) - #else - #define SBG_CONST_CAST_AA(x) x - #endif -#endif - -#ifndef SBG_CONST_CAST_PP - #ifdef __SBG_TYPEOF - #define SBG_CONST_CAST_PP(x) ((const __SBG_TYPEOF(**(x))**)(x)) - #else - #define SBG_CONST_CAST_PP(x) x - #endif -#endif - -/*! - * __BASE_FILE__ is gcc specific - */ -#ifndef __GNUC__ -#ifndef __BASE_FILE__ - #define __BASE_FILE__ __FILE__ -#endif -#endif - -#ifdef __cplusplus - #define SBG_DELETE(p) if (p){delete (p); (p) = NULL;} - #define SBG_DELETE_ARRAY(p) if (p){delete[] (p); (p) = NULL;} - #define SBG_FREE(p) if (p){free(p); (p) = NULL;} - #define SBG_FREE_ARRAY(p) if (p){free(p); (p) = NULL;} -#else - #define SBG_DELETE if (p){free(p); (p) = NULL;} - #define SBG_DELETE_ARRAY if (p){free(p); (p) = NULL;} - #define SBG_FREE(p) if (p){free(p); (p) = NULL;} - #define SBG_FREE_ARRAY(p) if (p){free(p); (p) = NULL;} -#endif - -//----------------------------------------------------------------------// -//- Compiler definitions -// -//----------------------------------------------------------------------// - -/*! - * Macro used to abstract the compiler specific inline keyword. - */ -#ifndef SBG_INLINE - #if defined(_MSC_VER) - #define SBG_INLINE __inline - #else - #define SBG_INLINE static inline - #endif -#endif - -/*! - * Macro used to avoid compiler warning when a variable is not used. - */ -#ifndef SBG_UNUSED_PARAMETER - #define SBG_UNUSED_PARAMETER(x) (void)(x) -#endif - -/*! - * The fallthrough attribute is used to avoid compiler warning in swith case statements - * when an intentional break is missing - */ -#ifndef SBG_FALLTHROUGH - #if __cplusplus >= 201703L - #define SBG_FALLTHROUGH [[fallthrough]] /* introduced in C++ 17 */ - #elif defined(__GNUC__) - #define SBG_FALLTHROUGH __attribute__ ((fallthrough)) - #else - #define SBG_FALLTHROUGH - #endif -#endif - -//----------------------------------------------------------------------// -//- Macro used to defined packed structures -// -//----------------------------------------------------------------------// - -/*! - * This macro is used to define a new section of packed structures. - * All structures defined after this macro will be packed. - */ -#ifdef __GNUC__ - #define SBG_BEGIN_PACKED() -#elif defined(__TI_COMPILER_VERSION__) - #define SBG_BEGIN_PACKED() -#elif defined(_MSC_VER) - #define SBG_BEGIN_PACKED() __pragma(pack(push, 1)) -#else - #error you must byte-align these structures with the appropriate compiler directives -#endif - -/*! - * This macro is used to specify that a structure is packed. - */ -#ifdef __GNUC__ - #define SBG_PACKED __attribute__((packed)) -#elif defined(__TI_COMPILER_VERSION__) - #define SBG_PACKED __attribute__((packed)) -#elif defined(_MSC_VER) - #define SBG_PACKED -#else - #error you must byte-align these structures with the appropriate compiler directives -#endif - -/*! - * This macro is used to close the section of packed structures and return to the default packing. - */ -#ifdef __GNUC__ - #define SBG_END_PACKED() -#elif defined(__TI_COMPILER_VERSION__) - #define SBG_END_PACKED() -#elif defined(_MSC_VER) - #define SBG_END_PACKED() __pragma(pack(pop)) -#else - #error you must byte-align these structures with the appropriate compiler directives -#endif - -//----------------------------------------------------------------------// -//- Deprecation definitions -// -//----------------------------------------------------------------------// - -/*! - * Macro used to indicate that a function is deprecated. - */ -#ifdef __GNUC__ - #define SBG_DEPRECATED(func) func __attribute__ ((deprecated)) -#elif defined(__TI_COMPILER_VERSION__) - #define SBG_DEPRECATED(func) func __attribute__ ((deprecated)) -#elif defined(_MSC_VER) - #define SBG_DEPRECATED(func) __declspec(deprecated) func -#else - //#warning "WARNING: You need to implement SBG_DEPRECATED for this compiler" - #define SBG_DEPRECATED(func) func -#endif - -/*! - * Macro used to indicate that a macro is deprecated. - */ -#ifdef __GNUC__ - #define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) -#elif defined(_MSC_VER) -#define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) -#else - //#warning "WARNING: You need to implement SBG_DEPRECATED_MACRO for this compiler" - #define SBG_DEPRECATED_MACRO(func) func -#endif - -//----------------------------------------------------------------------// -//- Basic maths definitions -// -//----------------------------------------------------------------------// -#ifndef SBG_PI - #define SBG_PI 3.14159265358979323846 -#endif - -#ifndef SBG_PI_F - #define SBG_PI_F 3.14159265358979323846f -#endif - -/*! - * Returns the absolute value of x. - * - * \param[in] x Signed integer value. - * \return The absolute value of x. - */ -#ifndef sbgAbs - #define sbgAbs(x) (((x) < 0) ? -(x) : (x)) -#endif - -/*! - * Returns the maximum between a and b - * \param[in] a First operand. - * \param[in] b Second operand. - * \return The maximum between a and b. - */ -#ifndef sbgMax - #define sbgMax(a,b) (((a) > (b)) ? (a) : (b)) -#endif - -/*! - * Returns the minimum between a and b - * \param[in] a First operand. - * \param[in] b Second operand. - * \return The minimum between a and b. - */ -#ifndef sbgMin - #define sbgMin(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -/*! - * Clamp a value between minValue and maxValue ie minValue <= value <= maxValue - * \param[in] value First operand. - * \param[in] minValue First operand. - * \param[in] maxValue Second operand. - * \return The clamped value. - */ -#ifndef sbgClamp - #define sbgClamp(value, minValue, maxValue) (((value) < (minValue))?(minValue): ((value) > (maxValue)?maxValue:value)) -#endif - -/*! - * Integer division with a result rounded up. - * \param[in] n Dividend. - * \param[in] d Divisor. - */ -#ifndef sbgDivCeil - #define sbgDivCeil(n, d) (((n) + (d) - 1) / (d)) -#endif - -/*! - * Convert an angle from radians to degrees using double precision. - * \param[in] angle The angle to convert in radians. - * \return The converted angle in degrees. - */ -SBG_INLINE double sbgRadToDegD(double angle) -{ - return angle * 180.0 / SBG_PI; -} - -/*! - * Convert an angle from degrees to radians using double precision. - * \param[in] angle The angle to convert in degrees. - * \return The converted angle in radians. - */ -SBG_INLINE double sbgDegToRadD(double angle) -{ - return angle * SBG_PI / 180.0; -} - -/*! - * Convert an angle from radians to degrees using single (float) precision. - * \param[in] angle The angle to convert in radians. - * \return The converted angle in degrees. - */ -SBG_INLINE float sbgRadToDegF(float angle) -{ - return angle * 180.0f / SBG_PI_F; -} - -/*! - * Convert an angle from degrees to radians using single (float) precision. - * \param[in] angle The angle to convert in degrees. - * \return The converted angle in radians. - */ -SBG_INLINE float sbgDegToRadF(float angle) -{ - return angle * SBG_PI_F / 180.0f; -} - -#endif /* __SBG_DEFINES_H__ */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgErrorCodes.h b/sbg_ros_driver/external/sbgECom/common/sbgErrorCodes.h deleted file mode 100644 index 09a1d5e..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgErrorCodes.h +++ /dev/null @@ -1,106 +0,0 @@ -/*! - * \file sbgErrorCodes.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Header file that defines all error codes for SBG Systems libraries. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ERROR_CODES_H -#define SBG_ERROR_CODES_H - -// Local headers -#include "sbgDefines.h" - -//----------------------------------------------------------------------// -//- Errors code definitions -// -//----------------------------------------------------------------------// - -/*! - * Generic errors definitions for SBG Systems projects. - */ -typedef enum _SbgErrorCode -{ - SBG_NO_ERROR = 0, /*!< The operation was successfully executed. */ - SBG_ERROR, /*!< We have a generic error. */ - SBG_NULL_POINTER, /*!< A pointer is null. */ - SBG_INVALID_CRC, /*!< The received frame has an invalid CRC. */ - SBG_INVALID_FRAME, /*!< The received frame is invalid
*/ - /*!< We have received an unexpected frame (not the cmd we are waiting for or with an invalid data size.
*/ - /*!< This could be caused by a desync between questions and answers.
*/ - /*!< You should flush the serial port to fix this. */ - SBG_TIME_OUT, /*!< We have started to receive a frame but not the end. */ - SBG_WRITE_ERROR, /*!< All bytes hasn't been written. */ - SBG_READ_ERROR, /*!< All bytes hasn't been read. */ - SBG_BUFFER_OVERFLOW, /*!< A buffer is too small to contain so much data. */ - SBG_INVALID_PARAMETER, /*!< An invalid parameter has been found. */ - SBG_NOT_READY, /*!< A device isn't ready (Rx isn't ready for example). */ - SBG_MALLOC_FAILED, /*!< Failed to allocate a buffer. */ - SGB_CALIB_MAG_NOT_ENOUGH_POINTS, /*!< Not enough points were available to perform magnetometers calibration. */ - SBG_CALIB_MAG_INVALID_TAKE, /*!< The calibration procedure could not be properly executed due to insufficient precision. */ - SBG_CALIB_MAG_SATURATION, /*!< Saturation were detected when attempt to calibrate magnetos. */ - SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE, /*!< 2D calibration procedure could not be performed. */ - - SBG_DEVICE_NOT_FOUND, /*!< A device couldn't be founded or opened PC only error code */ - SBG_OPERATION_CANCELLED, /*!< An operation was canceled. PC only error code*/ - SBG_NOT_CONTINUOUS_FRAME, /*!< We have received a frame that isn't a continuous one. PC only error code*/ - - SBG_INCOMPATIBLE_HARDWARE, /*!< Hence valid; the command cannot be executed because of hardware incompatibility */ - SBG_INVALID_VERSION /*!< Incompatible version */ -} SbgErrorCode; - -//----------------------------------------------------------------------// -//- Error codes to string litteral conversion -// -//----------------------------------------------------------------------// - -/*! - * According to an error code, returns a human readable string. - * \param[in] errorCode The errorCode to convert to a string. - * \return Read only corresponding string. - */ -static inline const char *sbgErrorCodeToString(SbgErrorCode errorCode) -{ - /*! - * Array of string litterals that should be exactly ordered as the SbgErrorCode enum. - */ - static const char *sbgErrorCodeString[] = - { - "SBG_NO_ERROR", - "SBG_ERROR", - "SBG_NULL_POINTER", - "SBG_INVALID_CRC", - "SBG_INVALID_FRAME", - "SBG_TIME_OUT", - "SBG_WRITE_ERROR", - "SBG_READ_ERROR", - "SBG_BUFFER_OVERFLOW", - "SBG_INVALID_PARAMETER", - "SBG_NOT_READY", - "SBG_MALLOC_FAILED", - "SGB_CALIB_MAG_NOT_ENOUGH_POINTS", - "SBG_CALIB_MAG_INVALID_TAKE", - "SBG_CALIB_MAG_SATURATION", - "SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE", - "SBG_DEVICE_NOT_FOUND", - "SBG_OPERATION_CANCELLED", - "SBG_NOT_CONTINUOUS_FRAME", - "SBG_INCOMPATIBLE_HARDWARE", - "SBG_INVALID_VERSION" - }; - - assert(errorCode < SBG_ARRAY_SIZE(sbgErrorCodeString)); - return sbgErrorCodeString[errorCode]; -} - -#endif /* SBG_ERROR_CODES_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgPlatform.h b/sbg_ros_driver/external/sbgECom/common/sbgPlatform.h deleted file mode 100644 index bf45f40..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgPlatform.h +++ /dev/null @@ -1,89 +0,0 @@ -/*! - * \file sbgPlatform.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Header file that contains all platform specific definitions. - * - * This file should be modified to each targeted platform. - * For example, all common headers should be included from this file. - * - * The platform endianness should be defined here. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_PLATFORM_H -#define SBG_PLATFORM_H - -// System headers -#include - -// Local headers -#include "sbgDefines.h" -#include "sbgDebug.h" -#include "sbgErrorCodes.h" - -//----------------------------------------------------------------------// -//- Add here any additional includes you want to share -// -//----------------------------------------------------------------------// - -#ifdef _WIN32 -#include -#elif defined(__unix__) -#include -#endif - -//----------------------------------------------------------------------// -//- Specific timing methods to reimplement for your platform -// -//----------------------------------------------------------------------// - -/*! - * Returns the current time in ms. - * \return The current time in ms. - */ -SBG_COMMON_LIB_API uint32_t sbgGetTime(void); - -/*! - * Sleep for the specified number of ms. - * \param[in] ms Number of millisecondes to wait. - */ -SBG_COMMON_LIB_API void sbgSleep(uint32_t ms); - -//----------------------------------------------------------------------// -//- Specific logging methods to reimplement for your platform -// -//----------------------------------------------------------------------// - -/*! - * The method is called when one of the SBG_LOG_ERROR, SBG_LOG_WARNING, SBG_LOG_INFO or SBG_LOG_VERBOSE is called. - * It logs an error message with debug information and support a variable list of arguments - * \param[in] pFileName File name where the error occurred. - * \param[in] pFunctionName Function name where the error occurred. - * \param[in] line Line number where the error occurred. - * \param[in] pCategory Category for this log or "None" if no category has been specified. - * \param[in] logType Define if we have an error, a warning, an info or a debug log. - * \param[in] errorCode The error code associated with the message. - * \param[in] pFormat The error message that will be used with the variable list of arguments. - */ -SBG_COMMON_LIB_API void sbgPlatformDebugLogMsg(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pFormat, ...); - -//----------------------------------------------------------------------// -//- Errors and warning managment -// -//----------------------------------------------------------------------// - -/*! - * Run time assert that is raised if the expression is false. - * \param[in] expression The boolean expression to test, the execution is interrupted if the expression is evaluated as false. - */ -#define SBG_PLATFORM_ASSERT(expression) assert((expression)) - -#endif /* SBG_PLATFORM_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/sbgTypes.h b/sbg_ros_driver/external/sbgECom/common/sbgTypes.h deleted file mode 100644 index bfa4dca..0000000 --- a/sbg_ros_driver/external/sbgECom/common/sbgTypes.h +++ /dev/null @@ -1,172 +0,0 @@ -/*! - * \file sbgTypes.h - * \author SBG Systems (Raphael Siryani) - * \date 17 March 2015 - * - * \brief Header file that defines all scalar types. - * - * The platform endianness should be defined here. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_TYPES_H -#define SBG_TYPES_H - -// Standard headers -#include - -//----------------------------------------------------------------------// -//- Limits definitions -// -//----------------------------------------------------------------------// -#define SBG_MIN_INT_24 (-8388608l) -#define SBG_MAX_INT_24 (8388607l) -#define SBG_MAX_UINT_24 (16777215ul) - -#define SBG_MIN_INT_40 (-549755813887ll - 1) -#define SBG_MAX_INT_40 (549755813887ll) -#define SBG_MAX_UINT_40 (1099511627775ull) - -#define SBG_MIN_INT_48 (-140737488355327ll - 1) -#define SBG_MAX_INT_48 (140737488355327ll) -#define SBG_MAX_UINT_48 (281474976710655ull) - -#define SBG_MIN_INT_56 (-36028797018963967ll - 1) -#define SBG_MAX_INT_56 (36028797018963967ll) -#define SBG_MAX_UINT_56 (72057594037927935ull) - -//----------------------------------------------------------------------// -//- DEPRECATED: Scalar types definitions -// -//----------------------------------------------------------------------// -typedef unsigned char uint8; // 8 bits -typedef unsigned short uint16; // 16 bits -typedef unsigned int uint32; // 32 bits -typedef unsigned long long int uint64; // 64 bits - -typedef signed char int8; // 8 bits -typedef signed short int16; // 16 bits -typedef signed int int32; // 32 bits -typedef signed long long int int64; // 64 bits - - -//----------------------------------------------------------------------// -//- Misc types definitions -// -//----------------------------------------------------------------------// -typedef uint32_t sbgIpAddress; /*!< Define an IP v4 address stored in 4 bytes. The format is A.B.C.D, each component is 8 bits and stored in Big Endian. */ - -//------------------------------------------------------------------// -//- Type punning safe conversion unions -// -//------------------------------------------------------------------// - -/*! - * Used to get a uint32_t from a uint8_t array. - */ -typedef union _Uint8PtrToUint32Ptr -{ - uint8_t *m_pointerUint8; /*!< Set the address used to access the uint32_t. */ - uint32_t *m_pointerUint32; /*!< Store the unint32 value. */ -} Uint8PtrToUint32Ptr; - -/*! - * Union used to convert a buffer or 2 unit8 two's complement values to a int16_t - */ -typedef union _Uint8ToInt16 -{ - int16_t value; - uint8_t buffer[2]; -} Uint8ToInt16; - -/*! - * Union used to convert a buffer or 2 unit8 values to a uint16_t - */ -typedef union _Uint8ToUint16 -{ - uint16_t value; - uint8_t buffer[2]; -} Uint8ToUint16; - -/*! - * Union used to convert a buffer or 4 unit8 two's complement values to a int32_t - */ -typedef union _Uint8ToInt32 -{ - int32_t value; - uint8_t buffer[4]; -} Uint8ToInt32; - -/*! - * Union used to convert a buffer or 4 unit8 values to a uint32_t - */ -typedef union _Uint8ToUint32 -{ - uint32_t value; - uint8_t buffer[4]; -} Uint8ToUint32; - -/*! - * Union used to convert a buffer or 8 unit8 two's complement values to a int64_t - */ -typedef union _Uint8ToInt64 -{ - int64_t value; - uint8_t buffer[8]; -} Uint8ToInt64; - -/*! - * Union used to convert a buffer or 8 unit8 values to a uint64_t - */ -typedef union _Uint8ToUint64 -{ - uint64_t value; - uint8_t buffer[8]; -} Uint8ToUint64; - -/*! - * Union that allows type punning (access to a floating point number bits) - */ -typedef union _FloatNint -{ - float valF; - int32_t valI; - uint32_t valU; -} FloatNint; - -/*! - * Union that allows type punning (access to a double number bits) - */ -typedef union _DoubleNint -{ - double valF; - uint64_t valU; - int64_t valI; -} DoubleNint; - -/*! - * Structure that splits a 64bits - */ -typedef struct _Split64 -{ - uint32_t high; - uint32_t low; -} Split64; - -/*! - * This structure defines a date - */ -typedef struct _DateStructure -{ - uint16_t year; - uint8_t month; - uint8_t day; -} DateStructure; - -#endif /* SBG_TYPES_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.c b/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.c deleted file mode 100644 index e43ad8d..0000000 --- a/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.c +++ /dev/null @@ -1 +0,0 @@ -#include "sbgSplitBuffer.h" diff --git a/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.h b/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.h deleted file mode 100644 index 49ec8ee..0000000 --- a/sbg_ros_driver/external/sbgECom/common/splitBuffer/sbgSplitBuffer.h +++ /dev/null @@ -1,240 +0,0 @@ -/*! - * \file sbgSplitBuffer.h - * \author SBG Systems (Maxime Renaudet) - * \date 19 November 2013 - * - * \brief Helper methods used to handle a splittable buffer. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_SPLIT_BUFFER_H -#define SBG_SPLIT_BUFFER_H - -#include - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -//----------------------------------------------------------------------// -//- Structs definitions -// -//----------------------------------------------------------------------// - -/*! - * Defines the ECom transfer states - */ -typedef struct _SbgSplitBuffer -{ - uint8_t *pLinkedBuffer; /*!< Pointer on the buffer that needs to be split */ - size_t linkedBufferSize; /*!< Size of the original buffer */ - size_t subBufferSize; /*!< The size of the sub buffers */ - size_t subBufferNbr; /*!< The number of sub buffers in this split buffer */ -} SbgSplitBuffer; - -//----------------------------------------------------------------------// -//- Public methods definitions -// -//----------------------------------------------------------------------// - -/*! - * Initialize a split buffer for read operations. - * \param[in] pSplitBuffer Pointer to an allocated split buffer instance. - * \param[in] pBuffer The buffer that needs to be split (doesn't take the ownership). - * \param[in] bufferSize The total size of the buffer in bytes. - * \param[in] subBufferSize The size of each sub buffer in bytes. - */ -SBG_INLINE void sbgSplitBufferInitForRead(SbgSplitBuffer *pSplitBuffer, const void *pBuffer, size_t bufferSize, size_t subBufferSize) -{ - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Initialize split buffer parameters - // - pSplitBuffer->pLinkedBuffer = (uint8_t*)pBuffer; - pSplitBuffer->linkedBufferSize = bufferSize; - pSplitBuffer->subBufferSize = subBufferSize; - - // - // Compute sub buffer number - // - pSplitBuffer->subBufferNbr = (bufferSize + (subBufferSize - 1)) / subBufferSize; -} - -/*! - * Initialize a split buffer for write operations. - * \param[in] pSplitBuffer Pointer to an allocated split buffer instance. - * \param[in] pBuffer The buffer that needs to be split (doesn't take the ownership). - * \param[in] bufferSize The total size of the buffer in bytes. - * \param[in] subBufferSize The size of each sub buffer in bytes. - */ -SBG_INLINE void sbgSplitBufferInitForWrite(SbgSplitBuffer *pSplitBuffer, void *pBuffer, size_t bufferSize, size_t subBufferSize) -{ - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Initialize split buffer parameters - // - pSplitBuffer->pLinkedBuffer = (uint8_t*)pBuffer; - pSplitBuffer->linkedBufferSize = bufferSize; - pSplitBuffer->subBufferSize = subBufferSize; - - // - // Compute sub buffer number - // - pSplitBuffer->subBufferNbr = (bufferSize + (subBufferSize - 1)) / subBufferSize; -} - -/*! - * Returns the number of sub buffers that compose the whole buffer. - * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. - * \return The number of sub buffer the buffer has or 0 if there is an error. - */ -SBG_INLINE size_t sbgSplitBufferGetSubBufferNbr(const SbgSplitBuffer *pSplitBuffer) -{ - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Return subBufferNbr parameter - // - return pSplitBuffer->subBufferNbr; -} - -/*! - * Get one sub buffer given its index. - * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. - * \param[in] subBufferIdx Index of the sub buffer required. - * \return Pointer to the sub buffer or NULL if the subBuffer index is invalid. - */ -SBG_INLINE void *sbgSplitBufferGetSubBuffer(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) -{ - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Test input parameters - // - if (subBufferIdx < pSplitBuffer->subBufferNbr) - { - // - // Return pointer to buffer - // - return ((uint8_t*)pSplitBuffer->pLinkedBuffer + pSplitBuffer->subBufferSize*subBufferIdx); - } - else - { - // - // Invalid index - // - return NULL; - } -} - -/*! - * Return the offset in bytes of a sub buffer from the start of the buffer. - * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. - * \param[in] subBufferIdx Index of the sub buffer required. - * \return Offset to the sub buffer or 0 if the subBuffer index is invalid. - */ -SBG_INLINE size_t sbgSplitBufferGetSubBufferOffset(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) -{ - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Test input parameters - // - if (subBufferIdx < pSplitBuffer->subBufferNbr) - { - // - // Return pointer to buffer - // - return (pSplitBuffer->subBufferSize * subBufferIdx); - } - else - { - // - // Invalid index - // - return 0; - } -} - -/*! - * Get the size of a sub buffer given its index. - * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. - * \param[in] subBufferIdx Index of the sub buffer required. - * \return The size of the sub buffer of index subBufferIdx, or 0 if the subBuffer index is invalid. - */ -SBG_INLINE size_t sbgSplitBufferGetSubBufferSize(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) -{ - size_t subBufferSize = 0; - - // - // Test input arguments - // - assert(pSplitBuffer); - - // - // Test input parameters - // - if (pSplitBuffer->subBufferNbr > 0) - { - // - // Test that the sub buffer index is not the last one - // - if (subBufferIdx < (pSplitBuffer->subBufferNbr-1)) - { - // - // We can just return the sub buffer size because it's not the last sub buffer - // - subBufferSize = pSplitBuffer->subBufferSize; - } - else if (subBufferIdx == (pSplitBuffer->subBufferNbr-1) ) - { - // - // It's the last sub buffer so return the remaining size - // - subBufferSize = pSplitBuffer->linkedBufferSize - (subBufferIdx * pSplitBuffer->subBufferSize); - } - } - - // - // Return computed size - // - return subBufferSize; -} - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_SPLIT_BUFFER_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.c b/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.c deleted file mode 100644 index 9f4955a..0000000 --- a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.c +++ /dev/null @@ -1 +0,0 @@ -#include "sbgStreamBuffer.h" diff --git a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.h b/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.h deleted file mode 100644 index 5d1feb9..0000000 --- a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBuffer.h +++ /dev/null @@ -1,41 +0,0 @@ -/*! - * \file sbgStreamBuffer.h - * \author SBG Systems (Raphael Siryani) - * \date 02 January 2013 - * - * \brief Used to read/write data from/to a memory buffer stream. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_STREAM_BUFFER_H -#define SBG_STREAM_BUFFER_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include "sbgStreamBufferLE.h" -#include "sbgStreamBufferBE.h" - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_STREAM_BUFFER_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferBE.h b/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferBE.h deleted file mode 100644 index 99d301e..0000000 --- a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferBE.h +++ /dev/null @@ -1,1911 +0,0 @@ -/*! - * \file sbgStreamBufferBE.h - * \author SBG Systems (Maxime Renaudet) - * \date 17 February 2015 - * - * \brief Specific method of stream buffer for little endian readings/writings. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_STREAM_BUFFER_BE_H -#define SBG_STREAM_BUFFER_BE_H - -#include "sbgStreamBufferCommon.h" - -//----------------------------------------------------------------------// -//- Read operations methods -// -//----------------------------------------------------------------------// - -/*! - * Read an int16_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int16_t sbgStreamBufferReadInt16BE(SbgStreamBuffer *pHandle) -{ - int16_t bytesValues[2]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - bytesValues[0] = *((int16_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int16_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[0] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[1] | (bytesValues[0] << 8); - #else - return bytesValues[0] | (bytesValues[1] << 8); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint16_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint16_t sbgStreamBufferReadUint16BE(SbgStreamBuffer *pHandle) -{ - uint16_t bytesValues[2]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint16_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[0] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[1] | (bytesValues[0] << 8); - #else - return bytesValues[0] | (bytesValues[1] << 8); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int24 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int32_t sbgStreamBufferReadInt24BE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt32 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 24 bits - // - return value.value >> (32-24); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint24 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint32_t sbgStreamBufferReadUint24BE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint32 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 24 bits - // - return value.value >> (32-24); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int32_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int32_t sbgStreamBufferReadInt32BE(SbgStreamBuffer *pHandle) -{ - int32_t bytesValues[4]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int32_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[3] = *(pHandle->pCurrentPtr++); - bytesValues[2] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[0] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); - #else - return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint32_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint32_t sbgStreamBufferReadUint32BE(SbgStreamBuffer *pHandle) -{ - uint32_t bytesValues[4]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint32_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[3] = *(pHandle->pCurrentPtr++); - bytesValues[2] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[0] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); - #else - return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int40 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt40BE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 40 bits - // - return value.value >> (64-40); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint40 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint40BE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 40 bits - // - return value.value >> (64-40); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int48 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt48BE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 48 bits - // - return value.value >> (64-48); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint48 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint48BE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 48 bits - // - return value.value >> (64-48); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int56 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt56BE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 56 bits - // - return value.value >> (64-56); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint56 from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadUint56BE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB - #else - // - // Read the each bytes - // - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - #endif - - // - // Shift the value to handle the sign correctly for a 56 bits - // - return value.value >> (64-56); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int64_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt64BE(SbgStreamBuffer *pHandle) -{ - int64_t lowPart; - int64_t highPart; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - lowPart = *((int64_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int64_t); - - return lowPart; - #else - // - // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations - // - highPart = sbgStreamBufferReadUint32BE(pHandle); - lowPart = sbgStreamBufferReadUint32BE(pHandle); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return (lowPart << 32) | highPart; - #else - return lowPart | (highPart << 32); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0ll; -} - -/*! - * Read an uint64_t from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint64BE(SbgStreamBuffer *pHandle) -{ - uint64_t lowPart; - uint64_t highPart; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the current value - // - lowPart = *((uint64_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint64_t); - - return lowPart; - #else - // - // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations - // - highPart = sbgStreamBufferReadUint32BE(pHandle); - lowPart = sbgStreamBufferReadUint32BE(pHandle); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return (lowPart << 32) | highPart; - #else - return lowPart | (highPart << 32); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0ll; -} - -/*! - * Read a size_t from a stream buffer that has been stored in a uint32_t (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE size_t sbgStreamBufferReadSizeT32BE(SbgStreamBuffer *pHandle) -{ - // - // Just call the read method for uint32_t - // We assume that a size_t is at least 32 bits on all platforms - // - return (size_t)sbgStreamBufferReadUint32BE(pHandle); -} - -/*! - * Read a size_t from a stream buffer that has been stored in a uint64_t (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE size_t sbgStreamBufferReadSizeT64BE(SbgStreamBuffer *pHandle) -{ - uint64_t size; - - // - // Just call the read method for uint64_t - // - size = sbgStreamBufferReadUint64BE(pHandle); - - // - // Make sure the read size can fit in the size_t in size_t is 32 bits - // - assert((sizeof(size_t) == 8) || ((sizeof(size_t) == 4) && (size <= UINT32_MAX))); - - // - // Return the read value - // - return (size_t)size; -} - -/*! - * Read an float from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE float sbgStreamBufferReadFloatBE(SbgStreamBuffer *pHandle) -{ - FloatNint floatInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float)) - { - // - // Read the float as an uint32_t - // - floatInt.valU = sbgStreamBufferReadUint32BE(pHandle); - - // - // Return the float using an union to avoid compiller cast - // - return floatInt.valF; - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0.0f; -} - -/*! - * Read an double from a stream buffer (Big endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE double sbgStreamBufferReadDoubleBE(SbgStreamBuffer *pHandle) -{ - DoubleNint doubleInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double)) - { - // - // Read the float as an uint64_t - // - doubleInt.valU = sbgStreamBufferReadUint64BE(pHandle); - - // - // Return the double using an union to avoid compiller cast - // - return doubleInt.valF; - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0.0; -} - -//----------------------------------------------------------------------// -//- Write operations methods -// -//----------------------------------------------------------------------// - -/*! - * Write an int16_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt16BE(SbgStreamBuffer *pHandle, int16_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((int16_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int16_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint16_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16BE(SbgStreamBuffer *pHandle, uint16_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((uint16_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint16_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an int24 into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt24BE(SbgStreamBuffer *pHandle, int32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Make sure that the value is within 24 bit bonds - // - if ( (value >= SBG_MIN_INT_24) && (value <= SBG_MAX_INT_24) ) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(int8_t)) - { - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - else - { - // - // The input value is not within a 24 bit integer bounds - // - pHandle->errorCode = SBG_INVALID_PARAMETER; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint24 into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint24BE(SbgStreamBuffer *pHandle, uint32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Make sure that the value is within 24 bit bonds - // - if (value <= SBG_MAX_UINT_24) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - else - { - // - // The input value is not within a 24 bit integer bounds - // - pHandle->errorCode = SBG_INVALID_PARAMETER; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an int32_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt32BE(SbgStreamBuffer *pHandle, int32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((int32_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int32_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint32_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32BE(SbgStreamBuffer *pHandle, uint32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((uint32_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint32_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an int64_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt64BE(SbgStreamBuffer *pHandle, int64_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((int64_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int64_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint64_t into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint64BE(SbgStreamBuffer *pHandle, uint64_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Write the value - // - *((uint64_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint64_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an size_t into a stream buffer as a uint32_t (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT32BE(SbgStreamBuffer *pHandle, size_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Make sure the provided size_t value doesn't exceed a uint32_t storage - // - assert(value <= UINT32_MAX); - - // - // Call the write method to store a uint32_t - // - return sbgStreamBufferWriteUint32BE(pHandle, (uint32_t)value); -} - -/*! - * Write an size_t into a stream buffer as a uint64_t (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64BE(SbgStreamBuffer *pHandle, size_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Call the write method to store a uint64_t - // - return sbgStreamBufferWriteUint64BE(pHandle, (uint64_t)value); -} - -/*! - * Write an float into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatBE(SbgStreamBuffer *pHandle, float value) -{ - FloatNint floatInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We use an union to avoid compiler cast - // - floatInt.valF = value; - - // - // Write this float as an uint32_t - // - return sbgStreamBufferWriteUint32BE(pHandle, floatInt.valU); - } - - return pHandle->errorCode; -} - -/*! - * Write an double into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleBE(SbgStreamBuffer *pHandle, double value) -{ - DoubleNint doubleInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We use an union to avoid compiler cast - // - doubleInt.valF = value; - - // - // Write this float as an uint64_t - // - return sbgStreamBufferWriteUint64BE(pHandle, doubleInt.valU); - } - - return pHandle->errorCode; -} - -/*! - * Read a C String from a stream buffer (Big Endian Version). - * - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \param[out] pString Buffer that can hold the read NULL terminated C string. - * \param[in] maxSize Maximum number of bytes that can be stored in pString (including the NULL char). - * \return SBG_NO_ERROR if the string has been read successfully from the stream buffer. - * SBG_BUFFER_OVERFLOW if the provided string isn't big enough to hold the read string - */ -SBG_INLINE SbgErrorCode sbgStreamBufferReadStringBE(SbgStreamBuffer *pHandle, char *pString, size_t maxSize) -{ - size_t stringLength; - - // - // Check input parameters - // - assert(pHandle); - assert(pString); - assert(maxSize > 0); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // The C string are stored in a stream buffer with a 32 bit size length and then the buffer itself - // - stringLength = sbgStreamBufferReadSizeT32BE(pHandle); - - if (stringLength <= maxSize) - { - // - // Read the string buffer itself - // - sbgStreamBufferReadBuffer(pHandle, pString, stringLength); - } - else - { - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - SBG_LOG_ERROR(pHandle->errorCode, "Trying to store a string of %u bytes into a buffer of %u bytes.", stringLength, maxSize); - } - } - - return pHandle->errorCode; -} - -/*! - * Write a NULL terminated C String into a stream buffer (Big Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] pString NULL terminated C String to write to the stream buffer. - * \return SBG_NO_ERROR if the string has been written successfully to the stream buffer. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteStringBE(SbgStreamBuffer *pHandle, const char *pString) -{ - size_t stringLength; - - // - // Check input parameters - // - assert(pHandle); - assert(pString); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We write C string using a 32 bit size_t as the string length including the NULL char - // We should thus make sure the provided string isn't too big to fit in a 32 bits size_t - // - stringLength = strlen(pString)+1; - - if (stringLength <= UINT32_MAX) - { - // - // Write the string length - // - if (sbgStreamBufferWriteSizeT32BE(pHandle, stringLength) == SBG_NO_ERROR) - { - // - // Write the string buffer itself - // - sbgStreamBufferWriteBuffer(pHandle, pString, stringLength); - } - } - else - { - pHandle->errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(pHandle->errorCode, "The provided string is too big to fit in a 32 bit size_t"); - } - } - - return pHandle->errorCode; -} - -#endif /* SBG_STREAM_BUFFER_BE_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h b/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h deleted file mode 100644 index 9fe4722..0000000 --- a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h +++ /dev/null @@ -1,854 +0,0 @@ -/*! - * \file sbgStreamBufferCommon.h - * \author SBG Systems (Raphael Siryani) - * \date 02 January 2013 - * - * \brief Used to read/write data from/to a memory buffer stream. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_STREAM_BUFFER_COMMON_H -#define SBG_STREAM_BUFFER_COMMON_H - -#include - -//----------------------------------------------------------------------// -//- General definitions -// -//----------------------------------------------------------------------// - -/*! - * The default method should read and write using the platform endianness - */ -#if SBG_CONFIG_BIG_ENDIAN == 1 - /*! - * The platform is a big endian one so default methods should use big endian byte order. - */ - #define sbgStreamBufferReadUint16 sbgStreamBufferReadUint16BE - #define sbgStreamBufferReadInt16 sbgStreamBufferReadInt16BE - - #define sbgStreamBufferReadUint24 sbgStreamBufferReadUint24BE - #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24BE - - #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32BE - #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32BE - - #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40BE - #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40BE - - #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48BE - #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48BE - - #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56BE - #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56BE - - #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64BE - #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64BE - - #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32BE - #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64BE - - #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatBE - #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleBE - - #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16BE - #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16BE - - #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24BE - #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24BE - - #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32BE - #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32BE - - #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64BE - #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64BE - - #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32BE - #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64BE - - #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatBE - #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleBE - - #define sbgStreamBufferReadString sbgStreamBufferReadStringBE - #define sbgStreamBufferWriteString sbgStreamBufferWriteStringBE -#else - /*! - * The platform is a little endian one so default methods should use little endian byte order. - */ - #define sbgStreamBufferReadUint16 sbgStreamBufferReadUint16LE - #define sbgStreamBufferReadInt16 sbgStreamBufferReadInt16LE - - #define sbgStreamBufferReadUint24 sbgStreamBufferReadUint24LE - #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24LE - - #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32LE - #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32LE - - #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40LE - #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40LE - - #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48LE - #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48LE - - #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56LE - #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56LE - - #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64LE - #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64LE - - #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32LE - #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64LE - - #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatLE - #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleLE - - #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16LE - #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16LE - - #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24LE - #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24LE - - #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32LE - #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32LE - - #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64LE - #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64LE - - #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32LE - #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64LE - - #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatLE - #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleLE - - #define sbgStreamBufferReadString sbgStreamBufferReadStringLE - #define sbgStreamBufferWriteString sbgStreamBufferWriteStringLE -#endif - -/*! - * Some methods are common between big and little endian. - * This definitions just unify the API. - */ -#define sbgStreamBufferReadUint8LE sbgStreamBufferReadUint8 -#define sbgStreamBufferReadInt8LE sbgStreamBufferReadInt8 -#define sbgStreamBufferReadBooleanLE sbgStreamBufferReadBoolean -#define sbgStreamBufferReadBufferLE sbgStreamBufferReadBuffer - -#define sbgStreamBufferWriteUint8LE sbgStreamBufferWriteUint8 -#define sbgStreamBufferWriteInt8LE sbgStreamBufferWriteInt8 -#define sbgStreamBufferWriteBooleanLE sbgStreamBufferWriteBoolean -#define sbgStreamBufferWriteBufferLE sbgStreamBufferWriteBuffer - -#define sbgStreamBufferReadUint8BE sbgStreamBufferReadUint8 -#define sbgStreamBufferReadInt8BE sbgStreamBufferReadInt8 -#define sbgStreamBufferReadBooleanBE sbgStreamBufferReadBoolean -#define sbgStreamBufferReadBufferBE sbgStreamBufferReadBuffer - -#define sbgStreamBufferWriteUint8BE sbgStreamBufferWriteUint8 -#define sbgStreamBufferWriteInt8BE sbgStreamBufferWriteInt8 -#define sbgStreamBufferWriteBooleanBE sbgStreamBufferWriteBoolean -#define sbgStreamBufferWriteBufferBE sbgStreamBufferWriteBuffer - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Stream buffer modes. - */ -typedef enum _SbgSBMode -{ - SB_MODE_READ, /*!< This stream buffer can perform read operations. */ - SB_MODE_WRITE /*!< This stream buffer can perform write operations. */ -} SbgSBMode; - -/*! - * Enum used to define all seek modes - */ -typedef enum _SbgSBSeekOrigin -{ - SB_SEEK_SET, /*!< The offset is referenced to the begining of the stream. */ - SB_SEEK_CUR_INC, /*!< The offset is referenced to the current cursor position and increment the current cursor. */ - SB_SEEK_CUR_DEC, /*!< The offset is referenced to the current cursor position and decrement the current cursor. */ - SB_SEEK_END /*!< The offset is referenced to the end of the stream. */ -} SbgSBSeekOrigin; - -/*! - * Defines a stream buffer. - */ -typedef struct _SbgStreamBuffer -{ - SbgSBMode modes; /*!< Defines the stream buffer modes (read/write). */ - size_t bufferSize; /*!< Size in bytes of the linked buffer. */ - uint8_t *pBufferPtr; /*!< Pointer to the buffer linked with this stream. */ - uint8_t *pCurrentPtr; /*!< Current pointer within the buffer. */ - SbgErrorCode errorCode; /*!< Current error code on stream buffer. */ -} SbgStreamBuffer; - -//----------------------------------------------------------------------// -//- Common operations methods -// -//----------------------------------------------------------------------// - -/*! - * Initialize a stream buffer for both read and write operations and link it to a buffer. - * \param[in] pHandle Handle on an allocated stream buffer. - * \param[in] pLinkedBuffer Pointer on an allocated buffer to link with this stream. - * \param[in] bufferSize Size in bytes of the linked buffer. - * \return SBG_NO_ERROR if the stream buffer has been initialized successfully. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize) -{ - // - // Check input parameters - // - assert(pHandle); - assert(pLinkedBuffer); - - // - // Initialize stream parameters - // - pHandle->modes = SB_MODE_WRITE; - pHandle->bufferSize = bufferSize; - pHandle->errorCode = SBG_NO_ERROR; - - // - // Initialize the buffer - // - pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer; - pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer; - - // - // For now, we don't handle any error, maybe we could add checks in debug mode only - // - return SBG_NO_ERROR; -} - -/*! - * Initialize a stream buffer for both read and write operations and link it to a buffer. - * \param[in] pHandle Handle on an allocated stream buffer. - * \param[in] pLinkedBuffer Pointer on an allocated buffer to link with this stream. - * \param[in] bufferSize Size in bytes of the linked buffer. - * \return SBG_NO_ERROR if the stream buffer has been initialized successfully. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize) -{ - // - // Check input parameters - // - assert(pHandle); - assert(pLinkedBuffer); - - // - // Initialize stream parameters - // - pHandle->modes = SB_MODE_READ; - pHandle->bufferSize = bufferSize; - pHandle->errorCode = SBG_NO_ERROR; - - // - // Initialize the buffer - // - pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer; - pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer; - - // - // For now, we don't handle any error, maybe we could add checks in debug mode only - // - return SBG_NO_ERROR; -} - -/*! - * Return the error code that has occurred on the last stream buffer operation. - * \param[in] pHandle Pointer to a valid Stream Buffer handle - * \return Last stream buffer error code - */ -SBG_INLINE SbgErrorCode sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Return error code - // - return pHandle->errorCode; -} - -/*! - * Clear the last error code that has occurred on the last stream buffer operation. - * \param[in] pHandle Pointer to a valid Stream Buffer handle - */ -SBG_INLINE void sbgStreamBufferClearLastError(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Return error code - // - pHandle->errorCode = SBG_NO_ERROR; -} - -/*! - * Returns the size in bytes of this stream. - * The size is the linked buffer total size in bytes. - * For example, for a SbgStreamBuffer linked with a buffer of 256 bytes, - * this method will always returns 256 even if no data has been written or read. - * \param[in] pHandle Valid handle on a stream buffer. - * \return The allocated size of the linked buffer in bytes. - */ -SBG_INLINE size_t sbgStreamBufferGetSize(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Return the linked buffer size - // - return pHandle->bufferSize; -} - -/*! - * Returns the length in bytes of this stream. - * The length is computed using the current cursor position. - * If no data has been read or written, this method will return 0. - * If 4 uint32_t has been written, it should return 16. - * \param[in] pHandle Valid handle on a stream buffer. - * \return The current cursor position in bytes. - */ -SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Return the number of bytes between the begin of the stream and the current pointer - // - return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr); -} - -/*! - * Returns the available space in this stream. - * The available space is just the delta between the linked buffer size - * and the current buffer length (cursor position). - * \param[in] pHandle Valid handle on a stream buffer. - * \return The space available in this stream buffer in bytes. - */ -SBG_INLINE size_t sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Return the space left in bytes - // - return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle); -} - -/*! - * Move the current cursor position. - * \param[in] pHandle Valid handle on a stream buffer. - * \param[in] offset Offset in bytes to apply (only positive). - * \param[in] origin Origin reference point to apply the offset from. - * \return SBG_NO_ERROR if the stream current cursor position has been moved. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // According to the origin reference point - // - switch (origin) - { - case SB_SEEK_SET: - pHandle->pCurrentPtr = pHandle->pBufferPtr + offset; - break; - case SB_SEEK_CUR_INC: - pHandle->pCurrentPtr += offset; - break; - case SB_SEEK_CUR_DEC: - pHandle->pCurrentPtr -= offset; - break; - case SB_SEEK_END: - pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset); - break; - default: - pHandle->errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter"); - } - - // - // Make sure that no error has occurred - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if the current ptr is still within the buffer bounds - // - if (pHandle->pCurrentPtr < pHandle->pBufferPtr) - { - // - // We are before the buffer so clamp to the begining of the buffer and raise an error - // - pHandle->pCurrentPtr = pHandle->pBufferPtr; - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - - // - // Stream buffer underflow - // - SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer"); - } - else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize) - { - // - // We are after the buffer so clamp to the end of the buffer and raise an error - // - pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize; - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - - // - // Stream buffer overflow - // - SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer"); - } - } - } - - return pHandle->errorCode; -} - -/*! - * Returns the current offset in bytes from the beginning of the stream. - * \param[in] pHandle Valid handle on a stream buffer. - * \return Current offset in bytes from the beginning. - */ -SBG_INLINE size_t sbgStreamBufferTell(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr; -} - -/*! - * Returns a pointer on the internal buffer. - * \param[in] pHandle Valid handle on a stream buffer. - * \return Pointer on the begining of the internal buffer. - */ -SBG_INLINE void *sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - return pHandle->pBufferPtr; -} - -/*! - * Returns a pointer on the internal buffer at the current cursor. - * \param[in] pHandle Valid handle on a stream buffer. - * \return Pointer on the current cursor of the internal buffer. - */ -SBG_INLINE void *sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - return pHandle->pCurrentPtr; -} - -//----------------------------------------------------------------------// -//- Read operations methods -// -//----------------------------------------------------------------------// - -/*! - * Read an int8_t from a stream buffer. - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int8_t sbgStreamBufferReadInt8(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t)) - { - // - // Read the byte - // - return *((int8_t*)(pHandle->pCurrentPtr++)); - } - else - { - // - // We have a buffer overflow - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint8_t from a stream buffer. - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint8_t sbgStreamBufferReadUint8(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) - { - // - // Read the byte - // - return *((uint8_t*)(pHandle->pCurrentPtr++)); - } - else - { - // - // We have a buffer overflow - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read a boolean from a stream buffer. - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or false if we have an error. - */ -SBG_INLINE bool sbgStreamBufferReadBoolean(SbgStreamBuffer *pHandle) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) - { - // - // Read the byte and check if the value is different than zero or not - // - if (*((uint8_t*)(pHandle->pCurrentPtr++))) - { - return true; - } - else - { - return false; - } - } - else - { - // - // We have a buffer overflow - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return false - // - return false; -} - -/*! - * Read a buffer from a stream buffer. - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \param[out] pBuffer Allocated buffer used to hold read data. - * \param[in] numBytesToRead Number of bytes to read from the stream buffer and to store in pBuffer. - * \return SBG_NO_ERROR if the data has been read. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferReadBuffer(SbgStreamBuffer *pHandle, void *pBuffer, size_t numBytesToRead) -{ - // - // Check input parameters - // - assert(pHandle); - assert((pBuffer) || (numBytesToRead == 0)); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if enough bytes in stream - // - if (sbgStreamBufferGetSpace(pHandle) >= numBytesToRead) - { - // - // Copy from the stream buffer to the output buffer - // - memcpy(pBuffer, pHandle->pCurrentPtr, numBytesToRead); - - // - // Update the current pointer - // - pHandle->pCurrentPtr += numBytesToRead; - } - else - { - // - // Not enough data in stream - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -//----------------------------------------------------------------------// -//- Write operations methods -// -//----------------------------------------------------------------------// - -/*! - * Write an int8_t into a stream buffer - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt8(SbgStreamBuffer *pHandle, int8_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t)) - { - // - // Write each byte - // - *(pHandle->pCurrentPtr++) = (int8_t)(value); - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint8_t into a stream buffer - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint8(SbgStreamBuffer *pHandle, uint8_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) - { - // - // Write each byte - // - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write a boolean into a stream buffer - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteBoolean(SbgStreamBuffer *pHandle, bool value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) - { - // - // Write the boolean as an uint8_t value (1 byte) - // - if (value) - { - *(pHandle->pCurrentPtr++) = 1; - } - else - { - *(pHandle->pCurrentPtr++) = 0; - } - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write a buffer to a stream buffer. - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[out] pBuffer Buffer to write into the stream buffer. - * \param[in] numBytesToRead Number of bytes to write to the stream buffer. - * \return SBG_NO_ERROR if the data has been written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteBuffer(SbgStreamBuffer *pHandle, const void *pBuffer, size_t numBytesToWrite) -{ - // - // Check input parameters - // - assert(pHandle); - assert((pBuffer) || (numBytesToWrite == 0)); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= numBytesToWrite) - { - // - // Copy from the stream buffer to the output buffer - // - memcpy(pHandle->pCurrentPtr, pBuffer, numBytesToWrite); - - // - // Update the current pointer - // - pHandle->pCurrentPtr += numBytesToWrite; - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -#endif /* SBG_STREAM_BUFFER_COMMON_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferLE.h b/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferLE.h deleted file mode 100644 index 55d78ad..0000000 --- a/sbg_ros_driver/external/sbgECom/common/streamBuffer/sbgStreamBufferLE.h +++ /dev/null @@ -1,1912 +0,0 @@ -/*! - * \file sbgStreamBufferLE.h - * \author SBG Systems (Maxime Renaudet) - * \date 17 February 2015 - * - * \brief Specific method of stream buffer for little endian readings/writings. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_STREAM_BUFFER_LE_H -#define SBG_STREAM_BUFFER_LE_H - -#include "sbgStreamBufferCommon.h" - -//----------------------------------------------------------------------// -//- Read operations methods -// -//----------------------------------------------------------------------// - -/*! - * Read an int16_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int16_t sbgStreamBufferReadInt16LE(SbgStreamBuffer *pHandle) -{ - int16_t bytesValues[2]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - bytesValues[0] = *((int16_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int16_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[0] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[1] | (bytesValues[0] << 8); - #else - return bytesValues[0] | (bytesValues[1] << 8); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint16_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle) -{ - uint16_t bytesValues[2]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint16_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[0] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[1] | (bytesValues[0] << 8); - #else - return bytesValues[0] | (bytesValues[1] << 8); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int24 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int32_t sbgStreamBufferReadInt24LE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt32 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 24 bits - // - return value.value >> (32-24); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint24 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint32_t sbgStreamBufferReadUint24LE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint32 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 24 bits - // - return value.value >> (32-24); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int32_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int32_t sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle) -{ - int32_t bytesValues[4]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int32_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[0] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[2] = *(pHandle->pCurrentPtr++); - bytesValues[3] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); - #else - return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint32_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint32_t sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle) -{ - uint32_t bytesValues[4]; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint32_t); - - return bytesValues[0]; - #else - // - // Read the each bytes - // - bytesValues[0] = *(pHandle->pCurrentPtr++); - bytesValues[1] = *(pHandle->pCurrentPtr++); - bytesValues[2] = *(pHandle->pCurrentPtr++); - bytesValues[3] = *(pHandle->pCurrentPtr++); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); - #else - return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int40 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt40LE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 40 bits - // - return value.value >> (64-40); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint40 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadUint40LE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 40 bits - // - return value.value >> (64-40); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int48 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt48LE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 48 bits - // - return value.value >> (64-48); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint48 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint48LE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 48 bits - // - return value.value >> (64-48); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int56 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt56LE(SbgStreamBuffer *pHandle) -{ - Uint8ToInt64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 56 bits - // - return value.value >> (64-56); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an uint56 from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint56LE(SbgStreamBuffer *pHandle) -{ - Uint8ToUint64 value; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) - { - // - // Make sure the value is zero init - // - value.value = 0; - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - // - // Read the each bytes - // - value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[1] = *(pHandle->pCurrentPtr++); - value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB - #else - // - // Read the each bytes - // - value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB - value.buffer[2] = *(pHandle->pCurrentPtr++); - value.buffer[3] = *(pHandle->pCurrentPtr++); - value.buffer[4] = *(pHandle->pCurrentPtr++); - value.buffer[5] = *(pHandle->pCurrentPtr++); - value.buffer[6] = *(pHandle->pCurrentPtr++); - value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB - #endif - - // - // Shift the value to handle the sign correctly for a 56 bits - // - return value.value >> (64-56); - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0; -} - -/*! - * Read an int64_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE int64_t sbgStreamBufferReadInt64LE(SbgStreamBuffer *pHandle) -{ - int64_t lowPart; - int64_t highPart; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - lowPart = *((int64_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int64_t); - - return lowPart; - #else - // - // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations - // - lowPart = sbgStreamBufferReadUint32LE(pHandle); - highPart = sbgStreamBufferReadUint32LE(pHandle); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return (lowPart << 32) | highPart; - #else - return lowPart | (highPart << 32); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0ll; -} - -/*! - * Read an uint64_t from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE uint64_t sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle) -{ - uint64_t lowPart; - uint64_t highPart; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Read the current value - // - lowPart = *((uint64_t*)pHandle->pCurrentPtr); - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint64_t); - - return lowPart; - #else - // - // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations - // - lowPart = sbgStreamBufferReadUint32LE(pHandle); - highPart = sbgStreamBufferReadUint32LE(pHandle); - - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - return (lowPart << 32) | highPart; - #else - return lowPart | (highPart << 32); - #endif - #endif - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0ull; -} - -/*! - * Read a size_t from a stream buffer that has been stored in a uint32_t (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE size_t sbgStreamBufferReadSizeT32LE(SbgStreamBuffer *pHandle) -{ - // - // Just call the read method for uint32_t - // We assume that a size_t is at least 32 bits on all platforms - // - return (size_t)sbgStreamBufferReadUint32LE(pHandle); -} - -/*! - * Read a size_t from a stream buffer that has been stored in a uint64_t (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE size_t sbgStreamBufferReadSizeT64LE(SbgStreamBuffer *pHandle) -{ - uint64_t size; - - // - // Just call the read method for uint64_t - // - size = sbgStreamBufferReadUint64LE(pHandle); - - // - // Make sure the read size can fit in the size_t in size_t is 32 bits - // - assert((sizeof(size_t) == 8) || ((sizeof(size_t) == 4) && (size <= UINT32_MAX))); - - // - // Return the read value - // - return (size_t)size; -} - -/*! - * Read an float from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle) -{ - FloatNint floatInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float)) - { - // - // Read the float as an uint32_t - // - floatInt.valU = sbgStreamBufferReadUint32LE(pHandle); - - // - // Return the float using an union to avoid compiler cast - // - return floatInt.valF; - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0.0f; -} - -/*! - * Read an double from a stream buffer (Little endian version). - * \param[in] pHandle Valid stream buffer handle that supports read operations. - * \return The read value or 0 if we have an error. - */ -SBG_INLINE double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle) -{ - DoubleNint doubleInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double)) - { - // - // Read the float as an uint64_t - // - doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle); - - // - // Return the double using an union to avoid compiler cast - // - return doubleInt.valF; - } - else - { - // - // We have a buffer overflow so return 0 - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // If we are here, it means we have an error so return 0 - // - return 0.0; -} - -//----------------------------------------------------------------------// -//- Write operations methods -// -//----------------------------------------------------------------------// - -/*! - * Write an int16_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt16LE(SbgStreamBuffer *pHandle, int16_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((int16_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int16_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint16_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((uint16_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint16_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - - -/*! - * Write an int24 into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt24LE(SbgStreamBuffer *pHandle, int32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Make sure that the value is within 24 bit bonds - // - if ( (value >= SBG_MIN_INT_24) && (value <= SBG_MAX_INT_24) ) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(int8_t)) - { - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - else - { - // - // The input value is not within a 24 bit integer bounds - // - pHandle->errorCode = SBG_INVALID_PARAMETER; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint24 into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint24LE(SbgStreamBuffer *pHandle, uint32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Make sure that the value is within 24 bit bonds - // - if (value <= SBG_MAX_UINT_24) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) - { - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - else - { - // - // The input value is not within a 24 bit integer bounds - // - pHandle->errorCode = SBG_INVALID_PARAMETER; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an int32_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt32LE(SbgStreamBuffer *pHandle, int32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((int32_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int32_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint32_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32LE(SbgStreamBuffer *pHandle, uint32_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((uint32_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint32_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an int64_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt64LE(SbgStreamBuffer *pHandle, int64_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((int64_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(int64_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an uint64_t into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint64LE(SbgStreamBuffer *pHandle, uint64_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // Test if we can access this item - // - if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) - { - // - // Test if the platform supports un-aligned access and if the endianness is the same - // - #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) - // - // Write the value - // - *((uint64_t*)(pHandle->pCurrentPtr)) = value; - - // - // Increment the current pointer - // - pHandle->pCurrentPtr += sizeof(uint64_t); - #else - // - // Store data according to platform endianness - // - #if (SBG_CONFIG_BIG_ENDIAN == 1) - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - #else - *(pHandle->pCurrentPtr++) = (uint8_t)(value); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); - *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); - #endif - #endif - } - else - { - // - // We are accessing a data that is outside the stream buffer - // - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return pHandle->errorCode; -} - -/*! - * Write an size_t into a stream buffer as a uint32_t (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT32LE(SbgStreamBuffer *pHandle, size_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Make sure the provided size_t value doesn't exceed a uint32_t storage - // - assert(value <= UINT32_MAX); - - // - // Call the write method to store a uint32_t - // - return sbgStreamBufferWriteUint32LE(pHandle, (uint32_t)value); -} - -/*! - * Write an size_t into a stream buffer as a uint64_t (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64LE(SbgStreamBuffer *pHandle, size_t value) -{ - // - // Check input parameters - // - assert(pHandle); - - // - // Call the write method to store a uint64_t - // - return sbgStreamBufferWriteUint64LE(pHandle, (uint64_t)value); -} - -/*! - * Write an float into a stream buffer (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value) -{ - FloatNint floatInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We use an union to avoid compiler cast - // - floatInt.valF = value; - - // - // Write this float as an uint32_t - // - return sbgStreamBufferWriteUint32LE(pHandle, floatInt.valU); - } - - return pHandle->errorCode; -} - -/*! - * Write an double into a stream buffer. (Little Endian Version). - * \param[in] pHandle Valid stream buffer handle that supports write operations. - * \param[in] value The value to write. - * \return SBG_NO_ERROR if the value has been successfully written. - */ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleLE(SbgStreamBuffer *pHandle, double value) -{ - DoubleNint doubleInt; - - // - // Check input parameters - // - assert(pHandle); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We use an union to avoid compiler cast - // - doubleInt.valF = value; - - // - // Write this float as an uint64_t - // - return sbgStreamBufferWriteUint64LE(pHandle, doubleInt.valU); - } - - return pHandle->errorCode; -} - -/*! -* Read a C String from a stream buffer (Little Endian Version). -* -* \param[in] pHandle Valid stream buffer handle that supports read operations. -* \param[out] pString Buffer that can hold the read NULL terminated C string. -* \param[in] maxSize Maximum number of bytes that can be stored in pString (including the NULL char). -* \return SBG_NO_ERROR if the string has been read successfully from the stream buffer. -* SBG_BUFFER_OVERFLOW if the provided string isn't big enough to hold the read string -*/ -SBG_INLINE SbgErrorCode sbgStreamBufferReadStringLE(SbgStreamBuffer *pHandle, char *pString, size_t maxSize) -{ - size_t stringLength; - - // - // Check input parameters - // - assert(pHandle); - assert(pString); - assert(maxSize > 0); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // The C string are stored in a stream buffer with a 32 bit size length and then the buffer itself - // - stringLength = sbgStreamBufferReadSizeT32LE(pHandle); - - if (stringLength <= maxSize) - { - // - // Read the string buffer itself - // - sbgStreamBufferReadBuffer(pHandle, pString, stringLength); - } - else - { - pHandle->errorCode = SBG_BUFFER_OVERFLOW; - SBG_LOG_ERROR(pHandle->errorCode, "Trying to store a string of %u bytes into a buffer of %u bytes.", stringLength, maxSize); - } - } - - return pHandle->errorCode; -} - -/*! -* Write a NULL terminated C String into a stream buffer (Little Endian Version). -* \param[in] pHandle Valid stream buffer handle that supports write operations. -* \param[in] pString NULL terminated C String to write to the stream buffer. -* \return SBG_NO_ERROR if the string has been written successfully to the stream buffer. -*/ -SBG_INLINE SbgErrorCode sbgStreamBufferWriteStringLE(SbgStreamBuffer *pHandle, const char *pString) -{ - size_t stringLength; - - // - // Check input parameters - // - assert(pHandle); - assert(pString); - - // - // Test if we haven't already an error - // - if (pHandle->errorCode == SBG_NO_ERROR) - { - // - // We write C string using a 32 bit size_t as the string length including the NULL char - // We should thus make sure the provided string isn't too big to fit in a 32 bits size_t - // - stringLength = strlen(pString) + 1; - - if (stringLength <= UINT32_MAX) - { - // - // Write the string length - // - if (sbgStreamBufferWriteSizeT32LE(pHandle, stringLength) == SBG_NO_ERROR) - { - // - // Write the string buffer itself - // - sbgStreamBufferWriteBuffer(pHandle, pString, stringLength); - } - } - else - { - pHandle->errorCode = SBG_INVALID_PARAMETER; - SBG_LOG_ERROR(pHandle->errorCode, "The provided string is too big to fit in a 32 bit size_t"); - } - } - - return pHandle->errorCode; -} - -#endif /* SBG_STREAM_BUFFER_LE_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.c b/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.c deleted file mode 100644 index 2ebbd5c..0000000 --- a/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.c +++ /dev/null @@ -1 +0,0 @@ -#include "sbgSwap.h" diff --git a/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.h b/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.h deleted file mode 100644 index a5c823b..0000000 --- a/sbg_ros_driver/external/sbgECom/common/swap/sbgSwap.h +++ /dev/null @@ -1,132 +0,0 @@ -/*! - * \file sbgSwap.h - * \author SBG Systems (Raphael Siryani) - * \date 14 January 2013 - * - * \brief Set of functions used to swap numbers. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_SWAP_H -#define SBG_SWAP_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include - -//----------------------------------------------------------------------// -//- Internal swap functions -// -//----------------------------------------------------------------------// - -/*! - * Swap a uint16_t number. - * \param[in] x The uint16_t to swap. - * \return The swapped value. - */ -SBG_INLINE uint16_t sbgSwap16(uint16_t x) -{ - return ((x<<8)|(x>>8)); -} - -/*! - * Swap a uint32_t number. - * \param[in] x The uint32_t to swap. - * \return The swapped value. - */ -SBG_INLINE uint32_t sbgSwap32(uint32_t x) -{ - return ((x << 24) | ((x << 8) & (0xFF0000)) | ((x >> 8) & (0xFF00)) | (x >> 24)); -} - -/*! - * Swap a uint64_t number. - * \param[in] x The uint64_t to swap. - * \return The swapped value. - */ -SBG_INLINE uint64_t sbgSwap64(uint64_t x) -{ - uint32_t hi, lo; - - // - // Separate into high and low 32-bit values - // - lo = (uint32_t)(x&0xFFFFFFFF); - x >>= 32; - hi = (uint32_t)(x&0xFFFFFFFF); - - // - // Swap each part and rebuild our 64 bit vale - // - x = sbgSwap32(lo); - x <<= 32; - x |= sbgSwap32(hi); - - return x; -} - -/*! - * Swap a float number. - * \param[in] val The float to swap. - * \return The swapped value. - */ -SBG_INLINE float sbgSwapFloat(float val) -{ - FloatNint tmpFloat; - - // - // We use a union to do the type punning - // - tmpFloat.valF = val; - tmpFloat.valU = sbgSwap32(tmpFloat.valU); - - // - // Return the swapped float - // - return tmpFloat.valF; -} - -/*! - * Swap a double number. - * \param[in] val The double to swap. - * \return The swapped value. - */ -SBG_INLINE double sbgSwapDouble(double val) -{ - DoubleNint tmpDouble; - - // - // We use a union to do the type punning - // - tmpDouble.valF = val; - tmpDouble.valU = sbgSwap64(tmpDouble.valU); - - // - // Return the swapped double - // - return tmpDouble.valF; -} - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_SWAP_H */ diff --git a/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.c b/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.c deleted file mode 100644 index f14da33..0000000 --- a/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.c +++ /dev/null @@ -1,597 +0,0 @@ -#include "sbgVersion.h" - -//----------------------------------------------------------------------// -//- Version encoding / decoding methods -// -//----------------------------------------------------------------------// - -/*! - * Fill a SbgVersion structure based on an uint32_t that stores the 'compiled' version information. - * \param[in] encodedVersion The version information stored within a uint32_t. - * \param[out] pVersionInfo Pointer on an allocated SbgVersion structure to fill. - */ -SBG_COMMON_LIB_API void sbgVersionDecode(uint32_t encodedVersion, SbgVersion *pVersionInfo) -{ - // - // Check input parameters - // - assert(pVersionInfo); - - // - // Test if we have a software version scheme - // - if (encodedVersion&SBG_VERSION_SOFT_SCHEME) - { - // - // We have a software scheme, decode it - // - pVersionInfo->softwareScheme = true; - - // - // Decode the software scheme fields - // - pVersionInfo->qualifier = (SbgVersionQualifier)((encodedVersion >> SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT) & SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK); - pVersionInfo->major = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT) & SBG_VERSION_SOFT_SCHEME_MAJOR_MASK; - pVersionInfo->minor = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT) & SBG_VERSION_SOFT_SCHEME_MINOR_MASK; - pVersionInfo->build = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT) & SBG_VERSION_SOFT_SCHEME_BUILD_MASK; - - // - // Set the revision to zero as it's not used - // - pVersionInfo->rev = 0; - } - else - { - // - // We have a basic scheme, decode it - // - pVersionInfo->softwareScheme = false; - - // - // Decode the software scheme fields - // - pVersionInfo->major = (encodedVersion >> 24) & 0xFF; - pVersionInfo->minor = (encodedVersion >> 16) & 0xFF; - pVersionInfo->rev = (encodedVersion >> 8) & 0xFF; - pVersionInfo->build = (encodedVersion >> 0) & 0xFF; - - // - // Set the qualifier to zero - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_DEV; - } -} - -/*! - * Construct a uint32_t containing a version information based on a SbgVersion structure. - * \param[in] pVersionInfo Pointer on a read only version structure to encode. - * \return The encoded version information on an uint32_t or 0 if an error has occurred. - */ -SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo) -{ - uint32_t encodedVersion = 0; - - // - // Check input parameter - // - assert(pVersionInfo); - - // - // Test if we have a software scheme or a basic one - // - if (pVersionInfo->softwareScheme) - { - // - // We have a software, scheme, so test that the version is valid - // - assert((pVersionInfo->major <= 63) && (pVersionInfo->minor <= 63) && (pVersionInfo->rev == 0)); - - // - // Indicate that we have a software version scheme - // - encodedVersion = SBG_VERSION_SOFTWARE(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->build, pVersionInfo->qualifier); - } - else - { - // - // We have a basic version scheme so check parameter validty - // - assert(pVersionInfo->major <= 127); - - // - // Encode the basic version information - // - encodedVersion = SBG_VERSION_BASIC(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); - } - - return encodedVersion; -} - -//----------------------------------------------------------------------// -//- Version comparaison methods -// -//----------------------------------------------------------------------// - -/*! - * Compare two version information structures and return if the version A is greater, less or equal than the version B. - * The computation is roughly result = version A - version B - * We can define how far we will check if the version A is greater than the version B. - * For example, we can only check the major or major and minor fields. - * \param[in] pVersionA The first version to compare. - * \param[in] pVersionB The second version to compare. - * \param[in] thresold The comparaison thresold to know if we are checking the major, minor, revision, build, ... - * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the thresold). - */ -SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThresold thresold) -{ - int32_t result; - - // - // Check input parameter - // - assert((pVersionA) && (pVersionB)); - - // - // Check that the two versions are using the same scheme - // - assert(pVersionA->softwareScheme == pVersionB->softwareScheme); - - // - // Do the comparaison according to the selected thresold - // Start by compairing the major field - // - result = pVersionA->major - pVersionB->major; - - // - // Test if we have to also compare the minor field - // - if ( (result == 0) && ((thresold == SBG_VERSION_CMP_THRESOLD_MINOR) || (thresold == SBG_VERSION_CMP_THRESOLD_REVISION) || (thresold == SBG_VERSION_CMP_THRESOLD_BUILD) || (thresold == SBG_VERSION_CMP_THRESOLD_QUALIFIER)) ) - { - // - // Update the result using the minor indication - // - result = pVersionA->minor - pVersionB->minor; - - // - // Test if we have to also compare the revision field (for basic version only) - // - if ( (result == 0) && ((thresold == SBG_VERSION_CMP_THRESOLD_REVISION) || (thresold == SBG_VERSION_CMP_THRESOLD_BUILD) || (thresold == SBG_VERSION_CMP_THRESOLD_QUALIFIER)) ) - { - // - // Test if we have a software scheme or a basic scheme version - // - if (pVersionA->softwareScheme) - { - // - // We have a software scheme so set the result to 0 - // - result = 0; - } - else - { - // - // We have a basic scheme so we can compare the revision field - // - result = pVersionA->rev - pVersionB->rev; - } - - // - // Test if we have to also compare the build field - // - if ( (result == 0) && ((thresold == SBG_VERSION_CMP_THRESOLD_BUILD) || (thresold == SBG_VERSION_CMP_THRESOLD_QUALIFIER)) ) - { - // - // Compare the build field - // - result = pVersionA->build - pVersionB->build; - - // - // Test if we have to also compare the qualifier field - // - if ( (result == 0) && (thresold == SBG_VERSION_CMP_THRESOLD_QUALIFIER) ) - { - // - // Test if we have a software scheme - // - if (pVersionA->softwareScheme) - { - // - // We have a software scheme so set the result to 0 - // - result = pVersionA->qualifier - pVersionB->qualifier; - } - else - { - // - // We have a basic scheme so set the result to 0 - // - result = 0; - } - } - } - } - } - - return result; -} - -/*! - * Compare two encoded versions and return if the version A is greater, less or equal than the version B. - * The computation is roughly result = version A - version B - * We can define how far we will check if the version A is greater than the version B. - * For example, we can only check the major or major and minor fields. - * \param[in] versionA The first compiled version to compare. - * \param[in] versionB The second compiled version to compare. - * \param[in] thresold The comparaison thresold to know if we are checking the major, minor, revision, build, ... - * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the thresold). - */ -SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThresold thresold) -{ - SbgVersion versionAInfo; - SbgVersion versionBInfo; - - // - // Decode the versions - // - sbgVersionDecode(versionA, &versionAInfo); - sbgVersionDecode(versionB, &versionBInfo); - - // - // Do the comparaison - // - return sbgVersionCompare(&versionAInfo, &versionBInfo, thresold); -} - -/*! - * Check if the provided version is between the provided version interval. - * All versions should have the same scheme. - * - * \param[in] pLowerVersion The lower version bound of the interval. - * \param[in] pHigherVersion The hiver version bound of the interval. - * \param[in] pVersion The version to check. - * \return A negative value if the version is stricly below the lower version bound - * Zero if the version if equal or greater than lower version and equal or smaller than higer version - * A positive value if the version is strictly above the higher version bound - */ -SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRange(const SbgVersion *pLowerVersion, const SbgVersion *pHigherVersion, const SbgVersion *pVersion) -{ - assert(pLowerVersion); - assert(pHigherVersion); - assert(pVersion); - - // - // Use the encoded version to speed up the comparaison - // - return sbgVersionIsWithinRangeEncoded(sbgVersionEncode(pLowerVersion), sbgVersionEncode(pHigherVersion), sbgVersionEncode(pVersion)); -} - -/*! - * Check if the provided encoded version is between the provided version interval. - * All versions should have the same scheme. - * - * \param[in] lowerVersion The lower version bound of the interval. - * \param[in] higherVersion The hiver version bound of the interval. - * \param[in] version The version to check. - * \return A negative value if the version is stricly below the lower version bound - * Zero if the version if equal or greater than lower version and equal or smaller than higer version - * A positive value if the version is strictly above the higher version bound - */ -SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, uint32_t higherVersion, uint32_t version) -{ - // - // Make sure that all versions are using the same scheme - // - assert(lowerVersion <= higherVersion); - assert(( (sbgVersionIsUsingSoftwareScheme(lowerVersion) == sbgVersionIsUsingSoftwareScheme(higherVersion)) && - (sbgVersionIsUsingSoftwareScheme(lowerVersion) == sbgVersionIsUsingSoftwareScheme(version))) || - ( (sbgVersionIsUsingSoftwareScheme(lowerVersion) != sbgVersionIsUsingSoftwareScheme(higherVersion)) && - (sbgVersionIsUsingSoftwareScheme(lowerVersion) != sbgVersionIsUsingSoftwareScheme(version)))); - - // - // We can compare safely the encoded version directly - // - if (version < lowerVersion) - { - return -1; - } - else if (version > higherVersion) - { - return 1; - } - else - { - return 0; - } -} - -//----------------------------------------------------------------------// -//- Version to string methods -// -//----------------------------------------------------------------------// - -/*! - * Convert a version information to a human readable string. - * \param[in] pVersionInfo Pointer on a read only version structure to convert to a human readable string. - * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. - * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. - * \return SBG_NO_ERROR if the version information has been converted to a string. - * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionToString(const SbgVersion *pVersionInfo, char *pBuffer, uint32_t sizeOfBuffer) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - // - // Check input parameters - // - assert((pVersionInfo) && (pBuffer) && (sizeOfBuffer>0)); - - // - // Test if we have a basic or software version scheme - // - if (pVersionInfo->softwareScheme) - { - // - // We have a software version scheme - // Test that the buffer is big enough to store the generated string (31.31.65535-hotfix) - // - if (sizeOfBuffer >= 19) - { - // - // Generate the string version - // - sprintf(pBuffer, "%u.%u.%u-", pVersionInfo->major, pVersionInfo->minor, pVersionInfo->build); - - // - // Append the qualifier - // - switch (pVersionInfo->qualifier) - { - case SBG_VERSION_QUALIFIER_DEV: - strcat(pBuffer, "dev"); - break; - case SBG_VERSION_QUALIFIER_ALPHA: - strcat(pBuffer, "alpha"); - break; - case SBG_VERSION_QUALIFIER_BETA: - strcat(pBuffer, "beta"); - break; - case SBG_VERSION_QUALIFIER_RC: - strcat(pBuffer, "rc"); - break; - case SBG_VERSION_QUALIFIER_STABLE: - strcat(pBuffer, "stable"); - break; - case SBG_VERSION_QUALIFIER_HOT_FIX: - strcat(pBuffer, "hotfix"); - break; - default: - break; - } - } - else - { - // - // Output buffer is to small - // - errorCode = SBG_BUFFER_OVERFLOW; - } - - } - else - { - // - // We have a basic version scheme, generate the string - // Test that the buffer is big enough to store the generated string - // - if (sizeOfBuffer >= 16) - { - // - // Generate the string version - // - sprintf(pBuffer, "%u.%u.%u.%u", pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); - } - else - { - // - // Output buffer is to small - // - errorCode = SBG_BUFFER_OVERFLOW; - } - } - - // - // Return status of operation - // - return errorCode; -} - -/*! - * Convert an encoded version number to a human readable string. - * \param[in] version Encoded version value to to convert to a human readable string. - * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. - * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. - * \return SBG_NO_ERROR if the version information has been converted to a string. - * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, uint32_t sizeOfBuffer) -{ - SbgVersion versionInfo; - - // - // Decode the versions - // - sbgVersionDecode(version, &versionInfo); - - // - // Return the version as a string - // - return sbgVersionToString(&versionInfo, pBuffer, sizeOfBuffer); -} - -//----------------------------------------------------------------------// -//- String to version methods -// -//----------------------------------------------------------------------// - -/*! - * Convert a human readable string to a version structure. - * \param[in] pVersionStr The string containing the version to convert. - * \param[out] pVersionInfo Pointer to a version structure to store the parsed version info. - * \return SBG_NO_ERROR if the version information has been converted from a string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - char qualifierStr[32]; - uint32_t major; - uint32_t minor; - uint32_t rev; - uint32_t build; - - // - // Check input parameters - // - assert((pVersionStr) && (pVersionInfo)); - - // - // Zero init the returned version struct - // - memset(pVersionInfo, 0x00, sizeof(SbgVersion)); - - // - // Try to parse a basic version - // - if (sscanf(pVersionStr, "%u.%u.%u.%u", &major, &minor, &rev, &build) == 4) - { - // - // We have read successfully a basic version - // - pVersionInfo->softwareScheme = false; - - // - // Fill the version numbers - // - pVersionInfo->major = (uint8_t)major; - pVersionInfo->minor = (uint8_t)minor; - pVersionInfo->rev = (uint8_t)rev; - pVersionInfo->build = (uint8_t)build; - } - else if (sscanf(pVersionStr, "%u.%u.%u-%s", &major, &minor, &build, qualifierStr) == 4) - { - // - // We have read successfully a software scheme version - // - pVersionInfo->softwareScheme = true; - - // - // Fill the version numbers - // - pVersionInfo->major = (uint8_t)major; - pVersionInfo->minor = (uint8_t)minor; - pVersionInfo->build = (uint16_t)build; - - // - // Also convert the qualifier - // - if (!strcmp(qualifierStr, "dev")) - { - // - // Dev qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_DEV; - } - else if (!strcmp(qualifierStr, "alpha")) - { - // - // Alpha qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_ALPHA; - } - else if (!strcmp(qualifierStr, "beta")) - { - // - // Beta qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_BETA; - } - else if (!strcmp(qualifierStr, "rc")) - { - // - // Release Candidate qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_RC; - } - else if (!strcmp(qualifierStr, "stable")) - { - // - // Stable qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_STABLE; - } - else if (!strcmp(qualifierStr, "hotfix")) - { - // - // Hot Fix qualifier - // - pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_HOT_FIX; - } - else - { - // - // Unknown qualifier - // - errorCode = SBG_INVALID_PARAMETER; - } - } - else - { - // - // Invalid format - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; -} - -/*! - * Convert an encoded version number to a human readable string. - * \param[in] pVersionStr The string containing the version to convert. - * \param[out] pVersion Returned encoded version value parsed from the string. - * \return SBG_NO_ERROR if the version information has been converted from a string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgVersion versionInfo; - - // - // Check input parameters - // - assert((pVersionStr) && (pVersion)); - - // - // Parse the version from a string - // - errorCode = sbgVersionFromString(pVersionStr, &versionInfo); - - // - // Test that no error has occurred - // - if (errorCode == SBG_NO_ERROR) - { - // - // Encode the version and return it - // - *pVersion = sbgVersionEncode(&versionInfo); - } - else - { - // - // An error has occurred so return a zero version - // - *pVersion = 0; - } - - // - // Return error - // - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.h b/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.h deleted file mode 100644 index 77f3803..0000000 --- a/sbg_ros_driver/external/sbgECom/common/version/sbgVersion.h +++ /dev/null @@ -1,399 +0,0 @@ -/*! - * \file sbgVersion.h - * \author SBG Systems (Raphael Siryani) - * \date 23 April 2015 - * - * \brief Helper methods and definitions used to handle version. - * - * Version information is stored within a single uint32_t. - * There are two different versions schemes to better handle software revisions and file format / hardware ones. - * - * Software version are only used for firmware and software. - * This versions is defined as a ..- for example: 1.3.1845-RC - * Major and Minor can range from 0 to 63. - * Build is an uint16_t ranging from 0 to 65535. - * Qualifier is an enum encoded on 3 bits. - * - * Basic version is used for hardware or file formats and is the legacy one for software. - * This versions is defined as a ... - * Each element is stored in an uint8_t. - * The Major version should NEVER go above 31 - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2015, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_VERSION_H -#define SBG_VERSION_H - -//----------------------------------------------------------------------// -//- Header (open extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -extern "C" { -#endif - -#include - -//----------------------------------------------------------------------// -//- Version related definitions -// -//----------------------------------------------------------------------// -#define SBG_VERSION_SOFT_SCHEME (0x00000001u << 31) /*!< Set to 1 to indicate a version number that uses the software scheme. Set to 0 means a basic version scheme. */ - -#define SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK (0x07) /*!< Mask used to keep only the version qualifier enum. */ -#define SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT (28) /*!< Bitshift to apply to get the qualifier enum. */ - -#define SBG_VERSION_SOFT_SCHEME_MAJOR_MASK (0x3F) /*!< Mask used to keep only the major version field. */ -#define SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT (22) /*!< Bitshift used to get the major version field. */ - -#define SBG_VERSION_SOFT_SCHEME_MINOR_MASK (0x3F) /*!< Mask used to keep only the minor version field. */ -#define SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT (16) /*!< Bitshift used to get the minor version field. */ - -#define SBG_VERSION_SOFT_SCHEME_BUILD_MASK (0xFFFF) /*!< Mask used to keep only the build version field. */ -#define SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT (0) /*!< Bitshift used to get the build version field. */ - -/*! - * Version qualifier enum definition. - * This enum is used to identify if we have a basic version scheme or a software one. - * If it's a software one, we can easly know if the software version is a beta, a stable one. - */ -typedef enum _SbgVersionQualifier -{ - SBG_VERSION_QUALIFIER_DEV = 0, /*!< Development only version or pre-alpha. Customer shouldn't get this version qualifier. */ - SBG_VERSION_QUALIFIER_ALPHA = 1, /*!< Alpha version, missing features, can be unstable and could cause crashes or data loss. API can still change. */ - SBG_VERSION_QUALIFIER_BETA = 2, /*!< Beta version, features are freezed, can be unstable and could cause crashes or data loss. API shouldn't change. */ - SBG_VERSION_QUALIFIER_RC = 3, /*!< Release Candidate, features are freezed, with no known bug. API is freezed. */ - SBG_VERSION_QUALIFIER_STABLE = 4, /*!< Stable release, the version is the standard delivered one. */ - SBG_VERSION_QUALIFIER_HOT_FIX = 5 /*!< Hot fixes were applied on a stable release. It should be bug fixes. This qualifier is temporary as the version should return to stable release as soon as the test procedure has been performed. */ -} SbgVersionQualifier; - -/*! - * Version qualifier enum definition. - * This enum is used to identify if we have a basic version scheme or a software one. - * If it's a software one, we can easly know if the software version is a beta, a stable one. - */ -typedef enum _SbgVersionCmpThresold -{ - SBG_VERSION_CMP_THRESOLD_MAJOR = 0, /*!< Compare only the major field. */ - SBG_VERSION_CMP_THRESOLD_MINOR = 1, /*!< Compare the major and minor fields. */ - SBG_VERSION_CMP_THRESOLD_REVISION = 2, /*!< Compare the major, minor and revision fields (only for basic versions). */ - SBG_VERSION_CMP_THRESOLD_BUILD = 3, /*!< Compare the major, minor, revision and build fields. */ - SBG_VERSION_CMP_THRESOLD_QUALIFIER = 4 /*!< Compare the major, minor, revision, build and qualifier fields (only for software scheme). */ -} SbgVersionCmpThresold; - -/*! - * This structure contains all the fields provided by a version number. - * It handles both basic and software version numbers. - */ -typedef struct _SbgVersion -{ - bool softwareScheme; /*!< Set to true if it's a software scheme or false if it's a basic one. */ - SbgVersionQualifier qualifier; /*!< Qualifier of the current version */ - uint8_t major; /*!< Major version */ - uint8_t minor; /*!< Minor version */ - uint8_t rev; /*!< Revision number - left to 0 in case of software version */ - uint16_t build; /*!< Build number */ -} SbgVersion; - -//----------------------------------------------------------------------// -//- Macro definitions for encode versions -// -//----------------------------------------------------------------------// - -/*! - * Compile a version number using the basic scheme based on major, minor, revision and build information - * \param[in] maj The major version between 0 to 127. - * \param[in] min The minor version between 0 to 255. - * \param[in] rev The revision version between 0 to 255. - * \param[in] build The build number between 0 to 255. - * \return The encoded version number. - */ -#define SBG_VERSION_BASIC(major, minor, rev, build) ( (((uint32_t)(major)) << 24) | \ - (((uint32_t)(minor)) << 16) | \ - (((uint32_t)(rev)) << 8) | \ - (((uint32_t)(build))) ) - -/*! - * Compile a version number using the software scheme based on major, minor, build, qualifier. - * \param[in] major The major version between 0 to 63. - * \param[in] minor The major version between 0 to 63. - * \param[in] build The major version between 0 to 65535. - * \param[in] qualifier The version qualifier within the SbgVersionQualifier enum. - * \return The encoded version number. - */ -#define SBG_VERSION_SOFTWARE(major, minor, build, qualifier) ( SBG_VERSION_SOFT_SCHEME | \ - ((((uint32_t)(qualifier)) & SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK) << SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT) | \ - ((((uint32_t)(major)) & SBG_VERSION_SOFT_SCHEME_MAJOR_MASK) << SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT) | \ - ((((uint32_t)(minor)) & SBG_VERSION_SOFT_SCHEME_MINOR_MASK) << SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT) | \ - ((((uint32_t)(build)) & SBG_VERSION_SOFT_SCHEME_BUILD_MASK) << SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT) ) - -//----------------------------------------------------------------------// -//- Version encoding / decoding methods -// -//----------------------------------------------------------------------// - -/*! - * Fill a SbgVersion structure based on an uint32_t that stores the 'compiled' version information. - * \param[in] encodedVersion The version information stored within a uint32_t. - * \param[out] pVersionInfo Pointer on an allocated SbgVersion structure to fill. - */ -SBG_COMMON_LIB_API void sbgVersionDecode(uint32_t encodedVersion, SbgVersion *pVersionInfo); - -/*! - * Construct a uint32_t containing a version information based on a SbgVersion structure. - * \param[in] pVersionInfo Pointer on a read only version structure to encode. - * \return The encoded version information on an uint32_t or 0 if an error has occurred. - */ -SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo); - -//----------------------------------------------------------------------// -//- Version parameters methods -// -//----------------------------------------------------------------------// - -/*! - * Returns true if this encoded version number is using a software scheme. - * \param[in] encodedVersion The encoded version number to test stored in a uint32_t. - * \return true if the version number is using a software scheme. - */ -SBG_INLINE bool sbgVersionIsUsingSoftwareScheme(uint32_t encodedVersion) -{ - SbgVersion decodedVersion; - - // - // Decode the version first - // - sbgVersionDecode(encodedVersion, &decodedVersion); - - // - // Returns if the version is using a software scheme - // - return decodedVersion.softwareScheme; -} - -//----------------------------------------------------------------------// -//- Version comparaison methods -// -//----------------------------------------------------------------------// - -/*! - * Compare two version information structures and return if the version A is greater, less or equal than the version B. - * The computation is roughly result = version A - version B - * We can define how far we will check if the version A is greater than the version B. - * For example, we can only check the major or major and minor fields. - * \param[in] pVersionA The first version to compare. - * \param[in] pVersionB The second version to compare. - * \param[in] thresold The comparaison thresold to know if we are checking the major, minor, revision, build, ... - * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the thresold). - */ -SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThresold thresold); - -/*! - * Compare two encoded versions and return if the version A is greater, less or equal than the version B. - * The computation is roughly result = version A - version B - * We can define how far we will check if the version A is greater than the version B. - * For example, we can only check the major or major and minor fields. - * \param[in] versionA The first compiled version to compare. - * \param[in] versionB The second compiled version to compare. - * \param[in] thresold The comparaison thresold to know if we are checking the major, minor, revision, build, ... - * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the thresold). - */ -SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThresold thresold); - -/*! - * Check if the provided version is between the provided version interval. - * All versions should have the same scheme. - * - * \param[in] pLowerVersion The lower version bound of the interval. - * \param[in] pHigherVersion The hiver version bound of the interval. - * \param[in] pVersion The version to check. - * \return A negative value if the version is stricly below the lower version bound - * Zero if the version if equal or greater than lower version and equal or smaller than higer version - * A positive value if the version is strictly above the higher version bound - */ -SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRange(const SbgVersion *pLowerVersion, const SbgVersion *pHigherVersion, const SbgVersion *pVersion); - -/*! - * Check if the provided encoded version is between the provided version interval. - * All versions should have the same scheme. - * - * \param[in] lowerVersion The lower version bound of the interval. - * \param[in] higherVersion The hiver version bound of the interval. - * \param[in] version The version to check. - * \return A negative value if the version is stricly below the lower version bound - * Zero if the version if equal or greater than lower version and equal or smaller than higer version - * A positive value if the version is strictly above the higher version bound - */ -SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, uint32_t higherVersion, uint32_t version); - - -//----------------------------------------------------------------------// -//- Version to string methods -// -//----------------------------------------------------------------------// - -/*! - * Convert a version information to a human readable string. - * \param[in] pVersionInfo Pointer on a read only version structure to convert to a human readable string. - * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. - * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. - * \return SBG_NO_ERROR if the version information has been converted to a string. - * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionToString(const SbgVersion *pVersionInfo, char *pBuffer, uint32_t sizeOfBuffer); - -/*! - * Convert an encoded version number to a human readable string. - * \param[in] version Encoded version value to to convert to a human readable string. - * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. - * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. - * \return SBG_NO_ERROR if the version information has been converted to a string. - * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, uint32_t sizeOfBuffer); - -//----------------------------------------------------------------------// -//- String to version methods -// -//----------------------------------------------------------------------// - -/*! - * Convert a human readable string to a version structure. - * \param[in] pVersionStr The string containing the version to convert. - * \param[out] pVersionInfo Pointer to a version structure to store the parsed version info. - * \return SBG_NO_ERROR if the version information has been converted from a string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo); - -/*! - * Convert an encoded version number to a human readable string. - * \param[in] pVersionStr The string containing the version to convert. - * \param[out] pVersion Returned encoded version value parsed from the string. - * \return SBG_NO_ERROR if the version information has been converted from a string. - */ -SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion); - -//----------------------------------------------------------------------// -//- Legacy version system methods -// -//----------------------------------------------------------------------// - -/*! - * Define all these methods as deprecated. - */ -SBG_DEPRECATED(SBG_INLINE uint32_t SBG_VERSION(uint8_t major, uint8_t minor, uint8_t rev, uint8_t build)); -SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_MAJOR(uint32_t encodedVersion)); -SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_MINOR(uint32_t encodedVersion)); -SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_REV(uint32_t encodedVersion)); -SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion)); - -/*! - * DEPRECATED macro, please update your code - * Compile a version number using the basic scheme based on major, minor, revision and build information - * \param[in] maj The major version between 0 to 127. - * \param[in] min The minor version between 0 to 255. - * \param[in] rev The revision version between 0 to 255. - * \param[in] build The build number between 0 to 255. - * \return The encoded version number. - */ -SBG_INLINE uint32_t SBG_VERSION(uint8_t major, uint8_t minor, uint8_t rev, uint8_t build) -{ - // - // The SBG_VERSION macro is the basic version scheme. - // - return SBG_VERSION_BASIC(major, minor, rev, build); -} - -/*! - * DEPRECATED method, please update your code. - * Extract a basic version scheme using legacy methods. - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. - */ -SBG_INLINE uint8_t SBG_VERSION_GET_MAJOR(uint32_t encodedVersion) -{ - SbgVersion versionInfo; - - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); - - // - // Return the major version - // - return versionInfo.major; -} - -/*! - * DEPRECATED method, please update your code. - * Extract a basic version scheme using legacy methods. - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. - */ -SBG_INLINE uint8_t SBG_VERSION_GET_MINOR(uint32_t encodedVersion) -{ - SbgVersion versionInfo; - - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); - - // - // Return the major version - // - return versionInfo.minor; -} - -/*! - * DEPRECATED method, please update your code. - * Extract a basic version scheme using legacy methods. - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. - */ -SBG_INLINE uint8_t SBG_VERSION_GET_REV(uint32_t encodedVersion) -{ - SbgVersion versionInfo; - - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); - - // - // Return the major version - // - return versionInfo.rev; -} - -/*! - * DEPRECATED method, please update your code. - * Extract a basic version scheme using legacy methods. - * \param[in] encodedVersion The encoded version to extract the major version. - * \return The major version. - */ -SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion) -{ - SbgVersion versionInfo; - - // - // Decode the version information - // - sbgVersionDecode(encodedVersion, &versionInfo); - - // - // Return the major version - // - return (uint8_t)versionInfo.build; -} - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* SBG_VERSION_H */ diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c deleted file mode 100644 index 25d3543..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c +++ /dev/null @@ -1,66 +0,0 @@ -#include "sbgEComBinaryLogAirData.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); - - // - // The true airspeed fields have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pInputStream) > 0) - { - pOutputData->pressureDiff = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->trueAirspeed = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->airTemperature = sbgStreamBufferReadFloatLE(pInputStream); - } - else - { - pOutputData->pressureDiff = 0.0f; - pOutputData->trueAirspeed = 0.0f; - pOutputData->airTemperature = 0.0f; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureDiff); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->trueAirspeed); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->airTemperature); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h deleted file mode 100644 index 7754d5b..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h +++ /dev/null @@ -1,83 +0,0 @@ -/*! - * \file sbgEComBinaryLogAirData.h - * \author SBG Systems - * \date 20 February 2019 - * - * \brief This file is used to parse & received Air Data logs. - * - * Air Data logs are used to inject / return barometric altitude - * as well as true air speed. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_BINARY_LOG_AIR_DATA_H -#define SBG_ECOM_BINARY_LOG_AIR_DATA_H - -#include -#include - -//----------------------------------------------------------------------// -//- Log Air Data status definitions -// -//----------------------------------------------------------------------// - -/*! - * Air Data sensor status mask definitions - */ -#define SBG_ECOM_AIR_DATA_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the time stamp field represents a delay instead of an absolute time stamp. */ -#define SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID (0x0001u << 1) /*!< Set to 1 if the pressure field is filled and valid. */ -#define SBG_ECOM_AIR_DATA_ALTITUDE_VALID (0x0001u << 2) /*!< Set to 1 if the barometric altitude field is filled and valid. */ -#define SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID (0x0001u << 3) /*!< Set to 1 if the differential pressure field is filled and valid. */ -#define SBG_ECOM_AIR_DATA_AIRPSEED_VALID (0x0001u << 4) /*!< Set to 1 if the true airspeed field is filled and valid. */ -#define SBG_ECOM_AIR_DATA_TEMPERATURE_VALID (0x0001u << 5) /*!< Set to 1 if the output air temperature field is filled and valid. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for AirData. - */ -typedef struct _SbgLogAirData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up OR measurement delay in us. */ - uint16_t status; /*!< Airdata sensor status bitmask. */ - float pressureAbs; /*!< Raw absolute pressure measured by the barometer sensor in Pascals. */ - float altitude; /*!< Altitude computed from barometric altimeter in meters and positive upward. */ - float pressureDiff; /*!< Raw differential pressure measured by the pitot tube in Pascal. */ - float trueAirspeed; /*!< True airspeed measured by a pitot tube in m.s^-1 and positive forward. */ - float airTemperature; /*!< Outside air temperature in °C that could be used to compute true airspeed from differential pressure. */ -} SbgLogAirData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_AIR_DATA message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_AIR_DATA message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.c deleted file mode 100644 index e1055c8..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.c +++ /dev/null @@ -1,78 +0,0 @@ -#include "sbgEComBinaryLogDebug.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for SBG_ECOM_LOG_DEBUG_X messages and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDebugData(SbgStreamBuffer *pInputStream, SbgLogDebugData *pOutputData) -{ - SbgErrorCode errorCode; - - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload and return - // - pOutputData->id = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->offset = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->size = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->totalSize = sbgStreamBufferReadUint32LE(pInputStream); - - errorCode = sbgStreamBufferGetLastError(pInputStream); - - if (errorCode == SBG_NO_ERROR) - { - if (pOutputData->size <= SBG_ARRAY_SIZE(pOutputData->data)) - { - sbgStreamBufferReadBuffer(pInputStream, pOutputData->data, pOutputData->size); - errorCode = sbgStreamBufferGetLastError(pInputStream); - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - } - - return errorCode; -} - -/*! - * Write data for SBG_ECOM_LOG_DEBUG_X messages to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDebugData(SbgStreamBuffer *pOutputStream, const SbgLogDebugData *pInputData) -{ - SbgErrorCode errorCode; - - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->id); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->offset); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->size); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->totalSize); - - if (pInputData->size <= SBG_ARRAY_SIZE(pInputData->data)) - { - sbgStreamBufferWriteBuffer(pOutputStream, pInputData->data, pInputData->size); - errorCode = sbgStreamBufferGetLastError(pOutputStream); - } - else - { - errorCode = SBG_BUFFER_OVERFLOW; - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.h deleted file mode 100644 index 01c4868..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDebug.h +++ /dev/null @@ -1,67 +0,0 @@ -/*! - * \file sbgEComBinaryLogDebug.h - * \author SBG Systems (Alexis Guinamard) - * \date 03 October 2013 - * - * \brief This file is used to parse received debug frames - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_DEBUG_H__ -#define __SBG_ECOM_BINARY_LOG_DEBUG_H__ - -#include -#include - -#include "../protocol/sbgEComProtocol.h" - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for debug data. - * - * If debug data doesn't fit in a frame, multiple debug messages are sent - * with different message IDs but the same internal ID. - */ -typedef struct _SbgLogDebugData -{ - uint32_t id; /*!< Debug frame ID */ - uint32_t offset; /*!< Offset of the debug log */ - uint32_t size; /*!< Debug frame size */ - uint32_t totalSize; /*!< Total size of the debug log */ - uint8_t data[SBG_ECOM_MAX_PAYLOAD_SIZE - 16]; /*!< Debug data */ -} SbgLogDebugData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for SBG_ECOM_LOG_DEBUG_X messages and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDebugData(SbgStreamBuffer *pInputStream, SbgLogDebugData *pOutputData); - -/*! - * Write data for SBG_ECOM_LOG_DEBUG_X messages to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDebugData(SbgStreamBuffer *pOutputStream, const SbgLogDebugData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c deleted file mode 100644 index c433cbf..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c +++ /dev/null @@ -1,45 +0,0 @@ -#include "sbgEComBinaryLogDepth.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h deleted file mode 100644 index a5e8eeb..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h +++ /dev/null @@ -1,76 +0,0 @@ -/*! - * \file sbgEComBinaryLogDepth.h - * \author SBG Systems - * \date 20 February 2019 - * - * \brief This file is used to store depth measurements. - * - * Depth sensor are used for subsea navigation to improve height. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_BINARY_LOG_DEPTH_H -#define SBG_ECOM_BINARY_LOG_DEPTH_H - -#include -#include - -//----------------------------------------------------------------------// -//- Log Air Data status definitions -// -//----------------------------------------------------------------------// - -/*! - * Air Data sensor status mask definitions - */ -#define SBG_ECOM_DEPTH_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the time stamp field represents a delay instead of an absolute time stamp. */ -#define SBG_ECOM_DEPTH_PRESSURE_ABS_VALID (0x0001u << 1) /*!< Set to 1 if the pressure field is filled and valid. */ -#define SBG_ECOM_DEPTH_ALTITUDE_VALID (0x0001u << 2) /*!< Set to 1 if the depth altitude field is filled and valid. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for Depth sensor measurement (subsea). - */ -typedef struct _SbgLogDepth -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up OR measurement delay in us. */ - uint16_t status; /*!< Airdata sensor status bitmask. */ - float pressureAbs; /*!< Raw absolute pressure measured by the depth sensor in Pascals. */ - float altitude; /*!< Altitude computed from depth sensor in meters and positive upward. */ -} SbgLogDepth; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_DEPTH message and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_DEPTH message to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c deleted file mode 100644 index b4ada45..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c +++ /dev/null @@ -1,49 +0,0 @@ -// sbgCommonLib headers -#include -#include - -// Local headers -#include "sbgEComBinaryLogDiag.h" - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - pOutputData->timestamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->type = (SbgDebugLogType)sbgStreamBufferReadUint8(pInputStream); - pOutputData->errorCode = (SbgErrorCode)sbgStreamBufferReadUint8(pInputStream); - - sbgStreamBufferReadBuffer(pInputStream, pOutputData->string, sbgStreamBufferGetSpace(pInputStream)); - pOutputData->string[sizeof(pOutputData->string) - 1] = '\0'; - - return sbgStreamBufferGetLastError(pInputStream); -} - -SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData) -{ - size_t length; - - assert(pOutputStream); - assert(pInputData); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timestamp); - sbgStreamBufferWriteUint8(pOutputStream, pInputData->type); - sbgStreamBufferWriteUint8(pOutputStream, pInputData->errorCode); - - length = strlen(pInputData->string); - - if (length >= sizeof(pInputData->string)) - { - length = sizeof(pInputData->string) - 1; - } - - sbgStreamBufferWriteBuffer(pOutputStream, pInputData->string, length); - sbgStreamBufferWriteUint8(pOutputStream, 0); - - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h deleted file mode 100644 index 3875b7a..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h +++ /dev/null @@ -1,77 +0,0 @@ -/*! - * \file sbgEComBinaryLogDiag.h - * \author SBG Systems - * \date 12 June 2019 - * - * \brief Diagnostic log handling - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_BINARY_LOG_DIAG_H -#define SBG_ECOM_BINARY_LOG_DIAG_H - -// sbgCommonLib headers -#include -#include - -// Project headers -#include "../protocol/sbgEComProtocol.h" - -//----------------------------------------------------------------------// -//- Constant definitions -// -//----------------------------------------------------------------------// - -/*! - * Maximum size of the log string, in bytes. - */ -#define SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE (SBG_ECOM_MAX_PAYLOAD_SIZE - 6) - -//----------------------------------------------------------------------// -//- Structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Diagnostic log structure. - */ -typedef struct _SbgLogDiagData -{ - uint32_t timestamp; /*!< Timestamp, in microseconds. */ - SbgDebugLogType type; /*!< Log type. */ - SbgErrorCode errorCode; /*!< Error code. */ - char string[SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE]; /*!< Log string, null-terminated. */ -} SbgLogDiagData; - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * Parse data for SBG_ECOM_LOG_DIAG messages and fill the corresponding structure. - * - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData); - - -/*! - * Write data for SBG_ECOM_LOG_DIAG messages to the output stream buffer from the provided structure. - * - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c deleted file mode 100644 index da73bb2..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c +++ /dev/null @@ -1,67 +0,0 @@ -#include "sbgEComBinaryLogDvl.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->velocityQuality[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h deleted file mode 100644 index aa0fabd..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h +++ /dev/null @@ -1,72 +0,0 @@ -/*! - * \file sbgEComBinaryLogDvl.h - * \author SBG Systems - * \date 05 June 2013 - * - * \brief This file is used to parse received DVL binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_DVL_H__ -#define __SBG_ECOM_BINARY_LOG_DVL_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log DVL status definitions -// -//----------------------------------------------------------------------// - -/*! - * DVL status mask definitions - */ -#define SBG_ECOM_DVL_VELOCITY_VALID (0x0001u << 0) /*!< Set to 1 if the DVL equipment was able to measure a valid velocity. */ -#define SBG_ECOM_DVL_TIME_SYNC (0x0001u << 1) /*!< Set to 1 if the DVL data is correctly synchronized. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for DVL data. - */ -typedef struct _SbgLogDvlData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< DVL status bitmask. */ - float velocity[3]; /*!< X, Y, Z velocities in m.s^-1 expressed in the DVL instrument frame. */ - float velocityQuality[3]; /*!< X, Y, Z velocities quality indicators as provided by the DVL sensor and expressed in m.s^-1. - WARNING: This is typically just a residual information and not a real standard deviation. */ -} SbgLogDvlData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c deleted file mode 100644 index f5dadde..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c +++ /dev/null @@ -1,219 +0,0 @@ -#include "sbgEComBinaryLogEkf.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_EULER message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_EKF_EULER message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_QUAT message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->quaternion[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->quaternion[3] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_EKF_QUAT message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[3]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_NAV message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->position[0] = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->position[1] = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->position[2] = sbgStreamBufferReadDoubleLE(pInputStream); - - pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_EKF_NAV message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[2]); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[0]); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[1]); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h deleted file mode 100644 index 44e5008..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h +++ /dev/null @@ -1,197 +0,0 @@ -/*! - * \file sbgEComBinaryLogEkf.h - * \author SBG Systems (Raphael Siryani) - * \date 25 February 2013 - * - * \brief This file is used to parse received EKF compued data binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_EKF_H__ -#define __SBG_ECOM_BINARY_LOG_EKF_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Solution status definitions -// -//----------------------------------------------------------------------// - -/*! - * Solution status mode definitions. - */ -#define SBG_ECOM_SOLUTION_MODE_SHIFT (0u) /*!< Shift used to extract the clock status part. */ -#define SBG_ECOM_SOLUTION_MODE_MASK (0x0000000Fu) /*!< Mask used to keep only the clock status part. */ - -/*! - * Solution bit masks definitions. - */ -#define SBG_ECOM_SOL_ATTITUDE_VALID (0x00000001u << 4) /*!< Set to 1 if attitude data is reliable (Roll/Pitch error < 0,5°). */ -#define SBG_ECOM_SOL_HEADING_VALID (0x00000001u << 5) /*!< Set to 1 if geading data is reliable (Heading error < 1°). */ -#define SBG_ECOM_SOL_VELOCITY_VALID (0x00000001u << 6) /*!< Set to 1 if velocity data is reliable (velocity error < 1.5 m/s). */ -#define SBG_ECOM_SOL_POSITION_VALID (0x00000001u << 7) /*!< Set to 1 if position data is reliable (Position error < 10m). */ -#define SBG_ECOM_SOL_VERT_REF_USED (0x00000001u << 8) /*!< Set to 1 if vertical reference is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_MAG_REF_USED (0x00000001u << 9) /*!< Set to 1 if magnetometer is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS1_VEL_USED (0x00000001u << 10) /*!< Set to 1 if GPS1 velocity is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS1_POS_USED (0x00000001u << 11) /*!< Set to 1 if GPS1 Position is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS1_HDT_USED (0x00000001u << 13) /*!< Set to 1 if GPS1 True Heading is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS2_VEL_USED (0x00000001u << 14) /*!< Set to 1 if GPS2 velocity is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS2_POS_USED (0x00000001u << 15) /*!< Set to 1 if GPS2 Position is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_GPS2_HDT_USED (0x00000001u << 17) /*!< Set to 1 if GPS2 True Heading is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_ODO_USED (0x00000001u << 18) /*!< Set to 1 if Odometer is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_DVL_BT_USED (0x00000001u << 19) /*!< Set to 1 if DVL Bottom Tracking is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_DVL_WT_USED (0x00000001u << 20) /*!< Set to 1 if DVL Water Tracking is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_USER_POS_USED (0x00000001u << 21) /*!< Set to 1 if user velocity is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_USER_VEL_USED (0x00000001u << 22) /*!< Set to 1 if user Position is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_USER_HEADING_USED (0x00000001u << 23) /*!< Set to 1 if user Course is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_USBL_USED (0x00000001u << 24) /*!< Set to 1 if USBL / LBL is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_AIR_DATA_USED (0x00000001u << 25) /*!< Set to 1 if AirData (altimeter and/or true airspeed) is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_ZUPT_USED (0x00000001u << 26) /*!< Set to 1 if a ZUPT is used in solution (data used and valid since 3s). */ -#define SBG_ECOM_SOL_ALIGN_VALID (0x00000001u << 27) /*!< Set to 1 if sensor alignment and calibration parameters are valid */ -#define SBG_ECOM_SOL_DEPTH_USED (0x00000001u << 28) /*!< Set to 1 if Depth sensor (for subsea navigation) is used in solution (data used and valid since 3s). */ - -/*! - * Solution filter mode enum. - */ -typedef enum _SbgEComSolutionMode -{ - SBG_ECOM_SOL_MODE_UNINITIALIZED = 0, /*!< The Kalman filter is not initialized and the returned data are all invalid. */ - SBG_ECOM_SOL_MODE_VERTICAL_GYRO = 1, /*!< The Kalman filter only rely on a vertical reference to compute roll and pitch angles. Heading and navigation data drift freely. */ - SBG_ECOM_SOL_MODE_AHRS = 2, /*!< A heading reference is available, the Kalman filter provides full orientation but navigation data drift freely. */ - SBG_ECOM_SOL_MODE_NAV_VELOCITY = 3, /*!< The Kalman filter computes orientation and velocity. Position is freely integrated from velocity estimation. */ - SBG_ECOM_SOL_MODE_NAV_POSITION = 4 /*!< Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided. */ -} SbgEComSolutionMode; - -//----------------------------------------------------------------------// -//- Solution status helpers methods -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the solution mode from a solution status field. - * \param[in] status Status uint32_t value to extract the solution mode from it. - * \return The extracted solution mode. - */ -SBG_INLINE SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status) -{ - return (SbgEComSolutionMode)((status >> SBG_ECOM_SOLUTION_MODE_SHIFT) & SBG_ECOM_SOLUTION_MODE_MASK); -} - -/*! - * Method used to write the solution status field. - * \param[in] solutionMode The solution mode to set. - * \param[in] masks Bit mask to set. - * \return The build solution status field. - */ -SBG_INLINE uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)solutionMode)&SBG_ECOM_SOLUTION_MODE_MASK) << SBG_ECOM_SOLUTION_MODE_SHIFT) | masks; -} - - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * EKF computed orientation using euler angles. - */ -typedef struct _SbgLogEkfEulerData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - float euler[3]; /*!< Roll, Pitch and Yaw angles in rad. */ - float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */ - uint32_t status; /*!< EKF solution status bitmask and enum. */ -} SbgLogEkfEulerData; - -/*! - * EFK computed orientation using quaternion. - */ -typedef struct _SbgLogEkfQuatData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - float quaternion[4]; /*!< Orientation quaternion stored in W, X, Y, Z form. */ - float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */ - uint32_t status; /*!< EKF solution status bitmask and enum. */ -} SbgLogEkfQuatData; - -/*! - * EFK computed navigation data. - */ -typedef struct _SbgLogEkfNavData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - float velocity[3]; /*!< North, East, Down velocity in m.s^-1. */ - float velocityStdDev[3]; /*!< North, East, Down velocity 1 sigma standard deviation in m.s^-1. */ - double position[3]; /*!< Latitude, Longitude in degrees positive North and East. - Altitude above Mean Sea Level in meters. */ - float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ - float positionStdDev[3]; /*!< Latitude, longitude and altitude 1 sigma standard deviation in meters. */ - uint32_t status; /*!< EKF solution status bitmask and enum. */ -} SbgLogEkfNavData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_EULER message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_EKF_EULER message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_QUAT message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_EKF_QUAT message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_EKF_NAV message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_EKF_NAV message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c deleted file mode 100644 index 5f03f98..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c +++ /dev/null @@ -1,59 +0,0 @@ -#include "sbgEComBinaryLogEvent.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_EVENT_# message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset0 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset1 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset2 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOffset3 = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_EVENT_# message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset0); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset1); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset2); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset3); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h deleted file mode 100644 index 426a4a5..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h +++ /dev/null @@ -1,76 +0,0 @@ -/*! - * \file sbgEComBinaryLogEvent.h - * \author SBG Systems (Raphael Siryani) - * \date 28 October 2013 - * - * \brief This file is used to parse received event markers binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_EVENT_H__ -#define __SBG_ECOM_BINARY_LOG_EVENT_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log marker events definitions -// -//----------------------------------------------------------------------// - -/*! - * Log market events status mask definitions - */ -#define SBG_ECOM_EVENT_OVERFLOW (0x00000001u << 0) /*!< Set to 1 if we have received events at a higher rate than 1 kHz. */ -#define SBG_ECOM_EVENT_OFFSET_0_VALID (0x00000001u << 1) /*!< Set to 1 if at least two events have been received. */ -#define SBG_ECOM_EVENT_OFFSET_1_VALID (0x00000001u << 2) /*!< Set to 1 if at least three events have been received. */ -#define SBG_ECOM_EVENT_OFFSET_2_VALID (0x00000001u << 3) /*!< Set to 1 if at least four events have been received. */ -#define SBG_ECOM_EVENT_OFFSET_3_VALID (0x00000001u << 4) /*!< Set to 1 if at least five events have been received. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_EVENT_# message. - */ -typedef struct _SbgLogEvent -{ - uint32_t timeStamp; /*!< Measurement time since the sensor power up. */ - uint16_t status; /*!< Events status bitmask. */ - uint16_t timeOffset0; /*!< Time offset for the second received event. */ - uint16_t timeOffset1; /*!< Time offset for the third received event. */ - uint16_t timeOffset2; /*!< Time offset for the fourth received event. */ - uint16_t timeOffset3; /*!< Time offset for the fifth received event. */ -} SbgLogEvent; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_EVENT_# message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_EVENT_# message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c deleted file mode 100644 index 7f04ba0..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c +++ /dev/null @@ -1,292 +0,0 @@ -#include "sbgEComBinaryLogGps.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_VEL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->velocityAcc[2] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->course = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->courseAcc = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_VEL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[2]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->course); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->courseAcc); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Test if we have a additional information such as base station id (since version 1.4) - // - if (sbgStreamBufferGetSpace(pInputStream) >= 5) - { - // - // Read the additional information - // - pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream); - pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream); - } - else - { - // - // Default the additional information - // - pOutputData->numSvUsed = 0; - pOutputData->baseStationId = 0xFFFF; - pOutputData->differentialAge = 0xFFFF; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_POS message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->altitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitudeAccuracy); - - // - // Write the additional information added in version 1.4 - // - sbgStreamBufferWriteUint8LE(pOutputStream, pInputData->numSvUsed); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->baseStationId); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->differentialAge); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_HDT message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->heading = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->headingAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->pitch = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->pitchAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // The baseline field have been added in version 2.0 - // - if (sbgStreamBufferGetSpace(pInputStream) > 0) - { - pOutputData->baseline = sbgStreamBufferReadFloatLE(pInputStream); - } - else - { - pOutputData->baseline = 0.0f; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_HDT message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->heading); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->headingAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitch); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitchAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->baseline); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_RAW message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogGpsRaw *pOutputData) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - size_t payloadSize; - - assert(pInputStream); - assert(pOutputData); - - // - // This buffer is different from other because only a variable size raw buffer is stored - // We use the payload size (read from the low level protocol) to know the buffer size - // - // Please note that the provided stream buffer points to the payload part only so it's size is the payload size - // - payloadSize = sbgStreamBufferGetSize(pInputStream); - - // - // Check that the received buffer can be stored in the RAW message log - // - if (payloadSize <= SBG_ECOM_GPS_RAW_MAX_BUFFER_SIZE) - { - // - // Copy the buffer - // - errorCode = sbgStreamBufferReadBuffer(pInputStream, pOutputData->rawBuffer, payloadSize); - pOutputData->bufferSize = payloadSize; - } - else - { - // - // Unable to store the received buffer due to buffer overflow - // - errorCode = SBG_BUFFER_OVERFLOW; - } - - return errorCode; -} - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_RAW message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogGpsRaw *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the buffer and return if any error has occurred - // - return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->rawBuffer, pInputData->bufferSize); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h deleted file mode 100644 index 3bde0da..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h +++ /dev/null @@ -1,391 +0,0 @@ -/*! - * \file sbgEComBinaryLogGps.h - * \author SBG Systems (Raphael Siryani) - * \date 20 February 2013 - * - * \brief This file is used to parse received GPS binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_GPS_H__ -#define __SBG_ECOM_BINARY_LOG_GPS_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log GPS velocity const definitions -// -//----------------------------------------------------------------------// - -/*! - * Log GPS velocity status and type definitions. - */ -#define SBG_ECOM_GPS_VEL_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS velocity status part. */ -#define SBG_ECOM_GPS_VEL_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity status part. */ -#define SBG_ECOM_GPS_VEL_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS velocity type part. */ -#define SBG_ECOM_GPS_VEL_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity type part. */ - -//----------------------------------------------------------------------// -//- Log GPS position const definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS position status and type definitions. - */ -#define SBG_ECOM_GPS_POS_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS position status part. */ -#define SBG_ECOM_GPS_POS_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position status part. */ -#define SBG_ECOM_GPS_POS_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS position type part. */ -#define SBG_ECOM_GPS_POS_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position type part. */ - -/*! - * GPS position mask definitions - */ -#define SBG_ECOM_GPS_POS_GPS_L1_USED (0x00000001u << 12) /*!< Set to 1 if GPS L1 is used in solution. */ -#define SBG_ECOM_GPS_POS_GPS_L2_USED (0x00000001u << 13) /*!< Set to 1 if GPS L2 is used in solution. */ -#define SBG_ECOM_GPS_POS_GPS_L5_USED (0x00000001u << 14) /*!< Set to 1 if GPS L5 is used in solution. */ -#define SBG_ECOM_GPS_POS_GLO_L1_USED (0x00000001u << 15) /*!< Set to 1 if GLONASS L1 is used in solution. */ -#define SBG_ECOM_GPS_POS_GLO_L2_USED (0x00000001u << 16) /*!< Set to 1 if GLONASS L2 is used in solution. */ - -//----------------------------------------------------------------------// -//- Log GPS HDT const definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS HDT status definitions. - */ -#define SBG_ECOM_GPS_HDT_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS HDT status part. */ -#define SBG_ECOM_GPS_HDT_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS HDT status part. */ - -/*! - * GPS HDT status bitmasks - */ -#define SBG_ECOM_GPS_HDT_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ - -//----------------------------------------------------------------------// -//- Log GPS RAW const definitions -// -//----------------------------------------------------------------------// - -#define SBG_ECOM_GPS_RAW_MAX_BUFFER_SIZE (4086u) /*!< Maximum buffer size in bytes that can be stored in the GPS raw log. */ - -//----------------------------------------------------------------------// -//- Log GPS velocity enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS velocity status definitions. - */ -typedef enum _SbgEComGpsVelStatus -{ - SBG_ECOM_VEL_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_VEL_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_VEL_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_VEL_LIMIT = 3 /*!< Velocity limit exceeded. */ -} SbgEComGpsVelStatus; - -/*! - * GPS velocity types definitions. - */ -typedef enum _SbgEComGpsVelType -{ - SBG_ECOM_VEL_NO_SOLUTION = 0, /*!< No valid velocity solution available. */ - SBG_ECOM_VEL_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_VEL_DOPPLER = 2, /*!< A Doppler velocity has been computed. */ - SBG_ECOM_VEL_DIFFERENTIAL = 3 /*!< A differential velocity has been computed between two positions. */ -} SbgEComGpsVelType; - -//----------------------------------------------------------------------// -//- Log GPS position enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS position status definitions. - */ -typedef enum _SbgEComGpsPosStatus -{ - SBG_ECOM_POS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_POS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_POS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_POS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ -} SbgEComGpsPosStatus; - -/*! - * GPS position types definitions. - */ -typedef enum _SbgEComGpsPosType -{ - SBG_ECOM_POS_NO_SOLUTION = 0, /*!< No valid solution available. */ - SBG_ECOM_POS_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ - SBG_ECOM_POS_SINGLE = 2, /*!< Single point solution position. */ - SBG_ECOM_POS_PSRDIFF = 3, /*!< Standard Pseudorange Differential Solution (DGPS). */ - SBG_ECOM_POS_SBAS = 4, /*!< SBAS satellite used for differential corrections. */ - SBG_ECOM_POS_OMNISTAR = 5, /*!< Omnistar VBS Position (L1 sub-meter). */ - SBG_ECOM_POS_RTK_FLOAT = 6, /*!< Floating RTK ambiguity solution (20 cms RTK). */ - SBG_ECOM_POS_RTK_INT = 7, /*!< Integer RTK ambiguity solution (2 cms RTK). */ - SBG_ECOM_POS_PPP_FLOAT = 8, /*!< Precise Point Positioning with float ambiguities. */ - SBG_ECOM_POS_PPP_INT = 9, /*!< Precise Point Positioning with fixed ambiguities. */ - SBG_ECOM_POS_FIXED = 10 /*!< Fixed location solution position. */ -} SbgEComGpsPosType; - -//----------------------------------------------------------------------// -//- Log GPS HDT enums definitions -// -//----------------------------------------------------------------------// - -/*! - * GPS HDT status definitions. - */ -typedef enum _SbgEComGpsHdtStatus -{ - SBG_ECOM_HDT_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ - SBG_ECOM_HDT_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ - SBG_ECOM_HDT_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ - SBG_ECOM_HDT_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ -} SbgEComGpsHdtStatus; - -//----------------------------------------------------------------------// -//- Helpers methods for velocity status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS velocity status from a status field. - * \param[in] status Status uint32_t value to extract the velocity status from it. - * \return The extracted velocity status. - */ -SBG_INLINE SbgEComGpsVelStatus sbgEComLogGpsVelGetStatus(uint32_t status) -{ - return (SbgEComGpsVelStatus)((status >> SBG_ECOM_GPS_VEL_STATUS_SHIFT) & SBG_ECOM_GPS_VEL_STATUS_MASK); -} - -/*! - * Method used to read GPS velocity type from a status field. - * \param[in] status Status uint32_t value to extract the velocity type from it. - * \return The extracted velocity type. - */ -SBG_INLINE SbgEComGpsVelType sbgEComLogGpsVelGetType(uint32_t status) -{ - return (SbgEComGpsVelType)((status >> SBG_ECOM_GPS_VEL_TYPE_SHIFT) & SBG_ECOM_GPS_VEL_TYPE_MASK); -} - -/*! - * Method used to write the GPS velocity status to a status field. - * \param[in] status The velocity status to set. - * \param[in] type The velocity type to set. - * \return The build GpsVelData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsVelBuildStatus(SbgEComGpsVelStatus status, SbgEComGpsVelType type) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_VEL_STATUS_MASK) << SBG_ECOM_GPS_VEL_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_GPS_VEL_TYPE_MASK) << SBG_ECOM_GPS_VEL_TYPE_SHIFT); -} - -//----------------------------------------------------------------------// -//- Helpers methods for position status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS position status from a status field. - * \param[in] status Status uint32_t value to extract the position status from it. - * \return The extracted position status. - */ -SBG_INLINE SbgEComGpsPosStatus sbgEComLogGpsPosGetStatus(uint32_t status) -{ - return (SbgEComGpsPosStatus)((status >> SBG_ECOM_GPS_POS_STATUS_SHIFT) & SBG_ECOM_GPS_POS_STATUS_MASK); -} - -/*! - * Method used to read GPS position type from a status field. - * \param[in] status Status uint32_t value to extract the position type from it. - * \return The extracted position type. - */ -SBG_INLINE SbgEComGpsPosType sbgEComLogGpsPosGetType(uint32_t status) -{ - return (SbgEComGpsPosType)((status >> SBG_ECOM_GPS_POS_TYPE_SHIFT) & SBG_ECOM_GPS_POS_TYPE_MASK); -} - -/*! - * Method used to write the GPS position status to a status field. - * \param[in] status The position status to set. - * \param[in] type The position type to set. - * \param[in] masks Bit mask to set. - * \return The build GpsPosData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsPosBuildStatus(SbgEComGpsPosStatus status, SbgEComGpsPosType type, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_POS_STATUS_MASK) << SBG_ECOM_GPS_POS_STATUS_SHIFT) | - ((((uint32_t)type)&SBG_ECOM_GPS_POS_TYPE_MASK) << SBG_ECOM_GPS_POS_TYPE_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Helpers methods for HDT status access -// -//----------------------------------------------------------------------// - -/*! - * Method used to read GPS HDT status from a status field. - * \param[in] status Status uint32_t value to extract the HDT status from it. - * \return The extracted position status. - */ -SBG_INLINE SbgEComGpsHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status) -{ - return (SbgEComGpsHdtStatus)((status >> SBG_ECOM_GPS_HDT_STATUS_SHIFT) & SBG_ECOM_GPS_HDT_STATUS_MASK); -} - -/*! - * Method used to write the GPS HDT status to a status field. - * \param[in] status The HDT status to set. - * \param[in] masks Bit mask to set. - * \return The build GpsPosData status field. - */ -SBG_INLINE uint32_t sbgEComLogGpsHdtBuildStatus(SbgEComGpsHdtStatus status, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)status)&SBG_ECOM_GPS_HDT_STATUS_MASK) << SBG_ECOM_GPS_HDT_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_VEL message. - */ -typedef struct _SbgLogGpsVel -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< GPS velocity status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - float velocity[3]; /*!< GPS North, East, Down velocity in m.s^-1. */ - float velocityAcc[3]; /*!< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1. */ - float course; /*!< Track ground course in degrees. */ - float courseAcc; /*!< Course accuracy in degrees. */ -} SbgLogGpsVel; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_POS message. - */ -typedef struct _SbgLogGpsPos -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint32_t status; /*!< GPS position status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - double latitude; /*!< Latitude in degrees, positive north. */ - double longitude; /*!< Longitude in degrees, positive east. */ - double altitude; /*!< Altitude above Mean Sea Level in meters. */ - float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ - float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ - float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ - float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters. */ - uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution (since version 1.4). */ - uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4). */ - uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4). */ -} SbgLogGpsPos; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_HDT message. - */ -typedef struct _SbgLogGpsHdt -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< GPS HDT status, type and bitmask. */ - uint32_t timeOfWeek; /*!< GPS time of week in ms. */ - float heading; /*!< GPS true heading in degrees. */ - float headingAccuracy; /*!< 1 sigma GPS true heading accuracy in degrees. */ - float pitch; /*!< GPS pitch angle measured from the master to the rover in degrees. */ - float pitchAccuracy; /*!< 1 signa GPS pitch angle accuarcy in degrees. */ - float baseline; /*!< The distance between the main and aux antenna in meters. */ -} SbgLogGpsHdt; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_GPS#_RAW message. - */ -typedef struct _SbgLogGpsRaw -{ - uint8_t rawBuffer[SBG_ECOM_GPS_RAW_MAX_BUFFER_SIZE]; /*!< Buffer that contains raw GPS data. */ - size_t bufferSize; /*!< Raw buffer size in bytes. */ -} SbgLogGpsRaw; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_VEL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_VEL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_POS message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_POS message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_HDT message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_HDT message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_GPS#_RAW message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogGpsRaw *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_GPS#_RAW message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogGpsRaw *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c deleted file mode 100644 index eee4cc0..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c +++ /dev/null @@ -1,259 +0,0 @@ -#include "sbgEComBinaryLogImu.h" - -//----------------------------------------------------------------------// -//- Getter / helpers -// -//----------------------------------------------------------------------// - -/*! - * Return from an IMU Short log, the X, Y or Z delta angle value in rad.s^-1 - * \param[in] pImuShort Input IMU short message instance. - * \param[in] idx The component to return from 0 to 2. - * \return The delta angle value converted in rad.s^-1. - */ -float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx) -{ - assert(pImuShort); - assert(idx < 3); - - return pImuShort->deltaAngle[idx] / 67108864.0f; -} - -/*! - * Return from an IMU Short log, the X, Y or Z delta velocity value in m.s^-2 - * \param[in] pImuShort Input IMU short message instance. - * \param[in] idx The component to return from 0 to 2. - * \return The delta velocity value converted in m.s^-2. - */ -float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx) -{ - assert(pImuShort); - assert(idx < 3); - - return pImuShort->deltaVelocity[idx] / 1048576.0f; -} - -/*! - * Return from an IMU Short log, the temperature in °C - * \param[in] pImuShort Input IMU short message instance. - * \return The converted temperature in °C - */ -float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort) -{ - assert(pImuShort); - - return pImuShort->temperature / 256.0f; -} - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->gyroscopes[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->temperature = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->deltaVelocity[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaVelocity[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaVelocity[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->deltaAngle[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaAngle[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->deltaAngle[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_IMU_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->temperature); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! -* Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. -* \param[in] pInputStream Input stream buffer to read the payload from. -* \param[out] pOutputData Pointer on the output structure that stores parsed data. -* \return SBG_NO_ERROR if the payload has been parsed. -*/ -SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->deltaVelocity[0] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaVelocity[1] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaVelocity[2] = sbgStreamBufferReadInt32LE(pInputStream); - - pOutputData->deltaAngle[0] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaAngle[1] = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->deltaAngle[2] = sbgStreamBufferReadInt32LE(pInputStream); - - pOutputData->temperature = sbgStreamBufferReadInt16LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! -* Write data for the SBG_ECOM_LOG_IMU_SHORT message to the output stream buffer from the provided structure. -* \param[out] pOutputStream Output stream buffer to write the payload to. -* \param[in] pInputData Pointer on the input structure that stores data to write. -* \return SBG_NO_ERROR if the message has been generated in the provided buffer. -*/ -SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[0]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[1]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[2]); - - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[0]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[1]); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[2]); - - sbgStreamBufferWriteInt16LE(pOutputStream, pInputData->temperature); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->accelerometers[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - pOutputData->accelerometers[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - pOutputData->accelerometers[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; - - pOutputData->gyroscopes[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - pOutputData->gyroscopes[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - pOutputData->gyroscopes[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_FAST_IMU_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[0] * 100.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[1] * 100.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[2] * 100.0f)); - - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[0] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[1] * 1000.0f)); - sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[2] * 1000.0f)); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h deleted file mode 100644 index 519f7df..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h +++ /dev/null @@ -1,168 +0,0 @@ -/*! - * \file sbgEComBinaryLogImu.h - * \author SBG Systems (Raphael Siryani) - * \date 25 February 2013 - * - * \brief This file is used to parse received IMU binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_IMU_H__ -#define __SBG_ECOM_BINARY_LOG_IMU_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log Inertial Data definitions -// -//----------------------------------------------------------------------// - -/*! - * Log inertial data status mask definitions - */ -#define SBG_ECOM_IMU_COM_OK (0x00000001u << 0) /*!< Set to 1 if the communication with the IMU is ok. */ -#define SBG_ECOM_IMU_STATUS_BIT (0x00000001u << 1) /*!< Set to 1 if the IMU passes general Built in Tests (calibration, CPU, ...). */ - -#define SBG_ECOM_IMU_ACCEL_X_BIT (0x00000001u << 2) /*!< Set to 1 if the accelerometer X passes Built In Test. */ -#define SBG_ECOM_IMU_ACCEL_Y_BIT (0x00000001u << 3) /*!< Set to 1 if the accelerometer Y passes Built In Test. */ -#define SBG_ECOM_IMU_ACCEL_Z_BIT (0x00000001u << 4) /*!< Set to 1 if the accelerometer Z passes Built In Test. */ - -#define SBG_ECOM_IMU_GYRO_X_BIT (0x00000001u << 5) /*!< Set to 1 if the gyroscope X passes Built In Test. */ -#define SBG_ECOM_IMU_GYRO_Y_BIT (0x00000001u << 6) /*!< Set to 1 if the gyroscope Y passes Built In Test. */ -#define SBG_ECOM_IMU_GYRO_Z_BIT (0x00000001u << 7) /*!< Set to 1 if the gyroscope Z passes Built In Test. */ - -#define SBG_ECOM_IMU_ACCELS_IN_RANGE (0x00000001u << 8) /*!< Set to 1 if all accelerometers are within operating range. */ -#define SBG_ECOM_IMU_GYROS_IN_RANGE (0x00000001u << 9) /*!< Set to 1 if all gyroscopes are within operating range. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_IMU_DATA message. - */ -typedef struct _SbgLogImuData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< IMU status bitmask. */ - float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ - float gyroscopes[3]; /*!< X, Y, Z gyroscopes in rad.s^-1. */ - float temperature; /*!< Internal temperature in °C. */ - float deltaVelocity[3]; /*!< X, Y, Z delta velocity in m.s^-2. */ - float deltaAngle[3]; /*!< X, Y, Z delta angle in rad.s^-1. */ -} SbgLogImuData; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_IMU_SHORT message. - * This message is only sent asynchronously and is the preferred log for post processing. - */ -typedef struct _SbgLogImuShort -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< IMU status bitmask. */ - int32_t deltaVelocity[3]; /*!< X, Y, Z delta velocity. Unit is 1048576 LSB for 1 m.s^-2. */ - int32_t deltaAngle[3]; /*!< X, Y, Z delta angle. Unit is 67108864 LSB for 1 rad.s^-1. */ - int16_t temperature; /*!< IMU average temperature. Unit is 256 LSB for 1°C. */ -} SbgLogImuShort; - -/*! - * Structure that stores the data for SBG_ECOM_LOG_FAST_IMU_DATA message - */ -typedef struct _SbgLogFastImuData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< IMU status bitmask. */ - float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ - float gyroscopes[3]; /*!< X, Y, Z gyroscopes in rad.s^-1. */ -} SbgLogFastImuData; - -//----------------------------------------------------------------------// -//- Getter / helpers -// -//----------------------------------------------------------------------// - -/*! - * Return from an IMU Short log, the X, Y or Z delta angle value in rad.s^-1 - * \param[in] pImuShort Input IMU short message instance. - * \param[in] idx The component to return from 0 to 2. - * \return The delta angle value converted in rad.s^-1. - */ -float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx); - -/*! - * Return from an IMU Short log, the X, Y or Z delta velocity value in m.s^-2 - * \param[in] pImuShort Input IMU short message instance. - * \param[in] idx The component to return from 0 to 2. - * \return The delta velocity value converted in m.s^-2. - */ -float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx); - -/*! - * Return from an IMU Short log, the temperature in °C - * \param[in] pImuShort Input IMU short message instance. - * \return The converted temperature in °C - */ -float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort); - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_IMU_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_IMU_SHORT message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_FAST_IMU_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.c deleted file mode 100644 index dbe0a6f..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.c +++ /dev/null @@ -1,115 +0,0 @@ -#include "sbgEComBinaryLogImuRaw.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_IMU_RAW_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseImuRawData(SbgStreamBuffer *pInputStream, SbgLogImuRawData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->rawAccelerometers[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawAccelerometers[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawAccelerometers[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawGyroscopes[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawGyroscopes[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawGyroscopes[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawMagnetometers[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawMagnetometers[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawMagnetometers[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawTempAccels[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempAccels[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempAccels[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawTempGyros[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempGyros[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempGyros[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawTempMags[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempMags[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempMags[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawAuxValues[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawAuxValues[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawAuxValues[2] = sbgStreamBufferReadUint32LE(pInputStream); - - pOutputData->rawTempAuxValues[0] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempAuxValues[1] = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->rawTempAuxValues[2] = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_IMU_RAW_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteImuRawData(SbgStreamBuffer *pOutputStream, const SbgLogImuRawData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAccelerometers[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAccelerometers[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAccelerometers[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawGyroscopes[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawGyroscopes[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawGyroscopes[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawMagnetometers[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawMagnetometers[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawMagnetometers[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAccels[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAccels[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAccels[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempGyros[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempGyros[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempGyros[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempMags[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempMags[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempMags[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAuxValues[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAuxValues[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawAuxValues[2]); - - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAuxValues[0]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAuxValues[1]); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->rawTempAuxValues[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.h deleted file mode 100644 index a4c5278..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogImuRaw.h +++ /dev/null @@ -1,76 +0,0 @@ -/*! - * \file sbgEComBinaryLogImuRaw.h - * \author SBG Systems (Alexis Guinamard) - * \date 18 Jully 2017 - * - * \brief This file is used to parse received RAW IMU logs. - * - * Raw IMU logs are used for calibration and tests purposes. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2017, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_IMU_RAW_H__ -#define __SBG_ECOM_BINARY_LOG_IMU_RAW_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log Inertial Data definitions -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_IMU_RAW_DATA message. - */ -typedef struct _SbgLogImuRawData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< IMU status bitmask. */ - - int32_t rawAccelerometers[3]; /*!< X,Y,Z raw accelerometers signed data. No scale factor defined. */ - int32_t rawGyroscopes[3]; /*!< X,Y,Z raw gyroscopes signed data. No scale factor defined. */ - int32_t rawMagnetometers[3]; /*!< X,Y,Z raw magnetometers signed data. No scale factor defined. */ - - int32_t rawTempAccels[3]; /*!< X,Y,Z raw accelerometers temperature signed data. No scale factor defined. */ - int32_t rawTempGyros[3]; /*!< X,Y,Z raw gyroscopes temperature signed data. No scale factor defined. */ - int32_t rawTempMags[3]; /*!< X,Y,Z raw magnetometers temperature signed data. No scale factor defined. */ - - int32_t rawAuxValues[3]; /*!< Spare raw sensor values for specifics needs. */ - int32_t rawTempAuxValues[3]; /*!< Spare raw sensor temperature values for specifics needs. */ -} SbgLogImuRawData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_IMU_RAW_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseImuRawData(SbgStreamBuffer *pInputStream, SbgLogImuRawData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_IMU_RAW_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteImuRawData(SbgStreamBuffer *pOutputStream, const SbgLogImuRawData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c deleted file mode 100644 index 8c92169..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c +++ /dev/null @@ -1,113 +0,0 @@ -#include "sbgEComBinaryLogMag.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_MAG message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->magnetometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->magnetometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->magnetometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_MAG message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[2]); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Read the raw magnetic calibration data buffer - // - return sbgStreamBufferReadBuffer(pInputStream, pOutputData->magData, sizeof(pOutputData->magData)); -} - -/*! - * Write data for the SBG_ECOM_LOG_MAG_CALIB message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved); - - // - // Write the raw magnetic calibration data buffer - // - return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->magData, sizeof(pInputData->magData)); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h deleted file mode 100644 index 8a0a911..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h +++ /dev/null @@ -1,107 +0,0 @@ -/*! - * \file sbgEComBinaryLogMag.h - * \author SBG Systems (Raphael Siryani) - * \date 12 March 2013 - * - * \brief This file is used to parse received magnetometer binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_MAG_H__ -#define __SBG_ECOM_BINARY_LOG_MAG_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log magnetometers status definitions -// -//----------------------------------------------------------------------// - -/*! - * Log magnetometer data status mask definitions - */ -#define SBG_ECOM_MAG_MAG_X_BIT (0x00000001u << 0) /*!< Set to 1 if the magnetometer X passes Built In Test. */ -#define SBG_ECOM_MAG_MAG_Y_BIT (0x00000001u << 1) /*!< Set to 1 if the magnetometer Y passes Built In Test. */ -#define SBG_ECOM_MAG_MAG_Z_BIT (0x00000001u << 2) /*!< Set to 1 if the magnetometer Z passes Built In Test. */ - -#define SBG_ECOM_MAG_ACCEL_X_BIT (0x00000001u << 3) /*!< Set to 1 if the accelerometer X passes Built In Test. */ -#define SBG_ECOM_MAG_ACCEL_Y_BIT (0x00000001u << 4) /*!< Set to 1 if the accelerometer Y passes Built In Test. */ -#define SBG_ECOM_MAG_ACCEL_Z_BIT (0x00000001u << 5) /*!< Set to 1 if the accelerometer Z passes Built In Test. */ - -#define SBG_ECOM_MAG_MAGS_IN_RANGE (0x00000001u << 6) /*!< Set to 1 if all magnetometers are within operating range. */ -#define SBG_ECOM_MAG_ACCELS_IN_RANGE (0x00000001u << 7) /*!< Set to 1 if all accelerometers are within operating range. */ - -#define SBG_ECOM_MAG_CALIBRATION_OK (0x00000001u << 8) /*!< Set to 1 if the magnetometers seems to be calibrated. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_MAG message. - */ -typedef struct _SbgLogMag -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< Magnetometer status bitmask. */ - float magnetometers[3]; /*!< X, Y, Z magnetometer data in A.U. */ - float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ -} SbgLogMag; - -/*! - * Structure that stores data for the SBG_ECOM_LOG_MAG_CALIB message. - */ -typedef struct _SbgLogMagCalib -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t reserved; /*!< Reserved for future use. */ - uint8_t magData[16]; /*!< Magnetometers calibration data. */ -} SbgLogMagCalib; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_MAG message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_MAG message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData); - -/*! - * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_MAG_CALIB message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c deleted file mode 100644 index 6540e33..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "sbgEComBinaryLogOdometer.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_ODO_VEL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->velocity = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_ODO_VEL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h deleted file mode 100644 index 278b8e8..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h +++ /dev/null @@ -1,70 +0,0 @@ -/*! - * \file sbgEComBinaryLogOdometer.h - * \author SBG Systems (Raphael Siryani) - * \date 25 February 2013 - * - * \brief This file is used to parse received odometer binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_ODOMETER_H__ -#define __SBG_ECOM_BINARY_LOG_ODOMETER_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log odometer status definitions -// -//----------------------------------------------------------------------// - -/*! - * Odometer / velocity status mask definitions. - */ -#define SBG_ECOM_ODO_REAL_MEAS (0x0001 << 0) /*!< Set to 1 if this log comes from a real pulse measurement or from a timeout. */ -#define SBG_ECOM_ODO_TIME_SYNC (0x0001 << 1) /*!< Set to 1 if the velocity information is correctly time synchronized. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for odometer data. - */ -typedef struct _SbgLogOdometerData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< Odometer velocity status bitmask. */ - float velocity; /*!< Velocity in m.s^-1 in the odometer direction. */ -} SbgLogOdometerData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_ODO_VEL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_ODO_VEL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c deleted file mode 100644 index c17d64c..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c +++ /dev/null @@ -1,122 +0,0 @@ -#include "sbgEComBinaryLogShipMotion.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Read the main heave period in seconds - // - pOutputData->mainHeavePeriod = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Read the surge, sway and heave ship motion - // - pOutputData->shipMotion[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipMotion[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipMotion[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Read the ship accelerations - // - pOutputData->shipAccel[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipAccel[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipAccel[2] = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Test if we have a additional information such as ship velocity and status (since version 1.4) - // - if (sbgStreamBufferGetSpace(pInputStream) >= 14) - { - // - // Read new outputs - // - pOutputData->shipVel[0] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipVel[1] = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->shipVel[2] = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - } - else - { - // - // Those outputs are not available in previous versions - // - pOutputData->shipVel[0] = 0.0f; - pOutputData->shipVel[1] = 0.0f; - pOutputData->shipVel[2] = 0.0f; - - pOutputData->status = 0; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - - // - // Write the main heave period in seconds - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->mainHeavePeriod); - - // - // Write the surge, sway and heave ship motion - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[2]); - - // - // Write the ship accelerations - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[2]); - - // - // Write additional inforamtion added in version 1.4 - // - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[0]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[1]); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[2]); - - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h deleted file mode 100644 index cc81189..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h +++ /dev/null @@ -1,78 +0,0 @@ -/*! - * \file sbgEComBinaryLogShipMotion.h - * \author SBG Systems (Raphael Siryani) - * \date 30 March 2013 - * - * \brief This file is used to parse received ship motion binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_SHIP_MOTION_H__ -#define __SBG_ECOM_BINARY_LOG_SHIP_MOTION_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Heave status definitions -// -//----------------------------------------------------------------------// - -#define SBG_ECOM_HEAVE_VALID (0x0001u << 0) /*!< Set to 1 after heave convergence time. */ -#define SBG_ECOM_HEAVE_VEL_AIDED (0x0001u << 1) /*!< Set to 1 if heave output is compensated for transient accelerations. */ -#define SBG_ECOM_HEAVE_SURGE_SWAY_INCLUDED (0x0001u << 2) /*!< Set to 1 if surge and sway channels are provided in this output. */ -#define SBG_ECOM_HEAVE_PERIOD_INCLUDED (0x0001u << 3) /*!< Set to 1 if the heave period is provided in this output. */ -#define SBG_ECOM_HEAVE_PERIOD_VALID (0x0001u << 4) /*!< Set to 1 if the returned heave period is assumed to be valid. */ -#define SBG_ECOM_HEAVE_SWELL_MODE (0x0001u << 5) /*!< Set to 1 if the real time heave filter is using the swell mode computations. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message.
- * The data are expressed in the standard NED Ekinox coordiante frame. - * Surge is positive forward, sway is positive right and heave is positive down.
- * Note that status flag should be read before using the different parameters because it will provide validity information - * about all included outputs. Some frames may not provide the heave period or surge/sway axes for example - */ -typedef struct _SbgLogShipMotionData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< Ship Motion data status bitmask */ - float mainHeavePeriod; /*!< Main heave period in seconds. */ - float shipMotion[3]; /*!< Surge, sway and heave in meters. */ - float shipAccel[3]; /*!< Surge, sway and heave ship Acceleration in m.s^-2. */ - float shipVel[3]; /*!< Surge, sway and heave velocities */ -} SbgLogShipMotionData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c deleted file mode 100644 index b306b58..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c +++ /dev/null @@ -1,84 +0,0 @@ -#include "sbgEComBinaryLogStatus.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_STATUS message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->generalStatus = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->reserved1 = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->comStatus = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->aidingStatus = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved2 = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->reserved3 = sbgStreamBufferReadUint16LE(pInputStream); - - // - // Test if we have a additional information such as uptime (since version 1.7) - // - if (sbgStreamBufferGetSpace(pInputStream) >= sizeof(uint32_t)) - { - // - // Read the additional information - // - pOutputData->uptime = sbgStreamBufferReadUint32LE(pInputStream); - } - else - { - // - // Default the additional information - // - pOutputData->uptime = 0; - } - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_STATUS message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->generalStatus); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved1); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->comStatus); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->aidingStatus); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->reserved2); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved3); - - // - // Write the additional information added in version 1.7 - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->uptime); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h deleted file mode 100644 index 8a52299..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h +++ /dev/null @@ -1,176 +0,0 @@ -/*! - * \file sbgEComBinaryLogStatus.h - * \author SBG Systems (Raphael Siryani) - * \date 03 April 2013 - * - * \brief This file is used to parse received device status binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_STATUS_H__ -#define __SBG_ECOM_BINARY_LOG_STATUS_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- General status definitions -// -//----------------------------------------------------------------------// -#define SBG_ECOM_GENERAL_MAIN_POWER_OK (0x0001u << 0) /*!< Set to 1 when main power supply is OK. */ -#define SBG_ECOM_GENERAL_IMU_POWER_OK (0x0001u << 1) /*!< Set to 1 when IMU power supply is OK. */ -#define SBG_ECOM_GENERAL_GPS_POWER_OK (0x0001u << 2) /*!< Set to 1 when GPS power supply is OK. */ -#define SBG_ECOM_GENERAL_SETTINGS_OK (0x0001u << 3) /*!< Set to 1 if settings where correctly loaded. */ -#define SBG_ECOM_GENERAL_TEMPERATURE_OK (0x0001u << 4) /*!< Set to 1 when temperature is within specified limits. */ -#define SBG_ECOM_GENERAL_DATALOGGER_OK (0x0001u << 5) /*!< Set to 1 when the datalogger is working correctly. */ -#define SBG_ECOM_GENERAL_CPU_OK (0x0001u << 6) /*!< Set to 1 if the CPU headroom is correct.*/ - -//----------------------------------------------------------------------// -//- Communication status definitions -// -//----------------------------------------------------------------------// - -/*! - * Communication CAN status definitions. - */ -#define SBG_ECOM_CAN_STATUS_SHIFT (28u) /*!< Shift used to access the CAN status part. */ -#define SBG_ECOM_CAN_STATUS_MASK (0x00000007u) /*!< Mask used to keep only the CAN status part. */ - -/*! - * Communication status bit mask definitions. - */ -#define SBG_ECOM_PORTA_VALID (0x00000001u << 0) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTB_VALID (0x00000001u << 1) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTC_VALID (0x00000001u << 2) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTD_VALID (0x00000001u << 3) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_PORTE_VALID (0x00000001u << 4) /*!< Set to 0 in case of low level communication error. */ - -#define SBG_ECOM_PORTA_RX_OK (0x00000001u << 5) /*!< Set to 0 in case of error on PORT A input. */ -#define SBG_ECOM_PORTA_TX_OK (0x00000001u << 6) /*!< Set to 0 in case of error on PORT A output. */ -#define SBG_ECOM_PORTB_RX_OK (0x00000001u << 7) /*!< Set to 0 in case of error on PORT B input. */ -#define SBG_ECOM_PORTB_TX_OK (0x00000001u << 8) /*!< Set to 0 in case of error on PORT B output. */ -#define SBG_ECOM_PORTC_RX_OK (0x00000001u << 9) /*!< Set to 0 in case of error on PORT C input. */ -#define SBG_ECOM_PORTC_TX_OK (0x00000001u << 10) /*!< Set to 0 in case of error on PORT C output. */ -#define SBG_ECOM_PORTD_RX_OK (0x00000001u << 11) /*!< Set to 0 in case of error on PORT D input. */ -#define SBG_ECOM_PORTD_TX_OK (0x00000001u << 12) /*!< Set to 0 in case of error on PORT D input. */ -#define SBG_ECOM_PORTE_RX_OK (0x00000001u << 13) /*!< Set to 0 in case of error on PORT E input. */ -#define SBG_ECOM_PORTE_TX_OK (0x00000001u << 14) /*!< Set to 0 in case of error on PORT D input. */ - -#define SBG_ECOM_ETH0_VALID (0x00000001u << 15) /*!< Set to 0 in case of error on ETH0. */ -#define SBG_ECOM_ETH1_VALID (0x00000001u << 16) /*!< Set to 0 in case of error on ETH1. */ -#define SBG_ECOM_ETH2_VALID (0x00000001u << 17) /*!< Set to 0 in case of error on ETH2. */ -#define SBG_ECOM_ETH3_VALID (0x00000001u << 18) /*!< Set to 0 in case of error on ETH3. */ -#define SBG_ECOM_ETH4_VALID (0x00000001u << 19) /*!< Set to 0 in case of error on ETH4. */ - -#define SBG_ECOM_CAN_VALID (0x00000001u << 25) /*!< Set to 0 in case of low level communication error. */ -#define SBG_ECOM_CAN_RX_OK (0x00000001u << 26) /*!< Set to 0 in case of error on CAN Bus input buffer. */ -#define SBG_ECOM_CAN_TX_OK (0x00000001u << 27) /*!< Set to 0 in case of error on CAN Bus output buffer. */ - -/*! - * Communication status for the CAN Bus. - */ -typedef enum _SbgEComCanBusStatus -{ - SBG_ECOM_CAN_BUS_OFF = 0, /*!< Bus OFF operation due to too much errors. */ - SBG_ECOM_CAN_BUS_TX_RX_ERR = 1, /*!< Errors on Tx or Rx. */ - SBG_ECOM_CAN_BUS_OK = 2, /*!< Bus OK. */ - SBG_ECOM_CAN_BUS_ERROR = 3 /*!< Bus error. */ -} SbgEComCanBusStatus; - -//----------------------------------------------------------------------// -//- Aiding status definitions -// -//----------------------------------------------------------------------// -#define SBG_ECOM_AIDING_GPS1_POS_RECV (0x00000001u << 0) /*!< Set to 1 when valid GPS 1 position data is received. */ -#define SBG_ECOM_AIDING_GPS1_VEL_RECV (0x00000001u << 1) /*!< Set to 1 when valid GPS 1 velocity data is received. */ -#define SBG_ECOM_AIDING_GPS1_HDT_RECV (0x00000001u << 2) /*!< Set to 1 when valid GPS 1 true heading data is received. */ -#define SBG_ECOM_AIDING_GPS1_UTC_RECV (0x00000001u << 3) /*!< Set to 1 when valid GPS 1 UTC time data is received. */ -#define SBG_ECOM_AIDING_GPS2_POS_RECV (0x00000001u << 4) /*!< Set to 1 when valid GPS 2 position data is received. */ -#define SBG_ECOM_AIDING_GPS2_VEL_RECV (0x00000001u << 5) /*!< Set to 1 when valid GPS 2 velocity data is received. */ -#define SBG_ECOM_AIDING_GPS2_HDT_RECV (0x00000001u << 6) /*!< Set to 1 when valid GPS 2 true heading data is received. */ -#define SBG_ECOM_AIDING_GPS2_UTC_RECV (0x00000001u << 7) /*!< Set to 1 when valid GPS 2 UTC time data is received. */ -#define SBG_ECOM_AIDING_MAG_RECV (0x00000001u << 8) /*!< Set to 1 when valid Magnetometer data is received. */ -#define SBG_ECOM_AIDING_ODO_RECV (0x00000001u << 9) /*!< Set to 1 when Odometer pulse is received. */ -#define SBG_ECOM_AIDING_DVL_RECV (0x00000001u << 10) /*!< Set to 1 when valid DVL data is received. */ -#define SBG_ECOM_AIDING_USBL_RECV (0x00000001u << 11) /*!< Set to 1 when valid USBL data is received. */ -#define SBG_ECOM_AIDING_DEPTH_RECV (0x00000001u << 12) /*!< Set to 1 when valid Depth Log data is received. */ -#define SBG_ECOM_AIDING_AIR_DATA_RECV (0x00000001u << 13) /*!< Set to 1 when valid Air Data (altitude and/or true airspeed) is received. */ -#define SBG_ECOM_AIDING_USER_POS_RECV (0x00000001u << 14) /*!< Set to 1 when valid user position data is received. */ -#define SBG_ECOM_AIDING_USER_VEL_RECV (0x00000001u << 15) /*!< Set to 1 when valid user velocity data is received. */ -#define SBG_ECOM_AIDING_USER_HEADING_RECV (0x00000001u << 16) /*!< Set to 1 when valid user heading data is received. */ - -//----------------------------------------------------------------------// -//- Status definitions -// -//----------------------------------------------------------------------// - -/*! - * Stores global status data. - */ -typedef struct _SbgLogStatusData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t generalStatus; /*!< General status bitmask and enums. */ - uint16_t reserved1; /*!< Reserved status field for future use */ - uint32_t comStatus; /*!< Communication status bitmask and enums. */ - uint32_t aidingStatus; /*!< Aiding equipments status bitmask and enums. */ - uint32_t reserved2; /*!< Reserved status field for future use. */ - uint16_t reserved3; /*!< Reserved status field for future use. */ - uint32_t uptime; /*!< System uptime in seconds. */ -} SbgLogStatusData; - -//----------------------------------------------------------------------// -//- Communication status helpers methods -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the CAN bus status from a communication status field. - * \param[in] status Status field to extract the CAN bus status from it. - * \return The extracted CAN bus status. - */ -SBG_INLINE SbgEComCanBusStatus sbgEComLogStatusGetCanStatus(uint32_t status) -{ - return (SbgEComCanBusStatus)((status >> SBG_ECOM_CAN_STATUS_SHIFT) & SBG_ECOM_CAN_STATUS_MASK); -} - -/*! - * Method used to write the CAN bus status field. - * \param[in] canStatus The CAN bus status to set. - * \param[in] masks Bit mask to set. - * \return The build communication status field. - */ -SBG_INLINE uint32_t sbgEComLogStatusBuildCommunicationStatus(SbgEComCanBusStatus canStatus, uint32_t masks) -{ - // - // Create the combined status field - // - return ((((uint32_t)canStatus)&SBG_ECOM_CAN_STATUS_MASK) << SBG_ECOM_CAN_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_STATUS message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_STATUS message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c deleted file mode 100644 index b769d21..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c +++ /dev/null @@ -1,69 +0,0 @@ -#include "sbgEComBinaryLogUsbl.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_USBL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - - pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); - pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); - - pOutputData->depth = sbgStreamBufferReadFloatLE(pInputStream); - - pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - pOutputData->depthAccuracy = sbgStreamBufferReadFloatLE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_USBL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); - sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depth); - - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); - sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depthAccuracy); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h deleted file mode 100644 index 6a156ad..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h +++ /dev/null @@ -1,81 +0,0 @@ -/*! - * \file sbgEComBinaryLogUsbl.h - * \author SBG Systems (Raphael Siryani) - * \date 02 June 2014 - * - * \brief This file is used to parse received USBL binary logs. - * - * USBL binary logs contains underwater positioning data of a USBL beacon. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2014, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_USBL_H__ -#define __SBG_ECOM_BINARY_LOG_USBL_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Log USBL status definitions -// -//----------------------------------------------------------------------// - -/*! - * USBL sensor status mask definitions - */ -#define SBG_ECOM_USBL_TIME_SYNC (0x0001u << 0) /*!< Set to 1 if the USBL sensor data is correctly time synchronized. */ -#define SBG_ECOM_USBL_POSITION_VALID (0x0001u << 1) /*!< Set to 1 if the USBL data represents a valid 2D position. */ -#define SBG_ECOM_USBL_DEPTH_VALID (0x0001u << 2) /*!< Set to 1 if the USBL data has a valid depth information. */ - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Log structure for USBL data. - */ -typedef struct _SbgLogUsblData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< USBL system status bitmask. */ - - double latitude; /*!< Latitude in degrees, positive north. */ - double longitude; /*!< Longitude in degrees, positive east. */ - - float depth; /*!< Depth in meters below mean sea level (positive down). */ - - float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ - float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ - float depthAccuracy; /*!< 1 sigma depth accuracy in meters. */ -} SbgLogUsblData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_USBL message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_USBL message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c deleted file mode 100644 index 479c4c4..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c +++ /dev/null @@ -1,67 +0,0 @@ -#include "sbgEComBinaryLogUtc.h" - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_UTC_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData) -{ - assert(pInputStream); - assert(pOutputData); - - // - // Read the frame payload - // - pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); - pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream); - pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream); - pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream); - pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if any error has occurred while parsing the frame - // - return sbgStreamBufferGetLastError(pInputStream); -} - -/*! - * Write data for the SBG_ECOM_LOG_UTC_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData) -{ - assert(pOutputStream); - assert(pInputData); - - // - // Write the frame payload - // - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); - sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->year); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->month); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->day); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->hour); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->minute); - sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->second); - sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->nanoSecond); - sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->gpsTimeOfWeek); - - // - // Return if any error has occurred while writing the frame - // - return sbgStreamBufferGetLastError(pOutputStream); -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h deleted file mode 100644 index 38cf9fb..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h +++ /dev/null @@ -1,146 +0,0 @@ -/*! - * \file sbgEComBinaryLogUtc.h - * \author SBG Systems (Raphael Siryani) - * \date 20 February 2013 - * - * \brief This file is used to parse received UTC binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_BINARY_LOG_UTC_H__ -#define __SBG_ECOM_BINARY_LOG_UTC_H__ - -#include -#include - -//----------------------------------------------------------------------// -//- Clock status definitions -// -//----------------------------------------------------------------------// - -/*! - * Clock status and UTC time status definitions. - */ -#define SBG_ECOM_CLOCK_STATUS_SHIFT (1u) /*!< Shift used to extract the clock status part. */ -#define SBG_ECOM_CLOCK_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock status part. */ -#define SBG_ECOM_CLOCK_UTC_STATUS_SHIFT (6u) /*!< Shift used to extract the clock UTC status part. */ -#define SBG_ECOM_CLOCK_UTC_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock UTC status part. */ - -/*! - * Clock status mask definitions. - */ -#define SBG_ECOM_CLOCK_STABLE_INPUT (0x0001u << 0) /*!< Set to 1 if a stable input clock could be used to synchronized the internal clock. */ -#define SBG_ECOM_CLOCK_UTC_SYNC (0x0001u << 5) /*!< The UTC time is synchronized with a PPS. */ - -/*! - * Clock status enum. - */ -typedef enum _SbgEComClockStatus -{ - SBG_ECOM_CLOCK_ERROR = 0, /*!< An error has occurred on the clock estimation. */ - SBG_ECOM_CLOCK_FREE_RUNNING = 1, /*!< The clock is only based on the internal crystal. */ - SBG_ECOM_CLOCK_STEERING = 2, /*!< A PPS has been detected and the clock is converging to it. */ - SBG_ECOM_CLOCK_VALID = 3 /*!< The clock has converged to the PPS and is within 500ns. */ -} SbgEComClockStatus; - -/*! - * Status for the UTC time data. - */ -typedef enum _SbgEComClockUtcStatus -{ - SBG_ECOM_UTC_INVALID = 0, /*!< The UTC time is not known, we are just propagating the UTC time internally. */ - SBG_ECOM_UTC_NO_LEAP_SEC = 1, /*!< We have received valid UTC time information but we don't have the leap seconds information. */ - SBG_ECOM_UTC_VALID = 2 /*!< We have received valid UTC time data with valid leap seconds. */ -} SbgEComClockUtcStatus; - -//----------------------------------------------------------------------// -//- Clock status helpers methods -// -//----------------------------------------------------------------------// - -/*! - * Method used to read the clock status from a status field. - * \param[in] status Status field to extract the clock status from it. - * \return The extracted clock status. - */ -SBG_INLINE SbgEComClockStatus sbgEComLogUtcGetClockStatus(uint16_t status) -{ - return (SbgEComClockStatus)((status >> SBG_ECOM_CLOCK_STATUS_SHIFT) & SBG_ECOM_CLOCK_STATUS_MASK); -} - -/*! - * Method used to read the UTC time status from a clock status field. - * \param[in] status Status field to extract the UTC time status from it. - * \return The extracted UTC time status. - */ -SBG_INLINE SbgEComClockUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status) -{ - return (SbgEComClockUtcStatus)((status >> SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) & SBG_ECOM_CLOCK_UTC_STATUS_MASK); -} - -/*! - * Method used to write the clock status field. - * \param[in] clockStatus The clock status to set. - * \param[in] utcStatus The UTC time status to set. - * \param[in] masks Bit mask to set. - * \return The build clock status field. - */ -SBG_INLINE uint16_t sbgEComLogUtcBuildClockStatus(SbgEComClockStatus clockStatus, SbgEComClockUtcStatus utcStatus, uint16_t masks) -{ - // - // Create the combined status field - // - return ((((uint16_t)clockStatus)&SBG_ECOM_CLOCK_STATUS_MASK) << SBG_ECOM_CLOCK_STATUS_SHIFT) | - ((((uint16_t)utcStatus)&SBG_ECOM_CLOCK_UTC_STATUS_MASK) << SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) | masks; -} - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Structure that stores data for the SBG_ECOM_LOG_UTC_TIME message. - */ -typedef struct _SbgLogUtcData -{ - uint32_t timeStamp; /*!< Time in us since the sensor power up. */ - uint16_t status; /*!< UTC time and clock status information */ - uint16_t year; /*!< Year for example: 2013. */ - int8_t month; /*!< Month in year [1 .. 12]. */ - int8_t day; /*!< Day in month [1 .. 31]. */ - int8_t hour; /*!< Hour in day [0 .. 23]. */ - int8_t minute; /*!< Minute in hour [0 .. 59]. */ - int8_t second; /*!< Second in minute [0 .. 60]. (60 is used only when a leap second is added) */ - int32_t nanoSecond; /*!< Nanosecond of current second in ns. */ - uint32_t gpsTimeOfWeek; /*!< GPS time of week in ms. */ -} SbgLogUtcData; - -//----------------------------------------------------------------------// -//- Operations -// -//----------------------------------------------------------------------// - -/*! - * Parse data for the SBG_ECOM_LOG_UTC_DATA message and fill the corresponding structure. - * \param[in] pInputStream Input stream buffer to read the payload from. - * \param[out] pOutputData Pointer on the output structure that stores parsed data. - * \return SBG_NO_ERROR if the payload has been parsed. - */ -SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData); - -/*! - * Write data for the SBG_ECOM_LOG_UTC_DATA message to the output stream buffer from the provided structure. - * \param[out] pOutputStream Output stream buffer to write the payload to. - * \param[in] pInputData Pointer on the input structure that stores data to write. - * \return SBG_NO_ERROR if the message has been generated in the provided buffer. - */ -SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c deleted file mode 100644 index 8b62773..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c +++ /dev/null @@ -1,167 +0,0 @@ -#include "sbgEComBinaryLogs.h" -#include - -//----------------------------------------------------------------------// -//- Communication protocol operations -// -//----------------------------------------------------------------------// - -/*! - * Parse an incoming log and fill the output union. - * \param[in] msgClass Received message class - * \param[in] msg Received message ID - * \param[in] pPayload Read only pointer on the payload buffer. - * \param[in] payloadSize Payload size in bytes. - * \param[out] pOutputData Pointer on the output union that stores parsed data. - */ -SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer inputStream; - - assert(pPayload); - assert(payloadSize > 0); - assert(pOutputData); - - // - // Handle the different classes of messages differently - // - if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) - { - // - // Create an input stream buffer that points to the frame payload so we can easily parse it's content - // - sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize); - - // - // Parse the incoming log according to its type - // - switch (msg) - { - case SBG_ECOM_LOG_STATUS: - errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData); - break; - case SBG_ECOM_LOG_IMU_DATA: - errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData); - break; - case SBG_ECOM_LOG_IMU_SHORT: - errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort); - break; - case SBG_ECOM_LOG_EKF_EULER: - errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData); - break; - case SBG_ECOM_LOG_EKF_QUAT: - errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData); - break; - case SBG_ECOM_LOG_EKF_NAV: - errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData); - break; - case SBG_ECOM_LOG_SHIP_MOTION: - case SBG_ECOM_LOG_SHIP_MOTION_HP: - errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData); - break; - case SBG_ECOM_LOG_ODO_VEL: - errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData); - break; - case SBG_ECOM_LOG_UTC_TIME: - errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData); - break; - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: - errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData); - break; - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: - errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData); - break; - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: - errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData); - break; - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: - errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData); - break; - case SBG_ECOM_LOG_MAG: - errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData); - break; - case SBG_ECOM_LOG_MAG_CALIB: - errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData); - break; - case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: - errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); - break; - case SBG_ECOM_LOG_DVL_WATER_TRACK: - errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); - break; - case SBG_ECOM_LOG_AIR_DATA: - errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData); - break; - case SBG_ECOM_LOG_USBL: - errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData); - break; - case SBG_ECOM_LOG_DEPTH: - errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData); - break; - case SBG_ECOM_LOG_EVENT_A: - case SBG_ECOM_LOG_EVENT_B: - case SBG_ECOM_LOG_EVENT_C: - case SBG_ECOM_LOG_EVENT_D: - case SBG_ECOM_LOG_EVENT_E: - case SBG_ECOM_LOG_EVENT_OUT_A: - case SBG_ECOM_LOG_EVENT_OUT_B: - errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker); - break; - case SBG_ECOM_LOG_DEBUG_0: - case SBG_ECOM_LOG_DEBUG_1: - case SBG_ECOM_LOG_DEBUG_2: - case SBG_ECOM_LOG_DEBUG_3: - errorCode = sbgEComBinaryLogParseDebugData(&inputStream, &pOutputData->debugData); - break; - case SBG_ECOM_LOG_IMU_RAW_DATA: - errorCode = sbgEComBinaryLogParseImuRawData(&inputStream, &pOutputData->imuRawData); - break; - case SBG_ECOM_LOG_DIAG: - errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData); - break; - default: - // - // This log isn't handled - // - errorCode = SBG_ERROR; - } - } - else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) - { - // - // Create an input stream buffer that points to the frame payload so we can easily parse it's content - // - sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize); - - // - // Parse the message depending on the message ID - // - switch ((SbgEComLog1)msg) - { - case SBG_ECOM_LOG_FAST_IMU_DATA: - // - // Parse this binary log - // - errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData); - break; - default: - // - // This log isn't handled - // - errorCode = SBG_ERROR; - } - } - else - { - // - // Un-handled message class - // - errorCode = SBG_ERROR; - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h b/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h deleted file mode 100644 index 7b0144e..0000000 --- a/sbg_ros_driver/external/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h +++ /dev/null @@ -1,91 +0,0 @@ -/*! - * \file sbgEComBinaryLogs.h - * \author SBG Systems (Raphael Siryani) - * \date 06 February 2013 - * - * \brief This file is used to parse received binary logs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __BINARY_LOGS_H__ -#define __BINARY_LOGS_H__ - -#include -#include "../sbgEComIds.h" -#include "sbgEComBinaryLogAirData.h" -#include "sbgEComBinaryLogDebug.h" -#include "sbgEComBinaryLogDepth.h" -#include "sbgEComBinaryLogDiag.h" -#include "sbgEComBinaryLogDvl.h" -#include "sbgEComBinaryLogEkf.h" -#include "sbgEComBinaryLogEvent.h" -#include "sbgEComBinaryLogGps.h" -#include "sbgEComBinaryLogImu.h" -#include "sbgEComBinaryLogImuRaw.h" -#include "sbgEComBinaryLogMag.h" -#include "sbgEComBinaryLogOdometer.h" -#include "sbgEComBinaryLogShipMotion.h" -#include "sbgEComBinaryLogStatus.h" -#include "sbgEComBinaryLogUsbl.h" -#include "sbgEComBinaryLogUtc.h" - -//----------------------------------------------------------------------// -//- Log structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Union used to store received logs data. - */ -typedef union _SbgBinaryLogData -{ - SbgLogStatusData statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */ - SbgLogImuData imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */ - SbgLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */ - SbgLogEkfEulerData ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */ - SbgLogEkfQuatData ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */ - SbgLogEkfNavData ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */ - SbgLogShipMotionData shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */ - SbgLogOdometerData odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */ - SbgLogUtcData utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */ - SbgLogGpsPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */ - SbgLogGpsVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */ - SbgLogGpsHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */ - SbgLogGpsRaw gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */ - SbgLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */ - SbgLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ - SbgLogDvlData dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */ - SbgLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */ - SbgLogUsblData usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */ - SbgLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */ - SbgLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */ - SbgLogDebugData debugData; /*!< Stores debug information */ - SbgLogImuRawData imuRawData; /*!< Stores data for the SBG_ECOM_LOG_IMU_RAW_DATA message. */ - SbgLogFastImuData fastImuData; /*!< Stores Fast Imu Data for 1KHz output */ - SbgLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */ -} SbgBinaryLogData; - -//----------------------------------------------------------------------// -//- Communication protocol operations -// -//----------------------------------------------------------------------// - -/*! - * Parse an incoming log and fill the output union. - * \param[in] msgClass Received message class - * \param[in] msg Received message ID - * \param[in] pPayload Read only pointer on the payload buffer. - * \param[in] payloadSize Payload size in bytes. - * \param[out] pOutputData Pointer on the output union that stores parsed data. - */ -SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.c deleted file mode 100644 index 325d32f..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.c +++ /dev/null @@ -1 +0,0 @@ -#include "sbgEComCmd.h" diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.h deleted file mode 100644 index 32d1958..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmd.h +++ /dev/null @@ -1,40 +0,0 @@ -/*! - * \file sbgEComCmd.h - * \author SBG Systems (Maxime Renaudet) - * \date 16 June 2014 - * - * \brief This file describes the eCom command system. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_H__ -#define __SBG_ECOM_CMD_H__ - -#include -#include "sbgEComCmdAdvanced.h" -#include "sbgEComCmdAirData.h" -#include "sbgEComCmdDvl.h" -#include "sbgEComCmdEthernet.h" -#include "sbgEComCmdEvent.h" -#include "sbgEComCmdFeatures.h" -#include "sbgEComCmdGnss.h" -#include "sbgEComCmdInfo.h" -#include "sbgEComCmdInterface.h" -#include "sbgEComCmdLicense.h" -#include "sbgEComCmdMag.h" -#include "sbgEComCmdOdo.h" -#include "sbgEComCmdOutput.h" -#include "sbgEComCmdSensor.h" -#include "sbgEComCmdSettings.h" - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.c deleted file mode 100644 index b671121..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.c +++ /dev/null @@ -1,292 +0,0 @@ -#include "sbgEComCmdAdvanced.h" -#include - -//----------------------------------------------------------------------// -//- Event commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the advanced configurations. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAdvancedConf to contain the current configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedGetConf(SbgEComHandle *pHandle, SbgEComAdvancedConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command without payload since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ADVANCED_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pConf->timeReference = (SbgEComTimeReferenceSrc)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the advanced configurations. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComAdvancedConf that contains the new configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedSetConf(SbgEComHandle *pHandle, const SbgEComAdvancedConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->timeReference); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the current validity thresholds - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComValidityThresholds to contain the current configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ - -SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComValidityThresholds *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command without payload since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_VALIDITY_THRESHOLDS command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters and check payload consistency - // - pConf->positionThreshold = sbgStreamBufferReadFloatLE(&inputStream); - pConf->velocityThreshold = sbgStreamBufferReadFloatLE(&inputStream); - pConf->attitudeThreshold = sbgStreamBufferReadFloatLE(&inputStream); - pConf->headingThreshold = sbgStreamBufferReadFloatLE(&inputStream); - - errorCode = sbgStreamBufferGetLastError(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} -/*! - * Set the validity thresholds - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComValidityThresholds that contains the new configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedSetThresholds(SbgEComHandle *pHandle, const SbgEComValidityThresholds *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pConf->positionThreshold); - sbgStreamBufferWriteFloatLE(&outputStream, pConf->velocityThreshold); - sbgStreamBufferWriteFloatLE(&outputStream, pConf->attitudeThreshold); - sbgStreamBufferWriteFloatLE(&outputStream, pConf->headingThreshold); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.h deleted file mode 100644 index 1676835..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAdvanced.h +++ /dev/null @@ -1,100 +0,0 @@ -/*! - * \file sbgEComCmdAdvanced.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to advanced settings. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef __SBG_ECOM_CMD_ADVANCED_H__ -#define __SBG_ECOM_CMD_ADVANCED_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Advanced definitions -// -//----------------------------------------------------------------------// - -/*! - * List of available time reference source. - */ -typedef enum _SbgEComTimeReferenceSrc -{ - SBG_ECOM_TIME_REF_DISABLED = 0, /*!< The device is running it's internal clock without any time reference. */ - SBG_ECOM_TIME_REF_SYNC_IN_A, /*!< The main port sync in A is used as a time reference. */ - SBG_ECOM_TIME_REF_UTC_GPS_1 /*!< The GPS 1 module is used to provide both time reference and UTC data. */ -} SbgEComTimeReferenceSrc; - -//----------------------------------------------------------------------// -//- Advanced configurations -// -//----------------------------------------------------------------------// - -/*! - * Structure containing all the info for advanced configuration. - */ -typedef struct _SbgEComAdvancedConf -{ - SbgEComTimeReferenceSrc timeReference; /*!< Time reference source for clock alignment. */ -} SbgEComAdvancedConf; - -/*! - * Structure containing all validity thresholds (status outputs) - * Setting these thresholds to 0.0 will keep default configuration - */ -typedef struct _SbgEComValidityThresholds -{ - float positionThreshold; /*!< Norm of the position standard deviation threshold to raise position valid flag (m)*/ - float velocityThreshold; /*!< Norm of the velocity standard deviation threshold to raise velocity valid flag (m/s)*/ - float attitudeThreshold; /*!< Max of the roll/pitch standard deviations threshold to raise attitude valid flag (rad) */ - float headingThreshold; /*!< Heading standard deviations threshold to raise heading valid flag (rad) */ -} SbgEComValidityThresholds; - -//----------------------------------------------------------------------// -//- Advanced commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the advanced configurations. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAdvancedConf to contain the current configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedGetConf(SbgEComHandle *pHandle, SbgEComAdvancedConf *pConf); - -/*! - * Set the advanced configurations. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComAdvancedConf that contains the new configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedSetConf(SbgEComHandle *pHandle, const SbgEComAdvancedConf *pConf); - -/*! - * Retrieve the current validity thresholds - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComValidityThresholds to contain the current configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ - -SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComValidityThresholds *pConf); -/*! - * Set the validity thresholds - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComValidityThresholds that contains the new configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAdvancedSetThresholds(SbgEComHandle *pHandle, const SbgEComValidityThresholds *pConf); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.c deleted file mode 100644 index 08429f8..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.c +++ /dev/null @@ -1,312 +0,0 @@ -/* sbgCommonLib headers */ -#include - -/* Project headers */ -#include "transfer/sbgEComTransfer.h" - -/* Local headers */ -#include "sbgEComCmdAirData.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdAirDataSetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds modelId) -{ - assert(pHandle); - - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, modelId); -} - -SbgErrorCode sbgEComCmdAirDataGetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds *pModelId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t modelIdAsUint; - - assert(pHandle); - assert(pModelId); - - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, &modelIdAsUint); - - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComAirDataModelsIds)modelIdAsUint; - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdAirDataSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pLeverArm); - - // - // Create the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[2]); - - // - // Make sure the payload has been build correctly - // - errorCode = sbgStreamBufferGetLastError(&outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdAirDataGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[64]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pLeverArm); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to parse the payload - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return if an error has occurred during payload parsing - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdAirDataSetRejection(SbgEComHandle *pHandle, const SbgEComAirDataRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[2 * sizeof(uint8_t)]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Create the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->airspeed); - sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->altitude); - - // - // Make sure the payload has been build correctly - // - errorCode = sbgStreamBufferGetLastError(&outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdAirDataGetRejection(SbgEComHandle *pHandle, SbgEComAirDataRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[2 * sizeof(uint8_t)]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to parse payload - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Parse the payload - // - pRejectConf->airspeed = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - pRejectConf->altitude = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return if an error has occurred during payload parsing - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.h deleted file mode 100644 index a89b647..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdAirData.h +++ /dev/null @@ -1,110 +0,0 @@ -/*! - * \file sbgEComCmdAirData.h - * \author SBG Systems - * \date 18 February 2019 - * - * \brief This file implements sbgECom commands related to AirData module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_CMD_AIR_DATA_H -#define SBG_ECOM_CMD_AIR_DATA_H - -/* sbgCommonLib headers */ -#include - -/* Local headers */ -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Public definitions -// -//----------------------------------------------------------------------// - -/*! - * This enum defines the different AirData model IDs available in standard - */ -typedef enum _SbgEComAirDataModelsIds -{ - SBG_ECOM_AIR_DATA_MODEL_INTERNAL = 1, /*!< Use the internal barometer sensor if available. */ - SBG_ECOM_AIR_DATA_MODEL_GENERIC_ECOM = 2, /*!< Generic AirData model using sbgECom input protocol format. */ - SBG_ECOM_AIR_DATA_MODEL_AHRS_500 = 3, /*!< Crossbow AHRS-500 compatible input for barometric altitude and airspeed. */ -} SbgEComAirDataModelsIds; - -/*! - * Holds all necessary information for AirData module data rejection. - */ -typedef struct _SbgEComAirDataRejectionConf -{ - SbgEComRejectionMode airspeed; /*!< Rejection mode for the true air speed measurement. */ - SbgEComRejectionMode altitude; /*!< Rejection mode for the barometric altitude measurement. */ -} SbgEComAirDataRejectionConf; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Set the AirData model to use that both defines the protocol as well as the associated error model. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] modelId AirData model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataSetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds modelId); - -/*! - * Retrieve the AirData model id currently in use by the device. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Returns the AirData model ID currently in use by the device. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataGetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds *pModelId); - -/*! - * Set the lever arm configuration of the AirData module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pLeverArm The X, Y, Z airspeed sensor lever arm in meters from the pitot sensor to the IMU. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm); - -/*! - * Retrieve the lever arm configuration of the AirData module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pLeverArm Returns the airspeed sensor X,Y,Z lever arm in meters from the pitot sensor to the IMU. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm); - -/*! - * Set the rejection configuration of the AirData module (this command doesn't need a reboot to be applied) - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf The new rejection configuration to set. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataSetRejection(SbgEComHandle *pHandle, const SbgEComAirDataRejectionConf *pRejectConf); - -/*! - * Retrieve the current rejection configuration of the AirData module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf Return the rejection configuration currently in use. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdAirDataGetRejection(SbgEComHandle *pHandle, SbgEComAirDataRejectionConf *pRejectConf); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.c deleted file mode 100644 index 0572f35..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.c +++ /dev/null @@ -1,577 +0,0 @@ -#include "sbgEComCmdCommon.h" -#include - -//----------------------------------------------------------------------// -//- Common command reception operations -// -//----------------------------------------------------------------------// - -/*! - * Wait until any command that is not a output log is recevied during a specific time out. - * All binary logs received during this time are handled trough the standard callback system. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMsgClass Pointer used to hold the received command class. - * \param[out] pMsg Pointer used to hold the received command ID. - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \param[in] timeOut Time out in ms during which we can receive the command. - * \return SBG_NO_ERROR if we have received a valid frame.
- * SBG_NOT_READY if we haven't received a valid frame or if the serial buffer is empty.
- * SBG_INVALID_CRC if the received frame has an invalid CRC.
- * SBG_NULL_POINTER if an input parameter is NULL.
- * SBG_BUFFER_OVERFLOW if the received frame payload couldn't fit into the pData buffer. - * SBG_TIME_OUT if the command hasn't been received withint the specified time out. - */ -SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgBinaryLogData logData; - uint8_t receivedMsg; - uint8_t receivedMsgClass; - size_t payloadSize; - uint8_t payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint32_t lastValidTime; - - assert(pHandle); - - // - // Compute the last valid time according to the time out - // - lastValidTime = sbgGetTime() + timeOut; - - // - // Try to receive the desired frame within the specified time out - // - do - { - // - // Read a received frame - // - errorCode = sbgEComProtocolReceive(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsg, payloadData, &payloadSize, sizeof(payloadData)); - - // - // Test if we have received a valid frame - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test if the received frame is a binary log - // - if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) - { - // - // The received frame is a binary log one - // - errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, receivedMsg, payloadData, payloadSize, &logData); - - // - // Test if the incoming log has been parsed successfully - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test if we have a valid callback to handle received logs - // - if (pHandle->pReceiveLogCallback) - { - // - // Call the binary log callback using the new method - // - errorCode = pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsg, &logData, pHandle->pUserArg); - } - } - else - { - // - // Call the on error callback - // - } - } - else - { - // - // Return the received command - // - if (pMsg) - { - *pMsg = receivedMsg; - } - if (pMsgClass) - { - *pMsgClass = receivedMsgClass; - } - - // - // We have received a command so return the payload size - // - if (pSize) - { - *pSize = payloadSize; - } - - // - // Test if we have a payload to return - // - if (payloadSize > 0) - { - // - // Make sure that the payload can be stored and fit in the destination buffer - // - if ( (pData) && (payloadSize <= maxSize) ) - { - // - // Copy the payload - // - memcpy(pData, payloadData, payloadSize); - } - else - { - // - // We have a buffer overflow - // - return SBG_BUFFER_OVERFLOW; - } - } - - // - // We have received the frame we are looking for so return - // - return SBG_NO_ERROR; - } - } - else if (errorCode == SBG_NOT_READY) - { - // - // No more data are present in the reception buffer so release some CPU before the next try - // - sbgSleep(1); - } - } while (lastValidTime >= sbgGetTime()); - - // - // The time out has expired so return time out error - // - errorCode = SBG_TIME_OUT; - - return errorCode; -} - -/*! - * Wait for a specific command to be received given a time out.
- * All binary logs received during this time are handled trough the standard callback system.
- * NACK for the requested command is treated as a negative answer and the corresponding error code is returned to user. In case an ACK is returned, we condider it's an error as well as expected command is not an ACK but regular command. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] msgClass Command class we expect to receive - * \param[out] msg Message id we expect to receive - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \param[in] timeOut Time out in ms during which we can receive the command. - * \return SBG_NO_ERROR if we have received a valid frame.
- * Other error codes may come from the function itself, or from the device returning an NACK. - * - */ -SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgErrorCode ackErrorCode; - uint8_t receivedMsg; - uint8_t receivedMsgClass; - uint8_t ackMsg; - uint8_t ackMsgClass; - uint32_t lastValidTime; - SbgStreamBuffer inputStream; - - assert(pHandle); - - // - // Compute the last valid time according to the time out - // - lastValidTime = sbgGetTime() + timeOut; - - // - // Try to receive the desired frame within the specified time out - // - do - { - // - // Read a received frame - no timeout as this is handled in this function - // Content is directly copied to user parameters in order to avoid multiple data copy - // - errorCode = sbgEComReceiveAnyCmd(pHandle, &receivedMsgClass, &receivedMsg, pData, pSize, maxSize, 0); - - // - // Test if we have received a valid frame - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test if the received frame the requested one - // - if ((receivedMsgClass == msgClass) && (receivedMsg == msg)) - { - // - // We have received the frame we are looking for so return - // Output parameters are already filled by sbgEComReceiveAnyCmd - // - return SBG_NO_ERROR; - } - else if ((receivedMsgClass == SBG_ECOM_CLASS_LOG_CMD_0) && (receivedMsg == SBG_ECOM_CMD_ACK)) - { - // - // We have received an ACK frame. - // We can now parse the message and check if this gives an negative answer to our expected command - // - sbgStreamBufferInitForRead(&inputStream, pData, *pSize); - ackMsg = sbgStreamBufferReadUint8LE(&inputStream); - ackMsgClass = sbgStreamBufferReadUint8LE(&inputStream); - ackErrorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); - - // - // Check if the ACK corresponds to the frame we expected - // - if ((ackMsg == msg) && (ackMsgClass == msgClass)) - { - // - // return the error code if this is a negative one. in case of NO_ERROR, return SBG_ERROR instead - // - if (ackErrorCode != SBG_NO_ERROR) - { - return ackErrorCode; - } - else - { - return SBG_ERROR; - } - } - } - } - else if (errorCode == SBG_NOT_READY) - { - // - // No more data are present in the reception buffer so release some CPU before the next try - // - sbgSleep(1); - } - } while (lastValidTime >= sbgGetTime()); - - // - // The time out has expired so return time out error - // - errorCode = SBG_TIME_OUT; - - return errorCode; -} - -//----------------------------------------------------------------------// -//- ACK related commands operations -// -//----------------------------------------------------------------------// - -/*! - * Wait for an ACK for a specified amount of time. - *\param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass The message class that we want to check - * \param[in] msg The message ID that we want to check - * \param[in] timeOut Time out in ms during which we can receive the ACK. - * \return SBG_NO_ERROR if the ACK has been received. - */ -SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint8_t payload[2*sizeof(uint16_t)]; - SbgStreamBuffer inputStream; - size_t receivedSize; - uint8_t ackClass; - uint8_t ackMsg; - - assert(pHandle); - - // - // Try to receive the ACK - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, payload, &receivedSize, sizeof(payload), timeOut); - - // - // Test if an ACK frame has been received - // - if (errorCode == SBG_NO_ERROR) - { - // - // Validate the received ACK frame - // - if (receivedSize == 2*sizeof(uint16_t)) - { - // - // Initialize a stream buffer to parse the received payload - // - sbgStreamBufferInitForRead(&inputStream, payload, sizeof(payload)); - - // - // The ACK frame contains the ack message ID and class, and a uint16_t for the return error code - // We make sure that the ACK is for the correct command - // - ackMsg = sbgStreamBufferReadUint8LE(&inputStream); - ackClass = sbgStreamBufferReadUint8LE(&inputStream); - - if ((ackMsg == msg) && (ackClass == msgClass)) - { - // - // Parse the error code and return it - // - errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); - } - else - { - // - // We have received an ACK but not for this frame! - // - errorCode = SBG_INVALID_FRAME; - } - } - else - { - // - // The ACK is invalid - // - errorCode = SBG_INVALID_FRAME; - } - } - - return errorCode; -} - -/*! - * Send an ACK for a specific command with an associated error code. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass The message class that we want to send - * \param[in] msg The message ID that we want to send. - * \param[in] cmdError The associated error code. - * \return SBG_NO_ERROR if the ACK has been sent. - */ -SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgErrorCode cmdError) -{ - SbgStreamBuffer outputStream; - uint8_t payload[2*sizeof(uint8_t)+sizeof(uint16_t)]; - - assert(pHandle); - - // - // Initialize a stream buffer to write the command payload - // - sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload)); - - // - // Write the message ID and class and then the error code - // - sbgStreamBufferWriteUint8LE(&outputStream, msg); - sbgStreamBufferWriteUint8LE(&outputStream, msgClass); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)cmdError); - - // - // Send the ACK command - // - return sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); -} - -//----------------------------------------------------------------------// -//- Generic command definitions -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t modelId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[sizeof(uint32_t)]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Init stream buffer for output and Build payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint32LE(&outputStream, modelId); - - // - // Make sure the payload has been build correctly - // - errorCode = sbgStreamBufferGetLastError(&outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdGenericGetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t *pModelId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[sizeof(uint32_t)]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pModelId); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, msgClass, msg, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a the specified command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - *pModelId = sbgStreamBufferReadUint32LE(&inputStream); - - // - // The command has been executed successfully so return - // We return the stream buffer error code to catch any overflow error on the payload - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgEComModelInfo *pModelInfo) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[2*sizeof(uint32_t)]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pModelInfo); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, msgClass, msg, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a the specified command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pModelInfo->id = sbgStreamBufferReadUint32LE(&inputStream); - pModelInfo->revision = sbgStreamBufferReadUint32LE(&inputStream); - - // - // The command has been executed successfully so return - // We return the stream buffer error code to catch any overflow error on the payload - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.h deleted file mode 100644 index 1ec6e9e..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdCommon.h +++ /dev/null @@ -1,169 +0,0 @@ -/*! - * \file sbgEComCmdCommon.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file groups all common definitions required by all commands. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_COMMON_H__ -#define __SBG_ECOM_CMD_COMMON_H__ - -#include "../sbgECom.h" - -//----------------------------------------------------------------------// -//- Defintions -// -//----------------------------------------------------------------------// - -#define SBG_ECOM_DEFAULT_CMD_TIME_OUT (500) /*!< Default time out in ms for commands reception. */ - -/*! - * List of all rejection modes for aiding inputs. - */ -typedef enum _SbgEComRejectionMode -{ - SBG_ECOM_NEVER_ACCEPT_MODE = 0, /*!< Measurement is not taken into account. */ - SBG_ECOM_AUTOMATIC_MODE = 1, /*!< Measurement is accepted and rejected automatically depending on consistency checks */ - SBG_ECOM_ALWAYS_ACCEPT_MODE = 2 /*!< Measurement is always accepted. Should be used with caution */ -} SbgEComRejectionMode; - -/*! - * List of all axis directions for modules/sensor alignment. - */ -typedef enum _SbgEComAxisDirection -{ - SBG_ECOM_ALIGNMENT_FORWARD = 0, /*!< IMU/module Axis is turned in vehicle's forward direction. */ - SBG_ECOM_ALIGNMENT_BACKWARD = 1, /*!< IMU/module Axis is turned in vehicle's backward direction. */ - SBG_ECOM_ALIGNMENT_LEFT = 2, /*!< IMU/module Axis is turned in vehicle's left direction. */ - SBG_ECOM_ALIGNMENT_RIGHT = 3, /*!< IMU/module Axis is turned in vehicle's right direction. */ - SBG_ECOM_ALIGNMENT_UP = 4, /*!< IMU/module Axis is turned in vehicle's up direction. */ - SBG_ECOM_ALIGNMENT_DOWN = 5 /*!< IMU/module Axis is turned in vehicle's down direction. */ -} SbgEComAxisDirection; - -/*! - * Common model information structure. - * This is used for motion profile or Magnetometer,Gps, or other aiding sensor error model. - */ -typedef struct _SbgEComModelInfo -{ - uint32_t id; /*!< Identifier of the model */ - uint32_t revision; /*!< Revision of the model */ -} SbgEComModelInfo; - -//----------------------------------------------------------------------// -//- Common command reception operations -// -//----------------------------------------------------------------------// - -/*! - * Wait until any command that is not a output log is recevied during a specific time out. - * All binary logs received during this time are handled trough the standard callback system. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMsgClass Pointer used to hold the received command class. - * \param[out] pMsg Pointer used to hold the received command ID. - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \param[in] timeOut Time out in ms during which we can receive the command. - * \return SBG_NO_ERROR if we have received a valid frame.
- * SBG_NOT_READY if we haven't received a valid frame or if the serial buffer is empty.
- * SBG_INVALID_CRC if the received frame has an invalid CRC.
- * SBG_NULL_POINTER if an input parameter is NULL.
- * SBG_BUFFER_OVERFLOW if the received frame payload couldn't fit into the pData buffer. - * SBG_TIME_OUT if the command hasn't been received withint the specified time out. - */ -SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); - -/*! - * Wait for a specific command to be received given a time out. - * All binary logs received during this time are handled trough the standard callback system. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] msgClass Command class we expect to receive - * \param[out] msg Message id we expect to receive - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \param[in] timeOut Time out in ms during which we can receive the command. - * \return SBG_NO_ERROR if we have received a valid frame.
- * SBG_NOT_READY if we haven't received a valid frame or if the serial buffer is empty.
- * SBG_INVALID_CRC if the received frame has an invalid CRC.
- * SBG_NULL_POINTER if an input parameter is NULL.
- * SBG_BUFFER_OVERFLOW if the received frame payload couldn't fit into the pData buffer. - * SBG_TIME_OUT if the command hasn't been received withint the specified time out. - */ -SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); - -//----------------------------------------------------------------------// -//- ACK related commands operations -// -//----------------------------------------------------------------------// - -/*! - * Wait for an ACK for a specified amount of time. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass The message class that we want to check - * \param[in] msg The message ID that we want to check - * \param[in] timeOut Time out in ms during which we can receive the ACK. - * \return SBG_NO_ERROR if the ACK has been received. - */ -SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut); - -/*! - * Send an ACK for a specific command with an associated error code. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass The message class that we want to send - * \param[in] msg The message ID that we want to send. - * \param[in] cmdError The associated error code. - * \return SBG_NO_ERROR if the ACK has been sent. - */ -SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgErrorCode cmdError); - -//----------------------------------------------------------------------// -//- Generic command definitions -// -//----------------------------------------------------------------------// - -/*! - * Generic function to set an error model ID - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass Original message class - * \param[in] msg Original message ID - * \param[in] modelId Model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t modelId); - -/*! - * Generic function to get an error model ID - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass Original message class - * \param[in] msg Original message ID - * \param[out] pModelId Returns the currently used model ID. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGenericGetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t *pModelId); - -/*! - * Generic function to retrieve error model information. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] msgClass Original message class - * \param[in] msg Original message ID - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain model info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgEComModelInfo *pModelInfo); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.c deleted file mode 100644 index c5536c7..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.c +++ /dev/null @@ -1,330 +0,0 @@ -/* sbgCommonLib headers */ -#include - -/* Project headers */ -#include "transfer/sbgEComTransfer.h" - -/* Local headers */ -#include "sbgEComCmdDvl.h" - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdDvlSetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds modelId) -{ - assert(pHandle); - - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, modelId); -} - -SbgErrorCode sbgEComCmdDvlGetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds *pModelId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t modelIdAsUint; - - assert(pHandle); - assert(pModelId); - - errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, &modelIdAsUint); - - if (errorCode == SBG_NO_ERROR) - { - *pModelId = (SbgEComDvlModelsIds)modelIdAsUint; - } - - return errorCode; -} - -/*! - * Set the lever arm and alignment configuration of the DVL module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pDvlInstallation The DVL lever arm and alignment configuration to apply. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlInstallationSet(SbgEComHandle *pHandle, const SbgEComDvlInstallation *pDvlInstallation) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[64]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pDvlInstallation); - - // - // Create the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[2]); - - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[2]); - - sbgStreamBufferWriteBooleanLE(&outputStream, pDvlInstallation->preciseInstallation); - - // - // Make sure the payload has been build correctly - // - errorCode = sbgStreamBufferGetLastError(&outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} -SbgErrorCode sbgEComCmdDvlInstallationGet(SbgEComHandle *pHandle, SbgEComDvlInstallation *pDvlInstallation) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[64]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pDvlInstallation); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to parse the payload - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - pDvlInstallation->leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - pDvlInstallation->leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - pDvlInstallation->leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - pDvlInstallation->alignment[0] = sbgStreamBufferReadFloatLE(&inputStream); - pDvlInstallation->alignment[1] = sbgStreamBufferReadFloatLE(&inputStream); - pDvlInstallation->alignment[2] = sbgStreamBufferReadFloatLE(&inputStream); - - pDvlInstallation->preciseInstallation = sbgStreamBufferReadBooleanLE(&inputStream); - - // - // The command has been executed successfully so return if an error has occurred during payload parsing - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdDvlSetRejection(SbgEComHandle *pHandle, const SbgEComDvlRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[2*sizeof(uint8_t)]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Create the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->bottomLayer); - sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->waterLayer); - - // - // Make sure the payload has been build correctly - // - errorCode = sbgStreamBufferGetLastError(&outputStream); - - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdDvlGetRejection(SbgEComHandle *pHandle, SbgEComDvlRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[2*sizeof(uint8_t)]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to parse payload - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Parse the payload - // - pRejectConf->bottomLayer = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - pRejectConf->waterLayer = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return if an error has occurred during payload parsing - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.h deleted file mode 100644 index c6ed250..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdDvl.h +++ /dev/null @@ -1,118 +0,0 @@ -/*! - * \file sbgEComCmdDvl.h - * \author SBG Systems - * \date 13 December 2018 - * - * \brief This file implements sbgECom commands related to DVL module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2018, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_CMD_DVL_H -#define SBG_ECOM_CMD_DVL_H - -/* sbgCommonLib headers */ -#include - -/* Local headers */ -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Public definitions -// -//----------------------------------------------------------------------// - -/*! - * This enum defines the different DVL model IDs available in standard - */ -typedef enum _SbgEComDvlModelsIds -{ - SBG_ECOM_DVL_MODEL_GENERIC_PD6 = 202, /*!< Generic DVL using PD6 protocol format. */ -} SbgEComDvlModelsIds; - -/*! - * DVL mechanical installation parameters such as lever arm and alignment - */ -typedef struct _SbgEComDvlInstallation -{ - float leverArm[3]; /*!< X, Y, Z DVL lever arm in meters expressed from the DVL to the IMU. */ - float alignment[3]; /*!< Roll, pitch, yaw DVL alignment expressed in radians. */ - bool preciseInstallation; /*!< Set to true if both the DVL lever arm and DVL alignment are precise and don't require in-run estimation. */ -} SbgEComDvlInstallation; - -/*! - * Holds all necessary information for DVL module data rejection. - */ -typedef struct _SbgEComDvlRejectionConf -{ - SbgEComRejectionMode bottomLayer; /*!< Rejection mode for the bottom tracking (ie when the velocity measurement is in respect to the seabed). */ - SbgEComRejectionMode waterLayer; /*!< Rejection mode for the water tracking (ie when the velocity measurement is relative to a water layer). */ -} SbgEComDvlRejectionConf; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Set the DVL model to use that both defines the protocol as well as the associated error model. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] modelId DVL model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlSetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds modelId); - -/*! - * Retrieve the DVL model id currently in use by the device. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pModelId Returns the DVL model ID currently in use by the device. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlGetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds *pModelId); - -/*! - * Set the lever arm and alignment configuration of the DVL module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pDvlInstallation The DVL lever arm and alignment configuration to apply. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlInstallationSet(SbgEComHandle *pHandle, const SbgEComDvlInstallation *pDvlInstallation); - -/*! - * Retrieve the lever arm and alignment configuration of the DVL module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pDvlInstallation Returns the DVL lever arm and alignment configuration currently in use. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlInstallationGet(SbgEComHandle *pHandle, SbgEComDvlInstallation *pDvlInstallation); - -/*! - * Set the rejection configuration of the DVL module (this command doesn't need a reboot to be applied) - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf The new DVL rejection configuration to set. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlSetRejection(SbgEComHandle *pHandle, const SbgEComDvlRejectionConf *pRejectConf); - -/*! - * Retrieve the current rejection configuration of the DVL module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf Return the DVL rejection configuration currently in use. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdDvlGetRejection(SbgEComHandle *pHandle, SbgEComDvlRejectionConf *pRejectConf); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.c deleted file mode 100644 index 3a6a0b1..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.c +++ /dev/null @@ -1,280 +0,0 @@ -#include "sbgEComCmdEthernet.h" -#include - -//----------------------------------------------------------------------// -//- Private methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Write in the output stream buffer, the provided Ethernet configuration. - * \param[out] pOutputStream Pointer on the output stream buffer to write to. - * \param[in] pEthernetConf Structure used to hold the parameters to write to the payload buffer. - * \return SBG_NO_ERROR if the structure has been written correctly. - */ -static SbgErrorCode sbgEComEthernetConfWrite(SbgStreamBuffer *pOutputStream, const SbgEComEthernetConf *pEthernetConf) -{ - assert(pOutputStream); - assert(pEthernetConf); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(pOutputStream, (uint8_t)pEthernetConf->mode); - sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->ipAddress); - sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->netmask); - sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->gateway); - sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->dns1); - sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->dns2); - - // - // Return if an error has occurred during the parse - // - return sbgStreamBufferGetLastError(pOutputStream); -} - -/*! - * Parse the input stream buffer to extract all parameters and fill the corresponding structure. - * \param[in] pInputStream Pointer on the input stream buffer to read from. - * \param[out] pEthernetConf Structure used to store the parsed parameters. - * \return SBG_NO_ERROR if the structure has been parsed correctly. - */ -static SbgErrorCode sbgEComEthernetConfParse(SbgStreamBuffer *pInputStream, SbgEComEthernetConf *pEthernetConf) -{ - assert(pInputStream); - assert(pEthernetConf); - - // - // Read all parameters from the payload - // - pEthernetConf->mode = (SbgEComEthernetMode)sbgStreamBufferReadUint8LE(pInputStream); - pEthernetConf->ipAddress = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); - pEthernetConf->netmask = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); - pEthernetConf->gateway = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); - pEthernetConf->dns1 = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); - pEthernetConf->dns2 = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); - - // - // Return if an error has occurred during the parse - // - return sbgStreamBufferGetLastError(pInputStream); -} - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! -* Get the configuration for the Ethernet interface. -* Warning: this method only returns the Ethernet configuration and NOT the ip address currently used by the device. -* You should rather use sbgEComEthernetInfo to retreive the current assigned IP. -* \param[in] pHandle A valid sbgECom handle. -* \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read configuration from the device. -* \return SBG_NO_ERROR if the command has been executed successfully. -*/ -SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pEthernetConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with no payload to retreive the network configuration - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read all parameters from the payload and return any error during the parse - // - errorCode = sbgEComEthernetConfParse(&inputStream, pEthernetConf); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! -* Set the configuration for the Ethernet interface. -* \param[in] pHandle A valid sbgECom handle. -* \param[in] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the new configuration to apply. -* \return SBG_NO_ERROR if the command has been executed successfully. -*/ -SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pEthernetConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - errorCode = sbgEComEthernetConfWrite(&outputStream, pEthernetConf); - - // - // Send the payload if no error has occurred - // - if (errorCode == SBG_NO_ERROR) - { - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - } - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! -* Get the current assigned and used IP address as well as network inforamtion. -* In opposition to sbgEComEthernetGetConf, this method will not return the Ethernet configuration. -* It will rather return the IP address currently used by the device. -* \param[in] pHandle A valid sbgECom handle. -* \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read IP settings from the device. -* \return SBG_NO_ERROR if the command has been executed successfully. -*/ -SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pEthernetConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with no payload to retreive the network configuration - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read all parameters from the payload and return any error during the parse - // - errorCode = sbgEComEthernetConfParse(&inputStream, pEthernetConf); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.h deleted file mode 100644 index 2444d1c..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEthernet.h +++ /dev/null @@ -1,83 +0,0 @@ -/*! - * \file sbgEComCmdEthernet.h - * \author SBG Systems (Maxime Renaudet) - * \date 14 November 2016 - * - * \brief This file implements SbgECom commands related to Ethernet configuration. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2016, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_ETHERNET_H__ -#define __SBG_ECOM_CMD_ETHERNET_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Ethernet interface configuration -// -//----------------------------------------------------------------------// - -/*! - * Enum that defines the different type of IP acquisition method. - */ -typedef enum _SbgEComEthernetMode -{ - SBG_ECOM_ETHERNET_DHCP = 0, /*!< The TCP/IP configuration should be acquired from a DHCP server. */ - SBG_ECOM_ETHERNET_STATIC = 1 /*!< The TCP/IP configuration is manually defined. */ -} SbgEComEthernetMode; - -/*! - * Structure that contains all Ethernet configuration or settings. - */ -typedef struct _SbgEComEthernetConf -{ - SbgEComEthernetMode mode; /*!< Define how the device will acquiere its IP address, either DHCP or Static. */ - sbgIpAddress ipAddress; /*!< For static mode, defines the device IP address. */ - sbgIpAddress netmask; /*!< For static mode, defines the device net mask. */ - sbgIpAddress gateway; /*!< For static mode, defines the gateway to use. */ - sbgIpAddress dns1; /*!< For static mode, defines the primary DNS to use. */ - sbgIpAddress dns2; /*!< For static mode, defines the secondary DNS to use. */ -} SbgEComEthernetConf; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * Get the configuration for the Ethernet interface. - * Warning: this method only returns the Ethernet configuration and NOT the ip address currently used by the device. - * You should rather use sbgEComEthernetInfo to retreive the current assigned IP. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read configuration from the device. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf); - -/*! - * Set the configuration for the Ethernet interface. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the new configuration to apply. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf); - -/*! - * Get the current assigned and used IP address as well as network inforamtion. - * In opposition to sbgEComEthernetGetConf, this method will not return the Ethernet configuration. - * It will rather return the IP address currently used by the device. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read IP settings from the device. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.c deleted file mode 100644 index 07641e0..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.c +++ /dev/null @@ -1,306 +0,0 @@ -#include "sbgEComCmdEvent.h" -#include - -//----------------------------------------------------------------------// -//- Event commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the configuration of a Sync In. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncInId The id of the sync whose configuration is to be retrieved. - * \param[out] pConf Pointer to a SbgEComSyncInConf to contain the current configuration of the sync in. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncInGetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, SbgEComSyncInConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t outputBuffer; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with syncInId as a 1-byte payload - // - outputBuffer = (uint8_t)syncInId; - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &outputBuffer, sizeof(uint8_t)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_SYNC_IN_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned the id of the sync, then the sensitivity and the delay at last. - // - syncInId = (SbgEComSyncInId)sbgStreamBufferReadUint8LE(&inputStream); - pConf->sensitivity = (SbgEComSyncInSensitivity)sbgStreamBufferReadUint8LE(&inputStream); - pConf->delay = sbgStreamBufferReadInt32LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the configuration of a Sync In. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncInId The id of the sync whose configuration is to be set. - * \param[in] pConf Pointer to a SbgEComSyncInConf that contains the new configuration for the sync in. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, const SbgEComSyncInConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)syncInId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->sensitivity); - sbgStreamBufferWriteInt32LE(&outputStream, pConf->delay); - - // - // Send the message over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the configuration of a Sync Out. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncOutId The id of the sync whose configuration is to be retrieved. - * \param[out] pConf Pointer to a SbgEComSyncOutConf to contain the current configuration of the sync out. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncOutGetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, SbgEComSyncOutConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with syncOutId as a 1-byte payload - // - outputBuffer = (uint8_t)syncOutId; - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &outputBuffer, sizeof(uint8_t)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_SYNC_OUT_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned the id of the sync, then a reserved field, the output function, polarity and the duration at last. - // - syncOutId = (SbgEComSyncOutId)sbgStreamBufferReadUint8LE(&inputStream); - sbgStreamBufferReadUint8LE(&inputStream); - pConf->outputFunction = (SbgEComSyncOutFunction)sbgStreamBufferReadUint16LE(&inputStream); - pConf->polarity = (SbgEComSyncOutPolarity)sbgStreamBufferReadUint8LE(&inputStream); - pConf->duration = sbgStreamBufferReadUint32LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the configuration of a Sync Out. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncOutId The id of the sync whose configuration is to be set. - * \param[in] pConf Pointer to a SbgEComSyncOutConf that contains the new configuration for the sync Out. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, const SbgEComSyncOutConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)syncOutId); - sbgStreamBufferWriteUint8LE(&outputStream, 0); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)pConf->outputFunction); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->polarity); - sbgStreamBufferWriteUint32LE(&outputStream, pConf->duration); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.h deleted file mode 100644 index 40d57c8..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdEvent.h +++ /dev/null @@ -1,159 +0,0 @@ -/*! - * \file sbgEComCmdEvent.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to events. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_EVENT_H__ -#define __SBG_ECOM_CMD_EVENT_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Event definitions -// -//----------------------------------------------------------------------// - -/*! - * List of sync in signals available. - */ -typedef enum _SbgEComSyncInId -{ - SBG_ECOM_SYNC_IN_A = 0, /*!< Sync IN A */ - SBG_ECOM_SYNC_IN_B = 1, /*!< Sync IN B */ - SBG_ECOM_SYNC_IN_C = 2, /*!< Sync IN C */ - SBG_ECOM_SYNC_IN_D = 3 /*!< Sync IN D */ -} SbgEComSyncInId; - -/*! - * List of available sensitivities for sync in signals. - */ -typedef enum _SbgEComSyncInSensitivity -{ - SBG_ECOM_SYNC_IN_DISABLED = 0, /*!< This trigger is turned OFF. */ - SBG_ECOM_SYNC_IN_FALLING_EDGE = 1, /*!< The trigger will be activated by a falling edge. */ - SBG_ECOM_SYNC_IN_RISING_EDGE = 2, /*!< The trigger will be activated by a rising edge. */ - SBG_ECOM_SYNC_IN_BOTH_EDGES = 3 /*!< The trigger is activated by a level change (rising or falling edge). */ -} SbgEComSyncInSensitivity; - -/*! - * List of sync out signals available. - */ -typedef enum _SbgEComSyncOutId -{ - SBG_ECOM_SYNC_OUT_A = 0, /*!< Synchronization output A */ - SBG_ECOM_SYNC_OUT_B = 1 /*!< Synchronization output B */ -} SbgEComSyncOutId; - -/*! - * Logic and synchronization output types - */ -typedef enum _SbgEComSyncOutFunction -{ - SBG_ECOM_SYNC_OUT_MODE_DISABLED = 0, /*!< Output is disabled. */ - SBG_ECOM_SYNC_OUT_MODE_MAIN_LOOP = 1, /*!< Output is generated at 200Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_2 = 2, /*!< Output is generated at 100Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_4 = 4, /*!< Output is generated at 50Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_8 = 8, /*!< Output is generated at 25Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_10 = 10, /*!< Output is generated at 20Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_20 = 20, /*!< Output is generated at 10Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_40 = 40, /*!< Output is generated at 5Hz. */ - SBG_ECOM_SYNC_OUT_MODE_DIV_200 = 200, /*!< Output is generated at 1Hz. */ - SBG_ECOM_SYNC_OUT_MODE_PPS = 10000, /*!< Pulse Per Second. Same mode as above. */ - SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_A = 10003, /*!< Output is generated on a Sync In A event. */ - SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_B = 10004, /*!< Output is generated on a Sync In B event. */ - SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_C = 10005, /*!< Output is generated on a Sync In C event. */ - SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_D = 10006, /*!< Output is generated on a Sync In D event. */ - - SBG_ECOM_SYNC_OUT_MODE_DIRECT_PPS = 10100, /*!< The internal GNSS PPS signal is directly routed to the Sync Out. - This mode is only valid for ELLIPSE-N with hardware revisions above 1.2.1.0. - Polarity and duration parameters are ignored with this specific mode. */ - -} SbgEComSyncOutFunction; - -/*! - * Logic output polarity - */ -typedef enum _SbgEComSyncOutPolarity -{ - SBG_ECOM_SYNC_OUT_FALLING_EDGE = 0, /*!< The output pin will generate a falling edge*/ - SBG_ECOM_SYNC_OUT_RISING_EDGE = 1, /*!< The output pin will generate a rising edge */ - SBG_ECOM_SYNC_OUT_TOGGLE = 2, /*!< The pulse is a level change */ -} SbgEComSyncOutPolarity; - -//----------------------------------------------------------------------// -//- Event configurations -// -//----------------------------------------------------------------------// - -/*! - * Helper structure for sync in configuration. - */ -typedef struct _SbgEComSyncInConf -{ - SbgEComSyncInSensitivity sensitivity; /*!< Sensitivity of the sync in. */ - int32_t delay; /*!< Delay to take into account for the sync in. (in us)*/ -} SbgEComSyncInConf; - -/*! - * Helper structure for sync out configuration. - */ -typedef struct _SbgEComSyncOutConf -{ - SbgEComSyncOutFunction outputFunction; /*!< Output function of the sync out pin */ - SbgEComSyncOutPolarity polarity; /*!< Polarity of the sync out. */ - uint32_t duration; /*!< Pulse width for the sync out (in ns). */ -} SbgEComSyncOutConf; - -//----------------------------------------------------------------------// -//- Event commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the configuration of a Sync In. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncInId The id of the sync whose configuration is to be retrieved. - * \param[out] pConf Pointer to a SbgEComSyncInConf to contain the current configuration of the sync in. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncInGetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, SbgEComSyncInConf *pConf); - -/*! - * Set the configuration of a Sync In. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncInId The id of the sync whose configuration is to be set. - * \param[in] pConf Pointer to a SbgEComSyncInConf that contains the new configuration for the sync in. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, const SbgEComSyncInConf *pConf); - -/*! - * Retrieve the configuration of a Sync Out. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncOutId The id of the sync whose configuration is to be retrieved. - * \param[out] pConf Pointer to a SbgEComSyncOutConf to contain the current configuration of the sync out. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncOutGetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, SbgEComSyncOutConf *pConf); - -/*! - * Set the configuration of a Sync Out. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] syncOutId The id of the sync whose configuration is to be set. - * \param[in] pConf Pointer to a SbgEComSyncOutConf that contains the new configuration for the sync Out. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, const SbgEComSyncOutConf *pConf); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.c deleted file mode 100644 index e922633..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.c +++ /dev/null @@ -1,82 +0,0 @@ -#include "sbgEComCmdFeatures.h" -#include - -//----------------------------------------------------------------------// -//- Features commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the device and embedded GPS receiver features. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pFeatures A pointer to a structure to hold features. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGetFeatures(SbgEComHandle *pHandle, SbgEComFeatures *pFeatures) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pFeatures); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GPS_FEATURES command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pFeatures->sensorFeaturesMask = sbgStreamBufferReadUint32LE(&inputStream); - pFeatures->gnssType = (SbgEComGnssType)sbgStreamBufferReadUint8LE(&inputStream); - pFeatures->gnssUpdateRate = sbgStreamBufferReadUint8LE(&inputStream); - pFeatures->gnssSignalsMask = sbgStreamBufferReadUint32LE(&inputStream); - pFeatures->gnssFeaturesMask = sbgStreamBufferReadUint32LE(&inputStream); - sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssProductCode, 32*sizeof(char)); - sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssSerialNumber, 32*sizeof(char)); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.h deleted file mode 100644 index 4214c84..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdFeatures.h +++ /dev/null @@ -1,112 +0,0 @@ -/*! - * \file sbgEComCmdFeatures.h - * \author SBG Systems (Alexis Guinamard) - * \date 19 March 2015 - * - * \brief This file implements SbgECom commands related to device features. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_FEATURES_H__ -#define __SBG_ECOM_CMD_FEATURES_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Feature commands definitions -// -//----------------------------------------------------------------------// - -// -// Main sensor features -// -#define SBG_ECOM_SENSOR_FEATURE_IMU (0x00000001 << 0) /*!< This unit can provide IMU data */ -#define SBG_ECOM_SENSOR_FEATURE_AHRS (0x00000001 << 1) /*!< This unit can provide orientation data */ -#define SBG_ECOM_SENSOR_FEATURE_NAVIGATION (0x00000001 << 2) /*!< This unit can provide position and velocity data */ -#define SBG_ECOM_SENSOR_FEATURE_SHIP_MOTION (0x00000001 << 3) /*!< This unit can provide ship motion data output (heave) */ - -// -// GPS Signals bitmask defining every signal -// -#define SBG_ECOM_GNSS_SIGNAL_GPS_L1 (0x00000001 << 0) /*!< This GNSS receiver tracks GPS L1 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GPS_L2 (0x00000001 << 1) /*!< This GNSS receiver tracks GPS L2 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GPS_L5 (0x00000001 << 2) /*!< This GNSS receiver tracks GPS L5 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L1 (0x00000001 << 3) /*!< This GNSS receiver tracks GLONASS L1 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L2 (0x00000001 << 4) /*!< This GNSS receiver tracks GLONASS L2 band. */ -#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B1 (0x00000001 << 5) /*!< This GNSS receiver tracks BEIDOU B1 band. */ -#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B2 (0x00000001 << 6) /*!< This GNSS receiver tracks BEIDOU B2 band. */ -#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B3 (0x00000001 << 7) /*!< This GNSS receiver tracks BEIDOU B3 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E1 (0x00000001 << 8) /*!< This GNSS receiver tracks GALILEO E1 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E5 (0x00000001 << 9) /*!< This GNSS receiver tracks GALILEO E5 band. */ -#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E6 (0x00000001 << 10) /*!< This GNSS receiver tracks GALILEO E6 band. */ -#define SBG_ECOM_GNSS_SIGNAL_QZSS (0x00000001 << 11) /*!< This GNSS receiver tracks QZSS signals */ -#define SBG_ECOM_GNSS_SIGNAL_SBAS (0x00000001 << 12) /*!< This GNSS receiver tracks SBAS signals */ -#define SBG_ECOM_GNSS_SIGNAL_L_BAND (0x00000001 << 13) /*!< This GNSS receiver tracks L-Band (for PPP services) */ - -// -// GPS capabilities -// -#define SBG_ECOM_GNSS_FEATURE_DUAL_ANT (0x00000001 << 0) /*!< This GNSS receiver provides a dual antenna heading */ -#define SBG_ECOM_GNSS_FEATURE_RTK_LIMITED (0x00000001 << 1) /*!< This GNSS receiver has limited RTK accuracy (eg. Trimble RTK 30/30) */ -#define SBG_ECOM_GNSS_FEATURE_RTK (0x00000001 << 2) /*!< This GNSS receiver provides full RTK accuracy */ -#define SBG_ECOM_GNSS_FEATURE_PPP (0x00000001 << 3) /*!< This GNSS receiver provides PPP computations */ -#define SBG_ECOM_GNSS_FEATURE_RAW_DATA (0x00000001 << 4) /*!< This GNSS receiver provides RAW data output */ -#define SBG_ECOM_GNSS_FEATURE_RAIM (0x00000001 << 5) /*!< This GNSS receiver provides Receiver Autonomous Integrity Monitoring */ -#define SBG_ECOM_GNSS_FEATURE_HIGH_SPEED (0x00000001 << 6) /*!< This GNSS receiver has no high speed limitation (> 515m/s) */ - -//----------------------------------------------------------------------// -//- Feature commands types definition -// -//----------------------------------------------------------------------// - -/*! - * This enum defines the different types of internal GNSS receiver that can provide specific features. - * Note External type is considered as not handled by the feature system - */ -typedef enum _SbgEComGnssType -{ - SBG_ECOM_GNSS_TYPE_DISABLED = 0, /*!< GNSS module disabled */ - SBG_ECOM_GNSS_TYPE_EXTERNAL = 1, /*!< External GNSS module (all features are unknown) */ - SBG_ECOM_GNSS_TYPE_UBX_MAX_M8 = 2, /*!< Ublox MAX-M8 module */ - SBG_ECOM_GNSS_TYPE_NOV_OEM615 = 3, /*!< Novatel OEM615 device */ - SBG_ECOM_GNSS_TYPE_NOV_OEM615_DUAL = 4, /*!< Two Novatel OEM615 devices for dual antenna */ - SBG_ECOM_GNSS_TYPE_NOV_OEM617D = 5, /*!< Novatel OEM617D device */ - SBG_ECOM_GNSS_TYPE_SEP_AX4 = 6, /*!< Septentrio Asterx m4 */ - SBG_ECOM_GNSS_TYPE_SEP_AXM2A = 7 /*!< Septentrio Asterx m2a */ -} SbgEComGnssType; - -/*! - * This structure contains all the information provided by the SBG_ECOM_CMD_GET_FEATURES command - */ -typedef struct _SbgEComFeatures -{ - uint32_t sensorFeaturesMask; /*!< The different measurement capabilities of this unit */ - SbgEComGnssType gnssType; /*!< The type of GNSS receiver used (brand and model) */ - uint8_t gnssUpdateRate; /*!< The actual GNSS update rate */ - uint32_t gnssSignalsMask; /*!< GNSS receiver signals tracking */ - uint32_t gnssFeaturesMask; /*!< GNSS receiver computation and output features */ - char gnssProductCode[32]; /*!< String containing the GNSS receiver product code (“\0” if unknown) */ - char gnssSerialNumber[32]; /*!< String containing the GNSS receiver serial number (“\0” if unknown) */ -} SbgEComFeatures; - -//----------------------------------------------------------------------// -//- Info commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the device and embedded GPS receiver features. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pFeatures A pointer to a structure to hold features. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGetFeatures(SbgEComHandle *pHandle, SbgEComFeatures *pFeatures); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.c deleted file mode 100644 index 33d42ae..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.c +++ /dev/null @@ -1,546 +0,0 @@ -/* sbgCommonLib headers */ -#include -#include - -/* Project headers */ - -/* Local headers */ -#include "sbgEComCmdGnss.h" -#include "transfer/sbgEComTransfer.h" - -//----------------------------------------------------------------------// -//- Private methods -// -//----------------------------------------------------------------------// - -/*! - * Set GNSS error model id. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] id Model ID to set - * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssSetModelId(SbgEComHandle *pHandle, uint32_t id, SbgEComCmd cmdId) -{ - assert(pHandle); - - // - // Call generic function with specific command name - // - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, id); -} - -/*! - * Retrieve GNSS error model information. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current GNSS error model info. - * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo, SbgEComCmd cmdId) -{ - assert(pHandle); - assert(pModelInfo); - - // - // Call generic function with specific command name - // - return sbgEComCmdGenericGetModelInfo(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pModelInfo); -} - -/*! - * Retrieve the mechanical installation parameters for the GNSS # module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pGnssInstallation Used to store the retrieved the GNSS installation parameters. - * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssInstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pGnssInstallation); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pGnssInstallation->leverArmPrimary[0] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmPrimary[1] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmPrimary[2] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmPrimaryPrecise = sbgStreamBufferReadBooleanLE(&inputStream); - - pGnssInstallation->leverArmSecondary[0] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmSecondary[1] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmSecondary[2] = sbgStreamBufferReadFloatLE(&inputStream); - pGnssInstallation->leverArmSecondaryMode = (SbgEComGnssInstallationMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the mechanical installation parameters for the GNSS # module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pGnssInstallation The GNSS installation parameters to set. - * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssInstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pGnssInstallation); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[2]); - sbgStreamBufferWriteBooleanLE(&outputStream, pGnssInstallation->leverArmPrimaryPrecise); - - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[0]); - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[1]); - sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[2]); - sbgStreamBufferWriteUint8LE(&outputStream, pGnssInstallation->leverArmSecondaryMode); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the rejection configuration of the gnss module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct to hold rejection configuration of the gnss module. - * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssGetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pRejectConf->position = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - sbgStreamBufferReadUint8LE(&inputStream); // Skipped for backward compatibility - pRejectConf->hdt = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the rejection configuration of the gnss module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct holding rejection configuration for the gnss module. - * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -static SbgErrorCode sbgEComCmdGnssSetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->position); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)SBG_ECOM_NEVER_ACCEPT_MODE); // Reserved parameter - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->hdt); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- GNSS public commands -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, uint32_t id) -{ - assert(pHandle); - - return sbgEComCmdGnssSetModelId(pHandle, id, SBG_ECOM_CMD_GNSS_1_MODEL_ID); -} - -SbgErrorCode sbgEComCmdGnss1GetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo) -{ - assert(pHandle); - assert(pModelInfo); - - return sbgEComCmdGnssGetModelInfo(pHandle, pModelInfo, SBG_ECOM_CMD_GNSS_1_MODEL_ID); -} - -SbgErrorCode sbgEComCmdGnss1GetLeverArmAlignment(SbgEComHandle *pHandle, SbgEComGnssAlignmentInfo *pAlignConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pAlignConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pAlignConf->leverArmX = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->leverArmY = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->leverArmZ = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->pitchOffset = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->yawOffset = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->antennaDistance = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdGnss1SetLeverArmAlignment(SbgEComHandle *pHandle, const SbgEComGnssAlignmentInfo *pAlignConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pAlignConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmX); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmY); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmZ); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->pitchOffset); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->yawOffset); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->antennaDistance); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation) -{ - assert(pHandle); - assert(pGnssInstallation); - - return sbgEComCmdGnssInstallationGet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); -} - -SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation) -{ - assert(pHandle); - assert(pGnssInstallation); - - return sbgEComCmdGnssInstallationSet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); -} - -SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf) -{ - assert(pHandle); - assert(pRejectConf); - - return sbgEComCmdGnssGetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); -} - -SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf) -{ - assert(pHandle); - assert(pRejectConf); - - return sbgEComCmdGnssSetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.h deleted file mode 100644 index 6fce50e..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdGnss.h +++ /dev/null @@ -1,174 +0,0 @@ -/*! - * \file sbgEComCmdGnss.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to GNSS module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_CMD_GNSS_H -#define SBG_ECOM_CMD_GNSS_H - -/* sbgCommonLib headers */ -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Public definitions (DEPRECATED) -// -//----------------------------------------------------------------------// - -/*! - * DEPRECATED: Command SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT has been replaced by SBG_ECOM_CMD_GNSS_1_INSTALLATION - * Holds all necessary information for GNSS module alignment. - */ -typedef struct _SbgEComGnssAlignmentInfo -{ - float leverArmX; /*!< GNSS antenna lever arm in IMU X axis in meters */ - float leverArmY; /*!< GNSS antenna lever arm in IMU Y axis in meters */ - float leverArmZ; /*!< GNSS antenna lever arm in IMU Z axis in meters */ - float pitchOffset; /*!< Pitch offset for dual antenna GNSS in rad */ - float yawOffset; /*!< Yaw offset for dual antenna GNSS in rad */ - float antennaDistance; /*!< Distance between two GNSS antennas in meters */ -} SbgEComGnssAlignmentInfo; - -//----------------------------------------------------------------------// -//- Public definitions -// -//----------------------------------------------------------------------// - -/*! - * This enum defines the different GNSS model IDs available in standard - */ -typedef enum _SbgEComGnssModelsStdIds -{ - SBG_ECOM_GNSS_MODEL_UBLOX_GPS_GLONASS = 101, /*!< Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS */ - SBG_ECOM_GNSS_MODEL_NMEA = 102, /*!< Used on ELLIPSE-E to accept an external GNSS using NMEA protocol */ - SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU = 103, /*!< Used on ELLIPSE-N to setup the internal GNSS in GPS+BEIDOU */ - SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL = 104, /*!< Used on ELLIPSE-E to accept an external Ublox GNSS (read only) */ - SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_DYNAMICS = 105, /*!< Used on ELLIPSE-N to setup the internal GNSS to high dynamic motion constraints. */ - SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL = 106, /*!< Used on ELLIPSE-E to accept an external Novatel GNSS (read only) */ - SBG_ECOM_GNSS_MODEL_ELLIPSE_D_INTERNAL = 107, /*!< Used on ELLIPSE-D to setup the internal GNSS receiver. */ - SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_SPEED = 108, /*!< Used on ELLIPSE-N to setup the internal GNSS for high speed applications (airborne > 200 km/h) */ - SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL = 109, /*!< Used on ELLIPSE-E to accept an external Septentrio GNSS (read only) */ - SBG_ECOM_GNSS_MODEL_UBLOX_LOW_SPEED = 110 /*!< Used on ELLIPSE-N to setup the internal GNSS receiver for pedestrian applications. */ -} SbgEComGnssModelsStdIds; - -/*! - * GNSS mechanical installation modes for the dual antenna mode. - */ -typedef enum _SbgEComGnssInstallationMode -{ - SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE = 1, /*!< The GNSS will be used in single antenna mode only and the secondary lever arm is not used. */ - SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_AUTO = 2, /*!< [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. */ - SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_ROUGH = 3, /*!< The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. */ - SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_PRECISE = 4 /*!< The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. */ -} SbgEComGnssInstallationMode; - -/*! - * GNSS mechanical installation parameters to be used with command SBG_ECOM_CMD_GNSS_#_INSTALLATION - */ -typedef struct _SbgEComGnssInstallation -{ - float leverArmPrimary[3]; /*!< GNSS primary antenna lever arm in IMU X, Y, Z axis in meters */ - bool leverArmPrimaryPrecise; /*!< If set to true, the primary lever arm has been accurately entered and doesn't need online re-estimation. */ - - float leverArmSecondary[3]; /*!< GNSS secondary antenna lever arm in IMU X, Y, Z axis in meters */ - SbgEComGnssInstallationMode leverArmSecondaryMode; /*!< Define the secondary antenna (dual antenna) operating mode. */ -} SbgEComGnssInstallation; - -/*! - * Holds all necessary information for GNSS module data rejection. - */ -typedef struct _SbgEComGnssRejectionConf -{ - SbgEComRejectionMode position; /*!< Rejection mode for position. */ - SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ - SbgEComRejectionMode hdt; /*!< Rejection mode for true heading. */ -} SbgEComGnssRejectionConf; - -//----------------------------------------------------------------------// -//- GNSS public commands -// -//----------------------------------------------------------------------// - -/*! - * Set GNSS error model id. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] id Model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, uint32_t id); - -/*! - * Retrieve GNSS error model information. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current GNSS error model info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1GetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo); - -/*! - * Retrieve the lever arm and alignment configuration of the gnss 1 module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComGnssAlignmentInfo struct to hold alignment configuration of the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SBG_DEPRECATED(SbgErrorCode sbgEComCmdGnss1GetLeverArmAlignment(SbgEComHandle *pHandle, SbgEComGnssAlignmentInfo *pAlignConf)); - -/*! - * Set the lever arm and alignment configuration of the gnss 1 module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pAlignConf Pointer to a SbgEComGnssAlignmentInfo struct holding alignment configuration for the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SBG_DEPRECATED(SbgErrorCode sbgEComCmdGnss1SetLeverArmAlignment(SbgEComHandle *pHandle, const SbgEComGnssAlignmentInfo *pAlignConf)); - -/*! - * Retrieve the mechanical installation parameters for the GNSS 1 module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pGnssInstallation Used to store the retrieved the GNSS installation parameters. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation); - -/*! - * Set the mechanical installation parameters for the GNSS 1 module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pGnssInstallation The GNSS installation parameters to set. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation); - -/*! - * Retrieve the rejection configuration of the gnss module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct to hold rejection configuration of the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf); - -/*! - * Set the rejection configuration of the gnss module. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct holding rejection configuration for the gnss module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf); - -#endif /* SBG_ECOM_CMD_GNSS_H */ diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.c deleted file mode 100644 index 24d806f..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.c +++ /dev/null @@ -1,98 +0,0 @@ -#include "sbgEComCmdInfo.h" -#include - -//----------------------------------------------------------------------// -//- Info commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the device information. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pInfo A pointer to a structure to hold device information. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pInfo); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have correctly received a message - // - if (errorCode == SBG_NO_ERROR) - { - // - // Make sure we have received a payload - // - if (receivedSize > 0) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - sbgStreamBufferReadBuffer(&inputStream, pInfo->productCode, SBG_ECOM_INFO_PRODUCT_CODE_LENGTH); - pInfo->serialNumber = sbgStreamBufferReadUint32LE(&inputStream); - pInfo->calibationRev = sbgStreamBufferReadUint32LE(&inputStream); - pInfo->calibrationYear = sbgStreamBufferReadUint16LE(&inputStream); - pInfo->calibrationMonth = sbgStreamBufferReadUint8LE(&inputStream); - pInfo->calibrationDay = sbgStreamBufferReadUint8LE(&inputStream); - pInfo->hardwareRev = sbgStreamBufferReadUint32LE(&inputStream); - pInfo->firmwareRev = sbgStreamBufferReadUint32LE(&inputStream); - - // - // We have parsed a message so return immediately but report any error during payload parsing - // - errorCode = sbgStreamBufferGetLastError(&inputStream); - - break; - } - else - { - // - // We should have received a non empty payload so we have received an invalid frame - // - errorCode = SBG_INVALID_FRAME; - } - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.h deleted file mode 100644 index 795b014..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInfo.h +++ /dev/null @@ -1,63 +0,0 @@ -/*! - * \file sbgEComCmdInfo.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to device info. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_INFO_H__ -#define __SBG_ECOM_CMD_INFO_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Device info definitions -// -//----------------------------------------------------------------------// - -/* Misc */ -#define SBG_ECOM_INFO_PRODUCT_CODE_LENGTH (32) - -//----------------------------------------------------------------------// -//- Device Info structure -// -//----------------------------------------------------------------------// - -/*! - * Helper structure to retrieve device info. - */ -typedef struct _SbgEComDeviceInfo -{ - uint8_t productCode[SBG_ECOM_INFO_PRODUCT_CODE_LENGTH]; /*!< Human readable Product Code. */ - uint32_t serialNumber; /*!< Device serial number */ - uint32_t calibationRev; /*!< Calibration data revision */ - uint16_t calibrationYear; /*!< Device Calibration Year */ - uint8_t calibrationMonth; /*!< Device Calibration Month */ - uint8_t calibrationDay; /*!< Device Calibration Day */ - uint32_t hardwareRev; /*!< Device hardware revision */ - uint32_t firmwareRev; /*!< Firmware revision */ -} SbgEComDeviceInfo; - -//----------------------------------------------------------------------// -//- Info commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the device information. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pInfo A pointer to a structure to hold device information. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.c deleted file mode 100644 index 0336dce..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.c +++ /dev/null @@ -1,285 +0,0 @@ -#include "sbgEComCmdInterface.h" -#include - -//----------------------------------------------------------------------// -//- Interface commands -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdInterfaceGetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, SbgEComInterfaceConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command and the interfaceId as a 1-byte payload - // - outputBuffer = (uint8_t)interfaceId; - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &outputBuffer, sizeof(uint8_t)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received correctly the answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned interfaceId, then baud rate and the mode at last. - // - interfaceId = (SbgEComPortId)sbgStreamBufferReadUint8LE(&inputStream); - pConf->baudRate = sbgStreamBufferReadUint32LE(&inputStream); - pConf->mode = (SbgEComPortMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdInterfaceSetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, const SbgEComInterfaceConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)interfaceId); - sbgStreamBufferWriteUint32LE(&outputStream, pConf->baudRate); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->mode); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdInterfaceGetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate *pBitrate, SbgEComCanMode *pMode) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pBitrate); - assert(pMode); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with no payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_CAN_BUS_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read bit rate returned by the device - // - *pBitrate = (SbgEComCanBitRate)sbgStreamBufferReadUint16LE(&inputStream); - - // - // Check if we can parse the CAN mode that has been introduced in sbgECom version 2.0 - // - if (sbgStreamBufferGetSpace(&inputStream) > 0) - { - // - // Read mode returned by the device - // - *pMode = (SbgEComCanMode)sbgStreamBufferReadUint8(&inputStream); - } - else - { - // - // Default the mode to the behavior prior to CAN mode setting introduction - // - *pMode = SBG_ECOM_CAN_MODE_NORMAL; - } - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdInterfaceSetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate bitrate, SbgEComCanMode mode) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[3]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(bitrate <= UINT16_MAX); - assert(mode <= UINT8_MAX); - - // - // Build the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)bitrate); - sbgStreamBufferWriteUint8(&outputStream, (uint8_t)mode); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.h deleted file mode 100644 index c9bd092..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdInterface.h +++ /dev/null @@ -1,149 +0,0 @@ -/*! - * \file sbgEComCmdInterface.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to interfaces. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_INTERFACE_H__ -#define __SBG_ECOM_CMD_INTERFACE_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Serial interface definitions -// -//----------------------------------------------------------------------// - -/*! - * List of serial interfaces available. - */ -typedef enum _SbgEComPortId -{ - SBG_ECOM_IF_COM_A = 0, /*!< Main communication interface. Full duplex. */ - SBG_ECOM_IF_COM_B = 1, /*!< Auxiliary input interface for RTCM. */ - SBG_ECOM_IF_COM_C = 2, /*!< Auxiliary communication interface. Full duplex. */ - SBG_ECOM_IF_COM_D = 3, /*!< Auxiliary input interface. */ - SBG_ECOM_IF_COM_E = 4, /*!< Auxiliary output interface. */ - - SBG_ECOM_IF_ETH_0 = 10, /*!< Ethernet interface 0. */ - SBG_ECOM_IF_ETH_1 = 11, /*!< Ethernet interface 1. */ - SBG_ECOM_IF_ETH_2 = 12, /*!< Ethernet interface 2. */ - SBG_ECOM_IF_ETH_3 = 13, /*!< Ethernet interface 3. */ - SBG_ECOM_IF_ETH_4 = 14, /*!< Ethernet interface 4. */ - - SBG_ECOM_IF_DATA_LOGGER = 20, /*!< Data logger interface. */ -} SbgEComPortId; - -/*! - * List of serial modes available. - */ -typedef enum _SbgEComPortMode -{ - SBG_ECOM_UART_MODE_OFF = 0, /*!< This interface is turned OFF. */ - SBG_ECOM_UART_MODE_232 = 1, /*!< This interface is using RS-232 communications. */ - SBG_ECOM_UART_MODE_422 = 2, /*!< This interface is using RS-422 communications. */ -} SbgEComPortMode; - -//----------------------------------------------------------------------// -//- Serial interface configuration -// -//----------------------------------------------------------------------// - -/*! - * Helper structure to configure a serial interface - */ -typedef struct _SbgEComInterfaceConf -{ - uint32_t baudRate; /*!< The baud rate of the interface. */ - SbgEComPortMode mode; /*!< The mode of the interface. */ -} SbgEComInterfaceConf; - -//----------------------------------------------------------------------// -//- CAN interface definitions -// -//----------------------------------------------------------------------// - -/*! - * Enum containing the list of all available bit rates (in KB/s). - */ -typedef enum _SbgEComCanBitRate -{ - SBG_ECOM_CAN_BITRATE_DISABLED = 0, /*!< The CAN interface is disabled. */ - SBG_ECOM_CAN_BITRATE_10 = 10, /*!< 10Kb/s. */ - SBG_ECOM_CAN_BITRATE_20 = 20, /*!< 20Kb/s. */ - SBG_ECOM_CAN_BITRATE_25 = 25, /*!< 25Kb/s. */ - SBG_ECOM_CAN_BITRATE_50 = 50, /*!< 50Kb/s. */ - SBG_ECOM_CAN_BITRATE_100 = 100, /*!< 100Kb/s. */ - SBG_ECOM_CAN_BITRATE_125 = 125, /*!< 125Kb/s. */ - SBG_ECOM_CAN_BITRATE_250 = 250, /*!< 250Kb/s. */ - SBG_ECOM_CAN_BITRATE_500 = 500, /*!< 500Kb/s. */ - SBG_ECOM_CAN_BITRATE_750 = 750, /*!< 750Kb/s. */ - SBG_ECOM_CAN_BITRATE_1000 = 1000, /*!< 1Mb/s. */ -} SbgEComCanBitRate; - -/*! - * Enum containing the list of different CAN modes - */ -typedef enum _SbgEComCanMode -{ - SBG_ECOM_CAN_MODE_UNDEFINED = 0, /*!< CAN Mode undefined. */ - SBG_ECOM_CAN_MODE_SPY = 1, /*!< Only listening on the CAN bus and doesn't sent anything (even RX ACK bit). */ - SBG_ECOM_CAN_MODE_NORMAL = 2, /*!< The device is allowed to both send and receive over the CAN bus. */ - SBG_ECOM_CAN_NR_MODE -} SbgEComCanMode; - -//----------------------------------------------------------------------// -//- Interface commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the configuration of one of the interfaces. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] interfaceId The interface from which the configuration is to be retrieved. - * \param[out] pConf Pointer to a SbgEComInterfaceConf struct to hold configuration of the interface. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdInterfaceGetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, SbgEComInterfaceConf *pConf); - -/*! - * Set the configuration of one of the interfaces. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] interfaceId The interface from which the configuration is to be retrieved. - * \param[in] pConf Pointer to a SbgEComInterfaceConf struct that holds the new configuration for the interface. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdInterfaceSetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, const SbgEComInterfaceConf *pConf); - -/*! - * Retrieve the configuration of the CAN interface. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pBitRate The bitrate of the CAN interface. - * \param[out] pCanMode Mode of the CAN interface. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdInterfaceGetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate *pBitrate, SbgEComCanMode *pMode); - -/*! - * Set the configuration of the CAN interface. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] bitRate The bitrate of the CAN interface. - * \param[in] pCanMode Mode of the CAN interface. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdInterfaceSetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate bitrate, SbgEComCanMode mode); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.c deleted file mode 100644 index f0b6233..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.c +++ /dev/null @@ -1,42 +0,0 @@ -#include "sbgEComCmdLicense.h" -#include "transfer/sbgEComTransfer.h" - -//----------------------------------------------------------------------// -//- License commands -// -//----------------------------------------------------------------------// - -/*! - * Upload and apply a new license to a device. - * The device will reboot automatically to use the new license. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Read only buffer containing the license. - * \param[in] size Size of the buffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdLicenseApply(SbgEComHandle *pHandle, const void *pBuffer, size_t size) -{ - SbgErrorCode errorCode; - uint32_t currentTimeOut; - - assert(pHandle); - assert(pBuffer); - assert(size > 0); - - // - // Define a time out of 10s to let enough time for the GNSS receiver to apply the license - // - currentTimeOut = pHandle->cmdDefaultTimeOut; - pHandle->cmdDefaultTimeOut = 10000; - - // - // Call function that handle data transfer - // - errorCode = sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LICENSE_APPLY, pBuffer, size); - - // - // Restore the default time out - // - pHandle->cmdDefaultTimeOut = currentTimeOut; - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.h deleted file mode 100644 index 3ef46cb..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdLicense.h +++ /dev/null @@ -1,39 +0,0 @@ -/*! - * \file sbgEComCmdLicense.h - * \author SBG Systems (Maxime Renaudet) - * \date 25 February 2015 - * - * \brief This file implements SbgECom commands related to licenses. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_LICENSE_H__ -#define __SBG_ECOM_CMD_LICENSE_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- License commands -// -//----------------------------------------------------------------------// - -/*! - * Upload and apply a new license to a device. - * The device will reboot automatically to use the new license. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Read only buffer containing the license. - * \param[in] size Size of the buffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdLicenseApply(SbgEComHandle *pHandle, const void *pBuffer, size_t size); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.c deleted file mode 100644 index 1e0598e..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.c +++ /dev/null @@ -1,445 +0,0 @@ -#include "sbgEComCmdMag.h" -#include -#include "transfer/sbgEComTransfer.h" - -//----------------------------------------------------------------------// -//- Magnetometer commands -// -//----------------------------------------------------------------------// - -/*! - * Set magnetometer error model ID. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] id Magnetometer model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32_t id) -{ - // - // Call generic function with specific command name - // - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, id); -} - -/*! - * Retrieve magnetometer error model information. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current magnetometer error model info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo) -{ - // - // Call generic function with specific command name - // - return sbgEComCmdGenericGetModelInfo(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, pModelInfo); -} - -/*! - * Send a command that set the magnetometers calibration parameters. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] offset Magnetometers calibration offset vector. - * \param[in] matix Magnetometers calibration 3x3 matrix. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outputStream; - uint8_t payload[12*sizeof(float)]; - uint32_t trial; - uint32_t i; - - assert(pHandle); - assert(offset); - assert(matrix); - - // - // Initialize a stream buffer to write the command payload - // - errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload)); - - // - // Write the offset vector - // - sbgStreamBufferWriteFloatLE(&outputStream, offset[0]); - sbgStreamBufferWriteFloatLE(&outputStream, offset[1]); - sbgStreamBufferWriteFloatLE(&outputStream, offset[2]); - - // - // Write the matrix - // - for (i = 0; i < 9; i++) - { - sbgStreamBufferWriteFloatLE(&outputStream, matrix[i]); - } - - // - // Make sure that the stream buffer has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, payload, sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -/*! - * Retrieve the rejection configuration of the magnetometer module. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf Pointer to a SbgEComMagRejectionConf struct to hold rejection configuration of the magnetometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received frame was OK - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pRejectConf->magneticField = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the rejection configuration of the magnetometer module. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pRejectConf Pointer to a SbgEComMagRejectionConf struct holding rejection configuration for the magnetometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // Build payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->magneticField); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- Magnetometer onboard calibration commands -// -//----------------------------------------------------------------------// - -/*! - * Start the magnetic calibration process. - * As soon as this command is sent, the device will start logging magnetic field data internally. - * This set of data will be used later by the magnetic calibration algorithms to map the surrounding magnetic field. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] mode Define which magnetic calibration type to perform. It could be 3D or 2D. - * \param[in] bandwidth Tell the device that we should have low, medium or high dynamics during the magnetic calibration process. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outputStream; - uint8_t payload[2]; - uint32_t trial; - - assert(pHandle); - - // - // Initialize a stream buffer to write the command payload - // - errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload)); - - // - // Write the calibration mode and bandwith - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)bandwidth); - - // - // Make sure that the stream buffer has been initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, payload, sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - - return errorCode; -} - -/*! - * This command computes a magnetic calibration solution based on the magnetic field logged since the last call to the command SBG_ECOM_CMD_START_MAG_CALIB (15). - * As soon as the computations are done, the device will answer with quality indicators, status flags and if possible a valid magnetic calibration matrix and offset. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pCalibResults Pointer on a SbgEComMagCalibResults structure that can hold onboard magnetic calibration results and status. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint32_t i; - - assert(pHandle); - assert(pCalibResults); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 5 s because the onboard magnetic computation can take some time - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, receivedBuffer, &receivedSize, sizeof(receivedBuffer), 5000); - - // - // Test if we have received the correct command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read quality and status parameters - // - pCalibResults->quality = (SbgEComMagCalibQuality)sbgStreamBufferReadUint8LE(&inputStream); - pCalibResults->confidence = (SbgEComMagCalibConfidence)sbgStreamBufferReadUint8LE(&inputStream); - pCalibResults->advancedStatus = sbgStreamBufferReadUint16LE(&inputStream); - - pCalibResults->beforeMeanError = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->beforeStdError = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->beforeMaxError = sbgStreamBufferReadFloatLE(&inputStream); - - pCalibResults->afterMeanError = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->afterStdError = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->afterMaxError = sbgStreamBufferReadFloatLE(&inputStream); - - pCalibResults->meanAccuracy = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->stdAccuracy = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->maxAccuracy = sbgStreamBufferReadFloatLE(&inputStream); - - pCalibResults->numPoints = sbgStreamBufferReadUint16LE(&inputStream); - pCalibResults->maxNumPoints = sbgStreamBufferReadUint16LE(&inputStream); - - // - // Read the computed hard iron offset vector - // - pCalibResults->offset[0] = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->offset[1] = sbgStreamBufferReadFloatLE(&inputStream); - pCalibResults->offset[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // Read the computed soft iron matrix - // - for (i = 0; i < 9; i++) - { - pCalibResults->matrix[i] = sbgStreamBufferReadFloatLE(&inputStream); - } - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.h deleted file mode 100644 index 7631efd..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdMag.h +++ /dev/null @@ -1,203 +0,0 @@ -/*! - * \file sbgEComCmdMag.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to Magnetometer module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_MAG_H__ -#define __SBG_ECOM_CMD_MAG_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Magnetometer definitions -// -//----------------------------------------------------------------------// - -/*! - * Define if the onboard magnetic calibration should acquiere points for a 3D or 2D calibration. - */ -typedef enum _SbgEComMagCalibMode -{ - SBG_ECOM_MAG_CALIB_MODE_2D = 1, /*!< Tell the device that the magnetic calibration will be performed with limited motions. - This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. - To work correctly, the device should be rotated through at least a full circle. */ - SBG_ECOM_MAG_CALIB_MODE_3D = 2 /*!< Tell the device to start a full 3D magnetic calibration procedure. - The 3D magnetic calibration offers the best accuracy but needs at least motion of ± 30° on the roll and pitch angles. */ -} SbgEComMagCalibMode; - - -/*! - * Used to select the expected dynamics during the magnetic calibration. - */ -typedef enum _SbgEComMagCalibBandwidth -{ - SBG_ECOM_MAG_CALIB_LOW_BW = 0, /*!< Tell the device that low dynamics will be observed during the magnetic calibration process. */ - SBG_ECOM_MAG_CALIB_MEDIUM_BW = 1, /*!< Tell the device that normal dynamics will be observed during the magnetic calibration process. */ - SBG_ECOM_MAG_CALIB_HIGH_BW = 2 /*!< Tell the device that high dynamics will be observed during the magnetic calibration process. */ -} SbgEComMagCalibBandwidth; - -/*! - * General quality indicator of an onboard magnetic calibration. - */ -typedef enum _SbgEComMagCalibQuality -{ - SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL = 0, /*!< All acquired points fit very well on a unit sphere after the calibration. */ - SBG_ECOM_MAG_CALIB_QUAL_GOOD = 1, /*!< Small deviations of the magnetic field norm have been detected. The magnetic calibration should although provide accurate heading. */ - SBG_ECOM_MAG_CALIB_QUAL_POOR = 2, /*!< Large deviations of the magnetic field norm have been detected. It may come from external magnetic distortions during the calibration. */ - SBG_ECOM_MAG_CALIB_QUAL_INVALID = 3 /*!< No valid magnetic calibration has been computed. It could comes from too much magnetic disturbances, insufficient or invalid motions. */ -} SbgEComMagCalibQuality; - -/*! - * Confidence indicator on results of an onbard magnetic calibration. - */ -typedef enum _SbgEComMagCalibConfidence -{ - SBG_ECOM_MAG_CALIB_TRUST_HIGH = 0, /*!< Reported quality indicator can be trusted as enough remarkable magnetic field points have been acquired. */ - SBG_ECOM_MAG_CALIB_TRUST_MEDIUM = 1, /*!< Few remarkable magnetic field points have been used to compute the magnetic calibration leading to a medium confidence in reported quality indicators. */ - SBG_ECOM_MAG_CALIB_TRUST_LOW = 2 /*!< Even if the quality indicator could report an excellent calibration, - The data set used to compute the magnetic calibration was not meaningful enough to compute meaningful quality indicators. - This calibration should be used carefully. */ -} SbgEComMagCalibConfidence; - -/*! - * Status bit masks used to report advanced inforamtion on the onboard magnetic calibration. - */ -#define SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS (0x0001u) /*!< Not enough valid magnetic points have been acquired. */ -#define SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS (0x0002u) /*!< Unable to compute a magnetic calibration due to magnetic interferences or incorrect data set distribution. */ -#define SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE (0x0004u) /*!< For a 3D calibration: not enough motion on X axis. For a 2D calibration; too much motion on X axis. */ -#define SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE (0x0008u) /*!< For a 3D calibration: not enough motion on Y axis. For a 2D calibration; too much motion on Y axis. */ -#define SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE (0x0010u) /*!< For a 3D or 2D calibration: not enough motion on Z axis. */ -#define SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE (0x0020u) /*!< For a 3D calibration: the alignment between the magnetometers and the inertial frame seems to be invalid. */ - -/*! - * This enum defines the different magnetometer model IDs available in standard - */ -typedef enum _SbgEComMagModelsStdIds -{ - SBG_ECOM_MAG_MODEL_NORMAL = 201, /*!< Should be used in most applications */ - SBG_ECOM_MAG_MODEL_NOISY_MAG_TOLERANT = 202, /*!< Should be used in disturbed magnetic environment */ -} SbgEComMagModelsStdId; - - -//----------------------------------------------------------------------// -//- Magnetometer configuration -// -//----------------------------------------------------------------------// - -/*! - * Holds all necessary information for Magnetometer module data rejection. - */ -typedef struct _SbgEComMagRejectionConf -{ - SbgEComRejectionMode magneticField; /*!< Rejection mode for magnetic field. */ -} SbgEComMagRejectionConf; - -/*! - * Helper structure to retrieve onboard magnetic calibration results. - */ -typedef struct _SbgEComMagCalibResults -{ - SbgEComMagCalibQuality quality; /*!< General magnetic calibration quality indicator. */ - SbgEComMagCalibConfidence confidence; /*!< Confidence indicator that should be read to interpret the quality indicator. */ - uint16_t advancedStatus; /*!< Set of bit masks used to report advanced information on the magnetic calibration status.*/ - - float beforeMeanError; /*!< Mean magnetic field norm error observed before calibration. */ - float beforeStdError; /*!< Standard deviation of the magnetic field norm error observed before calibration. */ - float beforeMaxError; /*!< Maximum magnetic field norm error observed before calibration. */ - - float afterMeanError; /*!< Mean magnetic field norm error observed after calibration. */ - float afterStdError; /*!< Standard deviation of the magnetic field norm error observed after calibration. */ - float afterMaxError; /*!< Maximum magnetic field norm error observed after calibration. */ - - float meanAccuracy; /*!< Mean expected heading accuracy in radians. */ - float stdAccuracy; /*!< Standard deviation of the expected heading accuracy in radians. */ - float maxAccuracy; /*!< Maximum expected heading accuracy in radians. */ - - uint16_t numPoints; /*!< Number of magnetic field points stored internally and used to compute the magnetic calibration. */ - uint16_t maxNumPoints; /*!< Maximum number of magnetic field points that can be stored internally. */ - float offset[3]; /*!< Computed Hard Iron correction vector offset. */ - float matrix[9]; /*!< Computed Hard & Soft Iron correction matrix. */ -} SbgEComMagCalibResults; - -//----------------------------------------------------------------------// -//- Magnetometer commands -// -//----------------------------------------------------------------------// - -/*! - * Set magnetometer error model ID. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] id Magnetometer model ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32_t id); - -/*! - * Retrieve magnetometer error model information. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current magnetometer error model info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo); - -/*! - * Retrieve the rejection configuration of the magnetometer module. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf Pointer to a SbgEComMagRejectionConf struct to hold rejection configuration of the magnetometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf); - -/*! - * Set the rejection configuration of the magnetometer module. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pRejectConf Pointer to a SbgEComMagRejectionConf struct holding rejection configuration for the magnetometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf); - -/*! - * Send a command that set the magnetometers calibration parameters. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] offset Magnetometers calibration offset vector. - * \param[in] matix Magnetometers calibration 3x3 matrix. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9]); - -//----------------------------------------------------------------------// -//- Magnetometer onboard calibration commands -// -//----------------------------------------------------------------------// - -/*! - * Start the magnetic calibration process. - * As soon as this command is sent, the device will start logging magnetic field data internally. - * This set of data will be used later by the magnetic calibration algorithms to map the surrounding magnetic field. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] mode Define which magnetic calibration type to perform. It could be 3D or 2D. - * \param[in] bandwidth Tell the device that we should have low, medium or high dynamics during the magnetic calibration process. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth); - -/*! - * This command computes a magnetic calibration solution based on the magnetic field logged since the last call to the command SBG_ECOM_CMD_START_MAG_CALIB (15). - * As soon as the computations are done, the device will answer with quality indicators, status flags and if possible a valid magnetic calibration matrix and offset. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pCalibResults Pointer on a SbgEComMagCalibResults structure that can hold onboard magnetic calibration results and status. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.c deleted file mode 100644 index 00a146b..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.c +++ /dev/null @@ -1,576 +0,0 @@ -/*! - * \file sbgEComCmdOdo.c - * \author SBG Systems - * - * \brief This file implements SbgECom commands related to Odometer module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#include "sbgEComCmdOdo.h" -#include - -//----------------------------------------------------------------------// -//- Odometer commands -// -//----------------------------------------------------------------------// - -SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pOdometerConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ODO_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pOdometerConf->gain = sbgStreamBufferReadFloatLE(&inputStream); - pOdometerConf->gainError = sbgStreamBufferReadUint8LE(&inputStream); - pOdometerConf->reverseMode = sbgStreamBufferReadBooleanLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pOdometerConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, pOdometerConf->gain); - sbgStreamBufferWriteUint8LE(&outputStream, pOdometerConf->gainError); - sbgStreamBufferWriteBooleanLE(&outputStream, pOdometerConf->reverseMode); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(leverArm); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(leverArm); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[2]); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pRejectConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoCanGetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, SbgEComCmdOdoCanConf *pOdoCanConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t cmdPayload[16]; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pOdoCanConf); - assert(canChannel <= UCHAR_MAX); - - // - // Build the command payload used to ask the CAN odometer configuration for a specific channel - // - sbgStreamBufferInitForWrite(&outputStream, cmdPayload, sizeof(cmdPayload)); - sbgStreamBufferWriteUint8LE(&outputStream, canChannel); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_ODO_CAN_SPEED_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read fields from payload - // - canChannel = sbgStreamBufferReadUint8LE(&inputStream); - - pOdoCanConf->options = sbgStreamBufferReadUint16LE(&inputStream); - pOdoCanConf->canId = sbgStreamBufferReadUint32LE(&inputStream); - - pOdoCanConf->startBit = sbgStreamBufferReadUint8LE(&inputStream); - pOdoCanConf->dataSize = sbgStreamBufferReadUint8LE(&inputStream); - - pOdoCanConf->scale = sbgStreamBufferReadFloatLE(&inputStream); - pOdoCanConf->offset = sbgStreamBufferReadFloatLE(&inputStream); - pOdoCanConf->minValue = sbgStreamBufferReadFloatLE(&inputStream); - pOdoCanConf->maxValue = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pOdoCanConf); - assert(canChannel <= UCHAR_MAX); - - // - // A CAN message has a payload of up to 64 bits so the offset can range from 0 to 63 and size from 1 to 64 - // - assert(pOdoCanConf->startBit < 64); - assert(pOdoCanConf->dataSize > 0); - assert(pOdoCanConf->dataSize <= 64); - - // - // Build the command payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)canChannel); - - sbgStreamBufferWriteUint16LE(&outputStream, pOdoCanConf->options); - sbgStreamBufferWriteUint32LE(&outputStream, pOdoCanConf->canId); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->startBit); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->dataSize); - - sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->scale); - sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->offset); - sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->minValue); - sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->maxValue); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.h deleted file mode 100644 index 9e9e4b9..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOdo.h +++ /dev/null @@ -1,161 +0,0 @@ -/*! - * \file sbgEComCmdOdo.h - * \author SBG Systems - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to Odometer module. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_ECOM_CMD_ODO_H -#define SBG_ECOM_CMD_ODO_H - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Public definitions -// -//----------------------------------------------------------------------// - -/*! - * Holds all necessary information for Odometer module parameter configuration. - */ -typedef struct _SbgEComOdoConf -{ - float gain; /*!< Odometer's gain in pulses / meter. */ - uint8_t gainError; /*!< User gain average error in % */ - bool reverseMode; /*!< Whether the odometer is in reverse mode or not. */ -} SbgEComOdoConf; - -/*! - * Holds all necessary information for Odometer module data rejection. - */ -typedef struct _SbgEComOdoRejectionConf -{ - SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ -} SbgEComOdoRejectionConf; - -/*! - * CAN odometer channels definition - * A channel is an inforamtion that can be decoded / used by the device. - */ -typedef enum _SbgEComCmdOdoCanChannel -{ - SBG_ECOM_CMD_ODO_CAN_CH_VELOCITY = 0, /*!< Channel used to decode the vehicle velocity information */ - SBG_ECOM_CMD_ODO_CAN_CH_REVERSE = 1 /*!< Channel used to decode the vehicle velocity reverse info (if available). */ -} SbgEComCmdOdoCanChannel; - -/* - * Define CAN odometer options bitmask - */ -#define SBG_ECOM_CMD_ODO_CAN_ENABLE (uint16_t)(0x0001 << 0) /*!< Set to enable CAN odometer information decoding. */ -#define SBG_ECOM_CMD_ODO_CAN_ID_EXTENDED (uint16_t)(0x0001 << 1) /*!< Set for a 29 bit extended CAN message, otherwise standard 11 bit */ -#define SBG_ECOM_CMD_ODO_CAN_BIG_ENDIAN (uint16_t)(0x0001 << 2) /*!< Set if the velocity is encoded in big endian, otherwise little endian */ -#define SBG_ECOM_CMD_ODO_CAN_SIGNED (uint16_t)(0x0001 << 3) /*!< Set to interpret the parsed value as signed, otherwise unsigned. */ - -/*! - * Holds all necessary information for CAN Odometer parameter configuration. - * This format is very similar to info contained in a DBC file. - */ -typedef struct _SbgEComCmdOdoCanConf -{ - uint16_t options; /*!< Set of options bit masks such as CAN extended. */ - uint32_t canId; /*!< CAN message ID from which the odometer velocity will be parsed. */ - - size_t startBit; /*!< Index of field MSB in big endian or LSB in little endian within the payload (any value from 0 to 63). */ - size_t dataSize; /*!< Length in bits of the odometer velocity field (any value from 1 to 64 minus dataOffset). */ - - float scale; /*!< Value to multiply the parsed field with to get physical unit^in m.s-1. */ - float offset; /*!< Offset to add on the scaled velocity information in m.s-1 (after applying scale factor). */ - float minValue; /*!< The minimum velocity to consider the message valid in m.s-1 */ - float maxValue; /*!< The maximum velocity to consider the message valid in m.s-1 */ -} SbgEComCmdOdoCanConf; - -//----------------------------------------------------------------------// -//- Public methods -// -//----------------------------------------------------------------------// - -/*! - * For quadrature and/or pulse based odometer, retrieve the configuration. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pOdometerConf Pointer to a SbgEComOdoConf struct to hold configuration of the odometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf); - -/*! - * For quadrature and/or pulse base odometer, define the configuration. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pOdometerConf Pointer to a SbgEComOdoConf struct holding configuration for the odometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf); - -/*! - * Retrieve the lever arm applicable for both quadrature or CAN based odometer. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] leverArm Array of three values, one for each axis. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3]); - -/*! - * Set the lever arm applicable for both quadrature or CAN based odometer. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] leverArm Array of three values, one for each axis. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3]); - -/*! - * Retrieve the velocity rejection configuration for both quadrature or CAN based odometer. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pRejectConf Pointer to a SbgEComOdoRejectionConf struct to hold rejection configuration of the odometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf); - -/*! - * Set the velocity rejection configuration for both quadrature or CAN based odometer. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pRejectConf Pointer to a SbgEComOdoRejectionConf struct holding rejection configuration for the odometer module. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf); - -/*! - * Retrieve the CAN odometer configuration for a specific CAN information channel - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] canChannel The CAN channel to retreive associated DBC configuration. - * \param[out] pOdoCanConf Struct to hold configuration of the CAN odometer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoCanGetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, SbgEComCmdOdoCanConf *pOdoCanConf); - -/*! - * Set the CAN odometer configuration for a specific CAN information channel - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] canChannel The CAN channel to define associated DBC configuration. - * \param[in] pOdoCanConf Struct holding configuration for the CAN odometer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf); - -#endif /* SBG_ECOM_CMD_ODO_H */ diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.c deleted file mode 100644 index 451ecf3..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.c +++ /dev/null @@ -1,843 +0,0 @@ -#include "sbgEComCmdOutput.h" -#include - -//----------------------------------------------------------------------// -//- Output commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] classId The class of the concerned log. - * \param[in] msgId The id of the concerned log. - * \param[out] pConf Pointer to a SbgEComOutputMode to contain the current output mode of the message. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - // - // Test that the input pointer are valid - // - if ((pHandle) && (pConf)) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - - // - // Initialize output stream buffer - // - sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - - // - // Send the command and the prepared payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_OUTPUT_CONF command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned outputPort, then messageId, classId rate and the output configuration at last. - // - outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); - msgId = sbgStreamBufferReadUint8LE(&inputStream); - classId = (SbgEComClass)sbgStreamBufferReadUint8LE(&inputStream); - *pConf = (SbgEComOutputMode)sbgStreamBufferReadUint16LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - else - { - // - // Null pointer. - // - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Set the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] classId The class of the concerned log. - * \param[in] msgId The id of the concerned log. - * \param[in] conf New output mode to set. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode conf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - // - // Test that the input pointer are valid - // - if (pHandle) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)conf); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - else - { - // - // Invalid protocol handle. - // - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Retrieve the enable of one of the output class message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port. - * \param[in] classId The class to enable or disable. - * \param[out] pEnable TRUE to enable message output of this class, FALSE to disable it. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - // - // Test that the input pointer are valid - // - if ((pHandle) && (pEnable)) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - - // - // Initialize output stream buffer - // - sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - - // - // Send the command and the prepared payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned outputPort, then messageId, classId rate and the output configuration at last. - // - outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); - classId = (SbgEComClass)sbgStreamBufferReadUint8LE(&inputStream); - *pEnable = (bool)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - else - { - // - // Null pointer. - // - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Set the enable of one of the output class message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port. - * \param[in] classId The class to enable or disable. - * \param[in] enable TRUE to enable message output of this class, FALSE to disable it. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - // - // Test that the input pointer are valid - // - if (pHandle) - { - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)enable); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - } - else - { - // - // Invalid protocol handle. - // - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Retrieve the configuration of one the message on the CAN interface. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] internalId The internal message id. - * \param[out] pMode Pointer to a SbgEComOutputMode to contain the current output mode of the message. - * \param[out] pUserId The user defined message id. - * \param[out] pExtended TRUE if the user id uses the extended format. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - uint8_t outputBuffer[2]; - SbgStreamBuffer outputStream; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pMode); - assert(pUserId); - assert(pExtended); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command and a 2-byte payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint16LE(&outputStream, internalId); - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct answer - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned outputPort, then messageId, classId rate and the output configuration at last. - // - internalId = (SbgECanMessageId)sbgStreamBufferReadUint16LE(&inputStream); - *pMode = (SbgEComOutputMode)sbgStreamBufferReadUint16LE(&inputStream); - *pUserId = sbgStreamBufferReadUint32LE(&inputStream); - *pExtended = (bool)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the configuration of one the message on the CAN interface - * \param[in] pHandle A valid sbgECom handle. - * \param[in] internalId The internal message id. - * \param[in] pMode Pointer to a SbgEComOutputMode containing the new output mode of the message. - * \param[in] pUserId The user defined message id. - * \param[in] pExtended TRUE if the user id uses the extended format. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)internalId); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)mode); - sbgStreamBufferWriteUint32LE(&outputStream, userId); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)extended); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] pConf Pointer to a SbgEComLegacyConf structure to contain legacy configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetLegacyConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComLegacyConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer[1]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Initialize output stream buffer - // - sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - - // - // Send the command and the prepared payload - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LEGACY_CONT_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LEGACY_CONT_OUTPUT_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a correct command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // First is returned outputPort, then messageId, classId rate and the output configuration at last. - // - outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); - pConf->mask = sbgStreamBufferReadUint32LE(&inputStream); - pConf->format = (SbgEComLegacyFormat)sbgStreamBufferReadUint8LE(&inputStream); - pConf->endian = (SbgEComLegacyEndian)sbgStreamBufferReadUint8LE(&inputStream); - pConf->mode = (SbgEComOutputMode)sbgStreamBufferReadUint16LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] pConf Pointer to a SbgEComLegacyConf structure containing new legacy configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetLegacyConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const SbgEComLegacyConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[9]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint32LE(&outputStream, pConf->mask); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->format); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->endian); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)pConf->mode); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LEGACY_CONT_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LEGACY_CONT_OUTPUT_CONF, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the NMEA talker id of one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] nmeaTalkerId A 2-char array to contain the nmea talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, char nmeaTalkerId[2]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - uint8_t outputBuffer[1]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(nmeaTalkerId); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command with the output port as a 1-byte payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint8(&outputStream, outputPort); - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_NMEA_TALKER_ID command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); - nmeaTalkerId[0] = (char)sbgStreamBufferReadUint8LE(&inputStream); - nmeaTalkerId[1] = (char)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the NMEA talker id of one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] nmeaTalkerId A 2-char array containint the new nmea talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char nmeaTalkerId[2]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[3]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(nmeaTalkerId); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(nmeaTalkerId[0])); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(nmeaTalkerId[1])); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.h deleted file mode 100644 index e5c1d84..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdOutput.h +++ /dev/null @@ -1,216 +0,0 @@ -/*! - * \file sbgEComCmdOutput.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to outputs. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_OUTPUT_H__ -#define __SBG_ECOM_CMD_OUTPUT_H__ - -#include "sbgEComCmdCommon.h" -#include "../sbgECanId.h" - -//----------------------------------------------------------------------// -//- Output definitions -// -//----------------------------------------------------------------------// - -/*! - * List of ouput ports available. - */ -typedef enum _SbgEComOutputPort -{ - SBG_ECOM_OUTPUT_PORT_A = 0, /*!< Main output port. */ - SBG_ECOM_OUTPUT_PORT_C = 2, /*!< Secondary output port only available on Ellipse-E devices */ - SBG_ECOM_OUTPUT_PORT_E = 4 /*!< Secondary output port only available on B1 devices */ -} SbgEComOutputPort; - -/*! - * List of available legacy output format. - */ -typedef enum _SbgEComLegacyFormat -{ - SBG_ECOM_LEGACY_FORMAT_FLOAT = 0, /*!< Data is output in floating point. */ - SBG_ECOM_LEGACY_FORMAT_FIXED /*!< Data is output in fixed point. */ -} SbgEComLegacyFormat; - -/*! - * List of available legacy output endiannesses. - */ -typedef enum _SbgEComLegacyEndian -{ - SBG_ECOM_LEGACY_LITTLE_ENDIAN = 0, /*!< Data is output in little endian format. */ - SBG_ECOM_LEGACY_BIG_ENDIAN /*!< Data is output in big endian format. */ -} SbgEComLegacyEndian; - -/*! - * List of output modes available. - */ -typedef enum _SbgEComOutputMode -{ - SBG_ECOM_OUTPUT_MODE_DISABLED = 0, /*!< This output is disabled. */ - SBG_ECOM_OUTPUT_MODE_MAIN_LOOP = 1, /*!< Output the message every main loop (ie 200 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_2 = 2, /*!< Output the message every 2 main loops (ie 100 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_4 = 4, /*!< Output the message every 4 main loops (ie 50 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_5 = 5, /*!< Output the message every 4 main loops (ie 40 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_8 = 8, /*!< Output the message every 8 main loops (ie 25 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_10 = 10, /*!< Output the message every 10 main loops (ie 20 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_20 = 20, /*!< Output the message every 20 main loops (ie 10 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_40 = 40, /*!< Output the message every 40 main loops (ie 5 Hz). */ - SBG_ECOM_OUTPUT_MODE_DIV_200 = 200, /*!< Output the message every 200 main loops (ie 1 Hz). */ - SBG_ECOM_OUTPUT_MODE_PPS = 10000, /*!< Output the message on a Pulse Per Second event. */ - SBG_ECOM_OUTPUT_MODE_NEW_DATA = 10001, /*!< Output sent when a new data is available. */ - SBG_ECOM_OUTPUT_MODE_EVENT_IN_A = 10003, /*!< Output the message when a Sync A is received. */ - SBG_ECOM_OUTPUT_MODE_EVENT_IN_B = 10004, /*!< Output the message when a Sync B is received. */ - SBG_ECOM_OUTPUT_MODE_EVENT_IN_C = 10005, /*!< Output the message when a Sync C is received. */ - SBG_ECOM_OUTPUT_MODE_EVENT_IN_D = 10006, /*!< Output the message when a Sync D is received. */ - SBG_ECOM_OUTPUT_MODE_EVENT_IN_E = 10007, /*!< Output the message when a Sync E is received. */ - SBG_ECOM_OUTPUT_MODE_HIGH_FREQ_LOOP = 20001 /*!< Output the message in the 1KHz IMU loop */ -} SbgEComOutputMode; - -/*! - * Defines which monitoring point to use for an output port. - * This feature enabled deporting measurements at a specific monitoring point. - */ -typedef enum _SbgEComOutputMonitoringPoint -{ - SBG_ECOM_OUTPUT_MONITORING_POINT_IMU = 0, /*!< Output measurements at the IMU location. */ - SBG_ECOM_OUTPUT_MONITORING_POINT_COG = 1, /*!< Output measurements at the center of rotation. */ - SBG_ECOM_OUTPUT_MONITORING_POINT_1 = 2, /*!< Output measurements at the user deported location 1 (only for Ekinox and Apogee). */ - SBG_ECOM_OUTPUT_MONITORING_POINT_2 = 3, /*!< Output measurements at the user deported location 2 (only for Ekinox and Apogee). */ - SBG_ECOM_OUTPUT_MONITORING_POINT_3 = 4, /*!< Output measurements at the user deported location 3 (only for Ekinox and Apogee). */ - SBG_ECOM_OUTPUT_MONITORING_NUM /*!< Number of output monitoring points. */ -} SbgEComOutputMonitoringPoint; - -//----------------------------------------------------------------------// -//- Helper structure definitions -// -//----------------------------------------------------------------------// - -/*! - * Helper structure to configure legacy output. - */ -typedef struct _SbgEComLegacyConf -{ - uint32_t mask; /*!< Legacy output bit mask. */ - SbgEComLegacyFormat format; /*!< Format of the output. */ - SbgEComLegacyEndian endian; /*!< Endianness of the output. */ - SbgEComOutputMode mode; /*!< Mode of output. */ -} SbgEComLegacyConf; - -//----------------------------------------------------------------------// -//- Output commands -// -//----------------------------------------------------------------------// - -/*! - * Retrieve the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] classId The class of the concerned log. - * \param[in] msgId The id of the concerned log. - * \param[out] pMode Pointer to a SbgEComOutputMode to contain the current output mode of the message. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pMode); - -/*! - * Set the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] classId The class of the concerned log. - * \param[in] msgId The id of the concerned log. - * \param[in] mode New output mode to set. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode mode); - -/*! - * Retrieve the enable of one of the output class message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port. - * \param[in] classId The class to enable or disable. - * \param[out] pEnable TRUE to enable message output of this class, FALSE to disable it. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable); - -/*! - * Set the enable of one of the output class message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port. - * \param[in] classId The class to enable or disable. - * \param[in] enable TRUE to enable message output of this class, FALSE to disable it. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable); - -/*! - * Retrieve the configuration of one the message on the CAN interface. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] internalId The internal message id. - * \param[out] pMode Pointer to a SbgEComOutputMode to contain the current output mode of the message. - * \param[out] pUserId The user defined message id. - * \param[out] pExtended TRUE if the user id uses the extended format. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended); - -/*! - * Set the configuration of one the message on the CAN interface - * \param[in] pHandle A valid sbgECom handle. - * \param[in] internalId The internal message id. - * \param[in] mode Pointer to a SbgEComOutputMode containing the new output mode of the message. - * \param[in] userId The user defined message id. - * \param[in] extended TRUE if the user id uses the extended format. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended); - -/*! - * Retrieve the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] pConf Pointer to a SbgEComLegacyConf structure to contain legacy configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetLegacyConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComLegacyConf *pConf); - -/*! - * Set the configuration of one the message on one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[in] pConf Pointer to a SbgEComLegacyConf structure containing new legacy configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetLegacyConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const SbgEComLegacyConf *pConf); - -/*! - * Retrieve the NMEA talker id of one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] nmeaTalkerId A 2-char array to contain the nmea talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputGetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, char nmeaTalkerId[2]); - -/*! - * Set the NMEA talker id of one of the output interfaces. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] outputPort The output port of the device for the log concerned. - * \param[out] nmeaTalkerId A 2-char array containint the new nmea talker id. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char nmeaTalkerId[2]); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.c deleted file mode 100644 index 6a94115..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.c +++ /dev/null @@ -1,506 +0,0 @@ -#include "sbgEComCmdSensor.h" -#include -#include "transfer/sbgEComTransfer.h" - -//----------------------------------------------------------------------// -//- Sensor commands -// -//----------------------------------------------------------------------// - -/*! - * Set a Motion profile ID - * \param[in] pHandle A valid sbgECom handle. - * \param[id] id Motion profile ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, uint32_t id) -{ - // - // Call generic function with specific command name - // - return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, id); -} - - -/*! - * Retrieve Motion profile information. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current motion profile info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetMotionProfileInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo) -{ - // - // Call generic function with specific command name - // - return sbgEComCmdGenericGetModelInfo(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, pModelInfo); -} - -/*! - * Retrieve the initial conditions settings. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComInitConditionConf to contain the current initial conditions settings. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_INIT_PARAMETERS command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pConf->latitude = sbgStreamBufferReadDoubleLE(&inputStream); - pConf->longitude = sbgStreamBufferReadDoubleLE(&inputStream); - pConf->altitude = sbgStreamBufferReadDoubleLE(&inputStream); - pConf->year = sbgStreamBufferReadUint16LE(&inputStream); - pConf->month = sbgStreamBufferReadUint8LE(&inputStream); - pConf->day = sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the initial condition configuration. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComInitConditionConf containing the new initial condition configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteDoubleLE(&outputStream, pConf->latitude); - sbgStreamBufferWriteDoubleLE(&outputStream, pConf->longitude); - sbgStreamBufferWriteDoubleLE(&outputStream, pConf->altitude); - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)pConf->year); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->month); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->day); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the assignment of the aiding sensors. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAidingAssignConf to contain the current assignment of the aiding sensors. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[16]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_AIDING_ASSIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pConf->gps1Port = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); - pConf->gps1Sync = (SbgEComModuleSyncAssignment)sbgStreamBufferReadUint8LE(&inputStream); - - sbgStreamBufferSeek(&inputStream, 4*sizeof(uint8_t), SB_SEEK_CUR_INC); /*!< Reserved fields to ignore */ - - pConf->dvlPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); - pConf->dvlSync = (SbgEComModuleSyncAssignment)sbgStreamBufferReadUint8LE(&inputStream); - - pConf->rtcmPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); - pConf->airDataPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); - pConf->odometerPinsConf = (SbgEComOdometerPinAssignment)sbgStreamBufferReadUint8LE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the assignment of the aiding sensors. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAidingAssignConf containing the new assignment of the aiding sensors. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[16]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pConf); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Port); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Sync); - - // - // Skip the 4 reserved bytes - // - sbgStreamBufferWriteUint8LE(&outputStream, 0); - sbgStreamBufferWriteUint8LE(&outputStream, 0); - sbgStreamBufferWriteUint8LE(&outputStream, 0); - sbgStreamBufferWriteUint8LE(&outputStream, 0); - - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->dvlPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->dvlSync); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->rtcmPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->airDataPort); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->odometerPinsConf); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Retrieve the alignment and lever arm configuration of the sensor. - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct to hold alignment configuration of the sensor. - * \param[out] leverArm Pointer to a table to contain lever arm X, Y, Z components in meters. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float leverArm[3]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - size_t receivedSize; - uint8_t receivedBuffer[32]; - SbgStreamBuffer inputStream; - - assert(pHandle); - assert(pAlignConf); - assert(leverArm); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command only since this is a no-payload command - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, NULL, 0); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a SBG_ECOM_CMD_IMU_ALIGNMENT command - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize stream buffer to read parameters - // - sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize); - - // - // Read parameters - // - pAlignConf->axisDirectionX = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); - pAlignConf->axisDirectionY = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); - pAlignConf->misRoll = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->misPitch = sbgStreamBufferReadFloatLE(&inputStream); - pAlignConf->misYaw = sbgStreamBufferReadFloatLE(&inputStream); - leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); - leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); - leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); - - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Set the alignment and lever arm configuration of the sensor. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct holding alignment configuration for the sensor. - * \param[in] leverArm Pointer to a table containing lever arm X, Y, Z components in meters. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float leverArm[3]) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[32]; - SbgStreamBuffer outputStream; - - assert(pHandle); - assert(pAlignConf); - assert(leverArm); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Init stream buffer for output - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload - // - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pAlignConf->axisDirectionX); - sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pAlignConf->axisDirectionY); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misRoll); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misPitch); - sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misYaw); - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[0]); - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[1]); - sbgStreamBufferWriteFloatLE(&outputStream, leverArm[2]); - - // - // Send the payload over ECom - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.h deleted file mode 100644 index 51d0860..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSensor.h +++ /dev/null @@ -1,203 +0,0 @@ -/*! - * \file sbgEComCmdSensor.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to sensor. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_SENSOR_H__ -#define __SBG_ECOM_CMD_SENSOR_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Sensor definitions -// -//----------------------------------------------------------------------// - -/*! - * Ports available for the modules. - */ -typedef enum _SbgEComModulePortAssignment -{ - SBG_ECOM_MODULE_PORT_B = 1, /*!< Module connected on PORT_B. */ - SBG_ECOM_MODULE_PORT_C = 2, /*!< Module connected on PORT_C. */ - SBG_ECOM_MODULE_PORT_D = 3, /*!< Module connected on PORT_D. */ - SBG_ECOM_MODULE_PORT_E = 4, /*!< Module connected on PORT_E. */ - SBG_ECOM_MODULE_INTERNAL = 5, /*!< Module is connected internally. */ - SBG_ECOM_MODULE_DISABLED = 0xFF /*!< Module is disabled. */ -} SbgEComModulePortAssignment; - -/*! - * Synchronization signals available for the modules. - */ -typedef enum _SbgEComModuleSyncAssignment -{ - SBG_ECOM_MODULE_SYNC_DISABLED = 0, /*!< Module is disabled. */ - SBG_ECOM_MODULE_SYNC_IN_A = 1, /*!< Synchronization is done using SYNC_IN_A pin. */ - SBG_ECOM_MODULE_SYNC_IN_B = 2, /*!< Synchronization is done using SYNC_IN_B pin. */ - SBG_ECOM_MODULE_SYNC_IN_C = 3, /*!< Synchronization is done using SYNC_IN_C pin. */ - SBG_ECOM_MODULE_SYNC_IN_D = 4, /*!< Synchronization is done using SYNC_IN_D pin. */ - SBG_ECOM_MODULE_SYNC_INTERNAL = 5, /*!< Synchronization is internal. */ - SBG_ECOM_MODULE_SYNC_OUT_A = 6, /*!< Synchronization signal is output on SYNC_OUT_A. */ - SBG_ECOM_MODULE_SYNC_OUT_B = 7, /*!< Synchronization signal is output on SYNC_OUT_B. */ -} SbgEComModuleSyncAssignment; - -/*! - * List of configurations available for the odometer. - */ -typedef enum _SbgEComOdometerPinAssignment -{ - SBG_ECOM_MODULE_ODO_DISABLED = 0, /*!< Odometer is disabled. */ - SBG_ECOM_MODULE_ODO_A = 1, /*!< Odometer connected only to ODO_A (unidirectional).. */ - SBG_ECOM_MODULE_ODO_A_B = 2, /*!< Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer.. */ - SBG_ECOM_MODULE_ODO_CAN = 3, /*!< Vehicle odometer using CAN (OBD-II). */ -} SbgEComOdometerPinAssignment; - -/*! - * This enum defines the different motion profile IDs available in standard - */ -typedef enum _SbgEComMotionProfileStdIds -{ - SBG_ECOM_MOTION_PROFILE_GENERAL_PURPOSE = 1, /*!< Should be used as a default when other profiles do not apply */ - SBG_ECOM_MOTION_PROFILE_AUTOMOTIVE = 2, /*!< Dedicated to car applications */ - SBG_ECOM_MOTION_PROFILE_MARINE = 3, /*!< Used in marine and underwater applications */ - SBG_ECOM_MOTION_PROFILE_AIRPLANE = 4, /*!< For fixed wings aircraft */ - SBG_ECOM_MOTION_PROFILE_HELICOPTER = 5, /*!< For rotary wing aircraft */ - SBG_ECOM_MOTION_PROFILE_UAV_ROTARY_WING = 7, /*!< For rotary wing UAVs that have low dynamics */ - SBG_ECOM_MOTION_PROFILE_HEAVY_MACHINERY = 8, /*!< For vibrating applications with low dynamics and no specific travel direction */ -} SbgEComMotionProfileStdIds; - -//----------------------------------------------------------------------// -//- Event configurations -// -//----------------------------------------------------------------------// - -/*! - * Helper structure for module assignments - */ -typedef struct _SbgEComAidingAssignConf -{ - SbgEComModulePortAssignment gps1Port; /*!< GNSS module port assignment. */ - SbgEComModuleSyncAssignment gps1Sync; /*!< GNSS module sync assignment. */ - SbgEComModulePortAssignment dvlPort; /*!< Port on which the DVL is connected */ - SbgEComModuleSyncAssignment dvlSync; /*!< Optional sync signal that could be used to time stamp the DVL data. */ - SbgEComModulePortAssignment rtcmPort; /*!< RTCM input port assignment for IGNG-N DGPS. */ - SbgEComModulePortAssignment airDataPort; /*!< Port on which Air Data aiding is connected. */ - SbgEComOdometerPinAssignment odometerPinsConf; /*!< Odometer module pin assignment. */ -} SbgEComAidingAssignConf; - -/*! - * Helper structure for sensor alignment details - */ -typedef struct _SbgEComSensorAlignmentInfo -{ - SbgEComAxisDirection axisDirectionX; /*!< Sensor X axis direction in vehicle */ - SbgEComAxisDirection axisDirectionY; /*!< Sensor Y axis direction in vehicle */ - float misRoll; /*!< Roll angle fine misalignment in rad */ - float misPitch; /*!< Pitch angle fine misalignment in rad */ - float misYaw; /*!< Yaw angle fine misalignment in rad */ -} SbgEComSensorAlignmentInfo; - -/*! - * Helper structure for sensor Initial condition details - */ -typedef struct _SbgEComInitConditionConf -{ - double latitude; /*!< Initial latitude in ° */ - double longitude; /*!< Initial longitude in ° */ - double altitude; /*!< Initial altitude above MSL in meters */ - uint16_t year; /*!< Initial Year */ - uint8_t month; /*!< Initial month */ - uint8_t day; /*!< Initial day */ -} SbgEComInitConditionConf; - -//----------------------------------------------------------------------// -//- Sensor commands -// -//----------------------------------------------------------------------// - -/*! - * Set a Motion profile ID. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[id] id Motion profile ID to set - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, uint32_t id); - -/*! - * Retrieve Motion profile information. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pMotionProfileInfo Pointer to a SbgEComModelInfo to contain the current motion profile info. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetMotionProfileInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pMotionProfileInfo); - -/*! - * Retrieve the initial conditions settings. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComInitConditionConf to contain the current initial conditions settings. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf); - -/*! - * Set the initial condition configuration. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pConf Pointer to a SbgEComInitConditionConf containing the new initial condition configuration. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf); - -/*! - * Retrieve the assignment of the aiding sensors. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAidingAssignConf to contain the current assignment of the aiding sensors. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf); - -/*! - * Set the assignment of the aiding sensors. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pConf Pointer to a SbgEComAidingAssignConf containing the new assignment of the aiding sensors. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf); - -/*! - * Retrieve the alignment and lever arm configuration of the sensor. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[out] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct to hold alignment configuration of the sensor. - * \param[out] leverArm Pointer to a table to contain lever arm X, Y, Z components in meters. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float leverArm[3]); - -/*! - * Set the alignment and lever arm configuration of the sensor. - * - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct holding alignment configuration for the sensor. - * \param[in] leverArm Pointer to a table containing lever arm X, Y, Z components in meters. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float leverArm[3]); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.c b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.c deleted file mode 100644 index ff1625e..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.c +++ /dev/null @@ -1,102 +0,0 @@ -#include "sbgEComCmdSettings.h" -#include "transfer/sbgEComTransfer.h" -#include - -//----------------------------------------------------------------------// -//- Settings commands -// -//----------------------------------------------------------------------// - -/*! - * Execute one of the available settings action :
- * - SBG_ECOM_REBOOT_ONLY : Only reboot the device.
- * - SBG_ECOM_SAVE_SETTINGS : Save the settings to non-volatile memory and then reboot the device.
- * - SBG_ECOM_RESTORE_DEFAULT_SETTINGS : Restore default settings, save them to non-volatile memory and reboot the device.
- * \param[in] pHandle A valid sbgECom handle. - * \param[in] action One of the available SbgEComSettingsAction. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint32_t trial; - uint8_t outputBuffer[1]; - SbgStreamBuffer outputStream; - - assert(pHandle); - - // - // Send the command three times - // - for (trial = 0; trial < pHandle->numTrials; trial++) - { - // - // Send the command and the action as a 1-byte payload - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - sbgStreamBufferWriteUint8(&outputStream, action); - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SETTINGS_ACTION, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - - // - // Make sure that the command has been sent - // - if (errorCode == SBG_NO_ERROR) - { - // - // Try to read the device answer for 500 ms - // - errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SETTINGS_ACTION, pHandle->cmdDefaultTimeOut); - - // - // Test if we have received a valid ACK - // - if (errorCode == SBG_NO_ERROR) - { - // - // The command has been executed successfully so return - // - break; - } - } - else - { - // - // We have a write error so exit the try loop - // - break; - } - } - - return errorCode; -} - -/*! - * Send a complete set of settings to the device and store them into the FLASH memory. - * The device will reboot automatically to use the new settings. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Read only buffer containing the settings. - * \param[in] size Size of the buffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdImportSettings(SbgEComHandle *pHandle, const void *pBuffer, size_t size) -{ - // - // Call function that handle data transfer - // - return sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMPORT_SETTINGS, pBuffer, size); -} - -/*! - * Retrieve a complete set of settings from the device as a buffer. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Allocated buffer that can hold the received settings. - * \param[out] pSize The number of bytes that have been stored into pBuffer. - * \param[in] maxSize The maximum buffer size in bytes that can be stored into pBuffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdExportSettings(SbgEComHandle *pHandle, void *pBuffer, size_t *pSize, size_t maxSize) -{ - // - // Call function that handle data transfer - // - return sbgEComTransferReceive(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_EXPORT_SETTINGS, pBuffer, pSize, maxSize); -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.h b/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.h deleted file mode 100644 index fa4a846..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/sbgEComCmdSettings.h +++ /dev/null @@ -1,74 +0,0 @@ -/*! - * \file sbgEComCmdSettings.h - * \author SBG Systems (Maxime Renaudet) - * \date 11 June 2014 - * - * \brief This file implements SbgECom commands related to settings. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef __SBG_ECOM_CMD_SETTINGS_H__ -#define __SBG_ECOM_CMD_SETTINGS_H__ - -#include "sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Settings action definition -// -//----------------------------------------------------------------------// - -/*! - * Defintion of all the settings actions available. - */ -typedef enum _SbgEComSettingsAction -{ - SBG_ECOM_REBOOT_ONLY = 0, /*!< Only reboot the device. */ - SBG_ECOM_SAVE_SETTINGS = 1, /*!< Save the settings to non-volatile memory and then reboot the device. */ - SBG_ECOM_RESTORE_DEFAULT_SETTINGS = 2 /*!< Restore default settings, save them to non-volatile memory and reboot the device. */ -} SbgEComSettingsAction; - -//----------------------------------------------------------------------// -//- Settings commands -// -//----------------------------------------------------------------------// - -/*! - * Execute one of the available settings action :
- * - SBG_ECOM_REBOOT_ONLY : Only reboot the device.
- * - SBG_ECOM_SAVE_SETTINGS : Save the settings to non-volatile memory and then reboot the device.
- * - SBG_ECOM_RESTORE_DEFAULT_SETTINGS : Restore default settings, save them to non-volatile memory and reboot the device.
- * \param[in] pHandle A valid sbgECom handle. - * \param[in] action One of the available SbgEComSettingsAction. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action); - -/*! - * Send a complete set of settings to the device and store them into the FLASH memory. - * The device will reboot automatically to use the new settings. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Read only buffer containing the settings. - * \param[in] size Size of the buffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdImportSettings(SbgEComHandle *pHandle, const void *pBuffer, size_t size); - -/*! - * Retrieve a complete set of settings from the device as a buffer. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pBuffer Allocated buffer that can hold the received settings. - * \param[out] pSize The number of bytes that have been stored into pBuffer. - * \param[in] maxSize The maximum buffer size in bytes that can be stored into pBuffer. - * \return SBG_NO_ERROR if the command has been executed successfully. - */ -SbgErrorCode sbgEComCmdExportSettings(SbgEComHandle *pHandle, void *pBuffer, size_t *pSize, size_t maxSize); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.c b/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.c deleted file mode 100644 index 06e65ed..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.c +++ /dev/null @@ -1,655 +0,0 @@ -#include "sbgEComTransfer.h" -#include "../sbgEComCmdCommon.h" -#include -#include - -//----------------------------------------------------------------------// -//- Internal transfer method definitions -// -//----------------------------------------------------------------------// - -/*! - * Initiates an upload transfer sequence with a device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] size Total size of the upload. - * \return SBG_NO_ERROR when the transfer was initiated successfully. - */ -static SbgErrorCode sbgEComTransferSendInit(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, size_t size) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer streamBuffer; - uint8_t outputBuffer[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint32_t i; - - assert(pHandle); - - // - // Initialize stream buffer that will contain payload - // - sbgStreamBufferInitForWrite(&streamBuffer, outputBuffer, sizeof(outputBuffer)); - - // - // Build transfer payload (a SBG_ECOM_TRANSFER_START command and the total size of the upload) - // - sbgStreamBufferWriteUint16LE(&streamBuffer, SBG_ECOM_TRANSFER_START); - sbgStreamBufferWriteSizeT32LE(&streamBuffer, size); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send transfer payload encapsulated in ECom protocol - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // If the device accepts the transfer, it returns an ack, wait for the answer. - // - errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); - - // - // Test if the response is positive from device - // - if (errorCode == SBG_NO_ERROR) - { - // - // Ack received, no need for other trial. - // - break; - } - } - } - - return errorCode; -} - -/*! - * Send one packet of data on a initiated upload transfer. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer containing the data to send. - * \param[in] offset The offset from the start of the transfer. - * \param[in] packetSize The size of this packet. - * \return SBG_NO_ERROR if the packet was sent and acknowledged by the device. - */ -static SbgErrorCode sbgEComTransferSendData(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t offset, size_t packetSize) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer streamBuffer; - uint8_t outputBuffer[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint32_t i; - - assert(pHandle); - assert(pBuffer); - assert(packetSize > 0); - - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&streamBuffer, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload: a SBG_ECOM_TRANSFER_DATA command, the offset from the start of the transfer, and the data - // - sbgStreamBufferWriteUint16LE(&streamBuffer, SBG_ECOM_TRANSFER_DATA); - sbgStreamBufferWriteSizeT32LE(&streamBuffer, offset); - sbgStreamBufferWriteBuffer(&streamBuffer, pBuffer, packetSize); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send transfer payload encapsulated in a ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // If the device receives the frame successfully received, it responds with an ACK, wait for the answer - // - errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); - - // - // Test if the response is positive from device - // - if (errorCode == SBG_NO_ERROR) - { - // - // Ack received, no need for other trial - // - break; - } - } - } - - return errorCode; -} - -/*! - * Ends ongoing upload transfer sequence with a device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \return SBG_NO_ERROR when the transfer ended successfully. - */ -static SbgErrorCode sbgEComTransferSendEnd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outStreamBuffer; - uint8_t outputBuffer[sizeof(uint16_t)]; - uint32_t i; - - assert(pHandle); - - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outStreamBuffer, outputBuffer, sizeof(outStreamBuffer)); - - // - // Build payload, only a SBG_ECOM_TRANSFER_END cmd - // - sbgStreamBufferWriteUint16LE(&outStreamBuffer, SBG_ECOM_TRANSFER_END); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send upload end payload encapsulated in a ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outStreamBuffer), sbgStreamBufferGetLength(&outStreamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // If the device finishes the sequence successfully, it responds with an ACK, wait for answer - // - errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); - - // - // Test if the response is positive from device - // - if (errorCode == SBG_NO_ERROR) - { - // - // ACK received, no need for other trial - // - break; - } - } - } - - return errorCode; -} - -/*! - * Initiates a download sequences with a device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[out] pSize Size of the transfer initiated, returned from the device. - * \return SBG_NO_ERROR when the transfer initiated successfully. - */ -static SbgErrorCode sbgEComTransferReceiveInit(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, size_t *pSize) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outStreamBuffer; - SbgStreamBuffer inStreamBuffer; - uint8_t inputBuffer[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint8_t outputBuffer[sizeof(uint16_t)]; - uint8_t receivedMsgClass; - uint8_t receivedMsg; - uint16_t transferCmd; - size_t inputSize; - size_t transferSize; - uint32_t i; - - assert(pHandle); - - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outStreamBuffer, outputBuffer, sizeof(outStreamBuffer)); - - // - // Build payload, only a SBG_ECOM_TRANSFER_START cmd - // - sbgStreamBufferWriteUint16LE(&outStreamBuffer, SBG_ECOM_TRANSFER_START); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send transfer payload encapsulated in an ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outStreamBuffer), sbgStreamBufferGetLength(&outStreamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // Wait for reponse, the device should respond with a ECOM_TRANSFER_START command and the transfer size - // If it can not initiate the transfer, it will respond with a NACK - // - errorCode = sbgEComReceiveAnyCmd(pHandle, &receivedMsgClass, &receivedMsg, inputBuffer, &inputSize, SBG_ECOM_MAX_PAYLOAD_SIZE, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - // - // Test if the command received is the one expected - // - if ((receivedMsgClass == msgClass) && (receivedMsg == msg)) - { - // - // Init stream buffer on received payload to process it - // - sbgStreamBufferInitForRead(&inStreamBuffer, inputBuffer, inputSize); - - // - // Retrieve parameters, the first one is the transfer command - // The second one is the total transfer size - // - transferCmd = sbgStreamBufferReadUint16LE(&inStreamBuffer); - transferSize = sbgStreamBufferReadSizeT32LE(&inStreamBuffer); - - // - // The device should have answered with SBG_ECOM_TRANSFER_START transfer command - // - if (transferCmd == SBG_ECOM_TRANSFER_START) - { - // - // Update output variable with the transfer size - // - *pSize = transferSize; - - // - // No need for other trials, exit loop/ - // - break; - } - else - { - // - // Invalid transfer command response - // - errorCode = SBG_ERROR; - } - } - else - { - // - // This is not the command expected - // - errorCode = SBG_ERROR; - } - } - } - } - - return errorCode; -} - -/*! - * Receive one packet of data on a initiated download transfer. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer where to write the packet. - * \param[in] offset The offset from the start of the buffer. - * \param[in] packetSize The size of the data asked to the device. - * \return SBG_NO_ERROR if the packet was successfully received. - */ -static SbgErrorCode sbgEComTransferReceiveData(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t offset, size_t packetSize) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outStreamBuffer; - SbgStreamBuffer inStreamBuffer; - uint8_t outputBuffer[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint8_t inputBuffer[SBG_ECOM_MAX_PAYLOAD_SIZE]; - uint16_t transferCmd; - uint8_t receivedMsgClass; - uint8_t receivedMsg; - size_t rcvdOffset; - size_t inputSize; - uint32_t i; - - assert(pHandle); - assert(pBuffer); - assert(packetSize > 0); - - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outStreamBuffer, outputBuffer, sizeof(outputBuffer)); - - // - // Build payload: an SBG_ECOM_TRANSFER_DATA transfer command, the offset from the start of the transfer, the size of the packet the device must send - // - sbgStreamBufferWriteUint16LE(&outStreamBuffer, SBG_ECOM_TRANSFER_DATA); - sbgStreamBufferWriteSizeT32LE(&outStreamBuffer, offset); - sbgStreamBufferWriteSizeT32LE(&outStreamBuffer, packetSize); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send transfer payload encapsulated in an ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outStreamBuffer), sbgStreamBufferGetLength(&outStreamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // Wait for reponse, the device should respond with a ECOM_TRANSFER_DATA, the offset from the start of the transfer and the data payload - // If it can not provide the data, it will respond with a NACK - // - errorCode = sbgEComReceiveAnyCmd(pHandle, &receivedMsgClass, &receivedMsg, inputBuffer, &inputSize, SBG_ECOM_MAX_PAYLOAD_SIZE, pHandle->cmdDefaultTimeOut); - - if (errorCode == SBG_NO_ERROR) - { - // - // Test if this is the protocol command expected - // - if ((receivedMsgClass == msgClass) && (receivedMsg == msg)) - { - // - // Initialize stream buffer for read on input buffer - // - sbgStreamBufferInitForRead(&inStreamBuffer, inputBuffer, inputSize); - - // - // Read response fields, first is the transfer command, second is the offset - // - transferCmd = sbgStreamBufferReadUint16LE(&inStreamBuffer); - rcvdOffset = sbgStreamBufferReadSizeT32LE(&inStreamBuffer); - - // - // Test that it's a SBG_ECOM_TRANSFER_DATA command - // The data is at the offset asked - // And the size corresponds - // - if ( (transferCmd == SBG_ECOM_TRANSFER_DATA) && (offset == rcvdOffset) && (packetSize == (inputSize - (sizeof(uint16_t) + sizeof(uint32_t)))) ) - { - // - // Read then all the buffer - // - sbgStreamBufferReadBuffer(&inStreamBuffer, pBuffer, inputSize - (sizeof(uint16_t) + sizeof(uint32_t))); - - // - // No need for other trials, exit loop - // - break; - } - } - else - { - // - // Not the command expected - // - errorCode = SBG_ERROR; - } - } - } - } - - return errorCode; -} - -/*! - * Function that ends a download sequence with a device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \return SBG_NO_ERROR when the transfer ended successfully. - */ -static SbgErrorCode sbgEComTransferReceiveEnd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgStreamBuffer outStreamBuffer; - uint8_t outputBuffer[sizeof(uint16_t)]; - uint32_t i; - - assert(pHandle); - - // - // Initialize stream buffer for output - // - sbgStreamBufferInitForWrite(&outStreamBuffer, outputBuffer, sizeof(outStreamBuffer)); - - // - // Build payload, only a SBG_ECOM_TRANSFER_END cmd - // - sbgStreamBufferWriteUint16LE(&outStreamBuffer, SBG_ECOM_TRANSFER_END); - - // - // Send command (multiple times in case of failures) - // - for (i = 0; i < 3; i++) - { - // - // Send upload end payload encapsulated in a ECom protocol frame - // - errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outStreamBuffer), sbgStreamBufferGetLength(&outStreamBuffer)); - - if (errorCode == SBG_NO_ERROR) - { - // - // If the device is able to finish transfer sequence, it responds with an ACK - // - errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); - - // - // Test if the response is positive from device - // - if (errorCode == SBG_NO_ERROR) - { - // - // No need for other trial, exit loop - // - break; - } - } - } - - return errorCode; -} - -//----------------------------------------------------------------------// -//- Public transfer method definitions -// -//----------------------------------------------------------------------// - -/*! - * Specific method to handle a large send into multiple frames. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer containing the data to send. - * \param[in] size The size of the buffer. - * \return SBG_NO_ERROR in case of a successful upload. - */ -SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t size) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgSplitBuffer splitBuffer; - size_t i; - - assert(pHandle); - assert(pBuffer); - assert(size > 0); - - // - // Make sure we are not trying to send a buffer that is too large - // - if (size <= SBG_ECOM_TRANSFER_MAX_SIZE) - { - // - // Initiate data transfer - // - errorCode = sbgEComTransferSendInit(pHandle, msgClass, msg, size); - - // - // Check that the transfer was correctly initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Initialize split buffer that will help with splitting up provided buffer - // - sbgSplitBufferInitForRead(&splitBuffer, pBuffer, size, SBG_ECOM_TRANSFER_PACKET_SIZE); - - // - // Transfer sub buffer one by one - // - for (i = 0; i < sbgSplitBufferGetSubBufferNbr(&splitBuffer); i++) - { - // - // Send a sub buffer - // - errorCode = sbgEComTransferSendData(pHandle, msgClass, msg, sbgSplitBufferGetSubBuffer(&splitBuffer, i), sbgSplitBufferGetSubBufferOffset(&splitBuffer, i), sbgSplitBufferGetSubBufferSize(&splitBuffer, i)); - - // - // Test if the sub buffer has been sent - // - if (errorCode != SBG_NO_ERROR) - { - // - // Unable to send a sub buffer, abort send operation. - // - break; - } - } - - // - // Test if any error occurred during data transfer - // - if (errorCode == SBG_NO_ERROR) - { - // - // End data transfer - // - errorCode = sbgEComTransferSendEnd(pHandle, msgClass, msg); - } - } - } - else - { - // - // Trying to send a buffer that is too large - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; -} - -/*! - * Specific method to handle a large receive from the device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer where to write data. - * \param[out] pActualSize The final size written into the buffer. - * \param[in] size The size of the buffer. - * \return SBG_NO_ERROR in case of a successful download. - */ -SbgErrorCode sbgEComTransferReceive(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t *pActualSize, size_t bufferSize) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgSplitBuffer splitBuffer; - size_t transferSize; - size_t i; - - assert(pHandle); - assert(pBuffer); - assert(pActualSize); - assert(bufferSize > 0); - - // - // initiate data transfer - // - errorCode = sbgEComTransferReceiveInit(pHandle, msgClass, msg, &transferSize); - - // - // Make sure the receive transfer has been correctly initialized - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test that the provided buffer is large enough to receive all data - // - if (transferSize <= bufferSize) - { - // - // Initialize Split buffer to help with sub buffer receive - // - sbgSplitBufferInitForWrite(&splitBuffer, pBuffer, transferSize, SBG_ECOM_TRANSFER_PACKET_SIZE); - - // - // Receive buffers one by one - // - for (i = 0; i < sbgSplitBufferGetSubBufferNbr(&splitBuffer); i++) - { - // - // Receive a sub buffer - // - errorCode = sbgEComTransferReceiveData(pHandle, msgClass, msg, sbgSplitBufferGetSubBuffer(&splitBuffer, i), sbgSplitBufferGetSubBufferOffset(&splitBuffer, i), sbgSplitBufferGetSubBufferSize(&splitBuffer, i)); - - // - // Make sure that the sub buffer has been correctly received - // - if (errorCode != SBG_NO_ERROR) - { - // - // An error occurred, abort data transfer - // - break; - } - } - - // - // Test if any error occurred during transfer - // - if (errorCode == SBG_NO_ERROR) - { - // - // End data transfer - // - errorCode = sbgEComTransferReceiveEnd(pHandle, msgClass, msg); - - // - // Make sure that the transfer has been correctly ended - // - if (errorCode == SBG_NO_ERROR) - { - // - // Since the transfer was successful update output variable pActualSize - // - *pActualSize = transferSize; - } - } - } - else - { - // - // Provided buffer is too small - // - errorCode = SBG_INVALID_PARAMETER; - } - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.h b/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.h deleted file mode 100644 index 4097e4d..0000000 --- a/sbg_ros_driver/external/sbgECom/src/commands/transfer/sbgEComTransfer.h +++ /dev/null @@ -1,75 +0,0 @@ -/*! - * \file sbgEComTransfer.h - * \author SBG Systems (Maxime Renaudet) - * \date 19 November 2013 - * - * \brief Handle large send/receive transfer for specific ECom Protocol commands. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef __SBG_ECOM_TRANSFER_H__ -#define __SBG_ECOM_TRANSFER_H__ - -#include -#include "../../sbgECom.h" - -//----------------------------------------------------------------------// -//- Global definitions -// -//----------------------------------------------------------------------// - -#define SBG_ECOM_TRANSFER_MAX_SIZE (8192u) /*!< Maximum buffer that can be transmitted using the sbgECom transfer protocol. */ -#define SBG_ECOM_TRANSFER_PACKET_SIZE (1300u) /*!< Max packet size transmitted in a single frame */ - -//----------------------------------------------------------------------// -//- Communication protocol structs and definitions -// -//----------------------------------------------------------------------// - -/*! - * Defines the ECom transfer commands - */ -typedef enum _SbgEComTransferCmd -{ - SBG_ECOM_TRANSFER_START = 0, /*!< Command to initiate a transfer. */ - SBG_ECOM_TRANSFER_DATA = 1, /*!< Command to transmit/receive data. */ - SBG_ECOM_TRANSFER_END = 2 /*!< Command to end a transfer. */ -} SbgEComTransferCmd; - -//----------------------------------------------------------------------// -//- Protocol transfer operations -// -//----------------------------------------------------------------------// - -/*! - * Specific method to handle a large send into multiple frames. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer containing the data to send. - * \param[in] size The size of the buffer. - * \return SBG_NO_ERROR in case of a successful upload. - */ -SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t size); - -/*! - * Specific method to handle a large receive from the device. - * \param[in] pHandle Pointer to a valid SbgEComHandle. - * \param[in] msgClass Original protocol class asking for transfer. - * \param[in] msg Original protocol message id asking for transfer. - * \param[in] pBuffer Pointer to the buffer where to write data. - * \param[out] pActualSize The final size written into the buffer. - * \param[in] size The size of the buffer. - * \return SBG_NO_ERROR in case of a successful download. - */ -SbgErrorCode sbgEComTransferReceive(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t *pActualSize, size_t bufferSize); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.c b/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.c deleted file mode 100644 index 4eb38f9..0000000 --- a/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.c +++ /dev/null @@ -1,581 +0,0 @@ -#include "sbgEComProtocol.h" -#include - -//----------------------------------------------------------------------// -//- Communication protocol operations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the protocol system used to communicate with the product and return the created handle. - * \param[in] pHandle Pointer on an allocated protocol structure to initialize. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialised the protocol system. - */ -SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pHandle, SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - assert(pHandle); - assert(pInterface); - - // - // Initialize the created protocol handle - // - pHandle->pLinkedInterface = pInterface; - pHandle->rxBufferSize = 0; - - return errorCode; -} - -/*! - * Close the protocol system. - * \param[in] pHandle A valid protocol handle to close. - * \return SBG_NO_ERROR if we have closed and released the protocol system. - */ -SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pHandle) -{ - assert(pHandle); - - // - // Reset all members to zero - // - pHandle->pLinkedInterface = NULL; - pHandle->rxBufferSize = 0; - - // - // Don't have to do anything - // - return SBG_NO_ERROR; -} - -/*! - * Send a frame to the device (size should be less than 4086 bytes). - * \param[in] pHandle A valid protocol handle. - * \param[in] msgClass Message class (0-255) - * \param[in] msg Message id (0-255) - * \param[in] pData Pointer on the data payload to send or NULL if no payload. - * \param[in] size Size in bytes of the data payload (less than 4086). - * \return SBG_NO_ERROR if the frame has been sent. - */ -SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; - SbgStreamBuffer outputStream; - uint16_t frameCrc; - - assert(pHandle); - - if ( (size <= SBG_ECOM_MAX_PAYLOAD_SIZE) && ( ((size > 0) && (pData)) || (size == 0) ) ) - { - // - // Create a stream buffer to write the frame - // - sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); - - // - // Write the header - // - sbgStreamBufferWriteUint8LE(&outputStream, SBG_ECOM_SYNC_1); - sbgStreamBufferWriteUint8LE(&outputStream, SBG_ECOM_SYNC_2); - - // - // Write the message ID and class - // - sbgStreamBufferWriteUint8LE(&outputStream, msg); - sbgStreamBufferWriteUint8LE(&outputStream, msgClass); - - // - // Write the length field - // - sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)size); - - // - // Write the payload part - // - sbgStreamBufferWriteBuffer(&outputStream, pData, size); - - // - // Compute the CRC, we skip the two sync chars - // - frameCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&outputStream)) + 2, sbgStreamBufferGetLength(&outputStream) - 2); - - // - // Write the CRC - // - sbgStreamBufferWriteUint16LE(&outputStream, frameCrc); - - // - // Write ETX char - // - sbgStreamBufferWriteUint8LE(&outputStream, SBG_ECOM_ETX); - - // - // The frame has been generated so send it - // - errorCode = sbgInterfaceWrite(pHandle->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); - } - else - { - // - // Invalid input parameters - // - errorCode = SBG_INVALID_PARAMETER; - } - - return errorCode; -} - -/*! - * Try to receive a frame from the device and returns the cmd, data and size of data field. - * \param[in] pHandle A valid protocol handle. - * \param[out] pMsgClass Pointer to hold the returned message class - * \param[out] pMsg Pointer to hold the returned message id - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \return SBG_NO_ERROR if we have received a valid frame.
- * SBG_NOT_READY if we haven't received a valid frame or if the serial buffer is empty.
- * SBG_INVALID_CRC if the received frame has an invalid CRC.
- * SBG_NULL_POINTER if an input parameter is NULL.
- * SBG_BUFFER_OVERFLOW if the received frame payload couldn't fit into the pData buffer. - */ -SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize) -{ - SbgErrorCode errorCode = SBG_NOT_READY; - SbgStreamBuffer inputStream; - bool syncFound; - size_t payloadSize = 0; - uint16_t frameCrc; - uint16_t computedCrc; - size_t i; - size_t numBytesRead; - uint8_t receivedMsgClass; - uint8_t receivedMsg; - size_t payloadOffset; - - assert(pHandle); - - // - // Set the return size to 0 in order to avoid possible bugs - // - if (pSize) - { - *pSize = 0; - } - - // - // Check if we can receive some new data (the receive buffer isn't full) - // - if (pHandle->rxBufferSize < SBG_ECOM_MAX_BUFFER_SIZE) - { - // - // First try to read as much data as we can - // - if (sbgInterfaceRead(pHandle->pLinkedInterface, pHandle->rxBuffer + pHandle->rxBufferSize, &numBytesRead, SBG_ECOM_MAX_BUFFER_SIZE - pHandle->rxBufferSize) == SBG_NO_ERROR) - { - // - // No error during reading so increment the number of bytes stored in the rx buffer - // - pHandle->rxBufferSize += numBytesRead; - } - } - - // - // We have read all available data and stored them into the rx buffer - // We will try to process all received data until we have found a valid frame. - // - while (pHandle->rxBufferSize > 0) - { - // - // For now, we haven't found any start of frame - // - syncFound = FALSE; - - // - // To find a valid start of frame we need at least 2 bytes in the reception buffer - // - if (pHandle->rxBufferSize >= 2) - { - // - // Try to find a valid start of frame by looking for SYNC_1 and SYNC_2 chars - // - for (i = 0; i < pHandle->rxBufferSize-1; i++) - { - // - // A valid start of frame should begin with SYNC and when STX chars - // - if ( (pHandle->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pHandle->rxBuffer[i+1] == SBG_ECOM_SYNC_2) ) - { - // - // We have found the sync char, test if we have dummy bytes at the begining of the frame - // - if (i > 0) - { - // - // Remove all dumy received bytes before the begining of the frame - // - memmove(pHandle->rxBuffer, pHandle->rxBuffer+i, pHandle->rxBufferSize-i); - pHandle->rxBufferSize = pHandle->rxBufferSize-i; - } - - // - // The sync has been found - // - syncFound = TRUE; - break; - } - } - } - - // - // Check if a valid start of frame has been found - // - if (syncFound) - { - // - // A valid start of frame has been found, try to extract the frame if we have at least a whole frame. - // - if (pHandle->rxBufferSize < 8) - { - // - // Don't have enough data for a valid frame - // - return SBG_NOT_READY; - } - - // - // Initialize an input stream buffer to parse the received frame - // - sbgStreamBufferInitForRead(&inputStream, pHandle->rxBuffer, pHandle->rxBufferSize); - - // - // Skip both the Sync 1 and Sync 2 chars - // - sbgStreamBufferSeek(&inputStream, sizeof(uint8_t)*2, SB_SEEK_CUR_INC); - - // - // Read the command - // - receivedMsg = sbgStreamBufferReadUint8LE(&inputStream); - receivedMsgClass = sbgStreamBufferReadUint8LE(&inputStream); - - // - // Read the payload size - // - payloadSize = (uint16_t)sbgStreamBufferReadUint16LE(&inputStream); - - // - // Check that the payload size is valid - // - if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) - { - // - // Check if we have received the whole frame - // - if (pHandle->rxBufferSize < payloadSize+9) - { - // - // Don't have received the whole frame - // - return SBG_NOT_READY; - } - - // - // We should have the whole frame so for now, skip the payload part but before store the current cursor - // - payloadOffset = sbgStreamBufferTell(&inputStream); - sbgStreamBufferSeek(&inputStream, payloadSize, SB_SEEK_CUR_INC); - - // - // Read the frame CRC - // - frameCrc = sbgStreamBufferReadUint16LE(&inputStream); - - // - // Read and test the frame ETX - // - if (sbgStreamBufferReadUint8(&inputStream) == SBG_ECOM_ETX) - { - // - // Go back at the beginning of the payload part - // - sbgStreamBufferSeek(&inputStream, payloadOffset, SB_SEEK_SET); - - // - // We have a frame so return the received command if needed even if the CRC is still not validated - // - if (pMsg) - { - *pMsg = receivedMsg; - } - if (pMsgClass) - { - *pMsgClass = receivedMsgClass; - } - - // - // Compute the CRC of the received frame (Skip SYNC 1 and SYNC 2 chars) - // - computedCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&inputStream)) + 2, payloadSize + 4); - - // - // Check if the received frame has a valid CRC - // - if (frameCrc == computedCrc) - { - // - // Extract the payload if needed - // - if (payloadSize > 0) - { - // - // Check if input parameters are valid - // - if ( (pData) && (pSize) ) - { - // - // Check if we have enough space to store the payload - // - if (payloadSize <= maxSize) - { - // - // Copy the payload and return the payload size - // - *pSize = payloadSize; - memcpy(pData, sbgStreamBufferGetCursor(&inputStream), payloadSize); - errorCode = SBG_NO_ERROR; - } - else - { - // - // Not enough space to store the payload, we will just drop the received data - // - errorCode = SBG_BUFFER_OVERFLOW; - } - } - else - { - errorCode = SBG_NULL_POINTER; - } - } - else - { - // - // No payload but the frame has been read successfully - // - errorCode = SBG_NO_ERROR; - } - } - else - { - // - // We have an invalid frame CRC and we will directly return this error - // - errorCode = SBG_INVALID_CRC; - } - - // - // We have read a whole valid frame so remove it from the buffer - // First, test if the reception buffer contains more than just the current frame - // - if (pHandle->rxBufferSize > payloadSize+9) - { - // - // We remove the read frame but we keep the remaining data - // - pHandle->rxBufferSize = pHandle->rxBufferSize-(payloadSize+9); - memmove(pHandle->rxBuffer, pHandle->rxBuffer+payloadSize+9, pHandle->rxBufferSize); - } - else - { - // - // We have parsed the whole received buffer so just empty it - // - pHandle->rxBufferSize = 0; - } - - // - // We have at least found a complete frame - // - return errorCode; - } - } - - // - // Frame size invalid or the found frame is invalid so we should have incorrectly detected a start of frame. - // Remove the SYNC 1 and SYNC 2 chars to retry to find a new frame - // - pHandle->rxBufferSize -= 2; - memmove(pHandle->rxBuffer, pHandle->rxBuffer+2, pHandle->rxBufferSize); - } - else - { - // - // Unable to find a valid start of frame so check if the last byte is a SYNC char in order to keep it for next time - // - if (pHandle->rxBuffer[pHandle->rxBufferSize-1] == SBG_ECOM_SYNC_1) - { - // - // Report the SYNC char and discard all other bytes in the buffer - // - pHandle->rxBuffer[0] = SBG_ECOM_SYNC_1; - pHandle->rxBufferSize = 1; - } - else - { - // - // Discard the whole buffer - // - pHandle->rxBufferSize = 0; - } - - // - // Unable to find a frame - // - return SBG_NOT_READY; - } - } - - // - // The whole buffer has been paresed and no valid frame has been found - // - return SBG_NOT_READY; -} - -//----------------------------------------------------------------------// -//- Frame generation to stream buffer -// -//----------------------------------------------------------------------// - -/*! - * Initialize an output stream for an sbgECom frame generation. - * This method is helpful to avoid memory copy compared to sbgEComProtocolSend one. - * - * \param[in] pOutputStream Pointer to an allocated and initialized output stream. - * \param[in] msgClass Message class (0-255) - * \param[in] msg Message id (0-255) - * \param[out] pStreamCursor The initial output stream cursor that thus points to the begining of the generated message. - * This value should be passed to sbgEComFinalizeFrameGeneration for correct operations. - * \return SBG_NO_ERROR in case of good operation. - */ -SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor) -{ - assert(pOutputStream); - assert(pStreamCursor); - - // - // Backup the current position in the stream buffer - // - *pStreamCursor = sbgStreamBufferTell(pOutputStream); - - // - // Write the header - // - sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_SYNC_1); - sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_SYNC_2); - - // - // Write the message ID and class - // - sbgStreamBufferWriteUint8LE(pOutputStream, msg); - sbgStreamBufferWriteUint8LE(pOutputStream, msgClass); - - // - // For now, we don't know the payload size so skip it - // - return sbgStreamBufferSeek(pOutputStream, sizeof(uint16_t), SB_SEEK_CUR_INC); -} - -/*! - * Finalize an output stream that has been initialized with sbgEComStartFrameGeneration. - * At return, the output stream buffer should point at the end of the generated message. - * You can thus easily create consecutive SBG_ECOM_LOGS with these methods. - * - * \param[in] pOutputStream Pointer to an allocated and initialized output stream. - * \param[in] streamCursor Position in the stream buffer of the generated message first byte. - * This value is returned by sbgEComStartFrameGeneration and is mandatory for correct operations. - * \return SBG_NO_ERROR in case of good operation. - */ -SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor) -{ - SbgErrorCode errorCode; - size_t payloadSize; - size_t currentPos; - uint16_t frameCrc; - - assert(pOutputStream); - - // - // Test if any error has occurred on the stream first - // - errorCode = sbgStreamBufferGetLastError(pOutputStream); - - // - // Is the stream buffer error free ? - // - if (errorCode == SBG_NO_ERROR) - { - // - // Compute the payload size (written data minus the header) - // - payloadSize = sbgStreamBufferGetLength(pOutputStream) - streamCursor - 6; - - // - // Test that the payload size is valid - // - if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) - { - // - // Backup the current cursor position - // - currentPos = sbgStreamBufferTell(pOutputStream); - - // - // Goto the payload size field (4th byte in the frame) - // - sbgStreamBufferSeek(pOutputStream, streamCursor+4, SB_SEEK_SET); - - // - // Write the payload size - // - sbgStreamBufferWriteUint16LE(pOutputStream, (uint16_t)payloadSize); - - // - // Go back to the previous position - // - sbgStreamBufferSeek(pOutputStream, currentPos, SB_SEEK_SET); - - // - // Compute the 16 bits CRC on the whole frame except Sync 1 and Sync 2 - // - frameCrc = sbgCrc16Compute((uint8_t*)sbgStreamBufferGetLinkedBuffer(pOutputStream) + streamCursor + 2, payloadSize + 4); - - // - // Append the CRC - // - sbgStreamBufferWriteUint16LE(pOutputStream, frameCrc); - - // - // Append the ETX - // - errorCode = sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_ETX); - } - else - { - // - // Invalid payload size - // - errorCode = SBG_BUFFER_OVERFLOW; - SBG_LOG_ERROR(errorCode, "Payload of %u bytes is too big for a valid sbgECom log", payloadSize); - } - } - else - { - // - // Notify error - // - SBG_LOG_ERROR(errorCode, "Unable to finalize frame because of an error on Stream Buffer"); - } - - return errorCode; -} diff --git a/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.h b/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.h deleted file mode 100644 index 78c79b1..0000000 --- a/sbg_ros_driver/external/sbgECom/src/protocol/sbgEComProtocol.h +++ /dev/null @@ -1,132 +0,0 @@ -/*! - * \file sbgEComProtocol.h - * \author SBG-Systems (Raphael Siryani) - * \date 06/02/13 - * - * \brief Implementation of the Ekinox binary communication protocol.
- * You can access low-level communication with the device.
- * Copyright 2007-20011 SBG Systems. All rights reserved. - * - * You will find below, the frame definition used by Ekinox devices.
- *
- * - * - * - * - * - *
Frame structure
Fields SYNC 1 SYNC 2 CMD LEN DATA CRC ETX
Size in bytes 1 1 2 2 (0-4086) 2 1
Value 0xFF 0x5A ? ? ? ? 0x33
- *
- * Size in bytes indicates the size of the data field.
- * The minimum frame size is 9 bytes and the maximum is 512 bytes.
- *
- * The CRC is calculated on the whole frame without:
- * SYNC STX CRC and ETX fields.
- */ -#ifndef __SBG_ECOM_PROTOCOL_H__ -#define __SBG_ECOM_PROTOCOL_H__ - -#include -#include -#include - -//----------------------------------------------------------------------// -//- Global definitions -// -//----------------------------------------------------------------------// -#define SBG_ECOM_MAX_BUFFER_SIZE (4096) /*!< Maximum reception buffer size in bytes. */ -#define SBG_ECOM_MAX_PAYLOAD_SIZE (4086) /*!< Maximum payload size in bytes. */ -#define SBG_ECOM_SYNC_1 (0xFF) /*!< First synchronization char of the frame. */ -#define SBG_ECOM_SYNC_2 (0x5A) /*!< Second synchronization char of the frame. */ -#define SBG_ECOM_ETX (0x33) /*!< End of frame byte. */ - -#define SBG_ECOM_RX_TIME_OUT (450) /*!< Default time out for new frame reception. */ - -//----------------------------------------------------------------------// -//- Communication protocol structs and definitions -// -//----------------------------------------------------------------------// - -/*! - * Struct containing all protocol related data. - */ -typedef struct _SbgEComProtocol -{ - SbgInterface *pLinkedInterface; /*!< Associated interface used by the protocol to read/write bytes. */ - uint8_t rxBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; /*!< The reception buffer. */ - size_t rxBufferSize; /*!< The current reception buffer size in bytes. */ -} SbgEComProtocol; - -//----------------------------------------------------------------------// -//- Communication protocol operations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the protocol system used to communicate with the product and return the created handle. - * \param[in] pHandle Pointer on an allocated protocol structure to initialize. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialised the protocol system. - */ -SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pHandle, SbgInterface *pInterface); - -/*! - * Close the protocol system. - * \param[in] pHandle A valid protocol handle to close. - * \return SBG_NO_ERROR if we have closed and released the protocol system. - */ -SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pHandle); - -/*! - * Send a frame to the device (size should be less than 4086 bytes). - * \param[in] pHandle A valid protocol handle. - * \param[in] msgClass Message class (0-255) - * \param[in] msg Message id (0-255) - * \param[in] pData Pointer on the data payload to send or NULL if no payload. - * \param[in] size Size in bytes of the data payload (less than 4086). - * \return SBG_NO_ERROR if the frame has been sent. - */ -SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size); - -/*! - * Try to receive a frame from the device and returns the cmd, data and size of data field. - * \param[in] pHandle A valid protocol handle. - * \param[out] pMsgClass Pointer to hold the returned message class - * \param[out] pMsg Pointer to hold the returned message id - * \param[out] pData Allocated buffer used to hold received data field. - * \param[out] pSize Pointer used to hold the received data field size. - * \param[in] maxSize Max number of bytes that can be stored in the pData buffer. - * \return SBG_NO_ERROR if we have received a valid frame.
- * SBG_NOT_READY if we haven't received a valid frame or if the serial buffer is empty.
- * SBG_INVALID_CRC if the received frame has an invalid CRC.
- * SBG_NULL_POINTER if an input parameter is NULL.
- * SBG_BUFFER_OVERFLOW if the received frame payload couldn't fit into the pData buffer. - */ -SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize); - -//----------------------------------------------------------------------// -//- Frame generation to stream buffer -// -//----------------------------------------------------------------------// - -/*! - * Initialize an output stream for an sbgECom frame generation. - * This method is helpful to avoid memory copy compared to sbgEComProtocolSend one. - * - * \param[in] pOutputStream Pointer to an allocated and initialized output stream. - * \param[in] msgClass Message class (0-255) - * \param[in] msg Message id (0-255) - * \param[out] pStreamCursor The initial output stream cursor that thus points to the begining of the generated message. - * This value should be passed to sbgEComFinalizeFrameGeneration for correct operations. - * \return SBG_NO_ERROR in case of good operation. - */ -SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor); - -/*! - * Finalize an output stream that has been initialized with sbgEComStartFrameGeneration. - * At return, the output stream buffer should point at the end of the generated message. - * You can thus easily create consecutive SBG_ECOM_LOGS with these methods. - * - * \param[in] pOutputStream Pointer to an allocated and initialized output stream. - * \param[in] streamCursor Position in the stream buffer of the generated message first byte. - * This value is returned by sbgEComStartFrameGeneration and is mandatory for correct operations. - * \return SBG_NO_ERROR in case of good operation. - */ -SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor); - -#endif diff --git a/sbg_ros_driver/external/sbgECom/src/sbgECanId.h b/sbg_ros_driver/external/sbgECom/src/sbgECanId.h deleted file mode 100644 index 3a203f0..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgECanId.h +++ /dev/null @@ -1,148 +0,0 @@ -/*! - * \file sbgECanIds.h - * \author SBG Systems (Maxime Renaudet) - * \date 10 October 2014 - * - * \brief Defines all sbgECom commands identifiers. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -/*! - * \mainpage SBG Systems Enhanced Communication library documentation - * Welcome to the sbgECom library documentation.
- * This documentation describes all functions implemented in the sbgECom library. - */ - -#ifndef __SBG_ECAN_IDS_H__ -#define __SBG_ECAN_IDS_H__ - -//----------------------------------------------------------------------// -//- Definition of all messages id for sbgECan -// -//----------------------------------------------------------------------// - -/*! - * Enum containing the list of messages that can be output on the can interface. - */ -typedef enum _SbgECanMessageId -{ - // - // Output Messages - // - SBG_ECAN_MSG_STATUS_01 = 0x100, - SBG_ECAN_MSG_STATUS_02 = 0x101, - SBG_ECAN_MSG_STATUS_03 = 0x102, - - SBG_ECAN_MSG_UTC_0 = 0x110, - SBG_ECAN_MSG_UTC_1 = 0x111, - - SBG_ECAN_MSG_IMU_INFO = 0x120, - SBG_ECAN_MSG_IMU_ACCEL = 0x121, - SBG_ECAN_MSG_IMU_GYRO = 0x122, - SBG_ECAN_MSG_IMU_DELTA_VEL = 0x123, - SBG_ECAN_MSG_IMU_DELTA_ANGLE = 0x124, - - SBG_ECAN_MSG_EKF_INFO = 0x130, - SBG_ECAN_MSG_EKF_QUAT = 0x131, - SBG_ECAN_MSG_EKF_EULER = 0x132, - SBG_ECAN_MSG_EKF_ORIENTATION_ACC = 0x133, - SBG_ECAN_MSG_EKF_POS = 0x134, - SBG_ECAN_MSG_EKF_ALTITUDE = 0x135, - SBG_ECAN_MSG_EKF_POS_ACC = 0x136, - SBG_ECAN_MSG_EKF_VEL_NED = 0x137, - SBG_ECAN_MSG_EKF_VEL_NED_ACC = 0x138, - SBG_ECAN_MSG_EKF_VEL_BODY = 0x139, - - SBG_ECAN_MSG_SHIP_MOTION_INFO = 0x140, - SBG_ECAN_MSG_SHIP_MOTION_0 = 0x141, - SBG_ECAN_MSG_SHIP_MOTION_1 = 0x145, - SBG_ECAN_MSG_SHIP_MOTION_2 = 0x149, - - SBG_ECAN_MSG_SHIP_MOTION_HP_INFO = 0x14A, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_SHIP_MOTION_HP_0 = 0x14B, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_SHIP_MOTION_HP_1 = 0x14C, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_SHIP_MOTION_HP_2 = 0x14D, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - - SBG_ECAN_MSG_MAG_0 = 0x150, - SBG_ECAN_MSG_MAG_1 = 0x151, - SBG_ECAN_MSG_MAG_2 = 0x152, - - SBG_ECAN_MSG_ODO_INFO = 0x160, - SBG_ECAN_MSG_ODO_VEL = 0x161, - - SBG_ECAN_MSG_AIR_DATA_INFO = 0x162, - SBG_ECAN_MSG_AIR_DATA_ALTITUDE = 0x163, - SBG_ECAN_MSG_AIR_DATA_AIRSPEED = 0x164, - - SBG_ECAN_MSG_DEPTH_INFO = 0x166, - SBG_ECAN_MSG_DEPTH_ALTITUDE = 0x167, - - SBG_ECAN_MSG_GPS1_VEL_INFO = 0x170, - SBG_ECAN_MSG_GPS1_VEL = 0x171, - SBG_ECAN_MSG_GPS1_VEL_ACC = 0x172, - SBG_ECAN_MSG_GPS1_VEL_COURSE = 0x173, - SBG_ECAN_MSG_GPS1_POS_INFO = 0x174, - SBG_ECAN_MSG_GPS1_POS = 0x175, - SBG_ECAN_MSG_GPS1_POS_ALT = 0x176, - SBG_ECAN_MSG_GPS1_POS_ACC = 0x177, - SBG_ECAN_MSG_GPS1_HDT_INFO = 0x178, - SBG_ECAN_MSG_GPS1_HDT = 0x179, - - SBG_ECAN_MSG_GPS2_VEL_INFO = 0x180, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_VEL = 0x181, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_VEL_ACC = 0x182, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_VEL_COURSE = 0x183, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_POS_INFO = 0x184, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_POS = 0x185, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_POS_ALT = 0x186, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_POS_ACC = 0x187, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_HDT_INFO = 0x188, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_GPS2_HDT = 0x189, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - - SBG_ECAN_MSG_EVENT_INFO_A = 0x200, - SBG_ECAN_MSG_EVENT_TIME_A = 0x201, - SBG_ECAN_MSG_EVENT_INFO_B = 0x202, - SBG_ECAN_MSG_EVENT_TIME_B = 0x203, - SBG_ECAN_MSG_EVENT_INFO_C = 0x204, - SBG_ECAN_MSG_EVENT_TIME_C = 0x205, - SBG_ECAN_MSG_EVENT_INFO_D = 0x206, - SBG_ECAN_MSG_EVENT_TIME_D = 0x207, - SBG_ECAN_MSG_EVENT_INFO_E = 0x208, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - SBG_ECAN_MSG_EVENT_TIME_E = 0x209, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ - - // - // Proprietary CASS logs - // - SBG_ECAN_MSG_CASS_DATINF = 0x210, - SBG_ECAN_MSG_CASS_ACCS = 0x211, - SBG_ECAN_MSG_CASS_OMGS = 0x212, - SBG_ECAN_MSG_CASS_NRPY = 0x213, - SBG_ECAN_MSG_CASS_VEL = 0x214, - SBG_ECAN_MSG_CASS_TIME = 0x215, - SBG_ECAN_MSG_CASS_GPS_INF = 0x216, - SBG_ECAN_MSG_CASS_GPS_COG = 0x217, - SBG_ECAN_MSG_CASS_ADDINF = 0x218, - SBG_ECAN_MSG_CASS_POS1 = 0x219, - SBG_ECAN_MSG_CASS_POS2 = 0x21A, - SBG_ECAN_MSG_CASS_SAT_INF = 0x21B, - SBG_ECAN_MSG_CASS_IACCS = 0x21C, - SBG_ECAN_MSG_CASS_IOMG = 0x21D, - SBG_ECAN_MSG_CASS_RR = 0x21E, - - // - // Automotive specific CAN output - // - SBG_ECAN_MSG_TRACK_SLIP_CURVATURE = 0x220 -} SbgECanMessageId; - -#endif /* __SBG_ECOM_CMDS_H__ */ diff --git a/sbg_ros_driver/external/sbgECom/src/sbgECom.c b/sbg_ros_driver/external/sbgECom/src/sbgECom.c deleted file mode 100644 index b006063..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgECom.c +++ /dev/null @@ -1,319 +0,0 @@ -#include "sbgECom.h" -#include -#include "commands/sbgEComCmdCommon.h" - -//----------------------------------------------------------------------// -//- Private methods declarations -// -//----------------------------------------------------------------------// - -//----------------------------------------------------------------------// -//- Public methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the protocol system used to communicate with the product and return the created handle. - * \param[out] pHandle Pointer used to store the allocated and initialized sbgECom handle. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialised the protocol system. - */ -SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - // - // Check input parameters - // - if ( (pInterface) && (pHandle) ) - { - // - // Initialize the sbgECom handle - // - pHandle->pReceiveLogCallback = NULL; - pHandle->pUserArg = NULL; - - // - // Initialize the default number of trials and time out - // - pHandle->numTrials = 3; - pHandle->cmdDefaultTimeOut = SBG_ECOM_DEFAULT_CMD_TIME_OUT; - - // - // Initialize the protocol - // - errorCode = sbgEComProtocolInit(&pHandle->protocolHandle, pInterface); - } - else - { - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Close the protocol system and release associated memory. - * \param[in] pHandle A valid sbgECom handle to close. - * \return SBG_NO_ERROR if we have closed and released the sbgECom system. - */ -SbgErrorCode sbgEComClose(SbgEComHandle *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - // - // Test that we have a valid protocol handle - // - if (pHandle) - { - // - // Close the protocol - // - errorCode = sbgEComProtocolClose(&pHandle->protocolHandle); - } - else - { - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Try to parse one log from the input interface and then return. - * \param[in] pHandle A valid sbgECom handle. - * \return SBG_NO_ERROR if no error occurs during incoming log parsing. - */ -SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - SbgBinaryLogData logData; - uint8_t receivedMsg; - uint8_t receivedMsgClass; - size_t payloadSize; - uint8_t payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE]; - - // - // Check input arguments - // - assert(pHandle); - - // - // Try to read a received frame - // - errorCode = sbgEComProtocolReceive(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsg, payloadData, &payloadSize, sizeof(payloadData)); - - // - // Test if we have received a valid frame - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test if the received frame is a binary log - // - if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) - { - // - // The received frame is a binary log one - // - errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg, payloadData, payloadSize, &logData); - - // - // Test if the incoming log has been parsed successfully - // - if (errorCode == SBG_NO_ERROR) - { - // - // Test if we have a valid callback to handle received logs - // - if (pHandle->pReceiveLogCallback) - { - // - // Call the binary log callback using the new method - // - errorCode = pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsg, &logData, pHandle->pUserArg); - } - } - else - { - // - // Call the on error callback - // - } - } - else - { - // - // We have received a command, it shouldn't happen - // - } - } - else if (errorCode != SBG_NOT_READY) - { - // - // We have received an invalid frame - // - SBG_LOG_WARNING(errorCode, "Invalid frame received"); - } - - return errorCode; -} - -/*! - * Handle all incoming logs until no more log are available in the input interface. - * \param[in] pHandle A valid sbgECom handle. - * \return SBG_NO_ERROR if no error occurs during incoming logs parsing. - */ -SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - // - // Check input arguments - // - assert(pHandle); - - // - // Try to read all received frames, we thus loop until we get an SBG_NOT_READY error - // - do - { - // - // Try to read and parse one frame - // - errorCode = sbgEComHandleOneLog(pHandle); - } while (errorCode != SBG_NOT_READY); - - return errorCode; -} - -/*! - * Define the callback that should be called each time a new binary log is received. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pReceiveLogCallback Pointer on the callback to call when a new log is received. - * \param[in] pUserArg Optional user argument that will be passed to the callback method. - * \return SBG_NO_ERROR if the callback and user argument have been defined successfully. - */ -SbgErrorCode sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg) -{ - SbgErrorCode errorCode = SBG_NO_ERROR; - - // - // Test that we have a valid protocol handle - // - if (pHandle) - { - // - // Define the callback and the user argument - // - pHandle->pReceiveLogCallback = pReceiveLogCallback; - pHandle->pUserArg = pUserArg; - } - else - { - errorCode = SBG_NULL_POINTER; - } - - return errorCode; -} - -/*! - * Define the default number of trials that should be done when a command is send to the device as well as the time out. - * \param[in] pHandle A valid sbgECom handle. - * \parma[in] numTrials Number of trials when a command is sent (starting at 1). - * \param[in] cmdDefaultTimeOut Default time out in milliseconds to wait to receive an answer from the device. - */ -void sbgEComSetCmdTrialsAndTimeOut(SbgEComHandle *pHandle, uint32_t numTrials, uint32_t cmdDefaultTimeOut) -{ - // - // Check input arguments - // - assert(pHandle); - assert(numTrials > 0); - assert(cmdDefaultTimeOut > 0); - - // - // Define the new settings - // - pHandle->numTrials = numTrials; - pHandle->cmdDefaultTimeOut = cmdDefaultTimeOut; -} - -/*! - * Convert an error code into a human readable string. - * \param[in] errorCode The errorCode to convert into a string. - * \param[out] errorMsg String buffer used to hold the error string. - */ -void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]) -{ - if (errorMsg) - { - // - // For each error code, copy the error msg - // - switch (errorCode) - { - case SBG_NO_ERROR: - strcpy(errorMsg, "SBG_NO_ERROR: No error."); - break; - case SBG_ERROR: - strcpy(errorMsg, "SBG_ERROR: Generic error."); - break; - case SBG_NULL_POINTER: - strcpy(errorMsg, "SBG_NULL_POINTER: A pointer is null."); - break; - case SBG_INVALID_CRC: - strcpy(errorMsg, "SBG_INVALID_CRC: The received frame has an invalid CRC."); - break; - case SBG_INVALID_FRAME: - strcpy(errorMsg, "SBG_INVALID_FRAME: The received frame is invalid."); - break; - case SBG_TIME_OUT: - strcpy(errorMsg, "SBG_TIME_OUT: We have a time out during frame reception."); - break; - case SBG_WRITE_ERROR: - strcpy(errorMsg, "SBG_WRITE_ERROR: All bytes hasn't been written."); - break; - case SBG_READ_ERROR: - strcpy(errorMsg, "SBG_READ_ERROR: All bytes hasn't been read."); - break; - case SBG_BUFFER_OVERFLOW: - strcpy(errorMsg, "SBG_BUFFER_OVERFLOW: A buffer is too small to contain so much data."); - break; - case SBG_INVALID_PARAMETER: - strcpy(errorMsg, "SBG_INVALID_PARAMETER: An invalid parameter has been founded."); - break; - case SBG_NOT_READY: - strcpy(errorMsg, "SBG_NOT_READY: A device isn't ready (Rx isn't ready for example)."); - break; - case SBG_MALLOC_FAILED: - strcpy(errorMsg, "SBG_MALLOC_FAILED: Failed to allocate a buffer."); - break; - case SGB_CALIB_MAG_NOT_ENOUGH_POINTS: - strcpy(errorMsg, "SGB_CALIB_MAG_NOT_ENOUGH_POINTS: Not enough points were available to perform magnetometers calibration."); - break; - case SBG_CALIB_MAG_INVALID_TAKE: - strcpy(errorMsg, "SBG_CALIB_MAG_INVALID_TAKE: The calibration procedure could not be properly executed due to insufficient precision."); - break; - case SBG_CALIB_MAG_SATURATION: - strcpy(errorMsg, "SBG_CALIB_MAG_SATURATION: Saturation were detected when attempt to calibrate magnetos."); - break; - case SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE: - strcpy(errorMsg, "SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE: 2D calibration procedure could not be performed."); - break; - case SBG_DEVICE_NOT_FOUND: - strcpy(errorMsg, "SBG_DEVICE_NOT_FOUND: A device couldn't be founded or opened."); - break; - case SBG_OPERATION_CANCELLED: - strcpy(errorMsg, "SBG_OPERATION_CANCELLED: An operation has been cancelled by a user."); - break; - case SBG_NOT_CONTINUOUS_FRAME: - strcpy(errorMsg, "SBG_NOT_CONTINUOUS_FRAME: We have received a frame that isn't a continuous one."); - break; - case SBG_INCOMPATIBLE_HARDWARE: - strcpy(errorMsg, "SBG_INCOMPATIBLE_HARDWARE: Hence valid, the configuration cannot be executed because of incompatible hardware."); - break; - default: - sprintf(errorMsg, "Undefined error code: %u", errorCode); - break; - } - } -} diff --git a/sbg_ros_driver/external/sbgECom/src/sbgECom.h b/sbg_ros_driver/external/sbgECom/src/sbgECom.h deleted file mode 100644 index aaaf4bf..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgECom.h +++ /dev/null @@ -1,146 +0,0 @@ -/*! - * \file sbgECom.h - * \author SBG Systems (Raphael Siryani) - * \date 05 February 2013 - * - * \brief Contains main sbgECom methods. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -/*! - * \mainpage SBG Systems Enhanced Communication library documentation - * Welcome to the sbgECom library documentation.
- * This documentation describes all functions implemented in the sbgECom library. - */ - -#ifndef __SBG_ECOM_H__ -#define __SBG_ECOM_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "sbgEComIds.h" -#include "protocol/sbgEComProtocol.h" -#include "binaryLogs/sbgEComBinaryLogs.h" - -//----------------------------------------------------------------------// -//- Predefinitions -// -//----------------------------------------------------------------------// - -/*! - * Interface structure pre-definition. - */ -typedef struct _SbgEComHandle SbgEComHandle; - -//----------------------------------------------------------------------// -//- Callbacks definitions -// -//----------------------------------------------------------------------// - -/*! - * Callback definition called each time a new log is received. - * \param[in] pHandle Valid handle on the sbgECom instance that has called this callback. - * \param[in] msgClass Class of the message we have received - * \param[in] msg Message ID of the log received. - * \param[in] pLogData Contains the received log data as an union. - * \param[in] pUserArg Optional user supplied argument. - * \return SBG_NO_ERROR if the received log has been used successfully. - */ -typedef SbgErrorCode (*SbgEComReceiveLogFunc)(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg); - -//----------------------------------------------------------------------// -//- Structures definitions -// -//----------------------------------------------------------------------// - -/*! - * Interface definition that stores methods used to communicate on the interface. - */ -struct _SbgEComHandle -{ - SbgEComProtocol protocolHandle; /*!< Handle on the protocol system. */ - - SbgEComReceiveLogFunc pReceiveLogCallback; /*!< Pointer on the method called each time a new binary log is received. */ - void *pUserArg; /*!< Optional user supplied argument for callbacks. */ - - uint32_t numTrials; /*!< Number of trials when a command is sent (default is 3). */ - uint32_t cmdDefaultTimeOut; /*!< Default time out in ms to get an answer from the device (default 500 ms). */ -}; - -//----------------------------------------------------------------------// -//- Public methods declarations -// -//----------------------------------------------------------------------// - -/*! - * Initialize the protocol system used to communicate with the product and return the created handle. - * \param[out] pHandle Pointer used to store the allocated and initialized sbgECom handle. - * \param[in] pInterface Interface to use for read/write operations. - * \return SBG_NO_ERROR if we have initialized the protocol system. - */ -SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface); - -/*! - * Close the protocol system and release associated memory. - * \param[in] pHandle A valid sbgECom handle to close. - * \return SBG_NO_ERROR if we have closed and released the sbgECom system. - */ -SbgErrorCode sbgEComClose(SbgEComHandle *pHandle); - -/*! - * Try to parse one log from the input interface and then return. - * \param[in] pHandle A valid sbgECom handle. - * \return SBG_NO_ERROR if no error occurs during incoming log parsing. - */ -SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle); - -/*! - * Handle all incoming logs until no more log are available in the input interface. - * \param[in] pHandle A valid sbgECom handle. - * \return SBG_NO_ERROR if no error occurs during incoming logs parsing. - */ -SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle); - -/*! - * Define the callback that should be called each time a new binary log is received. - * \param[in] pHandle A valid sbgECom handle. - * \param[in] pReceiveLogCallback Pointer on the callback to call when a new log is received. - * \param[in] pUserArg Optional user argument that will be passed to the callback method. - * \return SBG_NO_ERROR if the callback and user argument have been defined successfully. - */ -SbgErrorCode sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg); - -/*! - * Define the default number of trials that should be done when a command is send to the device as well as the time out. - * \param[in] pHandle A valid sbgECom handle. - * \parma[in] numTrials Number of trials when a command is sent (starting at 1). - * \param[in] cmdDefaultTimeOut Default time out in milliseconds to wait to receive an answer from the device. - */ -void sbgEComSetCmdTrialsAndTimeOut(SbgEComHandle *pHandle, uint32_t numTrials, uint32_t cmdDefaultTimeOut); - -/*! - * Convert an error code into a human readable string. - * \param[in] errorCode The errorCode to convert into a string. - * \param[out] errorMsg String buffer used to hold the error string. - */ -void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]); - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* __SBG_ECOM_H__ */ - diff --git a/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.c b/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.c deleted file mode 100644 index b1ee1b7..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.c +++ /dev/null @@ -1,23 +0,0 @@ -// sbgCommonLib headers -#include -#include - -// Project headers -#include "sbgEComVersion.h" - -// Local headers -#include "sbgEComGetVersion.h" - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -uint32_t sbgEComGetVersion(void) -{ - return SBG_E_COM_VERSION; -} - -const char *sbgEComGetVersionAsString(void) -{ - return SBG_E_COM_VERSION_STR; -} diff --git a/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.h b/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.h deleted file mode 100644 index 2000898..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgEComGetVersion.h +++ /dev/null @@ -1,45 +0,0 @@ -/*! - * \file sbgEComGetVersion.h - * \author SBG Systems - * - * \brief Version information. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2019, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -#ifndef SBG_ECOM_GET_VERSION_H -#define SBG_ECOM_GET_VERSION_H - -// sbgCommonLib headers -#include - -//----------------------------------------------------------------------// -//- Public functions -// -//----------------------------------------------------------------------// - -/*! - * Returns an integer representing the version of the sbgCom library. - * - * \return An integer representing the version of the sbgCom library.
- * Use #SBG_VERSION_GET_MAJOR, #SBG_VERSION_GET_MINOR, #SBG_VERSION_GET_REV and #SBG_VERSION_GET_BUILD. - */ -uint32_t sbgEComGetVersion(void); - -/*! - * Retrieve the sbgCom library version as a string (1.0.0.0). - * - * \return Null terminated string that contains the sbgCom library version. - */ -const char *sbgEComGetVersionAsString(void); - -#endif //SBG_ECOM_GET_VERSION_H diff --git a/sbg_ros_driver/external/sbgECom/src/sbgEComIds.h b/sbg_ros_driver/external/sbgECom/src/sbgEComIds.h deleted file mode 100644 index fe52e65..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgEComIds.h +++ /dev/null @@ -1,316 +0,0 @@ -/*! - * \file sbgEComIds.h - * \author SBG Systems (Raphael Siryani) - * \date 25 February 2013 - * - * \brief Defines all sbgECom commands identifiers. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -/*! - * \mainpage SBG Systems Enhanced Communication library documentation - * Welcome to the sbgECom library documentation.
- * This documentation describes all functions implemented in the sbgECom library. - */ - -#ifndef __SBG_ECOM_IDS_H__ -#define __SBG_ECOM_IDS_H__ - -//----------------------------------------------------------------------// -//- Macro definitions -// -//----------------------------------------------------------------------// - -/*! - * Helper macro to build an id with its class - */ -#define SBG_ECOM_BUILD_ID(classId, logId) (((uint16_t)classId << 8) | (uint8_t)logId) - -//----------------------------------------------------------------------// -//- Definition of all class id for sbgECom -// -//----------------------------------------------------------------------// - -/*! - * Enum that defines all the message classes available. - */ -typedef enum _SbgEComClass -{ - SBG_ECOM_CLASS_LOG_ECOM_0 = 0x00, /*!< Class that contains sbgECom protocol input/output log messages. */ - - SBG_ECOM_CLASS_LOG_ECOM_1 = 0x01, /*!< Class that contains special sbgECom output messages that handle high frequency output. */ - - SBG_ECOM_CLASS_LOG_NMEA_0 = 0x02, /*!< Class that contains NMEA (and NMEA like) output logs.
- Note: This class is only used for identification purpose and does not contain any sbgECom message. */ - SBG_ECOM_CLASS_LOG_NMEA_1 = 0x03, /*!< Class that contains proprietary NMEA (and NMEA like) output logs.
- Note: This class is only used for identification purpose and does not contain any sbgECom message. */ - SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 = 0x04, /*!< Class that contains third party output logs. - Note: This class is only used for identification purpose and does not contain any sbgECom message. */ - - SBG_ECOM_CLASS_LOG_CMD_0 = 0x10, /*!< Class that contains sbgECom protocol commands. */ -} SbgEComClass; - -//----------------------------------------------------------------------// -//- Definition of all messages id for sbgECom -// -//----------------------------------------------------------------------// - -/*! - * Enum that defines all the available ECom output logs from the sbgECom library. - */ -typedef enum _SbgEComLog -{ - SBG_ECOM_LOG_STATUS = 1, /*!< Status general, clock, com aiding, solution, heave */ - - SBG_ECOM_LOG_UTC_TIME = 2, /*!< Provides UTC time reference */ - - SBG_ECOM_LOG_IMU_DATA = 3, /*!< Includes IMU status, acc., gyro, temp delta speeds and delta angles values */ - - SBG_ECOM_LOG_MAG = 4, /*!< Magnetic data with associated accelerometer on each axis */ - SBG_ECOM_LOG_MAG_CALIB = 5, /*!< Magnetometer calibration data (raw buffer) */ - - SBG_ECOM_LOG_EKF_EULER = 6, /*!< Includes roll, pitch, yaw and their accuracies on each axis */ - SBG_ECOM_LOG_EKF_QUAT = 7, /*!< Includes the 4 quaternions values */ - SBG_ECOM_LOG_EKF_NAV = 8, /*!< Position and velocities in NED coordinates with the accuracies on each axis */ - - SBG_ECOM_LOG_SHIP_MOTION = 9, /*!< Heave, surge and sway and accelerations on each axis. */ - - SBG_ECOM_LOG_GPS1_VEL = 13, /*!< GPS velocities from primary or secondary GPS receiver */ - SBG_ECOM_LOG_GPS1_POS = 14, /*!< GPS positions from primary or secondary GPS receiver */ - SBG_ECOM_LOG_GPS1_HDT = 15, /*!< GPS true heading from dual antenna system */ - SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ - - SBG_ECOM_LOG_GPS2_VEL = 16, /*!< GPS 2 velocity log data. */ - SBG_ECOM_LOG_GPS2_POS = 17, /*!< GPS 2 position log data. */ - SBG_ECOM_LOG_GPS2_HDT = 18, /*!< GPS 2 true heading log data. */ - SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ - - SBG_ECOM_LOG_ODO_VEL = 19, /*!< Provides odometer velocity */ - - SBG_ECOM_LOG_EVENT_A = 24, /*!< Event markers sent when events are detected on sync in A pin */ - SBG_ECOM_LOG_EVENT_B = 25, /*!< Event markers sent when events are detected on sync in B pin */ - SBG_ECOM_LOG_EVENT_C = 26, /*!< Event markers sent when events are detected on sync in C pin */ - SBG_ECOM_LOG_EVENT_D = 27, /*!< Event markers sent when events are detected on sync in D pin */ - SBG_ECOM_LOG_EVENT_E = 28, /*!< Event markers sent when events are detected on sync in E pin */ - - SBG_ECOM_LOG_DVL_BOTTOM_TRACK = 29, /*!< Doppler Velocity Log for bottom tracking data. */ - SBG_ECOM_LOG_DVL_WATER_TRACK = 30, /*!< Doppler Velocity log for water layer data. */ - - SBG_ECOM_LOG_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */ - - SBG_ECOM_LOG_AIR_DATA = 36, /*!< Air Data aiding such as barometric altimeter and true air speed. */ - - SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for subsea navigation. */ - - SBG_ECOM_LOG_DEBUG_0 = 39, /*!< Debug Log. */ - SBG_ECOM_LOG_IMU_RAW_DATA = 40, /*!< Factory only log. */ - SBG_ECOM_LOG_DEBUG_1 = 41, /*!< Debug Log. */ - SBG_ECOM_LOG_DEBUG_2 = 42, /*!< Debug Log. */ - SBG_ECOM_LOG_DEBUG_3 = 43, /*!< Debug Log. */ - - SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Short IMU message recommended for post processing usages. */ - - SBG_ECOM_LOG_EVENT_OUT_A = 45, /*!< Event marker used to time stamp each generated Sync Out A signal. */ - SBG_ECOM_LOG_EVENT_OUT_B = 46, /*!< Event marker used to time stamp each generated Sync Out B signal. */ - - SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for subsea navigation. */ - SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */ - - SBG_ECOM_LOG_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ -} SbgEComLog; - -/*! - * Enum that defines all the available ECom output logs in the class SBG_ECOM_CLASS_LOG_ECOM_1 - */ -typedef enum _SbgEComLog1MsgId -{ - SBG_ECOM_LOG_FAST_IMU_DATA = 0, /*!< Provides accelerometers, gyroscopes, time and status at 1KHz rate. */ - SBG_ECOM_LOG_ECOM_1_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ -} SbgEComLog1; - -/*! - * Enum that defines all the available Nmea output logs from the sbgECom library. - */ -typedef enum _SbgEComNmeaLog -{ - SBG_ECOM_LOG_NMEA_GGA = 0, /*!< Latitude, Longitude, Altitude, Quality indicator. */ - SBG_ECOM_LOG_NMEA_RMC = 1, /*!< Latitude, Longitude, velocity, course over ground. */ - SBG_ECOM_LOG_NMEA_ZDA = 2, /*!< UTC Time. */ - SBG_ECOM_LOG_NMEA_HDT = 3, /*!< Heading (True). */ - SBG_ECOM_LOG_NMEA_GST = 4, /*!< GPS Pseudorange Noise Statistics. */ - SBG_ECOM_LOG_NMEA_VBW = 5, /*!< Water referenced and ground referenced speed data. */ - SBG_ECOM_LOG_NMEA_DPT = 7, /*!< Depth sensor output. */ - SBG_ECOM_LOG_NMEA_VTG = 8, /*!< Track an Speed over the ground. */ - SBG_ECOM_LOG_NMEA_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ -} SbgEComNmeaLog; - -/*! - * Enum that defines all the available Proprietary Nmea output logs from the sbgECom library. - */ -typedef enum _SbgEComIdNmea1Log -{ - SBG_ECOM_LOG_NMEA_1_PRDID = 0, /*!< RDI proprietary sentence. Pitch, Roll, Heading */ - SBG_ECOM_LOG_NMEA_1_PSBGI = 1, /*!< SBG Systems proprietary sentence. Rotation rates, accelerations. */ - SBG_ECOM_LOG_NMEA_1_PASHR = 2, /*!< Proprietary sentence. Roll, Pitch, Heading, Heave. */ - SBG_ECOM_LOG_NMEA_1_PSBGB = 3, /*!< SBG Systems proprietary sentence. Attitude, heading, heave, angular rates, velocity. */ - - SBG_ECOM_LOG_NMEA_1_PHINF = 5, /*!< Ixblue NMEA like log used to output Status information. */ - SBG_ECOM_LOG_NMEA_1_PHTRO = 6, /*!< Ixblue NMEA like log used to output Roll and Pitch. */ - SBG_ECOM_LOG_NMEA_1_PHLIN = 7, /*!< Ixblue NMEA like log used to output Surge, Sway and Heave. */ - SBG_ECOM_LOG_NMEA_1_PHOCT = 8, /*!< Ixblue NMEA like log used to output attitude and ship motion. */ - SBG_ECOM_LOG_NMEA_1_INDYN = 9, /*!< Ixblue NMEA like log used to output position, heading, attitude, attitude rate and speed. */ - - SBG_ECOM_LOG_NMEA_1_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ -} SbgEComIdNmea1Log; - -/*! - * Enum that defines all the available Proprietary output logs from the sbgECom library. - */ -typedef enum _SbgEComIdThirdParty -{ - SBG_ECOM_THIRD_PARTY_TSS1 = 0, /*!< Roll, Pitch, Heave, heave accelerations */ - SBG_ECOM_THIRD_PARTY_KVH = 1, /*!< Roll, Pitch, Yaw */ - - SBG_ECOM_THIRD_PARTY_PD0 = 2, /*!< Teledyne PD0 DVL proprietary frame. */ - SBG_ECOM_THIRD_PARTY_SIMRAD_1000 = 3, /*!< Konsberg SimRad 1000 proprietary frame that outputs Roll, Pitch and Heading. */ - SBG_ECOM_THIRD_PARTY_SIMRAD_3000 = 4, /*!< Konsberg SimRad 3000 proprietary frame that outputs Roll, Pitch and Heading. */ - - SBG_ECOM_THIRD_PARTY_SEAPATH_B26 = 5, /*!< Konsberg Seapth Binary Log 26 used for MBES FM mode. */ - SBG_ECOM_THIRD_PARTY_DOLOG_HRP = 6, /*!< DOLOG Heading, Roll, Pitch proprietary and binary message. */ - SBG_ECOM_THIRD_PARTY_AHRS_500 = 7, /*!< Crossbow AHRS-500 Data Packet output with attitude, rate, acceleration and status. */ - - SBG_ECOM_LOG_THIRD_PARTY_NUM_MESSAGES /*!< Helper definition to know the number of third party messages */ -} SbgEComIdThirdParty; - -/*! - * Enum that defines all the available commands for the sbgECom library. - */ -typedef enum _SbgEComCmd -{ - /* Acknowledge */ - SBG_ECOM_CMD_ACK = 0, /*!< Acknowledge */ - - /* Special settings commands */ - SBG_ECOM_CMD_SETTINGS_ACTION = 1, /*!< Performs various settings actions */ - SBG_ECOM_CMD_IMPORT_SETTINGS = 2, /*!< Imports a new settings structure to the sensor */ - SBG_ECOM_CMD_EXPORT_SETTINGS = 3, /*!< Export the whole configuration from the sensor */ - - /* Device info */ - SBG_ECOM_CMD_INFO = 4, /*!< Get basic device information */ - - /* Sensor parameters */ - SBG_ECOM_CMD_INIT_PARAMETERS = 5, /*!< Initial configuration */ - SBG_ECOM_CMD_MOTION_PROFILE_ID = 7, /*!< Set/get motion profile information */ - SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM = 8, /*!< Sensor alignment and lever arm on vehicle configuration */ - SBG_ECOM_CMD_AIDING_ASSIGNMENT = 9, /*!< Aiding assignments such as RTCM / GPS / Odometer configuration */ - - /* Magnetometer configuration */ - SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID = 11, /*!< Set/get magnetometer error model information */ - SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE = 12, /*!< Magnetometer aiding rejection mode */ - SBG_ECOM_CMD_SET_MAG_CALIB = 13, /*!< Set magnetic soft and hard Iron calibration data */ - - /* Magnetometer on-board calibration */ - SBG_ECOM_CMD_START_MAG_CALIB = 14, /*!< Start / reset internal magnetic field logging for calibration. */ - SBG_ECOM_CMD_COMPUTE_MAG_CALIB = 15, /*!< Compute a magnetic calibration based on previously logged data. */ - - /* GNSS configuration */ - SBG_ECOM_CMD_GNSS_1_MODEL_ID = 17, /*!< Set/get GNSS model information */ - SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT = 18, /*!< DEPRECATED: GNSS installation configuration (lever arm, antenna alignments) */ - SBG_ECOM_CMD_GNSS_1_INSTALLATION = 46, /*!< Define or retrieve the GNSS 1 main and secondary lever arms configuration. */ - SBG_ECOM_CMD_GNSS_1_REJECT_MODES = 19, /*!< GNSS aiding rejection modes configuration. */ - - /* Odometer configuration */ - SBG_ECOM_CMD_ODO_CONF = 20, /*!< Odometer gain, direction configuration */ - SBG_ECOM_CMD_ODO_LEVER_ARM = 21, /*!< Odometer installation configuration (lever arm) */ - SBG_ECOM_CMD_ODO_REJECT_MODE = 22, /*!< Odometer aiding rejection mode configuration. */ - - /* Interfaces configuration */ - SBG_ECOM_CMD_UART_CONF = 23, /*!< UART interfaces configuration */ - SBG_ECOM_CMD_CAN_BUS_CONF = 24, /*!< CAN bus interface configuration */ - SBG_ECOM_CMD_CAN_OUTPUT_CONF = 25, /*!< CAN identifiers configuration */ - - /* Events configuration */ - SBG_ECOM_CMD_SYNC_IN_CONF = 26, /*!< Synchronization inputs configuration */ - SBG_ECOM_CMD_SYNC_OUT_CONF = 27, /*!< Synchronization outputs configuration */ - - /* Output configuration */ - SBG_ECOM_CMD_NMEA_TALKER_ID = 29, /*!< NMEA talker ID configuration */ - SBG_ECOM_CMD_OUTPUT_CONF = 30, /*!< Output configuration */ - SBG_ECOM_CMD_LEGACY_CONT_OUTPUT_CONF = 31, /*!< Legacy serial output mode configuration */ - - /* Advanced configuration */ - SBG_ECOM_CMD_ADVANCED_CONF = 32, /*!< Advanced settings configuration */ - - /* Features related commands */ - SBG_ECOM_CMD_FEATURES = 33, /*!< Retrieve device features */ - - /* Licenses related commands */ - SBG_ECOM_CMD_LICENSE_APPLY = 34, /*!< Upload and apply a new license */ - - /* Message class output switch */ - SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE = 35, /*!< Enable/disable the output of an entire class */ - - /* Ethernet configuration */ - SBG_ECOM_CMD_ETHERNET_CONF = 36, /*!< Set/get Ethernet configuration such as DHCP mode and IP address. */ - SBG_ECOM_CMD_ETHERNET_INFO = 37, /*!< Return the current IP used by the device. */ - - /* Validity thresholds */ - SBG_ECOM_CMD_VALIDITY_THRESHOLDS = 38, /*!< Set/get Validity flag thresholds for position, velocity, attitude and heading */ - - /* DVL configuration */ - SBG_ECOM_CMD_DVL_MODEL_ID = 39, /*!< Set/get DVL model id to use */ - SBG_ECOM_CMD_DVL_INSTALLATION = 40, /*!< DVL installation configuration (lever arm, alignments) */ - SBG_ECOM_CMD_DVL_REJECT_MODES = 41, /*!< DVL aiding rejection modes configuration. */ - - /* AirData configuration */ - SBG_ECOM_CMD_AIRDATA_MODEL_ID = 42, /*!< Set/get AirData model id and protocol to use. */ - SBG_ECOM_CMD_AIRDATA_LEVER_ARM = 43, /*!< AirData installation configuration (lever arm, offsets) */ - SBG_ECOM_CMD_AIRDATA_REJECT_MODES = 44, /*!< AirData aiding rejection modes configuration. */ - - /* Odometer configuration (using CAN) */ - SBG_ECOM_CMD_ODO_CAN_CONF = 45, /*!< Configuration for CAN based odometer (CAN ID & DBC) */ - - /* Misc. */ - SBG_ECOM_LOG_ECOM_NUM_CMDS /*!< Helper definition to know the number of commands */ -} SbgEComCmd; - -/*! - * This type defines any message identifier. - * Because message identifiers enum will be different with each class id, we use a generic uint8_t rather than an enum. - */ -typedef uint8_t SbgEComMsgId; - -//----------------------------------------------------------------------// -//- Inline helpers for log IDs -// -//----------------------------------------------------------------------// - -/*! - * Test if the message class is a binary log one. - * - * \param[in] msgClass Message class. - * \return TRUE if the message class corresponds to a binary log. - */ -SBG_INLINE bool sbgEComMsgClassIsALog(SbgEComClass msgClass) -{ - // - // Test if this class id is part of the enum - // - if ((msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) || (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1)) - { - return TRUE; - } - else - { - return FALSE; - } -} - -#endif /* __SBG_ECOM_CMDS_H__ */ diff --git a/sbg_ros_driver/external/sbgECom/src/sbgEComLib.h b/sbg_ros_driver/external/sbgECom/src/sbgEComLib.h deleted file mode 100644 index 02def9f..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgEComLib.h +++ /dev/null @@ -1,61 +0,0 @@ -/*! - * \file sbgEComLib.h - * \author SBG Systems (Raphael Siryani) - * \date 05 February 2013 - * - * \brief Main header file for the SBG Systems Enhanced Communication Library. - * - * Only this main header file should be included to use the library. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ - -/*! - * \mainpage SBG Systems Enhanced Communication library documentation - * Welcome to the sbgECom library documentation.
- * This documentation describes all functions implemented in the sbgECom library. - */ - -#ifndef __SBG_ECOM_LIB_H__ -#define __SBG_ECOM_LIB_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "sbgECanId.h" -#include "sbgEComIds.h" -#include "commands/sbgEComCmd.h" -#include "protocol/sbgEComProtocol.h" -#include "binaryLogs/sbgEComBinaryLogs.h" -#include "sbgEComVersion.h" -#include "sbgEComGetVersion.h" - -//----------------------------------------------------------------------// -//- Footer (close extern C block) -// -//----------------------------------------------------------------------// -#ifdef __cplusplus -} -#endif - -#endif /* __SBG_ECOM_LIB_H__ */ diff --git a/sbg_ros_driver/external/sbgECom/src/sbgEComVersion.h b/sbg_ros_driver/external/sbgECom/src/sbgEComVersion.h deleted file mode 100644 index 35d761f..0000000 --- a/sbg_ros_driver/external/sbgECom/src/sbgEComVersion.h +++ /dev/null @@ -1,47 +0,0 @@ -/*! - * \file sbgEComVersion.h - * \author SBG Systems (Raphael Siryani) - * \date 05 February 2013 - * - * \brief Header file that contains all versions related information such as change log. - * - * \section CodeCopyright Copyright Notice - * Copyright (C) 2007-2013, SBG Systems SAS. All rights reserved. - * - * This source code is intended for use only by SBG Systems SAS and - * those that have explicit written permission to use it from - * SBG Systems SAS. - * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - */ -#ifndef SBG_E_COM_VERSION_H -#define SBG_E_COM_VERSION_H - -#include "sbgCommon.h" - -//----------------------------------------------------------------------// -//- Version definitions -// -//----------------------------------------------------------------------// - -#define SBG_E_COM_VERSION_MAJOR 1 -#define SBG_E_COM_VERSION_MINOR 11 -#define SBG_E_COM_VERSION_REV 920 -#define SBG_E_COM_VERSION_BUILD SBG_VERSION_QUALIFIER_STABLE - -#define SBG_E_COM_VERSION SBG_VERSION_SOFTWARE(SBG_E_COM_VERSION_MAJOR,SBG_E_COM_VERSION_MINOR,SBG_E_COM_VERSION_REV,SBG_E_COM_VERSION_BUILD) - -/* - * Backward compatibility macro definitions. - */ - #ifndef SBG_STR - #define SBG_STR(X) #X -#endif -#ifndef SBG_ASSTR - #define SBG_ASSTR(X) SBG_STR(X) -#endif -#define SBG_E_COM_VERSION_STR SBG_ASSTR(SBG_E_COM_VERSION_MAJOR) "." SBG_ASSTR(SBG_E_COM_VERSION_MINOR) "." SBG_ASSTR(SBG_E_COM_VERSION_REV) "-stable\0" - -#endif /* SBG_E_COM_VERSION_H */ diff --git a/sbg_ros_driver/include/sbg_driver/config_applier.h b/sbg_ros_driver/include/sbg_driver/config_applier.h deleted file mode 100644 index fe2db95..0000000 --- a/sbg_ros_driver/include/sbg_driver/config_applier.h +++ /dev/null @@ -1,215 +0,0 @@ -/*! -* \file config_applier.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Apply configuration to the device. -* -* Class takes a configuration from config_store and send all commands to the -* device to apply it. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - - -#ifndef CONFIG_APPLIER_H -#define CONFIG_APPLIER_H - -// Standard headers -#include -#include - -// Project headers -#include - -namespace sbg -{ -/*! - * Class to apply configuration to a device. - */ -class ConfigApplier -{ -private: - - bool m_reboot_needed_; - SbgEComHandle& m_ref_sbg_com_handle; - - //---------------------------------------------------------------------// - //- Private methods -// - //---------------------------------------------------------------------// - - /*! - * Check if the configuration getter worked properly. - * This function will log a warning information if the parameter is not available for this device. - * - * \param[in] ref_sbg_error_code Error code from the configuration getter. - * \param[in] ref_conf_title String to identify the configuration. - * \throw Unable to get the configuration. - */ - void checkConfigurationGet(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title) const; - - /*! - * Check if the configuration has been applied correctly. - * This function will log if the configuration has been applied. - * It will log a warning if the parameter is not available for this device. - * It will be the user responsability to check. - * - * \param[in] ref_sbg_error_code Error code from the configuration getter. - * \param[in] ref_conf_title String to identify the configuration. - * \throw Unable to configure. - */ - void checkConfigurationApplied(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title); - - /*! - * Configure the initial condition parameters. - * - * \param[in] ref_init_condition Initial condition conf to apply. - */ - void configureInitCondition(const SbgEComInitConditionConf& ref_init_condition); - - /*! - * Configure the motion profile. - * - * \param[in] ref_motion_profile Motion profile configuration to apply. - */ - void configureMotionProfile(const SbgEComModelInfo& ref_motion_profile); - - /*! - * Configure the IMU alignement. - * - * \param[in] ref_sensor_align Sensor IMU alignement configuration to apply. - * \param[in] ref_level_arms X, Y, Z level arms to apply. - */ - void configureImuAlignement(const SbgEComSensorAlignmentInfo& ref_sensor_align, const SbgVector3& ref_level_arms); - - /*! - * Configure the aiding assignement. - * - * \param[in] ref_aiding_assign Aiding assignement configuration to apply. - */ - void configureAidingAssignement(const SbgEComAidingAssignConf& ref_aiding_assign); - - /*! - * Configure the magnetometers model. - * - * \param[in] ref_mag_model Magnetometers model configuration to apply. - */ - void configureMagModel(const SbgEComModelInfo& ref_mag_model); - - /*! - * Configure the magnetometers rejection. - * - * \param[in] ref_mag_rejection Magnetometers rejection configuration to apply. - */ - void configureMagRejection(const SbgEComMagRejectionConf& ref_mag_rejection); - - /*! - * Configure the Gnss model. - * - * \param[in] ref_gnss_model Gnss model configuration to apply. - */ - void configureGnssModel(const SbgEComModelInfo& ref_gnss_model); - - /*! - * Configure the Gnss installation. - * - * \param[in] ref_gnss_installation Gnss installation configuration to apply. - */ - void configureGnssInstallation(const SbgEComGnssInstallation& ref_gnss_installation); - - /*! - * Configure the Gnss rejection. - * - * \param[in] ref_gnss_rejection Gnss rejection configuration to apply. - */ - void configureGnssRejection(const SbgEComGnssRejectionConf& ref_gnss_rejection); - - /*! - * Configure the odometer. - * - * \param[in] ref_odometer Odometer configuration to apply. - */ - void configureOdometer(const SbgEComOdoConf& ref_odometer); - - /*! - * Configure the odometer level arm. - * - * \param[in] odometer_level_arms X,Y,Z odometer level arms to apply. - */ - void configureOdometerLevelArm(const SbgVector3& odometer_level_arms); - - /*! - * Configure the odometer rejection. - * - * \param[in] ref_odometer_rejection Odometer rejection configuration to apply. - */ - void configureOdometerRejection(const SbgEComOdoRejectionConf& ref_odometer_rejection); - - /*! - * Configure the output for the SBG log. - * If a Log is not available for the connected device, a warning will be logged. - * It will be user responsability to check. - * - * \param[in] output_port Output communication port. - * \param[in] ref_log_output Log output to configure. - * \throw Unable to configure the output. - */ - void configureOutput(SbgEComOutputPort output_port, const ConfigStore::SbgLogOutput &ref_log_output); - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - * - * \param[in] ref_com_handle SBG communication handle. - */ - ConfigApplier(SbgEComHandle &ref_sbg_com_handle); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Apply a configuration to the SBG device. - * - * \param[in] ref_config_store Configuration to apply. - */ - void applyConfiguration(const ConfigStore& ref_config_store); - - /*! - * Save the configuration to the device. - */ - void saveConfiguration(void); -}; -} - -#endif // CONFIG_APPLIER_H diff --git a/sbg_ros_driver/include/sbg_driver/config_store.h b/sbg_ros_driver/include/sbg_driver/config_store.h deleted file mode 100644 index 2fc15aa..0000000 --- a/sbg_ros_driver/include/sbg_driver/config_store.h +++ /dev/null @@ -1,411 +0,0 @@ -/*! -* \file config_store.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Class to handle the device configuration. -* -* Methods to extract configuration values and load it to the ROS node. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_ROS_CONFIG_STORE_H -#define SBG_ROS_CONFIG_STORE_H - -// SbgECom headers -#include - -// ROS headers -#include - -// Project headers -#include "sbg_vector3.h" - -namespace sbg -{ -/*! - * Class to handle the device configuration. - */ -class ConfigStore -{ -public: - - /*! - * Structure to define the SBG log output. - */ - struct SbgLogOutput - { - SbgEComClass message_class; - SbgEComMsgId message_id; - SbgEComOutputMode output_mode; - }; - -private: - - std::string m_uart_port_name_; - SbgEComOutputPort m_output_port_; - uint32_t m_uart_baud_rate_; - bool m_serial_communication_; - - sbgIpAddress m_sbg_ip_address_; - uint32_t m_out_port_address_; - uint32_t m_in_port_address_; - bool m_upd_communication_; - - bool m_configure_through_ros_; - - SbgEComInitConditionConf m_init_condition_conf_; - SbgEComModelInfo m_motion_profile_model_info_; - - SbgEComSensorAlignmentInfo m_sensor_alignement_info_; - SbgVector3 m_sensor_lever_arm_; - - SbgEComAidingAssignConf m_aiding_assignement_conf_; - - SbgEComModelInfo m_mag_model_info_; - SbgEComMagRejectionConf m_mag_rejection_conf_; - SbgEComMagCalibMode m_mag_calib_mode_; - SbgEComMagCalibBandwidth m_mag_calib_bandwidth_; - - SbgEComModelInfo m_gnss_model_info_; - SbgEComGnssInstallation m_gnss_installation_; - SbgEComGnssRejectionConf m_gnss_rejection_conf_; - - SbgEComOdoConf m_odometer_conf_; - SbgVector3 m_odometer_level_arm_; - SbgEComOdoRejectionConf m_odometer_rejection_conf_; - - std::vector m_output_modes_; - bool m_ros_standard_output_; - uint32_t m_rate_frequency_; - - //---------------------------------------------------------------------// - //- Private methods -// - //---------------------------------------------------------------------// - - /*! - * Get the ROS integer parameter casted in the T type. - * This function has the same behavior as the param base function, however it enables an implicit cast, and the use of const NodeHandle. - * - * \template T Template type to cast the ROS param to. - * \param[in] ref_node_handle ROS NodeHandle. - * \param[in] param_key Parameter key. - * \param[in] default_value Default value for the parameter. - * \return ROS integer parameter casted. - */ - template - T getParameter(const ros::NodeHandle& ref_node_handle, std::string param_key, int default_value) const - { - if (ref_node_handle.hasParam(param_key)) - { - int parameter; - ref_node_handle.param(param_key, parameter, default_value); - - return static_cast(parameter); - } - else - { - return static_cast(default_value); - } - } - - /*! - * Load interface communication parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadCommunicationParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load sensor parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadSensorParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load IMU alignement parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadImuAlignementParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load aiding assignement parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadAidingAssignementParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load magnetometers parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadMagnetometersParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load Gnss parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadGnssParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load odometer parameters. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadOdometerParameters(const ros::NodeHandle& ref_node_handle); - - /*! - * Load the output configuration. - * - * \param[in] ref_node_handle ROS nodeHandle. - * \param[in] ref_key String key for the output config. - * \param[in] sbg_msg_class SBG message class. - * \param[in] sbg_msg_id ID of the SBG log. - */ - void loadOutputConfiguration(const ros::NodeHandle& ref_node_handle, const std::string& ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id); - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - */ - ConfigStore(void); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Check if the configuration should be done with ROS. - * - * \return True if the ROS driver has to configure the device. - */ - bool checkConfigWithRos(void) const; - - /*! - * Check if the interface configuration is a serial interface. - * - * \return True if the interface is serial, False otherwise. - */ - bool isInterfaceSerial(void) const; - - /*! - * Get the UART port name. - * - * \return UART serial port name. - */ - const std::string &getUartPortName(void) const; - - /*! - * Get the UART baudrate communication. - * - * \return UART serial baudrate. - */ - uint32_t getBaudRate(void) const; - - /*! - * Get the output port of the device. - * - * \return SBG device output port. - */ - SbgEComOutputPort getOutputPort(void) const; - - /*! - * Check if the interface configuration is a UDP interface. - * - * \return True if the interface is UDP, False otherwise. - */ - bool isInterfaceUdp(void) const; - - /*! - * Get the Ip address of the interface. - * - * \return Ip address. - */ - sbgIpAddress getIpAddress(void) const; - - /*! - * Get the output port. - * - * \return Output port. - */ - uint32_t getOutputPortAddress(void) const; - - /*! - * Get the input port. - * - * \return Input port. - */ - uint32_t getInputPortAddress(void) const; - - /*! - * Get the initial conditions configuration. - * - * \return Initial conditions configuration. - */ - const SbgEComInitConditionConf &getInitialConditions(void) const; - - /*! - * Get the motion profile configuration. - * - * \return Motion profile configuration. - */ - const SbgEComModelInfo &getMotionProfile(void) const; - - /*! - * Get the sensor alignement configuration. - * - * \return Sensor alignement configuration. - */ - const SbgEComSensorAlignmentInfo &getSensorAlignement(void) const; - - /*! - * Get the sensor level arms. - * - * \return Sensor level arms vector (in meters). - */ - const SbgVector3 &getSensorLevelArms(void) const; - - /*! - * Get the aiding assignement configuration. - * - * \return Aiding assignement configuration. - */ - const SbgEComAidingAssignConf &getAidingAssignement(void) const; - - /*! - * Get the magnetometer model configuration. - * - * \return Magnetometer model configuration. - */ - const SbgEComModelInfo &getMagnetometerModel(void) const; - - /*! - * Get the magnetometer rejection configuration. - * - * \return Magnetometer rejection configuration. - */ - const SbgEComMagRejectionConf &getMagnetometerRejection(void) const; - - /*! - * Get the magnetometer calibration mode. - * - * \return Magnetometer calibration mode. - */ - const SbgEComMagCalibMode &getMagnetometerCalibMode(void) const; - - /*! - * Get the magnetometer calibration bandwidth. - * - * \return Magnetometer calibration bandwidth. - */ - const SbgEComMagCalibBandwidth &getMagnetometerCalibBandwidth(void) const; - - /*! - * Get the Gnss model configuration. - * - * \return Gnss model configuration. - */ - const SbgEComModelInfo &getGnssModel(void) const; - - /*! - * Get the Gnss installation configuration. - * - * \return Gnss installation configuration. - */ - const SbgEComGnssInstallation &getGnssInstallation(void) const; - - /*! - * Get the Gnss rejection configuration. - * - * \return Gnss rejection configuration. - */ - const SbgEComGnssRejectionConf &getGnssRejection(void) const; - - /*! - * Get the odometer configuration. - * - * \return Odometer configuration. - */ - const SbgEComOdoConf &getOdometerConf(void) const; - - /*! - * Get the odometer level arms. - * - * \return Odometer level arms vector (in meters). - */ - const SbgVector3 &getOdometerLevelArms(void) const; - - /*! - * Get the odometer rejection. - * - * \return Odometer rejection configuration. - */ - const SbgEComOdoRejectionConf &getOdometerRejection(void) const; - - /*! - * Get all the output modes. - * - * \return Output mode for this config store. - */ - const std::vector &getOutputModes(void) const; - - /*! - * Check if the ROS standard outputs are defined. - * - * \return True if standard ROS messages output are defined. - */ - bool checkRosStandardMessages(void) const; - - /*! - * Get the reading frequency defined in settings. - * If this frequency is null, the driver will automatically configure the max output frequency according to the outputs. - * - * \return Rate frequency parameter (in Hz). - */ - uint32_t getReadingRateFrequency(void) const; - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Load the configuration from a ros parameter handle. - * - * \param[in] ref_node_handle ROS nodeHandle. - */ - void loadFromRosNodeHandle(const ros::NodeHandle& ref_node_handle); -}; -} - -#endif // SBG_ROS_CONFIG_STORE_H diff --git a/sbg_ros_driver/include/sbg_driver/message_publisher.h b/sbg_ros_driver/include/sbg_driver/message_publisher.h deleted file mode 100644 index 35eb9c0..0000000 --- a/sbg_ros_driver/include/sbg_driver/message_publisher.h +++ /dev/null @@ -1,231 +0,0 @@ -/*! -* \file message_publisher.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Manage publishment of messages from logs. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_ROS_MESSAGE_PUBLISHER_H -#define SBG_ROS_MESSAGE_PUBLISHER_H - -// Project headers -#include -#include - -namespace sbg -{ -/*! - * Class to publish all SBG-ROS messages to the corresponding publishers. - */ -class MessagePublisher -{ -private: - - ros::Publisher m_sbgStatus_pub_; - ros::Publisher m_sbgUtcTime_pub_; - ros::Publisher m_sbgImuData_pub_; - ros::Publisher m_sbgEkfEuler_pub_; - ros::Publisher m_sbgEkfQuat_pub_; - ros::Publisher m_sbgEkfNav_pub_; - ros::Publisher m_sbgShipMotion_pub_; - ros::Publisher m_sbgMag_pub_; - ros::Publisher m_sbgMagCalib_pub_; - ros::Publisher m_sbgGpsVel_pub_; - ros::Publisher m_sbgGpsPos_pub_; - ros::Publisher m_sbgGpsHdt_pub_; - ros::Publisher m_sbgGpsRaw_pub_; - ros::Publisher m_sbgOdoVel_pub_; - ros::Publisher m_sbgEventA_pub_; - ros::Publisher m_sbgEventB_pub_; - ros::Publisher m_sbgEventC_pub_; - ros::Publisher m_sbgEventD_pub_; - ros::Publisher m_sbgEventE_pub_; - ros::Publisher m_sbgImuShort_pub_; - ros::Publisher m_sbgAirData_pub_; - - ros::Publisher m_imu_pub_; - sbg_driver::SbgImuData m_sbg_imu_message_; - sbg_driver::SbgEkfQuat m_sbg_ekf_quat_message_; - sbg_driver::SbgEkfNav m_sbg_ekf_nav_message_; - sbg_driver::SbgEkfEuler m_sbg_ekf_euler_message_; - - ros::Publisher m_temp_pub_; - ros::Publisher m_mag_pub_; - ros::Publisher m_fluid_pub_; - ros::Publisher m_pos_ecef_pub_; - ros::Publisher m_velocity_pub_; - ros::Publisher m_utc_reference_pub_; - ros::Publisher m_nav_sat_fix_pub_; - - MessageWrapper m_message_wrapper_; - SbgEComOutputMode m_output_mode_; - uint32_t m_max_mesages_; - - //---------------------------------------------------------------------// - //- Private methods -// - //---------------------------------------------------------------------// - - /*! - * Update the maximal output frequency for the defined pubishers. - * Each time a new publisher is defined, update the maximal output frequency if required. - * - * \param[in] output_mode_freq Output mode. - */ - void updateMaxOutputFrequency(SbgEComOutputMode output_mode); - - /*! - * Get the corresponding frequency for the SBG output mode. - * - * \param[in] output_mode Output mode. - * \return Output frequency (in Hz). - */ - uint32_t getCorrespondingFrequency(SbgEComOutputMode output_mode) const; - - /*! - * Get the corresponding topic name output for the SBG output mode. - * - * \param[in] sbg_message_id SBG message ID. - * \return Output topic name. - */ - std::string getOutputTopicName(SbgEComMsgId sbg_message_id) const; - - /*! - * Initialize the publisher for the specified SBG Id, and the output configuration. - * - * \param[in] ref_ros_node_handle Ros NodeHandle to advertise the publisher. - * \param[in] sbg_msg_id Id of the SBG message. - * \param[in] output_conf Output configuration. - * \param[in] ref_output_topic Output topic for the publisher. - */ - void initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEComMsgId sbg_msg_id, SbgEComOutputMode output_conf, const std::string &ref_output_topic); - - /*! - * Define standard ROS publishers. - * - * \param[in] ref_ros_node_handle Ros NodeHandle to advertise the publisher. - */ - void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle); - - /*! - * Publish a received SBG IMU log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishIMUData(const SbgBinaryLogData &ref_sbg_log); - - /*! - * Process a ROS Velocity standard message. - */ - void processRosVelMessage(void); - - /*! - * Process a ROS IMU standard message. - */ - void processRosImuMessage(void); - - /*! - * Publish a received SBG Magnetic log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishMagData(const SbgBinaryLogData &ref_sbg_log); - - /*! - * Publish a received SBG Fluid pressure log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishFluidPressureData(const SbgBinaryLogData &ref_sbg_log); - - /*! - * Publish a received SBG EkfNav log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishEkfNavigationData(const SbgBinaryLogData &ref_sbg_log); - - /*! - * Publish a received SBG UTC log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishUtcData(const SbgBinaryLogData &ref_sbg_log); - - /*! - * Publish a received SBG GpsPos log. - * - * \param[in] ref_sbg_log SBG log. - */ - void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log); - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - */ - MessagePublisher(void); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Get the maximal output frequency for the publisher. - * - * \return Maixmal output frequency (in Hz). - */ - uint32_t getMaxOutputFrequency(void) const; - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Initialize the publishers for the output configuration. - * - * \param[in] ref_ros_node_handle Ros NodeHandle to advertise the publisher. - * \param[in] ref_config_store Store configuration for the publishers. - */ - void initPublishers(ros::NodeHandle& ref_ros_node_handle, const ConfigStore &ref_config_store); - - /*! - * Publish the received SbgLog if the corresponding publisher is defined. - * - * \param[in] ref_ros_time ROS processing time for the messages. - * \param[in] sbg_msg_class Class ID of the SBG message. - * \param[in] sbg_msg_id Id of the SBG message. - * \param[in] ref_sbg_log SBG binary log. - */ - void publish(const ros::Time& ref_ros_time, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log); -}; -} - -#endif // SBG_ROS_MESSAGE_PUBLISHER_H diff --git a/sbg_ros_driver/include/sbg_driver/message_wrapper.h b/sbg_ros_driver/include/sbg_driver/message_wrapper.h deleted file mode 100644 index 88ee496..0000000 --- a/sbg_ros_driver/include/sbg_driver/message_wrapper.h +++ /dev/null @@ -1,503 +0,0 @@ -/*! -* \file message_wrapper.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Handle creation of messages. -* -* Methods to create ROS messages from given data. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_ROS_MESSAGE_WRAPPER_H -#define SBG_ROS_MESSAGE_WRAPPER_H - -// SbgECom headers -#include -#include - -// Sbg header -#include - -// ROS headers -#include -#include -#include "ros/ros.h" -#include -#include -#include -#include -#include -#include - -// SbgRos message headers -#include "sbg_driver/SbgStatus.h" -#include "sbg_driver/SbgUtcTime.h" -#include "sbg_driver/SbgImuData.h" -#include "sbg_driver/SbgEkfEuler.h" -#include "sbg_driver/SbgEkfQuat.h" -#include "sbg_driver/SbgEkfNav.h" -#include "sbg_driver/SbgShipMotion.h" -#include "sbg_driver/SbgMag.h" -#include "sbg_driver/SbgMagCalib.h" -#include "sbg_driver/SbgGpsVel.h" -#include "sbg_driver/SbgGpsPos.h" -#include "sbg_driver/SbgGpsHdt.h" -#include "sbg_driver/SbgGpsRaw.h" -#include "sbg_driver/SbgOdoVel.h" -#include "sbg_driver/SbgEvent.h" -#include "sbg_driver/SbgImuShort.h" -#include "sbg_driver/SbgAirData.h" - -namespace sbg -{ -/*! - * Class to wrap the SBG logs into ROS messages. - */ -class MessageWrapper -{ -private: - - ros::Time m_ros_processing_time_; - sbg_driver::SbgUtcTime m_last_sbg_utc_; - bool m_first_valid_utc_; - - //---------------------------------------------------------------------// - //- Internal methods -// - //---------------------------------------------------------------------// - - /*! - * Create a geometry message Vector3 from a raw input vector. - * - * \template T Numeric template type. - * \param[in] p_array Raw input vector. - * \param[in] array_size Raw vector size, should be defined as 3. - * \return GeometryMsg Vector3. - */ - template - const geometry_msgs::Vector3 createGeometryVector3(const T* p_array, size_t array_size) const - { - assert(array_size == 3); - - geometry_msgs::Vector3 geometry_vector; - - geometry_vector.x = p_array[0]; - geometry_vector.y = p_array[1]; - geometry_vector.z = p_array[2]; - - return geometry_vector; - }; - - /*! - * Create a ROS message header. - * - * \param[in] device_timestamp SBG device timestamp (in microseconds). - * \return ROS header message. - */ - const std_msgs::Header createRosHeader(uint32_t device_timestamp) const; - - /*! - * Compute corrected ROS time for the device timestamp. - * - * \param[in] device_timestamp SBG device timestamp (in microseconds). - * \return ROS time. - */ - const ros::Time computeCorrectedRosTime(uint32_t device_timestamp) const; - - /*! - * Create SBG-ROS Ekf status message. - * - * \param[in] ekf_status SBG Ekf status. - * \return Ekf status message. - */ - const sbg_driver::SbgEkfStatus createEkfStatusMessage(uint32_t ekf_status) const; - - /*! - * Create SBG-ROS GPS Position status message. - * - * \param[in] ref_log_gps_pos SBG GPS position log. - * \return GPS Position status. - */ - const sbg_driver::SbgGpsPosStatus createGpsPosStatusMessage(const SbgLogGpsPos& ref_log_gps_pos) const; - - /*! - * Create SBG-ROS GPS Velocity status message. - * - * \param[in] ref_log_gps_vel SBG GPS Velocity log. - * \return GPS Velocity status. - */ - const sbg_driver::SbgGpsVelStatus createGpsVelStatusMessage(const SbgLogGpsVel& ref_log_gps_vel) const; - - /*! - * Create a SBG-ROS IMU status message. - * - * \param[in] sbg_imu_status SBG IMU status. - * \return IMU status message. - */ - const sbg_driver::SbgImuStatus createImuStatusMessage(uint16_t sbg_imu_status) const; - - /*! - * Create a SBG-ROS Magnetometer status message. - * - * \param[in] ref_log_mag SBG Magnetometer log. - * \return Magnetometer status message. - */ - const sbg_driver::SbgMagStatus createMagStatusMessage(const SbgLogMag& ref_log_mag) const; - - /*! - * Create a SBG-ROS Ship motion status message. - * - * \param[in] ref_log_ship_motion SBG Ship motion log. - * \return ship motion status message. - */ - const sbg_driver::SbgShipMotionStatus createShipMotionStatusMessage(const SbgLogShipMotionData& ref_log_ship_motion) const; - - /*! - * Create a SBG-ROS aiding status message. - * - * \param[in] ref_log_status SBG status log. - * \return Aiding status message. - */ - const sbg_driver::SbgStatusAiding createStatusAidingMessage(const SbgLogStatusData& ref_log_status) const; - - /*! - * Create a SBG-ROS com status message. - * - * \param[in] ref_log_status SBG status log. - * \return Com status message. - */ - const sbg_driver::SbgStatusCom createStatusComMessage(const SbgLogStatusData& ref_log_status) const; - - /*! - * Create a SBG-ROS general status message. - * - * \param[in] ref_log_status SBG status log. - * \return General status message. - */ - const sbg_driver::SbgStatusGeneral createStatusGeneralMessage(const SbgLogStatusData& ref_log_status) const; - - /*! - * Create a SBG-ROS UTC time status message. - * - * \param[in] ref_log_utc SBG UTC data log. - * \return UTC time status message. - */ - const sbg_driver::SbgUtcTimeStatus createUtcStatusMessage(const SbgLogUtcData& ref_log_utc) const; - - /*! - * Get the number of days in the year. - * - * \param[in] year Year to get the number of days. - * \return Number of days in the year. - */ - uint32_t getNumberOfDaysInYear(uint16_t year) const; - - /*! - * Get the number of days of the month index. - * - * \param[in] year Year. - * \param[in] month_index Month index [1..12]. - * \return Number of days in the month. - */ - uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const; - - /*! - * Check if the given year is a leap year. - * - * \param[in] year Year to check. - * \return True if the year is a leap year. - */ - bool isLeapYear(uint16_t year) const; - - /*! - * Convert the UTC time to an Epoch time. - * - * \param[in] ref_sbg_utc_msg SBG-ROS UTC message. - * \return Converted Epoch time (in s). - */ - const ros::Time convertUtcTimeToEpoch(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const; - - /*! - * Create a SBG-ROS air data status message. - * - * \param[in] ref_sbg_air_data SBG AirData log. - * \return SBG-ROS air data status message. - */ - const sbg_driver::SbgAirDataStatus createAirDataStatusMessage(const SbgLogAirData& ref_sbg_air_data) const; - - /*! - * Create a ROS standard TwistStamped message. - * - * \param[in] body_vel SBG Body velocity vector. - * \param[in] ref_sbg_air_data SBG IMU message. - * \return SBG TwistStamped message. - */ - const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f& body_vel, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const; - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - */ - MessageWrapper(void); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Set the wrapper processing ROS time. - * This method is call on the SbgDevice periodic handle, in order to have the same processing time for the messages. - * - * \param[in] ref_ros_time ROS processing time to set. - */ - void setRosProcessingTime(const ros::Time& ref_ros_time); - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Create a SBG-ROS Ekf Euler message. - * - * \param[in] ref_log_ekf_euler SBG Ekf Euler log. - * \return Ekf Euler message. - */ - const sbg_driver::SbgEkfEuler createSbgEkfEulerMessage(const SbgLogEkfEulerData& ref_log_ekf_euler) const; - - /*! - * Create a SBG-ROS Ekf Navigation message. - * - * \param[in] ref_log_ekf_nav SBG Ekf Navigation log. - * \return Ekf Navigation message. - */ - const sbg_driver::SbgEkfNav createSbgEkfNavMessage(const SbgLogEkfNavData& ref_log_ekf_nav) const; - - /*! - * Create a SBG-ROS Ekf Quaternion message. - * - * \param[in] ref_log_ekf_quat SBG Ekf Quaternion log. - * \return Ekf Quaternion message. - */ - const sbg_driver::SbgEkfQuat createSbgEkfQuatMessage(const SbgLogEkfQuatData& ref_log_ekf_quat) const; - - /*! - * Create a SBG-ROS event message. - * - * \param[in] ref_log_event SBG event log. - * \return Event message. - */ - const sbg_driver::SbgEvent createSbgEventMessage(const SbgLogEvent& ref_log_event) const; - - /*! - * Create SBG-ROS GPS-HDT message. - * - * \param[in] ref_log_gps_hdt SBG GPS HDT log. - * \return GPS HDT message. - */ - const sbg_driver::SbgGpsHdt createSbgGpsHdtMessage(const SbgLogGpsHdt& ref_log_gps_hdt) const; - - /*! - * Create a SBG-ROS GPS-Position message. - * - * \param[in] ref_log_gps_pos SBG GPS Position log. - * \return GPS Position message. - */ - const sbg_driver::SbgGpsPos createSbgGpsPosMessage(const SbgLogGpsPos& ref_log_gps_pos) const; - - /*! - * Create a SBG-ROS GPS raw message. - * - * \param[in] ref_log_gps_raw SBG GPS raw log. - * \return GPS raw message. - */ - const sbg_driver::SbgGpsRaw createSbgGpsRawMessage(const SbgLogGpsRaw& ref_log_gps_raw) const; - - /*! - * Create a SBG-ROS GPS Velocity message. - * - * \param[in] ref_log_gps_vel SBG GPS Velocity log. - * \return GPS Velocity message. - */ - const sbg_driver::SbgGpsVel createSbgGpsVelMessage(const SbgLogGpsVel& ref_log_gps_vel) const; - - /*! - * Create a SBG-ROS Imu data message. - * - * \param[in] ref_log_imu_data SBG Imu data log. - * \return Imu data message. - */ - const sbg_driver::SbgImuData createSbgImuDataMessage(const SbgLogImuData& ref_log_imu_data) const; - - /*! - * Create a SBG-ROS Magnetometer message. - * - * \param[in] ref_log_mag SBG Magnetometer log. - * \return Magnetometer message. - */ - const sbg_driver::SbgMag createSbgMagMessage(const SbgLogMag& ref_log_mag) const; - - /*! - * Create a SBG-ROS Magnetometer calibration message. - * - * \param[in] ref_log_mag_calib SBG Magnetometer calibration log. - * \return Magnetometer calibration message. - */ - const sbg_driver::SbgMagCalib createSbgMagCalibMessage(const SbgLogMagCalib& ref_log_mag_calib) const; - - /*! - * Create a SBG-ROS Odometer velocity message. - * - * \param[in] ref_log_odo SBG Odometer log. - * \return Odometer message. - */ - const sbg_driver::SbgOdoVel createSbgOdoVelMessage(const SbgLogOdometerData& ref_log_odo) const; - - /*! - * Create a SBG-ROS Shipmotion message. - * - * \param[in] ref_log_ship_motion SBG Ship motion log. - * \return Ship motion message. - */ - const sbg_driver::SbgShipMotion createSbgShipMotionMessage(const SbgLogShipMotionData& ref_log_ship_motion) const; - - /*! - * Create a SBG-ROS status message from a SBG status log. - * - * \param[in] ref_log_status SBG status log. - * \return Status message. - */ - const sbg_driver::SbgStatus createSbgStatusMessage(const SbgLogStatusData& ref_log_status) const; - - /*! - * Create a SBG-ROS UTC time message from a SBG UTC log. - * - * \param[in] ref_log_utc SBG UTC log. - * \return UTC time message. - */ - const sbg_driver::SbgUtcTime createSbgUtcTimeMessage(const SbgLogUtcData& ref_log_utc); - - /*! - * Create a SBG-ROS Air data message from a SBG log. - * - * \param[in] ref_air_data_log SBG AirData log. - * \return SBG-ROS airData message. - */ - const sbg_driver::SbgAirData createSbgAirDataMessage(const SbgLogAirData& ref_air_data_log) const; - - /*! - * Create a SBG-ROS Short Imu message. - * - * \param[in] ref_short_imu_log SBG Imu short log. - * \return SBG-ROS Imu short message. - */ - const sbg_driver::SbgImuShort createSbgImuShortMessage(const SbgLogImuShort& ref_short_imu_log) const; - - /*! - * Create a ROS standard IMU message from SBG messages. - * - * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. - * \param[in] ref_sbg_quat_msg SBG_ROS Quaternion message. - * \return ROS standard IMU message. - */ - const sensor_msgs::Imu createRosImuMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat& ref_sbg_quat_msg) const; - - /*! - * Create a ROS standard Temperature message from SBG message. - * - * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. - * \return ROS standard Temperature message. - */ - const sensor_msgs::Temperature createRosTemperatureMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg) const; - - /*! - * Create a ROS standard MagneticField message from SBG message. - * - * \param[in] ref_sbg_mag_msg SBG-ROS Mag message. - * \return ROS standard Mag message. - */ - const sensor_msgs::MagneticField createRosMagneticMessage(const sbg_driver::SbgMag& ref_sbg_mag_msg) const; - - /*! - * Create a ROS standard TwistStamped message from SBG messages. - * - * \param[in] ref_sbg_ekf_euler_msg SBG-ROS Ekf Euler message. - * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. - * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. - * \return ROS standard TwistStamped message. - */ - const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg_driver::SbgEkfEuler& ref_sbg_ekf_vel_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const; - - - /*! - * Create a ROS standard TwistStamped message from SBG messages. - * - * \param[in] ref_sbg_ekf_quat_msg SBG-ROS Ekf Quaternion message. - * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. - * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. - * \return ROS standard TwistStamped message. - */ - const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg_driver::SbgEkfQuat& ref_sbg_ekf_vel_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const; - - - /*! - * Create a ROS standard PointStamped message from SBG messages. - * - * \param[in] ref_sbg_ekf_msg SBG-ROS EkfNav message. - * \return ROS standard PointStamped message (ECEF). - */ - const geometry_msgs::PointStamped createRosPointStampedMessage(const sbg_driver::SbgEkfNav& ref_sbg_ekf_msg) const; - - /*! - * Create a ROS standard timeReference message for a UTC time. - * - * \param[in] ref_sbg_utc_msg SBG-ROS UTC message. - * \return ROS standard timeReference message. - */ - const sensor_msgs::TimeReference createRosUtcTimeReferenceMessage(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const; - - /*! - * Create a ROS standard NavSatFix message from a Gps message. - * - * \param[in] ref_sbg_gps_msg SBG-ROS GPS position message. - * \return ROS standard NavSatFix message. - */ - const sensor_msgs::NavSatFix createRosNavSatFixMessage(const sbg_driver::SbgGpsPos& ref_sbg_gps_msg) const; - - /*! - * Create a ROS standard FluidPressure message. - * - * \param[in] ref_sbg_air_msg SBG-ROS AirData message. - * \return ROS standard fluid pressure message. - */ - const sensor_msgs::FluidPressure createRosFluidPressureMessage(const sbg_driver::SbgAirData& ref_sbg_air_msg) const; -}; -} - -#endif // SBG_ROS_MESSAGE_WRAPPER_H diff --git a/sbg_ros_driver/include/sbg_driver/sbg_device.h b/sbg_ros_driver/include/sbg_driver/sbg_device.h deleted file mode 100644 index 55ad671..0000000 --- a/sbg_ros_driver/include/sbg_driver/sbg_device.h +++ /dev/null @@ -1,256 +0,0 @@ -/*! -* \file sbg_device.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Handle a connected SBG device. -* -* Methods to communicate with the device. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_ROS_SBG_DEVICE_H -#define SBG_ROS_SBG_DEVICE_H - -// Standard headers -#include -#include -#include - -// ROS headers -#include -#include - -// Project headers -#include -#include -#include - -namespace sbg -{ -/*! - * Class to handle a connected SBG device. - */ -class SbgDevice -{ -private: - - //---------------------------------------------------------------------// - //- Static members definition -// - //---------------------------------------------------------------------// - - static std::map g_mag_calib_quality_; - static std::map g_mag_calib_confidence_; - static std::map g_mag_calib_mode_; - static std::map g_mag_calib_bandwidth; - - //---------------------------------------------------------------------// - //- Private variables -// - //---------------------------------------------------------------------// - - SbgEComHandle m_com_handle_; - SbgInterface m_sbg_interface_; - ros::NodeHandle& m_ref_node_; - MessagePublisher m_message_publisher_; - ConfigStore m_config_store_; - - uint32_t m_rate_frequency_; - - bool m_mag_calibration_ongoing_; - bool m_mag_calibration_done_; - SbgEComMagCalibResults m_magCalibResults; - ros::ServiceServer m_calib_service_; - ros::ServiceServer m_calib_save_service_; - - ros::Time m_ros_processing_time_; - - //---------------------------------------------------------------------// - //- Private methods -// - //---------------------------------------------------------------------// - - /*! - * Callback definition called each time a new log is received. - * - * \param[in] pHandle Valid handle on the sbgECom instance that has called this callback. - * \param[in] msg_class Class of the message we have received - * \param[in] msg Message ID of the log received. - * \param[in] p_log_data Contains the received log data as an union. - * \param[in] p_user_arg Optional user supplied argument. - * \return SBG_NO_ERROR if the received log has been used successfully. - */ - static SbgErrorCode onLogReceivedCallback(SbgEComHandle* p_handle, SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData* p_log_data, void* p_user_arg); - - /*! - * Function to handle the received log. - * - * \param[in] msg_class Class of the message we have received - * \param[in] msg Message ID of the log received. - * \param[in] ref_sbg_data Contains the received log data as an union. - */ - void onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData& ref_sbg_data); - - /*! - * Load the parameters. - */ - void loadParameters(void); - - /*! - * Create the connection to the SBG device. - * - * \throw Unable to connect to the SBG device. - */ - void connect(void); - - /*! - * Read the device informations. - * - * \throw Unable to read the device information. - */ - void readDeviceInfo(void); - - /*! - * Get the SBG version as a string. - * - * \param[in] sbg_version_enc SBG version encoded. - * \return String version decoded. - */ - std::string getVersionAsString(uint32 sbg_version_enc) const; - - /*! - * Initialize the publishers according to the configuration. - */ - void initPublishers(void); - - /*! - * Configure the connected SBG device. - * This function will configure the device if the config file allows it. - * It will log warning for unavailable parameters for the connected device. - * - * \throw Unable to configure the connected device. - */ - void configure(void); - - /*! - * Process the magnetometer calibration. - * - * \param[in] ref_ros_request ROS service request. - * \param[in] ref_ros_response ROS service response. - * \return Return true if the calibration process has been succesfull. - */ - bool processMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response); - - /*! - * Save the magnetometer calibration. - * - * \param[in] ref_ros_request ROS service request. - * \param[in] ref_ros_response ROS service response. - * \return Return true if the calibration has been saved. - */ - bool saveMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response); - - /*! - * Start the magnetometer calibration process. - * - * \return True if the calibration process has started successfully. - */ - bool startMagCalibration(void); - - /*! - * End the magnetometer calibration process. - * - * \return True if the calibration process has ended successfully. - */ - bool endMagCalibration(void); - - /*! - * Upload the magnetometers calibration results to the device. - * - * \return True if the magnetometers calibration has been successfully uploaded to the device. - */ - bool uploadMagCalibrationToDevice(void); - - /*! - * Display magnetometers calibration status result. - */ - void displayMagCalibrationStatusResult(void) const; - - /*! - * Export magnetometers calibration results. - */ - void exportMagCalibrationResults(void) const; - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Default constructor. - * - * \param[in] ref_node_handle ROS NodeHandle. - */ - SbgDevice(ros::NodeHandle& ref_node_handle); - - /*! - * Default destructor. - */ - ~SbgDevice(void); - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - /*! - * Get the frequency to update the main rate loop for device handling. - * - * \return Device frequency to read the logs (in Hz). - */ - uint32_t getUpdateFrequency(void) const; - - //---------------------------------------------------------------------// - //- Public methods -// - //---------------------------------------------------------------------// - - /*! - * Initialize the SBG device for receiving data. - * - * \throw Unable to initialize the SBG device. - */ - void initDeviceForReceivingData(void); - - /*! - * Initialize the device for magnetometers calibration. - */ - void initDeviceForMagCalibration(void); - - /*! - * Periodic handle of the connected SBG device. - */ - void periodicHandle(void); -}; -} - -#endif // SBG_ROS_SBG_DEVICE_H diff --git a/sbg_ros_driver/include/sbg_driver/sbg_matrix3.h b/sbg_ros_driver/include/sbg_driver/sbg_matrix3.h deleted file mode 100644 index 93650a1..0000000 --- a/sbg_ros_driver/include/sbg_driver/sbg_matrix3.h +++ /dev/null @@ -1,260 +0,0 @@ -/*! -* \file sbg_matrix3.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Handle a 3x3 matrix. -* -* SBG Systems Ros driver needs some basic matrix operations. -* Ros uses Eigen for mathematical computations but to avoid dependancy on -* Eigen we chose to implement a basic custom matrix class with basic -* mathematical operations needed. -* This class also defines SbgMatrix3f and SbgMatrix3d for floats and doubles. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_MATRIX_3_H -#define SBG_MATRIX_3_H - -// Sbg headers -#include -#include "sbg_vector3.h" - -//SbgRos message headers -#include "sbg_driver/SbgEkfQuat.h" -#include "sbg_driver/SbgEkfEuler.h" - -namespace sbg -{ - -/*! - * - */ -template -class SbgMatrix3 -{ -private: - - //---------------------------------------------------------------------// - //- Private variables -// - //---------------------------------------------------------------------// - - std::array m_data; - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Empty constructor. - */ - SbgMatrix3(void) - { - m_data[0] = static_cast(0.0); - m_data[1] = static_cast(0.0); - m_data[2] = static_cast(0.0); - m_data[3] = static_cast(0.0); - m_data[4] = static_cast(0.0); - m_data[5] = static_cast(0.0); - m_data[6] = static_cast(0.0); - m_data[7] = static_cast(0.0); - m_data[8] = static_cast(0.0); - } - - /*! - * Constructor. - * - * \param[in] value00 Matrix X value. - * \param[in] value01 Matrix Y value. - * \param[in] value02 Matrix Z value. - * \param[in] value10 Matrix X value. - * \param[in] value11 Matrix Y value. - * \param[in] value12 Matrix Z value. - * \param[in] value20 Matrix X value. - * \param[in] value21 Matrix Y value. - * \param[in] value22 Matrix Z value. - */ - SbgMatrix3(T value00, T value01, T value02, T value10, T value11, T value12, T value20, T value21, T value22) - { - m_data[0] = value00; - m_data[1] = value01; - m_data[2] = value02; - m_data[3] = value10; - m_data[4] = value11; - m_data[5] = value12; - m_data[6] = value20; - m_data[7] = value21; - m_data[8] = value22; - }; - - /*! - * Constructor. - * - * \param[in] p_raw_data Pointer to data array. - * \param[in] array_size Array size (Should be defined as 9). - */ - SbgMatrix3(const T* p_raw_data, size_t array_size) - { - assert(array_size == 9); - - m_data[0] = p_raw_data[0]; - m_data[1] = p_raw_data[1]; - m_data[2] = p_raw_data[2]; - m_data[3] = p_raw_data[3]; - m_data[4] = p_raw_data[4]; - m_data[5] = p_raw_data[5]; - m_data[6] = p_raw_data[6]; - m_data[7] = p_raw_data[7]; - m_data[8] = p_raw_data[8]; - }; - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Getter parenthesis operator. - * - * \param[in] i Line. - * \param[in] j Column. - * \return Element at index. - */ - const T operator() (int i, int j) const - { - assert(i * 3 + j < 9); - - return m_data[i * 3 + j]; - } - - /*! - * Get the raw data of the sbgMatrix. - * - * \return Raw vector data. - */ - const T *data(void) const - { - return static_cast(m_data.data()); - }; - - const SbgVector3 operator*(const SbgVector3& vect) const - { - T x = m_data[0] * vect(0) + m_data[1] * vect(1) + m_data[2] * vect(2); - T y = m_data[3] * vect(0) + m_data[4] * vect(1) + m_data[5] * vect(2); - T z = m_data[6] * vect(0) + m_data[7] * vect(1) + m_data[8] * vect(2); - - return SbgVector3(x, y, z); - } - - void transpose(void) - { - T swap = m_data[1]; - m_data[1] = m_data[3]; - m_data[3] = swap; - - swap = m_data[2]; - m_data[2] = m_data[6]; - m_data[6] = swap; - - swap = m_data[5]; - m_data[5] = m_data[7]; - m_data[7] = swap; - } - - /*! - * Make DCM from euler angles vector - * - * \param[in] vect Euler angles vector. - * \param[in] x Quaternion x value. - * \param[in] y Quaternion y value. - * \param[in] z Quaternion z value. - */ - void makeDcm(const SbgVector3f& euler) - { - float cr = cosf(euler(0)); - float sr = sinf(euler(0)); - float cp = cosf(euler(1)); - float sp = sinf(euler(1)); - float cy = cosf(euler(2)); - float sy = sinf(euler(2)); - - m_data[0] = cp * cy; - m_data[3] = cp * sy; - m_data[6] = -sp; - - m_data[1] = (sr * sp * cy) - (cr * sy); - m_data[4] = (sr * sp * sy) + (cr * cy); - m_data[7] = sr * cp; - - m_data[2] = (cr * sp * cy) + (sy * sr); - m_data[5] = (cr * sp * sy) - (sr * cy); - m_data[8] = cr * cp; - - } - - /*! - * Make DCM from quaternion values - * - * \param[in] w Quaternion w value. - * \param[in] x Quaternion x value. - * \param[in] y Quaternion y value. - * \param[in] z Quaternion z value. - */ - void makeDcm(float w, float x, float y, float z) - { - float wx = w * x; - float wy = w * y; - float wz = w * z; - float xy = x * y; - float xz = x * z; - float yz = y * z; - - m_data[0] = (2 * powf(w, 2)) + (2 * powf(x, 2)) - 1; - m_data[3] = (2 * xy) + (2 * wz); - m_data[6] = (2 * xz) - (2 * wy); - - m_data[1] = (2 * xy) - (2 * wz); - m_data[4] = (2 * powf(w, 2)) + (2 * powf(y, 2)) - 1; - m_data[7] = (2 * yz) + (2 * wx); - - m_data[2] = (2 * wy) + (2 * xz); - m_data[5] = (2 * yz) - (2 * wx); - m_data[8] = (2 * powf(w, 2)) + (2 * powf(z, 2)) - 1; - } - -}; - - -typedef SbgMatrix3 SbgMatrix3f; -typedef SbgMatrix3 SbgMatrix3d; - -} - -#endif // SBG_MATRIX_3_H diff --git a/sbg_ros_driver/include/sbg_driver/sbg_vector3.h b/sbg_ros_driver/include/sbg_driver/sbg_vector3.h deleted file mode 100644 index 8fd34e9..0000000 --- a/sbg_ros_driver/include/sbg_driver/sbg_vector3.h +++ /dev/null @@ -1,183 +0,0 @@ -/*! -* \file sbg_vector3.h -* \author SBG Systems -* \date 13/03/2020 -* -* \brief Handle a three components vector. -* -* This custom vector3 class is compatible for operations with SbgMatrix3 in -* order to avoid a dependancy on Eigen. -* This class also defines SbgVector3f and SbgVector3d for floats and doubles. -* -* \section CodeCopyright Copyright Notice -* MIT License -* -* Copyright (c) 2020 SBG Systems -* -* 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. -*/ - -#ifndef SBG_VECTOR_3_H -#define SBG_VECTOR_3_H - -// ROS headers -#include - -// Sbg headers -#include - -namespace sbg -{ -/*! - * Check if the two input numbers are equals, taking into account the machine precision (float/double). - * - * \template T Numeric template type. - * \param[in] firstValue First numeric value. - * \param[in] secondValue Second numeric value. - * \return True if the numbers are considered as equals. - */ -template -bool areEquals(T firstValue, T secondValue) -{ - // - // The machine epsilon has to be scaled to the magnitude of the values used. - // - return std::fabs(firstValue - secondValue) <= (std::numeric_limits::epsilon() * std::fabs(firstValue + secondValue)); -} - -/*! - * Class to define a Vector3. - */ -template -class SbgVector3 -{ -private: - - //---------------------------------------------------------------------// - //- Private variables -// - //---------------------------------------------------------------------// - - std::array m_data; - -public: - - //---------------------------------------------------------------------// - //- Constructor -// - //---------------------------------------------------------------------// - - /*! - * Empty constructor. - */ - SbgVector3(void) - { - m_data[0] = static_cast(0.0); - m_data[1] = static_cast(0.0); - m_data[2] = static_cast(0.0); - } - - /*! - * Constructor. - * - * \param[in] x_value Vector X value. - * \param[in] y_value Vector Y value. - * \param[in] z_value Vector Z value. - */ - SbgVector3(T x_value, T y_value, T z_value) - { - m_data[0] = x_value; - m_data[1] = y_value; - m_data[2] = z_value; - }; - - /*! - * Constructor. - * - * \param[in] p_raw_data Pointer to data array. - * \param[in] array_size Array size (Should be defined as 3). - */ - SbgVector3(const T* p_raw_data, size_t array_size) - { - assert(array_size == 3); - - m_data[0] = p_raw_data[0]; - m_data[1] = p_raw_data[1]; - m_data[2] = p_raw_data[2]; - }; - - //---------------------------------------------------------------------// - //- Parameters -// - //---------------------------------------------------------------------// - - //---------------------------------------------------------------------// - //- Operations -// - //---------------------------------------------------------------------// - - /*! - * Comparison equal operator. - * - * \param[in] ref_vector Vector to compare. - * \return True if the vector are equals. - */ - bool operator==(const SbgVector3 &ref_vector) - { - return ((areEquals(m_data[0], ref_vector.m_data[0])) - && (areEquals(m_data[1], ref_vector.m_data[1])) - && (areEquals(m_data[2], ref_vector.m_data[2]))); - }; - - /*! - * Comparison not equal operator. - * - * \param[in] ref_vector Vector to compare. - * \return True if the vector are equals. - */ - bool operator!=(const SbgVector3 &ref_vector) - { - return !(*this == ref_vector); - }; - - /*! - * Getter parenthesis operator - * \param[in] index Index of value to retrieve. - * \return Value at index. - */ - const T operator()(size_t index) const - { - assert(index < 3); - - return m_data[index]; - } - - /*! - * Get the raw data of the sbgVector. - * - * \return Raw vector data. - */ - const T *data(void) const - { - return static_cast(m_data.data()); - }; -}; - -typedef SbgVector3 SbgVector3f; -typedef SbgVector3 SbgVector3d; - -} - -#endif // SBG_VECTOR_3_H diff --git a/sbg_ros_driver/launch/examples/sbg_ellipseA.launch b/sbg_ros_driver/launch/examples/sbg_ellipseA.launch deleted file mode 100644 index c3cb8b6..0000000 --- a/sbg_ros_driver/launch/examples/sbg_ellipseA.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/sbg_ros_driver/launch/examples/sbg_ellipseE.launch b/sbg_ros_driver/launch/examples/sbg_ellipseE.launch deleted file mode 100644 index afc83e9..0000000 --- a/sbg_ros_driver/launch/examples/sbg_ellipseE.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/sbg_ros_driver/launch/examples/sbg_ellipseN.launch b/sbg_ros_driver/launch/examples/sbg_ellipseN.launch deleted file mode 100644 index e027360..0000000 --- a/sbg_ros_driver/launch/examples/sbg_ellipseN.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/sbg_ros_driver/launch/sbg_device.launch b/sbg_ros_driver/launch/sbg_device.launch deleted file mode 100644 index 29fb59e..0000000 --- a/sbg_ros_driver/launch/sbg_device.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/sbg_ros_driver/launch/sbg_device_mag_calibration.launch b/sbg_ros_driver/launch/sbg_device_mag_calibration.launch deleted file mode 100644 index 0c43db5..0000000 --- a/sbg_ros_driver/launch/sbg_device_mag_calibration.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgAirData.msg b/sbg_ros_driver/msg/SbgAirData.msg deleted file mode 100644 index 8a6c7fe..0000000 --- a/sbg_ros_driver/msg/SbgAirData.msg +++ /dev/null @@ -1,23 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up micro s -uint32 time_stamp - -# Airdata sensor status -SbgAirDataStatus status - -# Raw absolute pressure measured by the barometer sensor in Pascals. -float64 pressure_abs - -# Altitude computed from barometric altimeter in meters and positive upward. -float64 altitude - -# Raw differential pressure measured by the pitot tube in Pascal. -float64 pressure_diff - -# True airspeed measured by a pitot tube in m.s^-1 and positive forward. -float64 true_air_speed - -# Outside air temperature in °C that could be used to compute true airspeed from differential pressure. -float64 air_temperature \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgAirDataStatus.msg b/sbg_ros_driver/msg/SbgAirDataStatus.msg deleted file mode 100644 index fb28a58..0000000 --- a/sbg_ros_driver/msg/SbgAirDataStatus.msg +++ /dev/null @@ -1,20 +0,0 @@ -# SBG Ellipse Messages -# Submessage - -# True if the time stamp field represents a delay instead of an absolute time stamp. -bool is_delay_time - -# True if the pressure field is filled and valid. -bool pressure_valid - -# True if the barometric altitude field is filled and valid. -bool altitude_valid - -# True if the differential pressure field is filled and valid. -bool pressure_diff_valid - -# True if the true airspeed field is filled and valid. -bool air_speed_valid - -# True if the output air temperature field is filled and valid. -bool air_temperature_valid \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgEkfEuler.msg b/sbg_ros_driver/msg/SbgEkfEuler.msg deleted file mode 100644 index f9f3c84..0000000 --- a/sbg_ros_driver/msg/SbgEkfEuler.msg +++ /dev/null @@ -1,15 +0,0 @@ -# SBG Ellipse Messages - -Header header - -# Time since sensor is powered up micro s -uint32 time_stamp - -# [Roll, Pitch, Yaw (heading)] angle rad -geometry_msgs/Vector3 angle - -# [Roll, Pitch, Yaw (heading)] angle accuracy (1 sigma) rad -geometry_msgs/Vector3 accuracy - -# Global solution status -SbgEkfStatus status diff --git a/sbg_ros_driver/msg/SbgEkfNav.msg b/sbg_ros_driver/msg/SbgEkfNav.msg deleted file mode 100644 index d4bcfcb..0000000 --- a/sbg_ros_driver/msg/SbgEkfNav.msg +++ /dev/null @@ -1,25 +0,0 @@ -# SBG Ellipse Messages - -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# Velocity (North, East, Down) direction m/s -geometry_msgs/Vector3 velocity - -# 1 sigma Velocity in (North, East, Down) direction accuracy m/s -geometry_msgs/Vector3 velocity_accuracy - -# [Latitude (deg), Longitude (deg), Altitude (above Mean Sea Level in meters)] -geometry_msgs/Vector3 position - -# Altitude difference between the geoid and the Ellipsoid (WGS-84 Altitude - MSL Altitude) -# (Height above Ellipsoid = altitude + undulation) -float32 undulation - -# Latitude, Longitude, Vertical accuracy m (1 sigma) -geometry_msgs/Vector3 position_accuracy - -# Global solution status -SbgEkfStatus status diff --git a/sbg_ros_driver/msg/SbgEkfQuat.msg b/sbg_ros_driver/msg/SbgEkfQuat.msg deleted file mode 100644 index 253213b..0000000 --- a/sbg_ros_driver/msg/SbgEkfQuat.msg +++ /dev/null @@ -1,15 +0,0 @@ -# SBG Ellipse Messages - -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# Quaternion parameter (ROS order X, Y, Z, W) -geometry_msgs/Quaternion quaternion - -# [Roll, Pitch, Yaw (heading)] angle accuracy (1 sigma) rad -geometry_msgs/Vector3 accuracy - -# Global solution status -SbgEkfStatus status diff --git a/sbg_ros_driver/msg/SbgEkfStatus.msg b/sbg_ros_driver/msg/SbgEkfStatus.msg deleted file mode 100644 index 0274454..0000000 --- a/sbg_ros_driver/msg/SbgEkfStatus.msg +++ /dev/null @@ -1,55 +0,0 @@ -# SBG Ellipse Messages -# Submessage - -# Defines the Kalman filter computation mode (see the table 4 below) -# 0 UNINITIALIZED The Kalman filter is not initialized and the returned data are all invalid. -# 1 VERTICAL_GYRO The Kalman filter only rely on a vertical reference to compute roll and pitch angles. Heading and navigation data drift freely. -# 2 AHRS A heading reference is available, the Kalman filter provides full orientation but navigation data drift freely. -# 3 NAV_VELOCITY The Kalman filter computes orientation and velocity. Position is freely integrated from velocity estimation. -# 4 NAV_POSITION Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided. -uint8 solution_mode - -# True if Attitude data is reliable (Roll/Pitch error < 0,5 deg) -bool attitude_valid - -# True if Heading data is reliable (Heading error < 1 deg) -bool heading_valid - -# True if Velocity data is reliable (velocity error < 1.5 m/s) -bool velocity_valid - -# True if Position data is reliable (Position error < 10m) -bool position_valid - -# True if vertical reference is used in solution (data used and valid since 3s) -bool vert_ref_used - -# True if magnetometer is used in solution (data used and valid since 3s) -bool mag_ref_used - -# True if GPS velocity is used in solution (data used and valid since 3s) -bool gps1_vel_used - -# True if GPS Position is used in solution (data used and valid since 3s) -bool gps1_pos_used - -# True if GPS Course is used in solution (data used and valid since 3s) -bool gps1_course_used - -# True if GPS True Heading is used in solution (data used and valid since 3s) -bool gps1_hdt_used - -# True if GPS2 velocity is used in solution (data used and valid since 3s) -bool gps2_vel_used - -# True if GPS2 Position is used in solution (data used and valid since 3s) -bool gps2_pos_used - -# True if GPS2 Course is used in solution (data used and valid since 3s) -bool gps2_course_used - -# True if GPS2 True Heading is used in solution (data used and valid since 3s) -bool gps2_hdt_used - -# True if Odometer is used in solution (data used and valid since 3s) -bool odo_used diff --git a/sbg_ros_driver/msg/SbgEvent.msg b/sbg_ros_driver/msg/SbgEvent.msg deleted file mode 100644 index deca02f..0000000 --- a/sbg_ros_driver/msg/SbgEvent.msg +++ /dev/null @@ -1,33 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# True if we have received events at a higher rate than 1 kHz. -bool overflow - -# True if at least two events have been received. -bool offset_0_valid - -# True if at least three events have been received. -bool offset_1_valid - -# True if at least four events have been received. -bool offset_2_valid - -# True if five events have been received. -bool offset_3_valid - - -# Time offset for the second received event. (us) -uint16 time_offset_0 - -# Time offset for the third received event. (us) -uint16 time_offset_1 - -# Time offset for the fourth received event. (us) -uint16 time_offset_2 - -# Time offset for the fifth received event. (us) -uint16 time_offset_3 diff --git a/sbg_ros_driver/msg/SbgGpsHdt.msg b/sbg_ros_driver/msg/SbgGpsHdt.msg deleted file mode 100644 index bfbc21d..0000000 --- a/sbg_ros_driver/msg/SbgGpsHdt.msg +++ /dev/null @@ -1,27 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# GPS True Heading status. -# 0 SOL_COMPUTED A valid solution has been computed. -# 1 INSUFFICIENT_OBS Not enough valid SV to compute a solution. -# 2 INTERNAL_ERROR An internal error has occurred. -# 3 HEIGHT_LIMIT The height limit has been exceeded. -uint16 status - -# GPS Time of Week ms -uint32 tow - -# True heading angle (0 to 360 deg). -float32 true_heading - -# 1 sigma True heading estimated accuracy (0 to 360 deg). -float32 true_heading_acc - -# Pitch angle from the master to the rover -float32 pitch - -# 1 sigma pitch estimated accuracy -float32 pitch_acc diff --git a/sbg_ros_driver/msg/SbgGpsPos.msg b/sbg_ros_driver/msg/SbgGpsPos.msg deleted file mode 100644 index e4f8a84..0000000 --- a/sbg_ros_driver/msg/SbgGpsPos.msg +++ /dev/null @@ -1,30 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# GPS position fix and status bitmask -SbgGpsPosStatus status - -# GPS Time of Week ms -uint32 gps_tow - -# [Latitude (positive North deg), Longitude (positive East deg), Altitude (Above Mean Sea Level m)] -geometry_msgs/Vector3 position - -# Altitude difference between the geoid and the Ellipsoid (WGS-84 Altitude - MSL Altitude) -# (Height above Ellipsoid = altitude + undulation) -float32 undulation - -# Latitude, Longitude, Vertical accuracy m (1 sigma) -geometry_msgs/Vector3 position_accuracy - -# Number of space vehicles used in GNSS solution -uint8 num_sv_used - -# ID of the DGPS/RTK base station in use -uint16 base_station_id - -# Differential data age 0.01 s -uint16 diff_age diff --git a/sbg_ros_driver/msg/SbgGpsPosStatus.msg b/sbg_ros_driver/msg/SbgGpsPosStatus.msg deleted file mode 100644 index 6f2dd3c..0000000 --- a/sbg_ros_driver/msg/SbgGpsPosStatus.msg +++ /dev/null @@ -1,39 +0,0 @@ -# SBG Ellipse Messages -# Submessage - - -# The raw GPS position status -# 0 SOL_COMPUTED A valid solution has been computed. -# 1 INSUFFICIENT_OBS Not enough valid SV to compute a solution. -# 2 INTERNAL_ERROR An internal error has occurred. -# 3 HEIGHT_LIMIT The height limit has been exceeded. -uint8 status - -# The raw GPS position type -# 0 NO_SOLUTION No valid solution available. -# 1 UNKNOWN_TYPE An unknown solution type has been computed. -# 2 SINGLE Single point solution position. -# 3 PSRDIFF Standard Pseudorange Differential Solution (DGPS). -# 4 SBAS SBAS satellite used for differential corrections. -# 5 OMNISTAR Omnistar VBS Position (L1 sub-meter). -# 6 RTK_FLOAT Floating RTK ambiguity solution (20 cms RTK). -# 7 RTK_INT Integer RTK ambiguity solution (2 cms RTK). -# 8 PPP_FLOAT Precise Point Positioning with float ambiguities -# 9 PPP_INT Precise Point Positioning with fixed ambiguities -# 10 FIXED Fixed location solution position -uint8 type - -# True if GPS L1 is used in the solution -bool gps_l1_used - -# True if GPS L2 is used in the solution -bool gps_l2_used - -# True if GPS L5 is used in the solution -bool gps_l5_used - -# True if GLONASS L1 is used in the solution -bool glo_l1_used - -# True if GLONASS L2 is used in the solution -bool glo_l2_used diff --git a/sbg_ros_driver/msg/SbgGpsRaw.msg b/sbg_ros_driver/msg/SbgGpsRaw.msg deleted file mode 100644 index 2d7bd4d..0000000 --- a/sbg_ros_driver/msg/SbgGpsRaw.msg +++ /dev/null @@ -1,5 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Raw Data -uint8[] data \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgGpsVel.msg b/sbg_ros_driver/msg/SbgGpsVel.msg deleted file mode 100644 index 5d306cb..0000000 --- a/sbg_ros_driver/msg/SbgGpsVel.msg +++ /dev/null @@ -1,23 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# GPS velocity fix and status bitmask - -SbgGpsVelStatus status - -# GPS Time of Week ms -uint32 gps_tow - -# Velocity in [North, East, Down] direction m/s -geometry_msgs/Vector3 vel - -# 1 sgima Accuracy in [North, East, Down] direction m/s -geometry_msgs/Vector3 vel_acc - -# True direction of motion over ground (0 to 360 deg) -float32 course - -# 1 sgima course accuracy (0 to 360 deg). -float32 course_acc diff --git a/sbg_ros_driver/msg/SbgGpsVelStatus.msg b/sbg_ros_driver/msg/SbgGpsVelStatus.msg deleted file mode 100644 index e9e1386..0000000 --- a/sbg_ros_driver/msg/SbgGpsVelStatus.msg +++ /dev/null @@ -1,16 +0,0 @@ -# SBG Ellipse Messages -# Submessage - -# The raw GPS velocity status -# 0 SOL_COMPUTED A valid solution has been computed. -# 1 INSUFFICIENT_OBS Not enough valid SV to compute a solution. -# 2 INTERNAL_ERROR An internal error has occurred. -# 3 LIMIT Velocity limit exceeded. -uint8 vel_status - -# The raw GPS velocity type -# 0 VEL_NO_SOLUTION No valid velocity solution available. -# 1 VEL_UNKNOWN_TYPE An unknown solution type has been computed. -# 2 VEL_DOPPLER A Doppler velocity has been computed. -# 3 VEL_DIFFERENTIAL A velocity has been computed between two positions. -uint8 vel_type \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgImuData.msg b/sbg_ros_driver/msg/SbgImuData.msg deleted file mode 100644 index e4bbcbb..0000000 --- a/sbg_ros_driver/msg/SbgImuData.msg +++ /dev/null @@ -1,23 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# IMU Status -SbgImuStatus imu_status - -# Filtered Accelerometer - X, Y, Z axis m/s2 -geometry_msgs/Vector3 accel - -# Filtered Gyroscope - X, Y, Z axis rad/s -geometry_msgs/Vector3 gyro - -# Internal Temperature degC -float32 temp - -# Sculling output - X, Y, Z axis m/s2 -geometry_msgs/Vector3 delta_vel - -# Coning output - X, Y, Z axis rad/s -geometry_msgs/Vector3 delta_angle diff --git a/sbg_ros_driver/msg/SbgImuShort.msg b/sbg_ros_driver/msg/SbgImuShort.msg deleted file mode 100644 index 616dff5..0000000 --- a/sbg_ros_driver/msg/SbgImuShort.msg +++ /dev/null @@ -1,17 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# IMU Status -SbgImuStatus imu_status - -# X, Y, Z delta velocity. Unit is 1048576 LSB for 1 m.s^-2. -geometry_msgs/Vector3 delta_velocity - -# X, Y, Z delta angle. Unit is 67108864 LSB for 1 rad.s^-1. -geometry_msgs/Vector3 delta_angle - -# IMU average temperature. Unit is 256 LSB for 1°C. -int16 temperature \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgImuStatus.msg b/sbg_ros_driver/msg/SbgImuStatus.msg deleted file mode 100644 index 1c755e9..0000000 --- a/sbg_ros_driver/msg/SbgImuStatus.msg +++ /dev/null @@ -1,32 +0,0 @@ -# SBG Ellipse Messages -# Submessage SbgImuData - -# True if the communication with the IMU is ok. -bool imu_com - -# True if internal IMU passes Built In Test (Calibration, CPU) -bool imu_status - -# True if accelerometer X passes Built In Test -bool imu_accel_x - -# True if accelerometer Y passes Built In Test -bool imu_accel_y - -# True if accelerometer Z passes Built In Test -bool imu_accel_z - -# True if gyroscope X passes Built In Test -bool imu_gyro_x - -# True if gyroscope Y passes Built In Test -bool imu_gyro_y - -# True if gyroscope Z passes Built In Test -bool imu_gyro_z - -# True if accelerometers are within operating range -bool imu_accels_in_range - -# True if gyroscopes are within operating range -bool imu_gyros_in_range diff --git a/sbg_ros_driver/msg/SbgMag.msg b/sbg_ros_driver/msg/SbgMag.msg deleted file mode 100644 index 3ca4c96..0000000 --- a/sbg_ros_driver/msg/SbgMag.msg +++ /dev/null @@ -1,14 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up (us) -uint32 time_stamp - -# Magnetometer output (X, Y, Z) -geometry_msgs/Vector3 mag - -# Accelerometer output (X, Y, Z) in m/s2 -geometry_msgs/Vector3 accel - -# Status -SbgMagStatus status diff --git a/sbg_ros_driver/msg/SbgMagCalib.msg b/sbg_ros_driver/msg/SbgMagCalib.msg deleted file mode 100644 index 670da15..0000000 --- a/sbg_ros_driver/msg/SbgMagCalib.msg +++ /dev/null @@ -1,2 +0,0 @@ -# SBG Ellipse Messages -Header header \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgMagStatus.msg b/sbg_ros_driver/msg/SbgMagStatus.msg deleted file mode 100644 index 5dd5dc4..0000000 --- a/sbg_ros_driver/msg/SbgMagStatus.msg +++ /dev/null @@ -1,29 +0,0 @@ -# SBG Ellipse Messages -# Submessage - -# True if the magnetometer X has passed the self test. -bool mag_x - -# True if the magnetometer Y has passed the self test. -bool mag_y - -# True if the magnetometer Z has passed the self test. -bool mag_z - -# True if the accelerometer X has passed the self test. -bool accel_x - -# True if the accelerometer Y has passed the self test. -bool accel_y - -# True if the accelerometer Z has passed the self test. -bool accel_z - -# True if magnetometer is not saturated -bool mags_in_range - -# True if accelerometer is not saturated -bool accels_in_range - -# True if magnetometer seems to be calibrated -bool calibration diff --git a/sbg_ros_driver/msg/SbgOdoVel.msg b/sbg_ros_driver/msg/SbgOdoVel.msg deleted file mode 100644 index b756823..0000000 --- a/sbg_ros_driver/msg/SbgOdoVel.msg +++ /dev/null @@ -1,13 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# Real Measurement -# True if this log comes from a real pulse measurement -# False if it comes from a timeout -bool status - -# Velocity in odometer direction (m/s) -float32 vel diff --git a/sbg_ros_driver/msg/SbgShipMotion.msg b/sbg_ros_driver/msg/SbgShipMotion.msg deleted file mode 100644 index f6c8cf4..0000000 --- a/sbg_ros_driver/msg/SbgShipMotion.msg +++ /dev/null @@ -1,21 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up us -uint32 time_stamp - -# Main heave period in seconds. s float 4 4 -uint16 heave_period - -# [Surge, Sway, Heave (positive down)] at main location (in m) -# Note : Surge & Sway are not fulfilled -geometry_msgs/Vector3 ship_motion - -# [Longitudinal, Lateral, Vertical (positive down)] acceleration (in m/s2) -geometry_msgs/Vector3 acceleration - -# [Longitudinal, Lateral, Vertical (positive down)] velocity (in m/s) -geometry_msgs/Vector3 velocity - -# Ship motion output status -SbgShipMotionStatus status diff --git a/sbg_ros_driver/msg/SbgShipMotionStatus.msg b/sbg_ros_driver/msg/SbgShipMotionStatus.msg deleted file mode 100644 index 4e5933a..0000000 --- a/sbg_ros_driver/msg/SbgShipMotionStatus.msg +++ /dev/null @@ -1,19 +0,0 @@ -# SBG Ellipse Messages -# SbgShipMotionStatus - -# True after heave convergence time. -# False in following conditions: -# - Turn occurred and no velocity aiding is available -# - Heave reached higher/lower limits -# - If a step is detected and filter has to re-converge -# - If internal failure -bool heave_valid - -# True if heave output is compensated for transient accelerations -bool heave_vel_aided - -# True if the swell period is provided in this output -bool period_available - -# True if the period returned is assumed to be valid or not. -bool period_valid \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgStatus.msg b/sbg_ros_driver/msg/SbgStatus.msg deleted file mode 100644 index 2d2f9d7..0000000 --- a/sbg_ros_driver/msg/SbgStatus.msg +++ /dev/null @@ -1,15 +0,0 @@ -# SBG Ellipse Messages - -Header header - -# Time since sensor is powered up (in us) -uint32 time_stamp - -# General status bitmask and enums -SbgStatusGeneral status_general - -# Communication status bitmask and enums. -SbgStatusCom status_com - -# Aiding equipments status bitmask and enums. -SbgStatusAiding status_aiding diff --git a/sbg_ros_driver/msg/SbgStatusAiding.msg b/sbg_ros_driver/msg/SbgStatusAiding.msg deleted file mode 100644 index e4bd877..0000000 --- a/sbg_ros_driver/msg/SbgStatusAiding.msg +++ /dev/null @@ -1,23 +0,0 @@ -# SBG Ellipse Messages -# SbgStatus submessage - -# AIDING_GPS1_POS_RECV true when valid GPS 1 position data is received -bool gps1_pos_recv - -# AIDING_GPS1_VEL_RECV true when valid GPS 1 velocity data is received -bool gps1_vel_recv - -# AIDING_GPS1_HDT_RECV true when valid GPS 1 true heading data is received -bool gps1_hdt_recv - -# AIDING_GPS1_UTC_RECV true when valid GPS 1 UTC time data is received -bool gps1_utc_recv - -# AIDING_MAG_RECV true when valid Magnetometer data is received -bool mag_recv - -# AIDING_ODO_RECV true when Odometer pulse is received -bool odo_recv - -# AIDING_DVL_RECV true when valid DVL data is received -bool dvl_recv diff --git a/sbg_ros_driver/msg/SbgStatusCom.msg b/sbg_ros_driver/msg/SbgStatusCom.msg deleted file mode 100644 index ca9f767..0000000 --- a/sbg_ros_driver/msg/SbgStatusCom.msg +++ /dev/null @@ -1,60 +0,0 @@ -# SBG Ellipse Messages -# SbgStatus submessage - -# PORT A: False in case of low level communication error. -bool port_a - -# PORT B: False in case of low level communication error. -bool port_b - -# PORT C: False in case of low level communication error. -bool port_c - -# PORT D: False in case of low level communication error. -bool port_d - -# PORT E: False in case of low level communication error. -bool port_e - -# PORT A RX: False in case of saturation on PORT A input -bool port_a_rx - -# PORT A TX: False in case of saturation on PORT A output -bool port_a_tx - -# PORT B RX: False in case of saturation on PORT B input -bool port_b_rx - -# PORT B TX: False in case of saturation on PORT B output -bool port_b_tx - -# PORT C RX: False in case of saturation on PORT C input -bool port_c_rx - -# PORT C TX: False in case of saturation on PORT C output -bool port_c_tx - -# PORT D RX: False in case of saturation on PORT D input -bool port_d_rx - -# PORT D TX: False in case of saturation on PORT D output -bool port_d_tx - -# PORT E RX: False in case of saturation on PORT E input -bool port_e_rx - -# PORT E TX: False in case of saturation on PORT E output -bool port_e_tx - -# CAN RX: False in case of saturation on CAN Bus output buffer -bool can_rx - -# CAN TX: False in case of saturation on CAN Bus input buffer -bool can_tx - -# CAN BUS -# 0 CAN BUS OFF Bus OFF operation due to too much errors. -# 1 CAN BUS TX_RX_ERR Transmit or received error. -# 2 CAN BUS OK The CAN bus is working correctly. -# 3 CAN BUS ERROR A general error has occurred on the CAN bus. -uint8 can_status \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgStatusGeneral.msg b/sbg_ros_driver/msg/SbgStatusGeneral.msg deleted file mode 100644 index 0ca2181..0000000 --- a/sbg_ros_driver/msg/SbgStatusGeneral.msg +++ /dev/null @@ -1,22 +0,0 @@ -# SBG Ellipse Messages -# SbgStatus submessage - -# General main power -# True when main power supply is OK. -bool main_power - -# General imu power -# True when IMU power supply is OK. -bool imu_power - -# General gps power -# Set to True when GPS power supply is OK. -bool gps_power - -# General Settings -# True if settings were correctly loaded -bool settings - -# General Temperature -# True when temperature is within specified limits. -bool temperature \ No newline at end of file diff --git a/sbg_ros_driver/msg/SbgUtcTime.msg b/sbg_ros_driver/msg/SbgUtcTime.msg deleted file mode 100644 index 346ec9e..0000000 --- a/sbg_ros_driver/msg/SbgUtcTime.msg +++ /dev/null @@ -1,32 +0,0 @@ -# SBG Ellipse Messages -Header header - -# Time since sensor is powered up (us) -uint32 time_stamp - -# General UTC time and clock sync status -SbgUtcTimeStatus clock_status - -# Year -uint16 year - -# Month in Year [1 ... 12] -uint8 month - -# Day in Month [1 ... 31] -uint8 day - -# Hour in day [0 ... 23] -uint8 hour - -# Minute in hour [0 ... 59] -uint8 min - -# Second in minute [0 ... 60], Note 60 is when a leap second is added. -uint8 sec - -# Nanosecond of second. -uint32 nanosec - -# GPS Time of week (ms) -uint32 gps_tow diff --git a/sbg_ros_driver/msg/SbgUtcTimeStatus.msg b/sbg_ros_driver/msg/SbgUtcTimeStatus.msg deleted file mode 100644 index 1be0664..0000000 --- a/sbg_ros_driver/msg/SbgUtcTimeStatus.msg +++ /dev/null @@ -1,20 +0,0 @@ -# SBG Ellipse Messages - -# True when a clock input can be used to synchronize the internal clock. -bool clock_stable - -# Define the internal clock estimation status -# 0 An error has occurred on the clock estimation. -# 1 The clock is only based on the internal crystal. -# 2 A PPS has been detected and the clock is converging to it. -# 3 The clock has converged to the PPS and is within 500ns. -uint8 clock_status - -# True if UTC time is synchronized with a PPS -bool clock_utc_sync - -# UTC validity status -# 0 The UTC time is not known, we are just propagating the UTC time internally. -# 1 We have received valid UTC time information but we don't have the leap seconds information. -# 2 We have received valid UTC time data with valid leap seconds. -uint8 clock_utc_status \ No newline at end of file diff --git a/sbg_ros_driver/package.xml b/sbg_ros_driver/package.xml deleted file mode 100644 index 81e5f77..0000000 --- a/sbg_ros_driver/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - sbg_driver - 2.0.3 - - ROS driver package for communication with the SBG navigation systems. - - SBG Systems - Hadrien Pomès - MIT - - http://wiki.ros.org/sbg_driver - - catkin - - roscpp - sensor_msgs - std_msgs - std_srvs - geometry_msgs - message_generation - - roscpp - sensor_msgs - geometry_msgs - std_msgs - std_srvs - message_runtime - - diff --git a/sbg_ros_driver/releasePackage.md b/sbg_ros_driver/releasePackage.md deleted file mode 100644 index e0c4ed4..0000000 --- a/sbg_ros_driver/releasePackage.md +++ /dev/null @@ -1,52 +0,0 @@ -# Release a new version for the sbg_driver - -This page will explain how the sbg_driver should be released, according to these docs : -* [ROS/ReleasingAPackage](http://wiki.ros.org/ROS/ReleasingAPackage) -* [bloom/ReleaseCatkinPackage](http://wiki.ros.org/bloom/Tutorials/ReleaseCatkinPackage) -* [bloom/FirstTimeRelease](http://wiki.ros.org/bloom/Tutorials/FirstTimeRelease#Configure_a_Release_Track) -* [ROS/PrereleaseTests](http://wiki.ros.org/regression_tests#How_do_I_setup_my_system_to_run_a_prerelease.3F) - -## Update the changelog -Once all changes have been committed in the master branch, and the driver is ready for release, update the changelog file. - -In the source directory of the driver, execute the following command. -Clean up the changelog file, but do not touch the "Forthcoming" title, it will be updated by bloom during the release process. -Once it has been modified, commit the changelog. - -``` -catkin_generate_changelog -``` - -## Update the sbg_driver version -Use the catkin command to prepare the driver for release. You can add the '--bump' option, to choose the version increment. -By default, it will increase the patch version, 2.0.2 -> 2.0.3 - -``` -catkin_prepare_release --bump {major, minor, patch} -``` - -## Running prerelease tests -A completed prerelease test is a quite good guarantee that the package will run perfectly. Follow the instructions from the -prerelease doc pages. -The following command could be used to generate the prerelease executables. - -``` -generate_prerelease_script.py \ -https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml \ -melodic default ubuntu bionic amd64 sbg_driver --level 0 --output-dir . -``` - -## Release the package -Execute the bloom command to release, for every required rosdistro (Melodic, Kinetic). -These command will ask for automatic push to the sbg_driver_release repository, and pull request to the rosdistro repository. - -``` -bloom-release sbg_driver --rosdistro -``` - -In case you want to release the driver for a new distro, use the following command, -and refer to the documentation page to release a package for the first time. - -``` -bloom-release --rosdistro --track sbg_driver --edit -``` diff --git a/sbg_ros_driver/src/config_applier.cpp b/sbg_ros_driver/src/config_applier.cpp deleted file mode 100644 index 19888b7..0000000 --- a/sbg_ros_driver/src/config_applier.cpp +++ /dev/null @@ -1,456 +0,0 @@ -// File header -#include - -using sbg::ConfigApplier; - -/*! - * Class to apply configuration to a device. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -ConfigApplier::ConfigApplier(SbgEComHandle &ref_sbg_com_handle): -m_reboot_needed_(false), -m_ref_sbg_com_handle(ref_sbg_com_handle) -{ - -} - -//---------------------------------------------------------------------// -//- Private methods -// -//---------------------------------------------------------------------// - -void ConfigApplier::checkConfigurationGet(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title) const -{ - if (ref_sbg_error_code == SBG_INVALID_PARAMETER) - { - ROS_WARN("SBG_DRIVER - [Config] Configuration %s is not available for the connected device.", ref_conf_title.c_str()); - } - else if (ref_sbg_error_code != SBG_NO_ERROR) - { - std::string error_message("[Config] Unable to get the "); - error_message.append(ref_conf_title); - error_message.append(" configuration : "); - error_message.append(sbgErrorCodeToString(ref_sbg_error_code)); - - throw ros::Exception(error_message); - } -} - -void ConfigApplier::checkConfigurationApplied(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title) -{ - if (ref_sbg_error_code == SBG_INVALID_PARAMETER) - { - ROS_WARN("SBG_DRIVER - [Config] Configuration %s is not available for the connected device.", ref_conf_title.c_str()); - } - else if (ref_sbg_error_code != SBG_NO_ERROR) - { - std::string error_message("[Config] Unable to set the "); - error_message.append(ref_conf_title); - error_message.append(" configuration : "); - error_message.append(sbgErrorCodeToString(ref_sbg_error_code)); - - throw ros::Exception(error_message); - } - else - { - ROS_INFO("SBG_DRIVER - [Config] %s updated on the device.", ref_conf_title.c_str()); - m_reboot_needed_ = true; - } -} - -void ConfigApplier::configureInitCondition(const SbgEComInitConditionConf& ref_init_condition) -{ - // - // Get the initial condition of the device, compare with the loaded parameters. - // If the conditions are different, update the device configuration with the loaded parameters. - // - SbgEComInitConditionConf init_condition; - SbgErrorCode error_code; - - error_code = sbgEComCmdSensorGetInitCondition(&m_ref_sbg_com_handle, &init_condition); - - checkConfigurationGet(error_code, std::string("Init conditions")); - - if ((init_condition.year != ref_init_condition.year) - || (init_condition.month != ref_init_condition.month) - || (init_condition.day != ref_init_condition.day) - || !areEquals(init_condition.altitude, ref_init_condition.altitude) - || !areEquals(init_condition.latitude, ref_init_condition.latitude) - || !areEquals(init_condition.longitude, ref_init_condition.longitude)) - { - error_code = sbgEComCmdSensorSetInitCondition(&m_ref_sbg_com_handle, &ref_init_condition); - - checkConfigurationApplied(error_code, std::string("Init conditions")); - } -} - -void ConfigApplier::configureMotionProfile(const SbgEComModelInfo& ref_motion_profile) -{ - // - // Get the motion profile ID, and compare with the loaded one parameter. - // If the profiles are different, update the device with the loaded one. - // - SbgEComModelInfo motion_profile; - SbgErrorCode error_code; - - error_code = sbgEComCmdSensorGetMotionProfileInfo(&m_ref_sbg_com_handle, &motion_profile); - - checkConfigurationGet(error_code, std::string("Motion profile")); - - if (motion_profile.id != ref_motion_profile.id) - { - error_code = sbgEComCmdSensorSetMotionProfileId(&m_ref_sbg_com_handle, ref_motion_profile.id); - - checkConfigurationApplied(error_code, std::string("Motion profile")); - } -} - -void ConfigApplier::configureImuAlignement(const SbgEComSensorAlignmentInfo& ref_sensor_align, const sbg::SbgVector3& ref_level_arms) -{ - // - // Get the IMU alignement and level arms, and compare with the parameters. - // If the alignement are differents, update the device with the loaded parameters. - // - SbgErrorCode error_code; - SbgEComSensorAlignmentInfo sensor_alignement; - float level_arms_device[3]; - - error_code = sbgEComCmdSensorGetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &sensor_alignement, level_arms_device); - - checkConfigurationGet(error_code, std::string("IMU alignement")); - - SbgVector3 level_arms_vector = SbgVector3(level_arms_device, 3); - - if ((level_arms_vector != ref_level_arms) - || (sensor_alignement.axisDirectionX != ref_sensor_align.axisDirectionX) - || (sensor_alignement.axisDirectionY != ref_sensor_align.axisDirectionY) - || !areEquals(sensor_alignement.misRoll, ref_sensor_align.misRoll) - || !areEquals(sensor_alignement.misPitch, ref_sensor_align.misPitch) - || !areEquals(sensor_alignement.misYaw, ref_sensor_align.misYaw)) - { - error_code = sbgEComCmdSensorSetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &ref_sensor_align, level_arms_vector.data()); - - checkConfigurationApplied(error_code, std::string("IMU alignement")); - } -} - -void ConfigApplier::configureAidingAssignement(const SbgEComAidingAssignConf& ref_aiding_assign) -{ - // - // Get the aiding assignement, and compare with the loaded parameters. - // If the assignement are differents, udpdate the device with the loaded parameters. - // - SbgEComAidingAssignConf aiding_assign; - SbgErrorCode error_code; - - error_code = sbgEComCmdSensorGetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign); - - checkConfigurationGet(error_code, std::string("Aiding assignement")); - - if ((aiding_assign.gps1Port != ref_aiding_assign.gps1Port) - || (aiding_assign.gps1Sync != ref_aiding_assign.gps1Sync) - || (aiding_assign.odometerPinsConf != ref_aiding_assign.odometerPinsConf) - || (aiding_assign.rtcmPort != ref_aiding_assign.rtcmPort)) - { - error_code = sbgEComCmdSensorSetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign); - - checkConfigurationApplied(error_code, std::string("Aiding assignement")); - } -} - -void ConfigApplier::configureMagModel(const SbgEComModelInfo& ref_mag_model) -{ - // - // Get the magnetometer model, and compare with the loaded parameter. - // If the model are different, update the device with the loaded parameter. - // - SbgEComModelInfo model_info; - SbgErrorCode error_code; - - error_code = sbgEComCmdMagGetModelInfo(&m_ref_sbg_com_handle, &model_info); - - checkConfigurationGet(error_code, std::string("Magnetometer model")); - - if (model_info.id != ref_mag_model.id) - { - error_code = sbgEComCmdMagSetModelId(&m_ref_sbg_com_handle, ref_mag_model.id); - - checkConfigurationApplied(error_code, std::string("Magnetometer model")); - } -} - -void ConfigApplier::configureMagRejection(const SbgEComMagRejectionConf& ref_mag_rejection) -{ - // - // Get the magnetometer rejection model, and compare with the loaded parameter. - // If the model are different, update the device with the loaded parameter. - // - SbgEComMagRejectionConf mag_rejection; - SbgErrorCode error_code; - - error_code = sbgEComCmdMagGetRejection(&m_ref_sbg_com_handle, &mag_rejection); - - checkConfigurationGet(error_code, std::string("Magnetometer rejection")); - - if (mag_rejection.magneticField != ref_mag_rejection.magneticField) - { - error_code = sbgEComCmdMagSetRejection(&m_ref_sbg_com_handle, &ref_mag_rejection); - - checkConfigurationApplied(error_code, std::string("Magnetometer rejection")); - } -} - -void ConfigApplier::configureGnssModel(const SbgEComModelInfo& ref_gnss_model) -{ - // - // Get the Gnss model, and compare with the loaded model. - // If the models are different, update the device with the loaded model. - // - SbgEComModelInfo model_info; - SbgErrorCode error_code; - - error_code = sbgEComCmdGnss1GetModelInfo(&m_ref_sbg_com_handle, &model_info); - - checkConfigurationGet(error_code, std::string("Gnss model")); - - if (model_info.id != ref_gnss_model.id) - { - error_code = sbgEComCmdGnss1SetModelId(&m_ref_sbg_com_handle, ref_gnss_model.id); - - checkConfigurationApplied(error_code, std::string("Gnss model")); - } -} - -void ConfigApplier::configureGnssInstallation(const SbgEComGnssInstallation& ref_gnss_installation) -{ - // - // Get the Gnss level arm, and compare with the loaded parameters. - // If the level arms are different, update the device with the loaded parameters. - // - SbgEComGnssInstallation gnss_installation; - SbgErrorCode error_code; - SbgVector3 gnss_device_primary; - SbgVector3 gnss_device_secondary; - SbgVector3 gnss_config_primary; - SbgVector3 gnss_config_secondary; - - error_code = sbgEComCmdGnss1InstallationGet(&m_ref_sbg_com_handle, &gnss_installation); - - checkConfigurationGet(error_code, std::string("Gnss level arms")); - - gnss_device_primary = SbgVector3(gnss_installation.leverArmPrimary, 3); - gnss_device_secondary = SbgVector3(gnss_installation.leverArmSecondary, 3); - gnss_config_primary = SbgVector3(ref_gnss_installation.leverArmPrimary, 3); - gnss_config_secondary = SbgVector3(ref_gnss_installation.leverArmSecondary, 3); - - if ((gnss_device_primary != gnss_config_primary) - || (gnss_device_secondary != gnss_config_secondary) - || (gnss_installation.leverArmPrimaryPrecise != ref_gnss_installation.leverArmPrimaryPrecise) - || (gnss_installation.leverArmSecondaryMode != ref_gnss_installation.leverArmSecondaryMode)) - { - error_code = sbgEComCmdGnss1InstallationSet(&m_ref_sbg_com_handle, &ref_gnss_installation); - - checkConfigurationApplied(error_code, std::string("Gnss level arms")); - } -} - -void ConfigApplier::configureGnssRejection(const SbgEComGnssRejectionConf& ref_gnss_rejection) -{ - // - // Get the Gnss rejection, and compare with the loaded parameters. - // If the rejection are different, update the device with the loaded parameters. - // - SbgEComGnssRejectionConf rejection; - SbgErrorCode error_code; - - error_code = sbgEComCmdGnss1GetRejection(&m_ref_sbg_com_handle, &rejection); - - checkConfigurationGet(error_code, std::string("Gnss rejection")); - - if ((rejection.hdt != ref_gnss_rejection.hdt) - || (rejection.position != ref_gnss_rejection.position) - || (rejection.velocity != ref_gnss_rejection.velocity)) - { - error_code = sbgEComCmdGnss1SetRejection(&m_ref_sbg_com_handle, &ref_gnss_rejection); - - checkConfigurationApplied(error_code, std::string("Gnss rejection")); - } -} - -void ConfigApplier::configureOdometer(const SbgEComOdoConf& ref_odometer) -{ - // - // Get the odometer configuration, and compare with the loaded parameters. - // If the conf are different, update the device with the loaded parameters. - // - SbgEComOdoConf odom_conf; - SbgErrorCode error_code; - - error_code = sbgEComCmdOdoGetConf(&m_ref_sbg_com_handle, &odom_conf); - - checkConfigurationGet(error_code, std::string("Odometer")); - - if (!areEquals(odom_conf.gain, ref_odometer.gain) - || (odom_conf.gainError != ref_odometer.gainError) - || (odom_conf.reverseMode != ref_odometer.reverseMode)) - { - error_code = sbgEComCmdOdoSetConf(&m_ref_sbg_com_handle, &ref_odometer); - - checkConfigurationApplied(error_code, std::string("Odometer")); - } -} - -void ConfigApplier::configureOdometerLevelArm(const SbgVector3& odometer_level_arms) -{ - // - // Get the odometer level arm, and compare with the loaded parameters. - // If the level arms are different, update the device with the loaded parameters. - // - float lever_arm[3]; - SbgErrorCode error_code; - - error_code = sbgEComCmdOdoGetLeverArm(&m_ref_sbg_com_handle, lever_arm); - - checkConfigurationGet(error_code, std::string("Odometer level arms")); - - SbgVector3 lever_arm_device = SbgVector3(lever_arm, 3); - - if (lever_arm_device != odometer_level_arms) - { - error_code = sbgEComCmdOdoSetLeverArm(&m_ref_sbg_com_handle, lever_arm_device.data()); - - checkConfigurationApplied(error_code, std::string("Odometer level arms")); - } -} - -void ConfigApplier::configureOdometerRejection(const SbgEComOdoRejectionConf& ref_odometer_rejection) -{ - // - // Get the odometer rejection mode, and compare with the loaded parameter. - // If the mode are different, update the device with the loaded parameter. - // - SbgEComOdoRejectionConf odom_rejection; - SbgErrorCode error_code; - - error_code = sbgEComCmdOdoGetRejection(&m_ref_sbg_com_handle, &odom_rejection); - - checkConfigurationGet(error_code, std::string("Odometer rejection")); - - if (odom_rejection.velocity != ref_odometer_rejection.velocity) - { - error_code = sbgEComCmdOdoSetRejection(&m_ref_sbg_com_handle, &ref_odometer_rejection); - - checkConfigurationApplied(error_code, std::string("Odometer rejection")); - } -} - -void ConfigApplier::configureOutput(SbgEComOutputPort output_port, const ConfigStore::SbgLogOutput &ref_log_output) -{ - SbgErrorCode error_code; - SbgEComOutputMode current_output_mode; - - // - // Get the current output mode for the device and the selected log ID. - // If output modes are different, udpate the device mode with the one loaded from the parameters. - // - error_code = sbgEComCmdOutputGetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, ¤t_output_mode); - - if (error_code == SBG_INVALID_PARAMETER) - { - ROS_WARN("SBG_DRIVER - [Config] Output is not available for this device : Class [%d] - Id [%d]", ref_log_output.message_class, ref_log_output.message_id); - } - else if (error_code != SBG_NO_ERROR) - { - std::string error_message("[Config] Unable to get output for the device : Class ["); - error_message.append(std::to_string(ref_log_output.message_class)); - error_message.append("] - Id ["); - error_message.append(std::to_string(ref_log_output.message_id)); - error_message.append("] : "); - error_message.append(sbgErrorCodeToString(error_code)); - - throw ros::Exception(error_message); - } - else if (current_output_mode != ref_log_output.output_mode) - { - error_code = sbgEComCmdOutputSetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, ref_log_output.output_mode); - - if (error_code != SBG_NO_ERROR) - { - std::string error_message("[Config] Unable to set the output configuration : Class["); - error_message.append(std::to_string(ref_log_output.message_class)); - error_message.append("] - Id ["); - error_message.append(std::to_string(ref_log_output.message_id)); - error_message.append("] : "); - error_message.append(sbgErrorCodeToString(error_code)); - - throw ros::Exception(error_message); - } - else - { - m_reboot_needed_ = true; - } - } -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -void ConfigApplier::applyConfiguration(const ConfigStore& ref_config_store) -{ - // - // Configure the connected device. - // - configureInitCondition(ref_config_store.getInitialConditions()); - configureMotionProfile(ref_config_store.getMotionProfile()); - configureImuAlignement(ref_config_store.getSensorAlignement(), ref_config_store.getSensorLevelArms()); - configureAidingAssignement(ref_config_store.getAidingAssignement()); - configureMagModel(ref_config_store.getMagnetometerModel()); - configureMagRejection(ref_config_store.getMagnetometerRejection()); - configureGnssModel(ref_config_store.getGnssModel()); - configureGnssInstallation(ref_config_store.getGnssInstallation()); - configureGnssRejection(ref_config_store.getGnssRejection()); - configureOdometer(ref_config_store.getOdometerConf()); - configureOdometerLevelArm(ref_config_store.getOdometerLevelArms()); - configureOdometerRejection(ref_config_store.getOdometerRejection()); - - // - // Configure the output, with all output defined in the store. - // - const std::vector& ref_output_modes = ref_config_store.getOutputModes(); - - for (const ConfigStore::SbgLogOutput& ref_output : ref_output_modes) - { - configureOutput(ref_config_store.getOutputPort(), ref_output); - } - - // - // Save configuration if needed. - // - if (m_reboot_needed_) - { - saveConfiguration(); - } -} - -void ConfigApplier::saveConfiguration(void) -{ - SbgErrorCode error_code; - - error_code = sbgEComCmdSettingsAction(&m_ref_sbg_com_handle, SBG_ECOM_SAVE_SETTINGS); - - if (error_code != SBG_NO_ERROR) - { - ROS_ERROR("Unable to save the settings on the SBG device - %s", sbgErrorCodeToString(error_code)); - } - else - { - ROS_INFO("SBG_DRIVER - Settings saved and device rebooted."); - } -} \ No newline at end of file diff --git a/sbg_ros_driver/src/config_store.cpp b/sbg_ros_driver/src/config_store.cpp deleted file mode 100644 index 4b73549..0000000 --- a/sbg_ros_driver/src/config_store.cpp +++ /dev/null @@ -1,323 +0,0 @@ -// File header -#include "config_store.h" - -using sbg::ConfigStore; - -/*! - * Class to handle the device configuration. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -ConfigStore::ConfigStore(void): -m_serial_communication_(false), -m_upd_communication_(false), -m_configure_through_ros_(false), -m_ros_standard_output_(false) -{ - -} - -//---------------------------------------------------------------------// -//- Private methods -// -//---------------------------------------------------------------------// - -void ConfigStore::loadCommunicationParameters(const ros::NodeHandle& ref_node_handle) -{ - ref_node_handle.param("confWithRos", m_configure_through_ros_, false); - - if (ref_node_handle.hasParam("uartConf")) - { - m_serial_communication_ = true; - ref_node_handle.param("uartConf/portName", m_uart_port_name_, "/dev/ttyUSB0"); - - m_uart_baud_rate_ = getParameter(ref_node_handle, "uartConf/baudRate", 0); - m_output_port_ = getParameter(ref_node_handle, "uartConf/portID", SBG_ECOM_OUTPUT_PORT_A); - } - else if (ref_node_handle.hasParam("ipConf")) - { - std::string ip_address; - ref_node_handle.param("ipConf/ipAddress", ip_address, "0.0.0.0"); - - m_upd_communication_ = true; - m_sbg_ip_address_ = sbgNetworkIpFromString(ip_address.c_str()); - m_out_port_address_ = getParameter(ref_node_handle, "ipConf/out_port", 0); - m_in_port_address_ = getParameter(ref_node_handle, "ipConf/in_port", 0); - } - else - { - throw ros::Exception("SBG DRIVER - Invalid communication interface parameters."); - } -} - -void ConfigStore::loadSensorParameters(const ros::NodeHandle& ref_node_handle) -{ - ref_node_handle.param("sensorParameters/initLat", m_init_condition_conf_.latitude, 48.419727); - ref_node_handle.param("sensorParameters/initLong", m_init_condition_conf_.longitude, -4.472119); - ref_node_handle.param("sensorParameters/initAlt", m_init_condition_conf_.altitude, 100); - - m_init_condition_conf_.year = getParameter(ref_node_handle, "sensorParameters/year", 2018); - m_init_condition_conf_.month = getParameter(ref_node_handle, "sensorParameters/year", 03); - m_init_condition_conf_.day = getParameter(ref_node_handle, "sensorParameters/year", 10); - - m_motion_profile_model_info_.id = getParameter(ref_node_handle, "sensorParameters/motionProfile", SBG_ECOM_MOTION_PROFILE_GENERAL_PURPOSE); -} - -void ConfigStore::loadImuAlignementParameters(const ros::NodeHandle& ref_node_handle) -{ - m_sensor_alignement_info_.axisDirectionX = getParameter(ref_node_handle, "imuAlignementLeverArm/axisDirectionX", SBG_ECOM_ALIGNMENT_FORWARD); - m_sensor_alignement_info_.axisDirectionY = getParameter(ref_node_handle, "imuAlignementLeverArm/axisDirectionY", SBG_ECOM_ALIGNMENT_FORWARD); - - ref_node_handle.param("imuAlignementLeverArm/misRoll", m_sensor_alignement_info_.misRoll, 0.0f); - ref_node_handle.param("imuAlignementLeverArm/misPitch", m_sensor_alignement_info_.misPitch, 0.0f); - ref_node_handle.param("imuAlignementLeverArm/misYaw", m_sensor_alignement_info_.misYaw, 0.0f); - - float sensor_level_arm[3]; - ref_node_handle.param("imuAlignementLeverArm/leverArmX", sensor_level_arm[0], 0.0f); - ref_node_handle.param("imuAlignementLeverArm/leverArmY", sensor_level_arm[1], 0.0f); - ref_node_handle.param("imuAlignementLeverArm/leverArmZ", sensor_level_arm[2], 0.0f); - - m_sensor_lever_arm_ = SbgVector3(sensor_level_arm, 3); -} - -void ConfigStore::loadAidingAssignementParameters(const ros::NodeHandle& ref_node_handle) -{ - m_aiding_assignement_conf_.gps1Port = getParameter(ref_node_handle, "aidingAssignment/gnss1ModulePortAssignment", SBG_ECOM_MODULE_PORT_B); - m_aiding_assignement_conf_.gps1Sync = getParameter(ref_node_handle, "aidingAssignment/gnss1ModuleSyncAssignment", SBG_ECOM_MODULE_SYNC_DISABLED); - m_aiding_assignement_conf_.rtcmPort = getParameter(ref_node_handle, "aidingAssignment/rtcmPortAssignment", SBG_ECOM_MODULE_DISABLED); - m_aiding_assignement_conf_.odometerPinsConf = getParameter(ref_node_handle, "aidingAssignment/odometerPinAssignment", SBG_ECOM_MODULE_ODO_DISABLED); -} - -void ConfigStore::loadMagnetometersParameters(const ros::NodeHandle& ref_node_handle) -{ - m_mag_model_info_.id = getParameter(ref_node_handle, "magnetometer/magnetometerModel", SBG_ECOM_MAG_MODEL_NORMAL); - m_mag_rejection_conf_.magneticField = getParameter(ref_node_handle, "magnetometer/magnetometerRejectMode", SBG_ECOM_AUTOMATIC_MODE); - - m_mag_calib_mode_ = getParameter(ref_node_handle, "magnetometer/calibration/mode", SBG_ECOM_MAG_CALIB_MODE_2D); - m_mag_calib_bandwidth_ = getParameter(ref_node_handle, "magnetometer/calibration/bandwidth", SBG_ECOM_MAG_CALIB_HIGH_BW); -} - -void ConfigStore::loadGnssParameters(const ros::NodeHandle& ref_node_handle) -{ - m_gnss_model_info_.id = getParameter(ref_node_handle, "gnss/gnss_model_id", SBG_ECOM_GNSS_MODEL_NMEA); - - ref_node_handle.param("gnss/primaryLeverArmX", m_gnss_installation_.leverArmPrimary[0], 0.0f); - ref_node_handle.param("gnss/primaryLeverArmY", m_gnss_installation_.leverArmPrimary[1], 0.0f); - ref_node_handle.param("gnss/primaryLeverArmZ", m_gnss_installation_.leverArmPrimary[2], 0.0f); - ref_node_handle.param("gnss/primaryLeverPrecise", m_gnss_installation_.leverArmPrimaryPrecise, true); - ref_node_handle.param("gnss/secondaryLeverArmX", m_gnss_installation_.leverArmSecondary[0], 0.0f); - ref_node_handle.param("gnss/secondaryLeverArmY", m_gnss_installation_.leverArmSecondary[1], 0.0f); - ref_node_handle.param("gnss/secondaryLeverArmZ", m_gnss_installation_.leverArmSecondary[2], 0.0f); - m_gnss_installation_.leverArmSecondaryMode = getParameter(ref_node_handle, "gnss/secondaryLeverMode", SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE); - - m_gnss_rejection_conf_.position = getParameter(ref_node_handle, "gnss/posRejectMode", SBG_ECOM_AUTOMATIC_MODE); - m_gnss_rejection_conf_.velocity = getParameter(ref_node_handle, "gnss/velRejectMode", SBG_ECOM_AUTOMATIC_MODE); - m_gnss_rejection_conf_.hdt = getParameter(ref_node_handle, "gnss/hdtRejectMode", SBG_ECOM_AUTOMATIC_MODE); -} - -void ConfigStore::loadOdometerParameters(const ros::NodeHandle& ref_node_handle) -{ - ref_node_handle.param("odom/gain", m_odometer_conf_.gain, 4800.0f); - ref_node_handle.param("odom/direction", m_odometer_conf_.reverseMode, false); - - float odometer_level_arm_[3]; - ref_node_handle.param("odom/leverArmX", odometer_level_arm_[0], 0.0f); - ref_node_handle.param("odom/leverArmY", odometer_level_arm_[1], 0.0f); - ref_node_handle.param("odom/leverArmZ", odometer_level_arm_[2], 0.0f); - - m_odometer_level_arm_ = SbgVector3(odometer_level_arm_, 3); - - m_odometer_conf_.gainError = getParameter(ref_node_handle, "odom/gain_error", 0.1); - m_odometer_rejection_conf_.velocity = getParameter(ref_node_handle, "odom/rejectMode", SBG_ECOM_AUTOMATIC_MODE); -} - -void ConfigStore::loadOutputConfiguration(const ros::NodeHandle& ref_node_handle, const std::string& ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id) -{ - SbgLogOutput log_output; - - log_output.message_class = sbg_msg_class; - log_output.message_id = sbg_msg_id; - log_output.output_mode = getParameter(ref_node_handle, ref_key, SBG_ECOM_OUTPUT_MODE_DISABLED); - - m_output_modes_.push_back(log_output); -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -bool ConfigStore::checkConfigWithRos(void) const -{ - return m_configure_through_ros_; -} - -bool ConfigStore::isInterfaceSerial(void) const -{ - return m_serial_communication_; -} - -const std::string &ConfigStore::getUartPortName(void) const -{ - return m_uart_port_name_; -} - -uint32_t ConfigStore::getBaudRate(void) const -{ - return m_uart_baud_rate_; -} - -SbgEComOutputPort ConfigStore::getOutputPort(void) const -{ - return m_output_port_; -} - -bool ConfigStore::isInterfaceUdp(void) const -{ - return m_upd_communication_; -} - -sbgIpAddress ConfigStore::getIpAddress(void) const -{ - return m_sbg_ip_address_; -} - -uint32_t ConfigStore::getOutputPortAddress(void) const -{ - return m_out_port_address_; -} - -uint32_t ConfigStore::getInputPortAddress(void) const -{ - return m_in_port_address_; -} - -const SbgEComInitConditionConf &ConfigStore::getInitialConditions(void) const -{ - return m_init_condition_conf_; -} - -const SbgEComModelInfo &ConfigStore::getMotionProfile(void) const -{ - return m_motion_profile_model_info_; -} - -const SbgEComSensorAlignmentInfo &ConfigStore::getSensorAlignement(void) const -{ - return m_sensor_alignement_info_; -} - -const sbg::SbgVector3 &ConfigStore::getSensorLevelArms(void) const -{ - return m_sensor_lever_arm_; -} - -const SbgEComAidingAssignConf &ConfigStore::getAidingAssignement(void) const -{ - return m_aiding_assignement_conf_; -} - -const SbgEComModelInfo &ConfigStore::getMagnetometerModel(void) const -{ - return m_mag_model_info_; -} - -const SbgEComMagRejectionConf &ConfigStore::getMagnetometerRejection(void) const -{ - return m_mag_rejection_conf_; -} - -const SbgEComMagCalibMode &ConfigStore::getMagnetometerCalibMode(void) const -{ - return m_mag_calib_mode_; -} - -const SbgEComMagCalibBandwidth &ConfigStore::getMagnetometerCalibBandwidth(void) const -{ - return m_mag_calib_bandwidth_; -} - -const SbgEComModelInfo &ConfigStore::getGnssModel(void) const -{ - return m_gnss_model_info_; -} - -const SbgEComGnssInstallation &ConfigStore::getGnssInstallation(void) const -{ - return m_gnss_installation_; -} - -const SbgEComGnssRejectionConf &ConfigStore::getGnssRejection(void) const -{ - return m_gnss_rejection_conf_; -} - -const SbgEComOdoConf &ConfigStore::getOdometerConf(void) const -{ - return m_odometer_conf_; -} - -const sbg::SbgVector3 &ConfigStore::getOdometerLevelArms(void) const -{ - return m_odometer_level_arm_; -} - -const SbgEComOdoRejectionConf &ConfigStore::getOdometerRejection(void) const -{ - return m_odometer_rejection_conf_; -} - -const std::vector &ConfigStore::getOutputModes(void) const -{ - return m_output_modes_; -} - -bool ConfigStore::checkRosStandardMessages(void) const -{ - return m_ros_standard_output_; -} - -uint32_t ConfigStore::getReadingRateFrequency(void) const -{ - return m_rate_frequency_; -} - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -void ConfigStore::loadFromRosNodeHandle(const ros::NodeHandle& ref_node_handle) -{ - loadCommunicationParameters(ref_node_handle); - loadSensorParameters(ref_node_handle); - loadImuAlignementParameters(ref_node_handle); - loadAidingAssignementParameters(ref_node_handle); - loadMagnetometersParameters(ref_node_handle); - loadGnssParameters(ref_node_handle); - loadOdometerParameters(ref_node_handle); - - loadOutputConfiguration(ref_node_handle, "output/log_status", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_STATUS); - loadOutputConfiguration(ref_node_handle, "output/log_imu_data", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA); - loadOutputConfiguration(ref_node_handle, "output/log_ekf_euler", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_EULER); - loadOutputConfiguration(ref_node_handle, "output/log_ekf_quat", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_QUAT); - loadOutputConfiguration(ref_node_handle, "output/log_ekf_nav", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_NAV); - loadOutputConfiguration(ref_node_handle, "output/log_ship_motion", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_SHIP_MOTION); - loadOutputConfiguration(ref_node_handle, "output/log_utc_time", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_UTC_TIME); - loadOutputConfiguration(ref_node_handle, "output/log_mag", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG); - loadOutputConfiguration(ref_node_handle, "output/log_mag_calib", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG_CALIB); - loadOutputConfiguration(ref_node_handle, "output/log_gps1_vel", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_VEL); - loadOutputConfiguration(ref_node_handle, "output/log_gps1_pos", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_POS); - loadOutputConfiguration(ref_node_handle, "output/log_gps1_hdt", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_HDT); - loadOutputConfiguration(ref_node_handle, "output/log_gps1_raw", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_RAW); - loadOutputConfiguration(ref_node_handle, "output/log_odo_vel", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_ODO_VEL); - loadOutputConfiguration(ref_node_handle, "output/log_event_a", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_A); - loadOutputConfiguration(ref_node_handle, "output/log_event_b", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_B); - loadOutputConfiguration(ref_node_handle, "output/log_event_c", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_C); - loadOutputConfiguration(ref_node_handle, "output/log_event_d", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_D); - loadOutputConfiguration(ref_node_handle, "output/log_event_e", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_E); - loadOutputConfiguration(ref_node_handle, "output/log_air_data", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA); - loadOutputConfiguration(ref_node_handle, "output/log_imu_short", SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_SHORT); - - ref_node_handle.param("output/ros_standard", m_ros_standard_output_, false); - m_rate_frequency_ = getParameter(ref_node_handle, "output/frequency", 0); -} \ No newline at end of file diff --git a/sbg_ros_driver/src/main.cpp b/sbg_ros_driver/src/main.cpp deleted file mode 100644 index 98cd4b3..0000000 --- a/sbg_ros_driver/src/main.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include - -using sbg::SbgDevice; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "sbg_device"); - ros::NodeHandle node_handle; - - try - { - ROS_INFO("SBG DRIVER - Init node, load params and connect to the device."); - SbgDevice sbg_device(node_handle); - - ROS_INFO("SBG DRIVER - Initialize device for receiving data"); - sbg_device.initDeviceForReceivingData(); - - ROS_INFO("SBG DRIVER - START RECEIVING DATA"); - ros::Rate loop_rate(sbg_device.getUpdateFrequency() * 2); - - while (ros::ok()) - { - sbg_device.periodicHandle(); - loop_rate.sleep(); - } - - return 0; - } - catch (ros::Exception const& refE) - { - ROS_ERROR("SBG_DRIVER - %s", refE.what()); - } - - return 0; -} \ No newline at end of file diff --git a/sbg_ros_driver/src/main_mag.cpp b/sbg_ros_driver/src/main_mag.cpp deleted file mode 100644 index c369521..0000000 --- a/sbg_ros_driver/src/main_mag.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "sbg_device.h" - -using sbg::SbgDevice; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "sbg_device_mag"); - ros::NodeHandle node_handle; - - try - { - ROS_INFO("SBG DRIVER - Init node, load params and connect to the device"); - SbgDevice sbg_device(node_handle); - - sbg_device.initDeviceForMagCalibration(); - - ros::spin(); - - return 0; - } - catch (ros::Exception const& refE) - { - ROS_ERROR("SBG_DRIVER - [MagNode] Error - %s.", refE.what()); - } - - return 0; -} \ No newline at end of file diff --git a/sbg_ros_driver/src/message_publisher.cpp b/sbg_ros_driver/src/message_publisher.cpp deleted file mode 100644 index 5a294cf..0000000 --- a/sbg_ros_driver/src/message_publisher.cpp +++ /dev/null @@ -1,700 +0,0 @@ -#include "message_publisher.h" - -using sbg::MessagePublisher; - -/*! - * Class to publish all SBG-ROS messages to the corresponding publishers. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -MessagePublisher::MessagePublisher(void): -m_output_mode_(SBG_ECOM_OUTPUT_MODE_DISABLED), -m_max_mesages_(10) -{ - -} - -//---------------------------------------------------------------------// -//- Private methods -// -//---------------------------------------------------------------------// - -void MessagePublisher::updateMaxOutputFrequency(SbgEComOutputMode output_mode) -{ - // - // Update the maximal output frequency if needed. - // - if (m_output_mode_ == SBG_ECOM_OUTPUT_MODE_DISABLED) - { - m_output_mode_ = output_mode; - } - else - { - if (getCorrespondingFrequency(output_mode) > getCorrespondingFrequency(m_output_mode_)) - { - m_output_mode_ = output_mode; - } - } - - // - // In case of sbg output event configuration, just define the output on a 25Hz frequency. - // - if (getCorrespondingFrequency(m_output_mode_) >= getCorrespondingFrequency(SBG_ECOM_OUTPUT_MODE_PPS)) - { - m_output_mode_ = SBG_ECOM_OUTPUT_MODE_DIV_8; - } -} - -uint32_t MessagePublisher::getCorrespondingFrequency(SbgEComOutputMode output_mode) const -{ - switch (output_mode) - { - case SBG_ECOM_OUTPUT_MODE_DISABLED: - return 0; - - case SBG_ECOM_OUTPUT_MODE_MAIN_LOOP: - return 200; - - case SBG_ECOM_OUTPUT_MODE_DIV_2: - return 100; - - case SBG_ECOM_OUTPUT_MODE_DIV_4: - return 50; - - case SBG_ECOM_OUTPUT_MODE_DIV_5: - return 40; - - case SBG_ECOM_OUTPUT_MODE_DIV_8: - return 25; - - case SBG_ECOM_OUTPUT_MODE_DIV_10: - return 20; - - case SBG_ECOM_OUTPUT_MODE_DIV_20: - return 10; - - case SBG_ECOM_OUTPUT_MODE_DIV_40: - return 5; - - case SBG_ECOM_OUTPUT_MODE_DIV_200: - return 1; - - case SBG_ECOM_OUTPUT_MODE_HIGH_FREQ_LOOP: - return 1000; - - case SBG_ECOM_OUTPUT_MODE_PPS: - return 10000; - - default: - return 0; - } -} - -std::string MessagePublisher::getOutputTopicName(SbgEComMsgId sbg_message_id) const -{ - switch (sbg_message_id) - { - case SBG_ECOM_LOG_STATUS: - return "sbg/status"; - - case SBG_ECOM_LOG_UTC_TIME: - return "sbg/utc_time"; - - case SBG_ECOM_LOG_IMU_DATA: - return "sbg/imu_data"; - - case SBG_ECOM_LOG_MAG: - return "sbg/mag"; - - case SBG_ECOM_LOG_MAG_CALIB: - return "sbg/mag_calib"; - - case SBG_ECOM_LOG_EKF_EULER: - return "sbg/ekf_euler"; - - case SBG_ECOM_LOG_EKF_QUAT: - return "sbg/ekf_quat"; - - case SBG_ECOM_LOG_EKF_NAV: - return "sbg/ekf_nav"; - - case SBG_ECOM_LOG_SHIP_MOTION: - return "sbg/ship_motion"; - - case SBG_ECOM_LOG_GPS1_VEL: - return "sbg/gps_vel"; - - case SBG_ECOM_LOG_GPS1_POS: - return "sbg/gps_pos"; - - case SBG_ECOM_LOG_GPS1_HDT: - return "sbg/gps_hdt"; - - case SBG_ECOM_LOG_GPS1_RAW: - return "sbg/gps_raw"; - - case SBG_ECOM_LOG_ODO_VEL: - return "sbg/odo_vel"; - - case SBG_ECOM_LOG_EVENT_A: - return "sbg/eventA"; - - case SBG_ECOM_LOG_EVENT_B: - return "sbg/eventB"; - - case SBG_ECOM_LOG_EVENT_C: - return "sbg/eventC"; - - case SBG_ECOM_LOG_EVENT_D: - return "sbg/eventD"; - - case SBG_ECOM_LOG_EVENT_E: - return "sbg/eventE"; - - case SBG_ECOM_LOG_AIR_DATA: - return "sbg/air_data"; - - case SBG_ECOM_LOG_IMU_SHORT: - return "sbg/imu_short"; - - default: - return "undefined"; - } -} - -void MessagePublisher::initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEComMsgId sbg_msg_id, SbgEComOutputMode output_conf, const std::string &ref_output_topic) -{ - // - // Check if the publisher has to be initialized. - // - if (output_conf != SBG_ECOM_OUTPUT_MODE_DISABLED) - { - updateMaxOutputFrequency(output_conf); - - switch (sbg_msg_id) - { - case SBG_ECOM_LOG_STATUS: - m_sbgStatus_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_UTC_TIME: - m_sbgUtcTime_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_IMU_DATA: - m_sbgImuData_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_MAG: - m_sbgMag_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_MAG_CALIB: - m_sbgMagCalib_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EKF_EULER: - m_sbgEkfEuler_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EKF_QUAT: - m_sbgEkfQuat_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EKF_NAV: - - m_sbgEkfNav_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_SHIP_MOTION: - - m_sbgShipMotion_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: - - m_sbgGpsVel_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: - - m_sbgGpsPos_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: - - m_sbgGpsHdt_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: - - m_sbgGpsRaw_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_ODO_VEL: - - m_sbgOdoVel_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EVENT_A: - - m_sbgEventA_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EVENT_B: - - m_sbgEventB_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EVENT_C: - - m_sbgEventC_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EVENT_D: - - m_sbgEventD_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_EVENT_E: - - m_sbgEventE_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_IMU_SHORT: - - m_sbgImuShort_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - case SBG_ECOM_LOG_AIR_DATA: - - m_sbgAirData_pub_ = ref_ros_node_handle.advertise(ref_output_topic, m_max_mesages_); - break; - - default: - break; - } - } -} - -void MessagePublisher::defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle) -{ - if (m_sbgImuData_pub_ && m_sbgEkfQuat_pub_) - { - m_imu_pub_ = ref_ros_node_handle.advertise("imu/data", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu and/or Quat output are not configured, the standard IMU can not be defined."); - } - - if (m_sbgImuData_pub_) - { - m_temp_pub_ = ref_ros_node_handle.advertise("imu/temp", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu data output are not configured, the standard Temperature publisher can not be defined."); - } - - if (m_sbgMag_pub_) - { - m_mag_pub_ = ref_ros_node_handle.advertise("imu/mag", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Mag data output are not configured, the standard Magnetic publisher can not be defined."); - } - - // - // We need either Euler or quat angles, and we must have Nav and IMU data to - // compute Body and angular velocity. - // - if ((m_sbgEkfEuler_pub_ || m_sbgEkfQuat_pub_) && m_sbgEkfNav_pub_ && m_sbgImuData_pub_) - { - m_velocity_pub_ = ref_ros_node_handle.advertise("imu/velocity", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu, Nav or Angles data outputs are not configured, the standard Velocity publisher can not be defined."); - } - - if (m_sbgAirData_pub_) - { - m_fluid_pub_ = ref_ros_node_handle.advertise("imu/pres", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG AirData output are not configured, the standard FluidPressure publisher can not be defined."); - } - - if (m_sbgEkfNav_pub_) - { - m_pos_ecef_pub_ = ref_ros_node_handle.advertise("imu/pos_ecef", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Ekf data output are not configured, the standard ECEF position publisher can not be defined."); - } - - if (m_sbgUtcTime_pub_) - { - m_utc_reference_pub_ = ref_ros_node_handle.advertise("imu/utc_ref", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG Utc data output are not configured, the UTC time reference publisher can not be defined."); - } - - if (m_sbgGpsPos_pub_) - { - m_nav_sat_fix_pub_ = ref_ros_node_handle.advertise("imu/nav_sat_fix", m_max_mesages_); - } - else - { - ROS_WARN("SBG_DRIVER - [Publisher] SBG GPS Pos data output are not configured, the NavSatFix publisher can not be defined."); - } -} - -void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) -{ - if (m_sbgImuData_pub_) - { - m_sbg_imu_message_ = m_message_wrapper_.createSbgImuDataMessage(ref_sbg_log.imuData); - m_sbgImuData_pub_.publish(m_sbg_imu_message_); - } - if (m_temp_pub_) - { - m_temp_pub_.publish(m_message_wrapper_.createRosTemperatureMessage(m_sbg_imu_message_)); - } - - processRosImuMessage(); - processRosVelMessage(); -} - -void MessagePublisher::processRosVelMessage(void) -{ - if (m_velocity_pub_) - { - if (m_sbgEkfQuat_pub_) - { - m_velocity_pub_.publish(m_message_wrapper_.createRosTwistStampedMessage(m_sbg_ekf_quat_message_, m_sbg_ekf_nav_message_, m_sbg_imu_message_)); - } - else if (m_sbgEkfEuler_pub_) - { - m_velocity_pub_.publish(m_message_wrapper_.createRosTwistStampedMessage(m_sbg_ekf_euler_message_, m_sbg_ekf_nav_message_, m_sbg_imu_message_)); - } - } -} - -void MessagePublisher::processRosImuMessage(void) -{ - if (m_imu_pub_) - { - if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_quat_message_.time_stamp) - { - m_imu_pub_.publish(m_message_wrapper_.createRosImuMessage(m_sbg_imu_message_, m_sbg_ekf_quat_message_)); - } - } -} - -void MessagePublisher::publishMagData(const SbgBinaryLogData &ref_sbg_log) -{ - sbg_driver::SbgMag sbg_mag_message; - sbg_mag_message = m_message_wrapper_.createSbgMagMessage(ref_sbg_log.magData); - - if (m_sbgMag_pub_) - { - m_sbgMag_pub_.publish(sbg_mag_message); - } - if (m_mag_pub_) - { - m_mag_pub_.publish(m_message_wrapper_.createRosMagneticMessage(sbg_mag_message)); - } -} - -void MessagePublisher::publishFluidPressureData(const SbgBinaryLogData &ref_sbg_log) -{ - sbg_driver::SbgAirData sbg_air_data_message; - sbg_air_data_message = m_message_wrapper_.createSbgAirDataMessage(ref_sbg_log.airData); - - if (m_sbgAirData_pub_) - { - m_sbgAirData_pub_.publish(sbg_air_data_message); - } - if (m_fluid_pub_) - { - m_fluid_pub_.publish(m_message_wrapper_.createRosFluidPressureMessage(sbg_air_data_message)); - } -} - -void MessagePublisher::publishEkfNavigationData(const SbgBinaryLogData &ref_sbg_log) -{ - m_sbg_ekf_nav_message_ = m_message_wrapper_.createSbgEkfNavMessage(ref_sbg_log.ekfNavData); - - if (m_sbgEkfNav_pub_) - { - m_sbgEkfNav_pub_.publish(m_sbg_ekf_nav_message_); - } - if (m_pos_ecef_pub_) - { - m_pos_ecef_pub_.publish(m_message_wrapper_.createRosPointStampedMessage(m_sbg_ekf_nav_message_)); - } - processRosVelMessage(); -} - -void MessagePublisher::publishUtcData(const SbgBinaryLogData &ref_sbg_log) -{ - sbg_driver::SbgUtcTime sbg_utc_message; - - sbg_utc_message = m_message_wrapper_.createSbgUtcTimeMessage(ref_sbg_log.utcData); - - if (m_sbgUtcTime_pub_) - { - m_sbgUtcTime_pub_.publish(sbg_utc_message); - } - if (m_utc_reference_pub_) - { - if (sbg_utc_message.clock_status.clock_utc_status != SBG_ECOM_UTC_INVALID) - { - m_utc_reference_pub_.publish(m_message_wrapper_.createRosUtcTimeReferenceMessage(sbg_utc_message)); - } - } -} - -void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log) -{ - sbg_driver::SbgGpsPos sbg_gps_pos_message; - - sbg_gps_pos_message = m_message_wrapper_.createSbgGpsPosMessage(ref_sbg_log.gpsPosData); - - if (m_sbgGpsPos_pub_) - { - m_sbgGpsPos_pub_.publish(sbg_gps_pos_message); - } - if (m_nav_sat_fix_pub_) - { - m_nav_sat_fix_pub_.publish(m_message_wrapper_.createRosNavSatFixMessage(sbg_gps_pos_message)); - } -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -uint32_t MessagePublisher::getMaxOutputFrequency(void) const -{ - return getCorrespondingFrequency(m_output_mode_); -} - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -void MessagePublisher::initPublishers(ros::NodeHandle& ref_ros_node_handle, const ConfigStore &ref_config_store) -{ - // - // Initialize all the publishers with the defined SBG output from the config store. - // - const std::vector &ref_output_modes = ref_config_store.getOutputModes(); - - for (const ConfigStore::SbgLogOutput &ref_output : ref_output_modes) - { - initPublisher(ref_ros_node_handle, ref_output.message_id, ref_output.output_mode, getOutputTopicName(ref_output.message_id)); - } - - if (ref_config_store.checkRosStandardMessages()) - { - defineRosStandardPublishers(ref_ros_node_handle); - } -} - -void MessagePublisher::publish(const ros::Time& ref_ros_time, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log) -{ - // - // Publish the message with the corresponding publisher and SBG message ID. - // For each log, check if the publisher has been initialized. - // - m_message_wrapper_.setRosProcessingTime(ref_ros_time); - - if(sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_0) - { - switch (sbg_msg_id) - { - case SBG_ECOM_LOG_STATUS: - - if (m_sbgStatus_pub_) - { - m_sbgStatus_pub_.publish(m_message_wrapper_.createSbgStatusMessage(ref_sbg_log.statusData)); - } - break; - - case SBG_ECOM_LOG_UTC_TIME: - - publishUtcData(ref_sbg_log); - break; - - case SBG_ECOM_LOG_IMU_DATA: - - publishIMUData(ref_sbg_log); - break; - - case SBG_ECOM_LOG_MAG: - - publishMagData(ref_sbg_log); - break; - - case SBG_ECOM_LOG_MAG_CALIB: - - if (m_sbgMagCalib_pub_) - { - m_sbgMagCalib_pub_.publish(m_message_wrapper_.createSbgMagCalibMessage(ref_sbg_log.magCalibData)); - } - break; - - case SBG_ECOM_LOG_EKF_EULER: - - if (m_sbgEkfEuler_pub_) - { - m_sbg_ekf_euler_message_ = m_message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); - m_sbgEkfEuler_pub_.publish(m_sbg_ekf_euler_message_); - processRosVelMessage(); - } - break; - - case SBG_ECOM_LOG_EKF_QUAT: - - if (m_sbgEkfQuat_pub_) - { - m_sbg_ekf_quat_message_ = m_message_wrapper_.createSbgEkfQuatMessage(ref_sbg_log.ekfQuatData); - m_sbgEkfQuat_pub_.publish(m_sbg_ekf_quat_message_); - processRosImuMessage(); - processRosVelMessage(); - } - break; - - case SBG_ECOM_LOG_EKF_NAV: - - publishEkfNavigationData(ref_sbg_log); - break; - - case SBG_ECOM_LOG_SHIP_MOTION: - - if (m_sbgShipMotion_pub_) - { - m_sbgShipMotion_pub_.publish(m_message_wrapper_.createSbgShipMotionMessage(ref_sbg_log.shipMotionData)); - } - break; - - case SBG_ECOM_LOG_GPS1_VEL: - case SBG_ECOM_LOG_GPS2_VEL: - - if (m_sbgGpsVel_pub_) - { - m_sbgGpsVel_pub_.publish(m_message_wrapper_.createSbgGpsVelMessage(ref_sbg_log.gpsVelData)); - } - break; - - case SBG_ECOM_LOG_GPS1_POS: - case SBG_ECOM_LOG_GPS2_POS: - - publishGpsPosData(ref_sbg_log); - break; - - case SBG_ECOM_LOG_GPS1_HDT: - case SBG_ECOM_LOG_GPS2_HDT: - - if (m_sbgGpsHdt_pub_) - { - m_sbgGpsHdt_pub_.publish(m_message_wrapper_.createSbgGpsHdtMessage(ref_sbg_log.gpsHdtData)); - } - break; - - case SBG_ECOM_LOG_GPS1_RAW: - case SBG_ECOM_LOG_GPS2_RAW: - - if (m_sbgGpsRaw_pub_) - { - m_sbgGpsRaw_pub_.publish(m_message_wrapper_.createSbgGpsRawMessage(ref_sbg_log.gpsRawData)); - } - break; - - case SBG_ECOM_LOG_ODO_VEL: - - if (m_sbgOdoVel_pub_) - { - m_sbgOdoVel_pub_.publish(m_message_wrapper_.createSbgOdoVelMessage(ref_sbg_log.odometerData)); - } - break; - - case SBG_ECOM_LOG_EVENT_A: - - if (m_sbgEventA_pub_) - { - m_sbgEventA_pub_.publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; - - case SBG_ECOM_LOG_EVENT_B: - - if (m_sbgEventB_pub_) - { - m_sbgEventB_pub_.publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; - - case SBG_ECOM_LOG_EVENT_C: - - if (m_sbgEventC_pub_) - { - m_sbgEventC_pub_.publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; - - case SBG_ECOM_LOG_EVENT_D: - - if (m_sbgEventD_pub_) - { - m_sbgEventD_pub_.publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; - - case SBG_ECOM_LOG_EVENT_E: - - if (m_sbgEventE_pub_) - { - m_sbgEventE_pub_.publish(m_message_wrapper_.createSbgEventMessage(ref_sbg_log.eventMarker)); - } - break; - - case SBG_ECOM_LOG_IMU_SHORT: - - if (m_sbgImuShort_pub_) - { - m_sbgImuShort_pub_.publish(m_message_wrapper_.createSbgImuShortMessage(ref_sbg_log.imuShort)); - } - break; - - case SBG_ECOM_LOG_AIR_DATA: - - publishFluidPressureData(ref_sbg_log); - break; - - default: - break; - } - } - else if (sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_1) - { - switch (sbg_msg_id) - { - default: - break; - } - } -} diff --git a/sbg_ros_driver/src/message_wrapper.cpp b/sbg_ros_driver/src/message_wrapper.cpp deleted file mode 100644 index a70a7a7..0000000 --- a/sbg_ros_driver/src/message_wrapper.cpp +++ /dev/null @@ -1,851 +0,0 @@ -// File header -#include "message_wrapper.h" - -// Project headers -#include - -using sbg::MessageWrapper; - -/*! - * Class to wrap the SBG logs into ROS messages. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -MessageWrapper::MessageWrapper(void): -m_first_valid_utc_(false) -{ - -} - -//---------------------------------------------------------------------// -//- Internal methods -// -//---------------------------------------------------------------------// - -const std_msgs::Header MessageWrapper::createRosHeader(uint32_t device_timestamp) const -{ - std_msgs::Header header; - - if (!m_first_valid_utc_) - { - header.stamp = ros::Time::now(); //m_ros_processing_time_; - header.frame_id = "System"; - header.frame_id = "imu_frame"; - - } - else - { - header.stamp = computeCorrectedRosTime(device_timestamp); - - std::string frame_header; - frame_header = "UTC"; - - if (m_last_sbg_utc_.clock_status.clock_utc_status == SBG_ECOM_UTC_INVALID) - { - frame_header += " INTERNAL"; - } - else - { - if (m_last_sbg_utc_.clock_status.clock_utc_status == SBG_ECOM_UTC_NO_LEAP_SEC) - { - frame_header += " NO LEAP"; - } - - if (m_last_sbg_utc_.clock_status.clock_utc_sync) - { - if (m_last_sbg_utc_.clock_status.clock_status == SBG_ECOM_CLOCK_STEERING) - { - frame_header += " | SYNCHRONIZING"; - } - else if (m_last_sbg_utc_.clock_status.clock_status == SBG_ECOM_CLOCK_VALID) - { - frame_header += " | SYNC"; - } - } - else - { - frame_header += " | NOT SYNC"; - } - } - - header.frame_id = frame_header; - header.frame_id = "imu_frame"; - } - - return header; -} - -const ros::Time MessageWrapper::computeCorrectedRosTime(uint32_t device_timestamp) const -{ - // - // Convert the UTC time to epoch from the last received message. - // Add the SBG timestamp difference (timestamp is in microsecond). - // - ros::Time utc_to_epoch; - uint32_t device_timestamp_diff; - uint64_t nanoseconds; - - utc_to_epoch = convertUtcTimeToEpoch(m_last_sbg_utc_); - device_timestamp_diff = device_timestamp - m_last_sbg_utc_.time_stamp; - - nanoseconds = utc_to_epoch.toNSec() + static_cast(device_timestamp_diff) * 1000; - - utc_to_epoch.fromNSec(nanoseconds); - - return utc_to_epoch; -} - -const sbg_driver::SbgEkfStatus MessageWrapper::createEkfStatusMessage(uint32_t ekf_status) const -{ - sbg_driver::SbgEkfStatus ekf_status_message; - - ekf_status_message.solution_mode = sbgEComLogEkfGetSolutionMode(ekf_status); - ekf_status_message.attitude_valid = (ekf_status & SBG_ECOM_SOL_ATTITUDE_VALID) != 0; - ekf_status_message.heading_valid = (ekf_status & SBG_ECOM_SOL_HEADING_VALID) != 0; - ekf_status_message.velocity_valid = (ekf_status & SBG_ECOM_SOL_VELOCITY_VALID) != 0; - ekf_status_message.position_valid = (ekf_status & SBG_ECOM_SOL_POSITION_VALID) != 0; - - ekf_status_message.vert_ref_used = (ekf_status & SBG_ECOM_SOL_VERT_REF_USED) != 0; - ekf_status_message.mag_ref_used = (ekf_status & SBG_ECOM_SOL_MAG_REF_USED) != 0; - ekf_status_message.gps1_vel_used = (ekf_status & SBG_ECOM_SOL_GPS1_VEL_USED) != 0; - ekf_status_message.gps1_pos_used = (ekf_status & SBG_ECOM_SOL_GPS1_POS_USED) != 0; - ekf_status_message.gps1_course_used = (ekf_status & SBG_ECOM_SOL_GPS1_HDT_USED) != 0; - ekf_status_message.gps1_hdt_used = (ekf_status & SBG_ECOM_SOL_GPS1_HDT_USED) != 0; - ekf_status_message.gps2_vel_used = (ekf_status & SBG_ECOM_SOL_GPS2_VEL_USED) != 0; - ekf_status_message.gps2_pos_used = (ekf_status & SBG_ECOM_SOL_GPS2_POS_USED) != 0; - ekf_status_message.gps2_course_used = (ekf_status & SBG_ECOM_SOL_GPS2_POS_USED) != 0; - ekf_status_message.gps2_hdt_used = (ekf_status & SBG_ECOM_SOL_GPS2_HDT_USED) != 0; - ekf_status_message.odo_used = (ekf_status & SBG_ECOM_SOL_ODO_USED) != 0; - - return ekf_status_message; -} - -const sbg_driver::SbgGpsPosStatus MessageWrapper::createGpsPosStatusMessage(const SbgLogGpsPos& ref_log_gps_pos) const -{ - sbg_driver::SbgGpsPosStatus gps_pos_status_message; - - gps_pos_status_message.status = sbgEComLogGpsPosGetStatus(ref_log_gps_pos.status); - gps_pos_status_message.type = sbgEComLogGpsPosGetType(ref_log_gps_pos.status); - gps_pos_status_message.gps_l1_used = (ref_log_gps_pos.status & SBG_ECOM_GPS_POS_GPS_L1_USED) != 0; - gps_pos_status_message.gps_l2_used = (ref_log_gps_pos.status & SBG_ECOM_GPS_POS_GPS_L2_USED) != 0; - gps_pos_status_message.gps_l5_used = (ref_log_gps_pos.status & SBG_ECOM_GPS_POS_GPS_L5_USED) != 0; - gps_pos_status_message.glo_l1_used = (ref_log_gps_pos.status & SBG_ECOM_GPS_POS_GLO_L1_USED) != 0; - gps_pos_status_message.glo_l2_used = (ref_log_gps_pos.status & SBG_ECOM_GPS_POS_GLO_L2_USED) != 0; - - return gps_pos_status_message; -} - -const sbg_driver::SbgGpsVelStatus MessageWrapper::createGpsVelStatusMessage(const SbgLogGpsVel& ref_log_gps_vel) const -{ - sbg_driver::SbgGpsVelStatus gps_vel_status_message; - - gps_vel_status_message.vel_status = sbgEComLogGpsVelGetStatus(ref_log_gps_vel.status); - gps_vel_status_message.vel_type = sbgEComLogGpsVelGetType(ref_log_gps_vel.status); - - return gps_vel_status_message; -} - -const sbg_driver::SbgImuStatus MessageWrapper::createImuStatusMessage(uint16_t sbg_imu_status) const -{ - sbg_driver::SbgImuStatus imu_status_message; - - imu_status_message.imu_com = (sbg_imu_status & SBG_ECOM_IMU_COM_OK) != 0; - imu_status_message.imu_status = (sbg_imu_status & SBG_ECOM_IMU_STATUS_BIT) != 0 ; - imu_status_message.imu_accel_x = (sbg_imu_status & SBG_ECOM_IMU_ACCEL_X_BIT) != 0; - imu_status_message.imu_accel_y = (sbg_imu_status & SBG_ECOM_IMU_ACCEL_Y_BIT) != 0; - imu_status_message.imu_accel_z = (sbg_imu_status & SBG_ECOM_IMU_ACCEL_Z_BIT) != 0; - imu_status_message.imu_gyro_x = (sbg_imu_status & SBG_ECOM_IMU_GYRO_X_BIT) != 0; - imu_status_message.imu_gyro_y = (sbg_imu_status & SBG_ECOM_IMU_GYRO_Y_BIT) != 0; - imu_status_message.imu_gyro_z = (sbg_imu_status & SBG_ECOM_IMU_GYRO_Z_BIT) != 0; - imu_status_message.imu_accels_in_range = (sbg_imu_status & SBG_ECOM_IMU_ACCELS_IN_RANGE) != 0; - imu_status_message.imu_gyros_in_range = (sbg_imu_status & SBG_ECOM_IMU_GYROS_IN_RANGE) != 0; - - return imu_status_message; -} - -const sbg_driver::SbgMagStatus MessageWrapper::createMagStatusMessage(const SbgLogMag& ref_log_mag) const -{ - sbg_driver::SbgMagStatus mag_status_message; - - mag_status_message.mag_x = (ref_log_mag.status & SBG_ECOM_MAG_MAG_X_BIT) != 0; - mag_status_message.mag_y = (ref_log_mag.status & SBG_ECOM_MAG_MAG_Y_BIT) != 0; - mag_status_message.mag_z = (ref_log_mag.status & SBG_ECOM_MAG_MAG_Z_BIT) != 0; - mag_status_message.accel_x = (ref_log_mag.status & SBG_ECOM_MAG_ACCEL_X_BIT) != 0; - mag_status_message.accel_y = (ref_log_mag.status & SBG_ECOM_MAG_ACCEL_Y_BIT) != 0; - mag_status_message.accel_z = (ref_log_mag.status & SBG_ECOM_MAG_ACCEL_Z_BIT) != 0; - mag_status_message.mags_in_range = (ref_log_mag.status & SBG_ECOM_MAG_MAGS_IN_RANGE) != 0; - mag_status_message.accels_in_range = (ref_log_mag.status & SBG_ECOM_MAG_ACCELS_IN_RANGE) != 0; - mag_status_message.calibration = (ref_log_mag.status & SBG_ECOM_MAG_CALIBRATION_OK) != 0; - - return mag_status_message; -} - -const sbg_driver::SbgShipMotionStatus MessageWrapper::createShipMotionStatusMessage(const SbgLogShipMotionData& ref_log_ship_motion) const -{ - sbg_driver::SbgShipMotionStatus ship_motion_status_message; - - ship_motion_status_message.heave_valid = (ref_log_ship_motion.status & SBG_ECOM_HEAVE_VALID) != 0; - ship_motion_status_message.heave_vel_aided = (ref_log_ship_motion.status & SBG_ECOM_HEAVE_VEL_AIDED) != 0; - ship_motion_status_message.period_available = (ref_log_ship_motion.status & SBG_ECOM_HEAVE_PERIOD_INCLUDED) != 0; - ship_motion_status_message.period_valid = (ref_log_ship_motion.status & SBG_ECOM_HEAVE_PERIOD_VALID) != 0; - - return ship_motion_status_message; -} - -const sbg_driver::SbgStatusAiding MessageWrapper::createStatusAidingMessage(const SbgLogStatusData& ref_log_status) const -{ - sbg_driver::SbgStatusAiding status_aiding_message; - - status_aiding_message.gps1_pos_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_GPS1_POS_RECV) != 0; - status_aiding_message.gps1_vel_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_GPS1_VEL_RECV) != 0; - status_aiding_message.gps1_hdt_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_GPS1_HDT_RECV) != 0; - status_aiding_message.gps1_utc_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_GPS1_UTC_RECV) != 0; - - status_aiding_message.mag_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_MAG_RECV) != 0; - status_aiding_message.odo_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_ODO_RECV) != 0; - status_aiding_message.dvl_recv = (ref_log_status.aidingStatus & SBG_ECOM_AIDING_DVL_RECV) != 0; - - return status_aiding_message; -} - -const sbg_driver::SbgStatusCom MessageWrapper::createStatusComMessage(const SbgLogStatusData& ref_log_status) const -{ - sbg_driver::SbgStatusCom status_com_message; - - status_com_message.port_a = (ref_log_status.comStatus & SBG_ECOM_PORTA_VALID) != 0; - status_com_message.port_b = (ref_log_status.comStatus & SBG_ECOM_PORTB_VALID) != 0; - status_com_message.port_c = (ref_log_status.comStatus & SBG_ECOM_PORTC_VALID) != 0; - status_com_message.port_d = (ref_log_status.comStatus & SBG_ECOM_PORTD_VALID) != 0; - status_com_message.port_e = (ref_log_status.comStatus & SBG_ECOM_PORTE_VALID) != 0; - - status_com_message.port_a_rx = (ref_log_status.comStatus & SBG_ECOM_PORTA_RX_OK) != 0; - status_com_message.port_a_tx = (ref_log_status.comStatus & SBG_ECOM_PORTA_TX_OK) != 0; - status_com_message.port_b_rx = (ref_log_status.comStatus & SBG_ECOM_PORTB_RX_OK) != 0; - status_com_message.port_b_tx = (ref_log_status.comStatus & SBG_ECOM_PORTB_TX_OK) != 0; - status_com_message.port_c_rx = (ref_log_status.comStatus & SBG_ECOM_PORTC_RX_OK) != 0; - status_com_message.port_c_tx = (ref_log_status.comStatus & SBG_ECOM_PORTC_TX_OK) != 0; - status_com_message.port_d_rx = (ref_log_status.comStatus & SBG_ECOM_PORTD_RX_OK) != 0; - status_com_message.port_d_tx = (ref_log_status.comStatus & SBG_ECOM_PORTD_TX_OK) != 0; - status_com_message.port_e_rx = (ref_log_status.comStatus & SBG_ECOM_PORTE_RX_OK) != 0; - status_com_message.port_e_tx = (ref_log_status.comStatus & SBG_ECOM_PORTE_TX_OK) != 0; - - status_com_message.can_rx = (ref_log_status.comStatus & SBG_ECOM_CAN_RX_OK) != 0; - status_com_message.can_tx = (ref_log_status.comStatus & SBG_ECOM_CAN_TX_OK) != 0; - status_com_message.can_status = (ref_log_status.comStatus & SBG_ECOM_CAN_VALID) != 0; - - return status_com_message; -} - -const sbg_driver::SbgStatusGeneral MessageWrapper::createStatusGeneralMessage(const SbgLogStatusData& ref_log_status) const -{ - sbg_driver::SbgStatusGeneral status_general_message; - - status_general_message.main_power = (ref_log_status.generalStatus & SBG_ECOM_GENERAL_MAIN_POWER_OK) != 0; - status_general_message.imu_power = (ref_log_status.generalStatus & SBG_ECOM_GENERAL_IMU_POWER_OK) != 0; - status_general_message.gps_power = (ref_log_status.generalStatus & SBG_ECOM_GENERAL_GPS_POWER_OK) != 0; - status_general_message.settings = (ref_log_status.generalStatus & SBG_ECOM_GENERAL_SETTINGS_OK) != 0; - status_general_message.temperature = (ref_log_status.generalStatus & SBG_ECOM_GENERAL_TEMPERATURE_OK) != 0; - - return status_general_message; -} - -const sbg_driver::SbgUtcTimeStatus MessageWrapper::createUtcStatusMessage(const SbgLogUtcData& ref_log_utc) const -{ - sbg_driver::SbgUtcTimeStatus utc_status_message; - - utc_status_message.clock_stable = (ref_log_utc.status & SBG_ECOM_CLOCK_STABLE_INPUT) != 0; - utc_status_message.clock_utc_sync = (ref_log_utc.status & SBG_ECOM_CLOCK_UTC_SYNC) != 0; - - utc_status_message.clock_status = static_cast(sbgEComLogUtcGetClockStatus(ref_log_utc.status)); - utc_status_message.clock_utc_status = static_cast(sbgEComLogUtcGetClockUtcStatus(ref_log_utc.status)); - - return utc_status_message; -} - -uint32_t MessageWrapper::getNumberOfDaysInYear(uint16_t year) const -{ - if (isLeapYear(year)) - { - return 366; - } - else - { - return 365; - } -} - -uint32_t MessageWrapper::getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const -{ - if ((month_index == 4) || (month_index == 6) || (month_index == 9) || (month_index == 11)) - { - return 30; - } - else if ((month_index == 2)) - { - if (isLeapYear(year)) - { - return 29; - } - else - { - return 28; - } - } - else - { - return 31; - } -} - -bool MessageWrapper::isLeapYear(uint16_t year) const -{ - return ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0); -} - -const ros::Time MessageWrapper::convertUtcTimeToEpoch(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const -{ - ros::Time utc_to_epoch; - uint32_t days; - uint64_t nanoseconds; - - // - // Convert the UTC time to Epoch(Unix) time, which is the elasped seconds since 1 Jan 1970. - // - days = 0; - nanoseconds = 0; - - for (uint16_t yearIndex = 1970; yearIndex < ref_sbg_utc_msg.year; yearIndex++) - { - days += getNumberOfDaysInYear(yearIndex); - } - - for (uint8_t monthIndex = 1; monthIndex < ref_sbg_utc_msg.month; monthIndex++) - { - days += getNumberOfDaysInMonth(ref_sbg_utc_msg.year, monthIndex); - } - - days += ref_sbg_utc_msg.day - 1; - - nanoseconds = days * 24; - nanoseconds = (nanoseconds + ref_sbg_utc_msg.hour) * 60; - nanoseconds = (nanoseconds + ref_sbg_utc_msg.min) * 60; - nanoseconds = nanoseconds + ref_sbg_utc_msg.sec; - nanoseconds = nanoseconds * 1000000000 + ref_sbg_utc_msg.nanosec; - - utc_to_epoch.fromNSec(nanoseconds); - - return utc_to_epoch; -} - -const sbg_driver::SbgAirDataStatus MessageWrapper::createAirDataStatusMessage(const SbgLogAirData& ref_sbg_air_data) const -{ - sbg_driver::SbgAirDataStatus air_data_status_message; - - air_data_status_message.is_delay_time = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_TIME_IS_DELAY) != 0; - air_data_status_message.pressure_valid = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID) != 0; - air_data_status_message.altitude_valid = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_ALTITUDE_VALID) != 0; - air_data_status_message.pressure_diff_valid = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID) != 0; - air_data_status_message.air_speed_valid = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_AIRPSEED_VALID) != 0; - air_data_status_message.air_temperature_valid = (ref_sbg_air_data.status & SBG_ECOM_AIR_DATA_TEMPERATURE_VALID) != 0; - - return air_data_status_message; -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -void MessageWrapper::setRosProcessingTime(const ros::Time& ref_ros_time) -{ - m_ros_processing_time_ = ref_ros_time; -} - -//---------------------------------------------------------------------// -//- Operations -// -//---------------------------------------------------------------------// - -const sbg_driver::SbgEkfEuler MessageWrapper::createSbgEkfEulerMessage(const SbgLogEkfEulerData& ref_log_ekf_euler) const -{ - sbg_driver::SbgEkfEuler ekf_euler_message; - - ekf_euler_message.header = createRosHeader(ref_log_ekf_euler.timeStamp); - ekf_euler_message.time_stamp = ref_log_ekf_euler.timeStamp; - ekf_euler_message.status = createEkfStatusMessage(ref_log_ekf_euler.status); - ekf_euler_message.angle = createGeometryVector3(ref_log_ekf_euler.euler, 3); - ekf_euler_message.accuracy = createGeometryVector3(ref_log_ekf_euler.eulerStdDev, 3); - - return ekf_euler_message; -} - -const sbg_driver::SbgEkfNav MessageWrapper::createSbgEkfNavMessage(const SbgLogEkfNavData& ref_log_ekf_nav) const -{ - sbg_driver::SbgEkfNav ekf_nav_message; - - ekf_nav_message.header = createRosHeader(ref_log_ekf_nav.timeStamp); - ekf_nav_message.time_stamp = ekf_nav_message.time_stamp; - ekf_nav_message.status = createEkfStatusMessage(ref_log_ekf_nav.status); - ekf_nav_message.undulation = ref_log_ekf_nav.undulation; - - ekf_nav_message.velocity = createGeometryVector3(ref_log_ekf_nav.velocity, 3); - ekf_nav_message.velocity_accuracy = createGeometryVector3(ref_log_ekf_nav.velocityStdDev, 3); - ekf_nav_message.position = createGeometryVector3(ref_log_ekf_nav.position, 3); - - return ekf_nav_message; -} - -const sbg_driver::SbgEkfQuat MessageWrapper::createSbgEkfQuatMessage(const SbgLogEkfQuatData& ref_log_ekf_quat) const -{ - sbg_driver::SbgEkfQuat ekf_quat_message; - - ekf_quat_message.header = createRosHeader(ref_log_ekf_quat.timeStamp); - ekf_quat_message.time_stamp = ref_log_ekf_quat.timeStamp; - ekf_quat_message.status = createEkfStatusMessage(ref_log_ekf_quat.status); - ekf_quat_message.accuracy = createGeometryVector3(ref_log_ekf_quat.eulerStdDev, 3); - ekf_quat_message.quaternion.w = ref_log_ekf_quat.quaternion[0]; - ekf_quat_message.quaternion.x = ref_log_ekf_quat.quaternion[1]; - ekf_quat_message.quaternion.y = ref_log_ekf_quat.quaternion[2]; - ekf_quat_message.quaternion.z = ref_log_ekf_quat.quaternion[3]; - - return ekf_quat_message; -} - -const sbg_driver::SbgEvent MessageWrapper::createSbgEventMessage(const SbgLogEvent& ref_log_event) const -{ - sbg_driver::SbgEvent event_message; - - event_message.header = createRosHeader(ref_log_event.timeStamp); - event_message.time_stamp = ref_log_event.timeStamp; - - event_message.overflow = (ref_log_event.status & SBG_ECOM_EVENT_OVERFLOW) != 0; - event_message.offset_0_valid = (ref_log_event.status & SBG_ECOM_EVENT_OFFSET_0_VALID) != 0; - event_message.offset_1_valid = (ref_log_event.status & SBG_ECOM_EVENT_OFFSET_1_VALID) != 0; - event_message.offset_2_valid = (ref_log_event.status & SBG_ECOM_EVENT_OFFSET_2_VALID) != 0; - event_message.offset_3_valid = (ref_log_event.status & SBG_ECOM_EVENT_OFFSET_3_VALID) != 0; - - event_message.time_offset_0 = ref_log_event.timeOffset0; - event_message.time_offset_1 = ref_log_event.timeOffset1; - event_message.time_offset_2 = ref_log_event.timeOffset2; - event_message.time_offset_3 = ref_log_event.timeOffset3; - - return event_message; -} - -const sbg_driver::SbgGpsHdt MessageWrapper::createSbgGpsHdtMessage(const SbgLogGpsHdt& ref_log_gps_hdt) const -{ - sbg_driver::SbgGpsHdt gps_hdt_message; - - gps_hdt_message.header = createRosHeader(ref_log_gps_hdt.timeStamp); - gps_hdt_message.time_stamp = ref_log_gps_hdt.timeStamp; - - gps_hdt_message.status = ref_log_gps_hdt.status; - gps_hdt_message.tow = ref_log_gps_hdt.timeOfWeek; - gps_hdt_message.true_heading = ref_log_gps_hdt.heading; - gps_hdt_message.true_heading_acc = ref_log_gps_hdt.headingAccuracy; - gps_hdt_message.pitch = ref_log_gps_hdt.pitch; - gps_hdt_message.pitch_acc = ref_log_gps_hdt.pitchAccuracy; - - return gps_hdt_message; -} - -const sbg_driver::SbgGpsPos MessageWrapper::createSbgGpsPosMessage(const SbgLogGpsPos& ref_log_gps_pos) const -{ - sbg_driver::SbgGpsPos gps_pos_message; - - gps_pos_message.header = createRosHeader(ref_log_gps_pos.timeStamp); - gps_pos_message.time_stamp = ref_log_gps_pos.timeStamp; - - gps_pos_message.status = createGpsPosStatusMessage(ref_log_gps_pos); - gps_pos_message.gps_tow = ref_log_gps_pos.timeOfWeek; - gps_pos_message.position.x = ref_log_gps_pos.latitude; - gps_pos_message.position.y = ref_log_gps_pos.longitude; - gps_pos_message.position.z = ref_log_gps_pos.altitude; - gps_pos_message.undulation = ref_log_gps_pos.undulation; - gps_pos_message.position_accuracy.x = ref_log_gps_pos.latitudeAccuracy; - gps_pos_message.position_accuracy.y = ref_log_gps_pos.longitudeAccuracy; - gps_pos_message.position_accuracy.z = ref_log_gps_pos.altitudeAccuracy; - gps_pos_message.num_sv_used = ref_log_gps_pos.numSvUsed; - gps_pos_message.base_station_id = ref_log_gps_pos.baseStationId; - gps_pos_message.diff_age = ref_log_gps_pos.differentialAge; - - return gps_pos_message; -} - -const sbg_driver::SbgGpsRaw MessageWrapper::createSbgGpsRawMessage(const SbgLogGpsRaw& ref_log_gps_raw) const -{ - sbg_driver::SbgGpsRaw gps_raw_message; - - gps_raw_message.data.assign(ref_log_gps_raw.rawBuffer, ref_log_gps_raw.rawBuffer + ref_log_gps_raw.bufferSize); - - return gps_raw_message; -} - -const sbg_driver::SbgGpsVel MessageWrapper::createSbgGpsVelMessage(const SbgLogGpsVel& ref_log_gps_vel) const -{ - sbg_driver::SbgGpsVel gps_vel_message; - - gps_vel_message.header = createRosHeader(ref_log_gps_vel.timeStamp); - gps_vel_message.time_stamp = ref_log_gps_vel.timeStamp; - gps_vel_message.status = createGpsVelStatusMessage(ref_log_gps_vel); - gps_vel_message.gps_tow = ref_log_gps_vel.timeOfWeek; - gps_vel_message.course = ref_log_gps_vel.course; - gps_vel_message.course_acc = ref_log_gps_vel.courseAcc; - gps_vel_message.vel = createGeometryVector3(ref_log_gps_vel.velocity, 3); - gps_vel_message.vel_acc = createGeometryVector3(ref_log_gps_vel.velocityAcc, 3); - - return gps_vel_message; -} - -const sbg_driver::SbgImuData MessageWrapper::createSbgImuDataMessage(const SbgLogImuData& ref_log_imu_data) const -{ - sbg_driver::SbgImuData imu_data_message; - - imu_data_message.header = createRosHeader(ref_log_imu_data.timeStamp); - imu_data_message.time_stamp = ref_log_imu_data.timeStamp; - imu_data_message.imu_status = createImuStatusMessage(ref_log_imu_data.status); - imu_data_message.temp = ref_log_imu_data.temperature; - imu_data_message.accel = createGeometryVector3(ref_log_imu_data.accelerometers, 3); - imu_data_message.gyro = createGeometryVector3(ref_log_imu_data.gyroscopes, 3); - imu_data_message.delta_vel = createGeometryVector3(ref_log_imu_data.deltaVelocity, 3); - imu_data_message.delta_angle = createGeometryVector3(ref_log_imu_data.deltaAngle, 3); - - return imu_data_message; -} - -const sbg_driver::SbgMag MessageWrapper::createSbgMagMessage(const SbgLogMag& ref_log_mag) const -{ - sbg_driver::SbgMag mag_message; - - mag_message.header = createRosHeader(ref_log_mag.timeStamp); - mag_message.time_stamp = ref_log_mag.timeStamp; - mag_message.status = createMagStatusMessage(ref_log_mag); - mag_message.mag = createGeometryVector3(ref_log_mag.magnetometers, 3); - mag_message.accel = createGeometryVector3(ref_log_mag.accelerometers, 3); - - return mag_message; -} - -const sbg_driver::SbgMagCalib MessageWrapper::createSbgMagCalibMessage(const SbgLogMagCalib& ref_log_mag_calib) const -{ - sbg_driver::SbgMagCalib mag_calib_message; - - // TODO. SbgMagCalib is not implemented. - mag_calib_message.header = createRosHeader(ref_log_mag_calib.timeStamp); - - return mag_calib_message; -} - -const sbg_driver::SbgOdoVel MessageWrapper::createSbgOdoVelMessage(const SbgLogOdometerData& ref_log_odo) const -{ - sbg_driver::SbgOdoVel odo_vel_message; - - odo_vel_message.header = createRosHeader(ref_log_odo.timeStamp); - odo_vel_message.time_stamp = ref_log_odo.timeStamp; - - odo_vel_message.status = ref_log_odo.status; - odo_vel_message.vel = ref_log_odo.velocity; - - return odo_vel_message; -} - -const sbg_driver::SbgShipMotion MessageWrapper::createSbgShipMotionMessage(const SbgLogShipMotionData& ref_log_ship_motion) const -{ - sbg_driver::SbgShipMotion ship_motion_message; - - ship_motion_message.header = createRosHeader(ref_log_ship_motion.timeStamp); - ship_motion_message.time_stamp = ref_log_ship_motion.timeStamp; - ship_motion_message.status = createShipMotionStatusMessage(ref_log_ship_motion); - ship_motion_message.ship_motion = createGeometryVector3(ref_log_ship_motion.shipMotion, 3); - ship_motion_message.acceleration = createGeometryVector3(ref_log_ship_motion.shipAccel, 3); - ship_motion_message.velocity = createGeometryVector3(ref_log_ship_motion.shipVel, 3); - - return ship_motion_message; -} - -const sbg_driver::SbgStatus MessageWrapper::createSbgStatusMessage(const SbgLogStatusData& ref_log_status) const -{ - sbg_driver::SbgStatus status_message; - - status_message.header = createRosHeader(ref_log_status.timeStamp); - status_message.time_stamp = ref_log_status.timeStamp; - - status_message.status_general = createStatusGeneralMessage(ref_log_status); - status_message.status_com = createStatusComMessage(ref_log_status); - status_message.status_aiding = createStatusAidingMessage(ref_log_status); - - return status_message; -} - -const sbg_driver::SbgUtcTime MessageWrapper::createSbgUtcTimeMessage(const SbgLogUtcData& ref_log_utc) -{ - sbg_driver::SbgUtcTime utc_time_message; - - utc_time_message.header = createRosHeader(ref_log_utc.timeStamp); - utc_time_message.time_stamp = ref_log_utc.timeStamp; - - utc_time_message.clock_status = createUtcStatusMessage(ref_log_utc); - utc_time_message.year = ref_log_utc.year; - utc_time_message.month = ref_log_utc.month; - utc_time_message.day = ref_log_utc.day; - utc_time_message.hour = ref_log_utc.hour; - utc_time_message.min = ref_log_utc.minute; - utc_time_message.sec = ref_log_utc.second; - utc_time_message.nanosec = ref_log_utc.nanoSecond; - utc_time_message.gps_tow = ref_log_utc.gpsTimeOfWeek; - - if (!m_first_valid_utc_) - { - if (utc_time_message.clock_status.clock_stable && utc_time_message.clock_status.clock_utc_sync) - { - if (utc_time_message.clock_status.clock_status == SBG_ECOM_CLOCK_VALID) - { - m_first_valid_utc_ = true; - ROS_INFO("A full valid UTC log has been detected, timestamp will be synchronized with the UTC data."); - } - } - } - - // - // Store the last UTC message. - // - m_last_sbg_utc_ = utc_time_message; - - return utc_time_message; -} - -const sbg_driver::SbgAirData MessageWrapper::createSbgAirDataMessage(const SbgLogAirData& ref_air_data_log) const -{ - sbg_driver::SbgAirData air_data_message; - - air_data_message.header = createRosHeader(ref_air_data_log.timeStamp); - air_data_message.time_stamp = ref_air_data_log.timeStamp; - air_data_message.status = createAirDataStatusMessage(ref_air_data_log); - air_data_message.pressure_abs = ref_air_data_log.pressureAbs; - air_data_message.altitude = ref_air_data_log.altitude; - air_data_message.pressure_diff = ref_air_data_log.pressureDiff; - air_data_message.true_air_speed = ref_air_data_log.trueAirspeed; - air_data_message.air_temperature = ref_air_data_log.airTemperature; - - return air_data_message; -} - -const sbg_driver::SbgImuShort MessageWrapper::createSbgImuShortMessage(const SbgLogImuShort& ref_short_imu_log) const -{ - sbg_driver::SbgImuShort imu_short_message; - - imu_short_message.header = createRosHeader(ref_short_imu_log.timeStamp); - imu_short_message.time_stamp = ref_short_imu_log.timeStamp; - imu_short_message.imu_status = createImuStatusMessage(ref_short_imu_log.status); - imu_short_message.temperature = ref_short_imu_log.temperature; - imu_short_message.delta_velocity = createGeometryVector3(ref_short_imu_log.deltaVelocity, 3); - imu_short_message.delta_angle = createGeometryVector3(ref_short_imu_log.deltaAngle, 3); - - return imu_short_message; -} - -const sensor_msgs::Imu MessageWrapper::createRosImuMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat& ref_sbg_quat_msg) const -{ - sensor_msgs::Imu imu_ros_message; - - imu_ros_message.header = createRosHeader(ref_sbg_imu_msg.time_stamp); - - imu_ros_message.header.stamp = ros::Time::now(); - - // NED - //imu_ros_message.orientation.w = ref_sbg_quat_msg.quaternion.w; - //imu_ros_message.orientation.x = ref_sbg_quat_msg.quaternion.x; - //imu_ros_message.orientation.y = ref_sbg_quat_msg.quaternion.y; - //imu_ros_message.orientation.z = ref_sbg_quat_msg.quaternion.z; - //imu_ros_message.angular_velocity.x = ref_sbg_imu_msg.gyro.x; - //imu_ros_message.angular_velocity.y = ref_sbg_imu_msg.gyro.y; - //imu_ros_message.angular_velocity.z = ref_sbg_imu_msg.gyro.z; - //imu_ros_message.linear_acceleration.x = ref_sbg_imu_msg.accel.x; - //imu_ros_message.linear_acceleration.y = ref_sbg_imu_msg.accel.y; - //imu_ros_message.linear_acceleration.z = ref_sbg_imu_msg.accel.z; - - //ENU - // swap x and y and negate z - imu_ros_message.orientation.w = ref_sbg_quat_msg.quaternion.w; - imu_ros_message.orientation.x = ref_sbg_quat_msg.quaternion.y; - imu_ros_message.orientation.y = ref_sbg_quat_msg.quaternion.x; - imu_ros_message.orientation.z = -ref_sbg_quat_msg.quaternion.z; - // acceleration swap x y negate z - imu_ros_message.angular_velocity.x = ref_sbg_imu_msg.gyro.y; - imu_ros_message.angular_velocity.y = ref_sbg_imu_msg.gyro.x; - imu_ros_message.angular_velocity.z = -ref_sbg_imu_msg.gyro.z; - // angular rates swap x y and negate z - imu_ros_message.linear_acceleration.x = ref_sbg_imu_msg.accel.y; - imu_ros_message.linear_acceleration.y = ref_sbg_imu_msg.accel.x; - imu_ros_message.linear_acceleration.z = -ref_sbg_imu_msg.accel.z; - - - imu_ros_message.orientation_covariance[0] = 0.001; - imu_ros_message.orientation_covariance[4] = 0.001; - imu_ros_message.orientation_covariance[8] = 0.001; - - imu_ros_message.angular_velocity_covariance[0] = 0.001; - imu_ros_message.angular_velocity_covariance[4] = 0.001; - imu_ros_message.angular_velocity_covariance[8] = 0.001; - - imu_ros_message.linear_acceleration_covariance[0] = 0.001; - imu_ros_message.linear_acceleration_covariance[4] = 0.001; - imu_ros_message.linear_acceleration_covariance[8] = 0.001; - - return imu_ros_message; -} - -const sensor_msgs::Temperature MessageWrapper::createRosTemperatureMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg) const -{ - sensor_msgs::Temperature temperature_message; - - temperature_message.header = createRosHeader(ref_sbg_imu_msg.time_stamp); - temperature_message.temperature = ref_sbg_imu_msg.temp; - temperature_message.variance = 0; - - return temperature_message; -} - -const sensor_msgs::MagneticField MessageWrapper::createRosMagneticMessage(const sbg_driver::SbgMag& ref_sbg_mag_msg) const -{ - sensor_msgs::MagneticField magnetic_message; - - magnetic_message.header = createRosHeader(ref_sbg_mag_msg.time_stamp); - magnetic_message.magnetic_field = ref_sbg_mag_msg.mag; - - return magnetic_message; -} - -const geometry_msgs::TwistStamped MessageWrapper::createRosTwistStampedMessage(const sbg_driver::SbgEkfEuler& ref_sbg_ekf_euler_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const -{ - sbg::SbgMatrix3f tdcm; - tdcm.makeDcm(sbg::SbgVector3f(ref_sbg_ekf_euler_msg.angle.x, ref_sbg_ekf_euler_msg.angle.y, ref_sbg_ekf_euler_msg.angle.z)); - tdcm.transpose(); - - const sbg::SbgVector3f res = tdcm * sbg::SbgVector3f(ref_sbg_ekf_nav_msg.velocity.x, ref_sbg_ekf_nav_msg.velocity.y, ref_sbg_ekf_nav_msg.velocity.z); - - return createRosTwistStampedMessage(res, ref_sbg_imu_msg); -} - -const geometry_msgs::TwistStamped MessageWrapper::createRosTwistStampedMessage(const sbg_driver::SbgEkfQuat& ref_sbg_ekf_quat_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const -{ - - sbg::SbgMatrix3f tdcm; - tdcm.makeDcm(ref_sbg_ekf_quat_msg.quaternion.w, ref_sbg_ekf_quat_msg.quaternion.x, ref_sbg_ekf_quat_msg.quaternion.y, ref_sbg_ekf_quat_msg.quaternion.z); - tdcm.transpose(); - - const sbg::SbgVector3f res = tdcm * sbg::SbgVector3f(ref_sbg_ekf_nav_msg.velocity.x, ref_sbg_ekf_nav_msg.velocity.y, ref_sbg_ekf_nav_msg.velocity.z); - return createRosTwistStampedMessage(res, ref_sbg_imu_msg); -} - -const geometry_msgs::TwistStamped MessageWrapper::createRosTwistStampedMessage(const sbg::SbgVector3f& body_vel, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const -{ - geometry_msgs::TwistStamped twist_stamped_message; - - twist_stamped_message.header = createRosHeader(ref_sbg_imu_msg.time_stamp); - twist_stamped_message.twist.angular = ref_sbg_imu_msg.gyro; - - twist_stamped_message.twist.linear.x = body_vel(0); - twist_stamped_message.twist.linear.y = body_vel(1); - twist_stamped_message.twist.linear.z = body_vel(2); - - return twist_stamped_message; -} - -const geometry_msgs::PointStamped MessageWrapper::createRosPointStampedMessage(const sbg_driver::SbgEkfNav& ref_sbg_ekf_msg) const -{ - geometry_msgs::PointStamped point_stamped_message; - - point_stamped_message.header = createRosHeader(ref_sbg_ekf_msg.time_stamp); - - // - // Conversion from Geodetic coordinates to ECEF is based on World Geodetic System 1984 (WGS84). - // Radius are expressed in meters, and latitute/longitude in radian. - // - double equatorial_radius; - double polar_radius; - double prime_vertical_radius; - double eccentricity; - double latitude; - double longitude; - - equatorial_radius = 6378137.0; - polar_radius = 6356752.314245; - eccentricity = 1 - pow(polar_radius, 2) / pow(equatorial_radius, 2); - latitude = sbgDegToRadD(ref_sbg_ekf_msg.position.x); - longitude = sbgDegToRadD(ref_sbg_ekf_msg.position.x); - - prime_vertical_radius = equatorial_radius / sqrt(1.0 - pow(eccentricity, 2) * pow(sin(latitude), 2)); - - point_stamped_message.point.x = (prime_vertical_radius + ref_sbg_ekf_msg.position.z) * cos(latitude) * cos(longitude); - point_stamped_message.point.y = (prime_vertical_radius + ref_sbg_ekf_msg.position.z) * cos(latitude) * sin(longitude); - point_stamped_message.point.z = ((pow(polar_radius, 2) / pow(equatorial_radius, 2)) * prime_vertical_radius + ref_sbg_ekf_msg.position.z) * sin(latitude); - - return point_stamped_message; -} - -const sensor_msgs::TimeReference MessageWrapper::createRosUtcTimeReferenceMessage(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const -{ - sensor_msgs::TimeReference utc_reference_message; - - // - // This message is defined to have comparison between the System time and the Utc reference. - // Header of the ROS message will always be the System time, and the source is the computed time from Utc data. - // - utc_reference_message.header.stamp = m_ros_processing_time_; - utc_reference_message.time_ref = computeCorrectedRosTime(ref_sbg_utc_msg.time_stamp); - utc_reference_message.source = "UTC time from device converted to Epoch"; - - return utc_reference_message; -} - -const sensor_msgs::NavSatFix MessageWrapper::createRosNavSatFixMessage(const sbg_driver::SbgGpsPos& ref_sbg_gps_msg) const -{ - sensor_msgs::NavSatFix nav_sat_fix_message; - - nav_sat_fix_message.header = createRosHeader(ref_sbg_gps_msg.time_stamp); - - if (ref_sbg_gps_msg.status.type == SBG_ECOM_POS_NO_SOLUTION) - { - nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_NO_FIX; - } - else if (ref_sbg_gps_msg.status.type == SBG_ECOM_POS_SBAS) - { - nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_SBAS_FIX; - } - else - { - nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_FIX; - } - - if (ref_sbg_gps_msg.status.glo_l1_used || ref_sbg_gps_msg.status.glo_l2_used) - { - nav_sat_fix_message.status.service = nav_sat_fix_message.status.SERVICE_GLONASS; - } - else - { - nav_sat_fix_message.status.service = nav_sat_fix_message.status.SERVICE_GPS; - } - - nav_sat_fix_message.latitude = ref_sbg_gps_msg.position.x; - nav_sat_fix_message.longitude = ref_sbg_gps_msg.position.y; - nav_sat_fix_message.altitude = ref_sbg_gps_msg.position.z; - - nav_sat_fix_message.position_covariance[0] = pow(ref_sbg_gps_msg.position_accuracy.x, 2); - nav_sat_fix_message.position_covariance[4] = pow(ref_sbg_gps_msg.position_accuracy.y, 2); - nav_sat_fix_message.position_covariance[8] = pow(ref_sbg_gps_msg.position_accuracy.z, 2); - - nav_sat_fix_message.position_covariance_type = nav_sat_fix_message.COVARIANCE_TYPE_DIAGONAL_KNOWN; - - return nav_sat_fix_message; -} - -const sensor_msgs::FluidPressure MessageWrapper::createRosFluidPressureMessage(const sbg_driver::SbgAirData& ref_sbg_air_msg) const -{ - sensor_msgs::FluidPressure fluid_pressure_message; - - fluid_pressure_message.header = createRosHeader(ref_sbg_air_msg.time_stamp); - fluid_pressure_message.fluid_pressure = ref_sbg_air_msg.pressure_abs; - fluid_pressure_message.variance = 0; - - return fluid_pressure_message; -} diff --git a/sbg_ros_driver/src/sbg_device.cpp b/sbg_ros_driver/src/sbg_device.cpp deleted file mode 100644 index 416b3d0..0000000 --- a/sbg_ros_driver/src/sbg_device.cpp +++ /dev/null @@ -1,496 +0,0 @@ -// File header -#include "sbg_device.h" - -// Standard headers -#include -#include -#include - -// Boost headers -#include -#include -#include -#include - -// SbgECom headers -#include - -using namespace std; -using sbg::SbgDevice; - -// From ros_com/recorder -std::string timeToStr(ros::WallTime ros_t) -{ - (void)ros_t; - std::stringstream msg; - const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); - boost::posix_time::time_facet *const f = new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S"); - msg.imbue(std::locale(msg.getloc(),f)); - msg << now; - return msg.str(); -} - -// -// Static magnetometers maps definition. -// -std::map SbgDevice::g_mag_calib_quality_ = { {SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL, "Quality: optimal"}, - {SBG_ECOM_MAG_CALIB_QUAL_GOOD, "Quality: good"}, - {SBG_ECOM_MAG_CALIB_QUAL_POOR, "Quality: poor"}, - {SBG_ECOM_MAG_CALIB_QUAL_INVALID, "Quality: invalid"}}; - -std::map SbgDevice::g_mag_calib_confidence_ = { {SBG_ECOM_MAG_CALIB_TRUST_HIGH, "Confidence: high"}, - {SBG_ECOM_MAG_CALIB_TRUST_MEDIUM, "Confidence: medium"}, - {SBG_ECOM_MAG_CALIB_TRUST_LOW, "Confidence: low"}}; - -std::map SbgDevice::g_mag_calib_mode_ = { {SBG_ECOM_MAG_CALIB_MODE_2D, "Mode 2D"}, - {SBG_ECOM_MAG_CALIB_MODE_3D, "Mode 3D"}}; - -std::map SbgDevice::g_mag_calib_bandwidth = {{SBG_ECOM_MAG_CALIB_HIGH_BW, "High Bandwidth"}, - {SBG_ECOM_MAG_CALIB_MEDIUM_BW, "Medium Bandwidth"}, - {SBG_ECOM_MAG_CALIB_LOW_BW, "Low Bandwidth"}}; - -/*! - * Class to handle a connected SBG device. - */ -//---------------------------------------------------------------------// -//- Constructor -// -//---------------------------------------------------------------------// - -SbgDevice::SbgDevice(ros::NodeHandle& ref_node_handle): -m_ref_node_(ref_node_handle), -m_mag_calibration_ongoing_(false), -m_mag_calibration_done_(false) -{ - loadParameters(); - connect(); -} - -SbgDevice::~SbgDevice(void) -{ - SbgErrorCode error_code; - - error_code = sbgEComClose(&m_com_handle_); - - if (error_code != SBG_NO_ERROR) - { - ROS_ERROR("Unable to close the SBG communication handle - %s.", sbgErrorCodeToString(error_code)); - } - - if (m_config_store_.isInterfaceSerial()) - { - error_code = sbgInterfaceSerialDestroy(&m_sbg_interface_); - } - else if (m_config_store_.isInterfaceUdp()) - { - error_code = sbgInterfaceUdpDestroy(&m_sbg_interface_); - } - - if (error_code != SBG_NO_ERROR) - { - ROS_ERROR("SBG DRIVER - Unable to close the communication interface."); - } -} - -//---------------------------------------------------------------------// -//- Private methods -// -//---------------------------------------------------------------------// - -SbgErrorCode SbgDevice::onLogReceivedCallback(SbgEComHandle* p_handle, SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData* p_log_data, void* p_user_arg) -{ - assert(p_user_arg); - - SBG_UNUSED_PARAMETER(p_handle); - - SbgDevice *p_sbg_device; - p_sbg_device = (SbgDevice*)(p_user_arg); - - p_sbg_device->onLogReceived(msg_class, msg, *p_log_data); - - return SBG_NO_ERROR; -} - -void SbgDevice::onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData& ref_sbg_data) -{ - // - // Publish the received SBG log. - // - m_message_publisher_.publish(m_ros_processing_time_, msg_class, msg, ref_sbg_data); -} - -void SbgDevice::loadParameters(void) -{ - // - // Get the ROS private nodeHandle, where the parameters are loaded from the launch file. - // - ros::NodeHandle n_private("~"); - m_config_store_.loadFromRosNodeHandle(n_private); -} - -void SbgDevice::connect(void) -{ - SbgErrorCode error_code; - error_code = SBG_NO_ERROR; - - // - // Initialize the communication interface from the config store, then initialize the sbgECom protocol to communicate with the device. - // - if (m_config_store_.isInterfaceSerial()) - { - error_code = sbgInterfaceSerialCreate(&m_sbg_interface_, m_config_store_.getUartPortName().c_str(), m_config_store_.getBaudRate()); - } - else if (m_config_store_.isInterfaceUdp()) - { - error_code = sbgInterfaceUdpCreate(&m_sbg_interface_, m_config_store_.getIpAddress(), m_config_store_.getInputPortAddress(), m_config_store_.getOutputPortAddress()); - } - else - { - throw ros::Exception("Invalid interface type for the SBG device."); - } - - if (error_code != SBG_NO_ERROR) - { - throw ros::Exception("SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code))); - } - - error_code = sbgEComInit(&m_com_handle_, &m_sbg_interface_); - - if (error_code != SBG_NO_ERROR) - { - throw ros::Exception("SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code))); - } - - readDeviceInfo(); -} - -void SbgDevice::readDeviceInfo(void) -{ - SbgEComDeviceInfo device_info; - SbgErrorCode error_code; - - error_code = sbgEComCmdGetInfo(&m_com_handle_, &device_info); - - if (error_code != SBG_NO_ERROR) - { - ROS_ERROR("Unable to get the device Info : %s", sbgErrorCodeToString(error_code)); - } - - ROS_INFO("SBG_DRIVER - productCode = %s", device_info.productCode); - ROS_INFO("SBG_DRIVER - serialNumber = %u", device_info.serialNumber); - - ROS_INFO("SBG_DRIVER - calibationRev = %s", getVersionAsString(device_info.calibationRev).c_str()); - ROS_INFO("SBG_DRIVER - calibrationDate = %u / %u / %u", device_info.calibrationDay, device_info.calibrationMonth, device_info.calibrationYear); - - ROS_INFO("SBG_DRIVER - hardwareRev = %s", getVersionAsString(device_info.hardwareRev).c_str()); - ROS_INFO("SBG_DRIVER - firmwareRev = %s", getVersionAsString(device_info.firmwareRev).c_str()); -} - -std::string SbgDevice::getVersionAsString(uint32 sbg_version_enc) const -{ - char version[32]; - sbgVersionToStringEncoded(sbg_version_enc, version, 32); - - return std::string(version); -} - -void SbgDevice::initPublishers(void) -{ - m_message_publisher_.initPublishers(m_ref_node_, m_config_store_); - - // - // Check if the rate frequency has to be defined according to the defined publishers. - // - if(m_config_store_.getReadingRateFrequency() == 0) - { - m_rate_frequency_ = m_message_publisher_.getMaxOutputFrequency(); - } - else - { - m_rate_frequency_ = m_config_store_.getReadingRateFrequency(); - } -} - -void SbgDevice::configure(void) -{ - if (m_config_store_.checkConfigWithRos()) - { - ConfigApplier configApplier(m_com_handle_); - configApplier.applyConfiguration(m_config_store_); - } -} - -bool SbgDevice::processMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response) -{ - SBG_UNUSED_PARAMETER(ref_ros_request); - - if (m_mag_calibration_ongoing_) - { - if (endMagCalibration()) - { - ref_ros_response.success = true; - ref_ros_response.message = "Magnetometer calibration is finished. See the output console to get calibration informations."; - } - else - { - ref_ros_response.success = false; - ref_ros_response.message = "Unable to end the calibration."; - } - - m_mag_calibration_ongoing_ = false; - m_mag_calibration_done_ = true; - } - else - { - if (startMagCalibration()) - { - ref_ros_response.success = true; - ref_ros_response.message = "Magnetometer calibration process started."; - } - else - { - ref_ros_response.success = false; - ref_ros_response.message = "Unable to start magnetometers calibration."; - } - - m_mag_calibration_ongoing_ = true; - } - - return ref_ros_response.success; -} - -bool SbgDevice::saveMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response) -{ - SBG_UNUSED_PARAMETER(ref_ros_request); - - if (m_mag_calibration_ongoing_) - { - ref_ros_response.success = false; - ref_ros_response.message = "Magnetometer calibration process is still ongoing, finish it before trying to save it."; - } - else if (m_mag_calibration_done_) - { - if (uploadMagCalibrationToDevice()) - { - ref_ros_response.success = true; - ref_ros_response.message = "Magnetometer calibration has been uploaded to the device."; - } - else - { - ref_ros_response.success = false; - ref_ros_response.message = "Magnetometer calibration has not been uploaded to the device."; - } - } - else - { - ref_ros_response.success = false; - ref_ros_response.message = "No magnetometer calibration has been done."; - } - - return ref_ros_response.success; -} - -bool SbgDevice::startMagCalibration(void) -{ - SbgErrorCode error_code; - SbgEComMagCalibMode mag_calib_mode; - SbgEComMagCalibBandwidth mag_calib_bandwidth; - - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); - mag_calib_bandwidth = m_config_store_.getMagnetometerCalibBandwidth(); - - error_code = sbgEComCmdMagStartCalib(&m_com_handle_, mag_calib_mode, mag_calib_bandwidth); - - if (error_code != SBG_NO_ERROR) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Unable to start the magnetometer calibration : %s", sbgErrorCodeToString(error_code)); - return false; - } - else - { - ROS_INFO("SBG DRIVER [Mag Calib] - Start calibration"); - ROS_INFO("SBG DRIVER [Mag Calib] - Mode : %s", g_mag_calib_mode_[mag_calib_mode].c_str()); - ROS_INFO("SBG DRIVER [Mag Calib] - Bandwidth : %s", g_mag_calib_bandwidth[mag_calib_bandwidth].c_str()); - return true; - } -} - -bool SbgDevice::endMagCalibration(void) -{ - SbgErrorCode error_code; - - error_code = sbgEComCmdMagComputeCalib(&m_com_handle_, &m_magCalibResults); - - if (error_code != SBG_NO_ERROR) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Unable to compute the magnetometer calibration results : %s", sbgErrorCodeToString(error_code)); - return false; - } - else - { - displayMagCalibrationStatusResult(); - exportMagCalibrationResults(); - - return true; - } -} - -bool SbgDevice::uploadMagCalibrationToDevice(void) -{ - SbgErrorCode error_code; - - if (m_magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID) - { - error_code = sbgEComCmdMagSetCalibData(&m_com_handle_, m_magCalibResults.offset, m_magCalibResults.matrix); - - if (error_code != SBG_NO_ERROR) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Unable to set the magnetometers calibration data to the device : %s", sbgErrorCodeToString(error_code)); - return false; - } - else - { - ROS_INFO("SBG DRIVER [Mag Calib] - Saving data to the device"); - ConfigApplier configApplier(m_com_handle_); - configApplier.saveConfiguration(); - return true; - } - } - else - { - ROS_ERROR("SBG DRIVER [Mag Calib] - The calibration was invalid, it can't be uploaded on the device."); - return false; - } -} - -void SbgDevice::displayMagCalibrationStatusResult(void) const -{ - ROS_INFO("SBG DRIVER [Mag Calib] - Quality of the calibration %s", g_mag_calib_quality_[m_magCalibResults.quality].c_str()); - ROS_INFO("SBG DRIVER [Mag Calib] - Calibration results confidence %s", g_mag_calib_confidence_[m_magCalibResults.confidence].c_str()); - - SbgEComMagCalibMode mag_calib_mode; - - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); - - // - // Check the magnetometers calibration status and display the warnings. - // - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Not enough valid points. Maybe you are moving too fast"); - } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Unable to find a calibration solution. Maybe there are too much non static distortions"); - } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment"); - } - if (mag_calib_mode == SBG_ECOM_MAG_CALIB_MODE_2D) - { - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Too much roll motion for a 2D magnetic calibration"); - } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Too much pitch motion for a 2D magnetic calibration"); - } - } - else - { - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Not enough roll motion for a 3D magnetic calibration"); - } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Not enough pitch motion for a 3D magnetic calibration."); - } - } - if (m_magCalibResults.advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE) - { - ROS_WARN("SBG DRIVER [Mag Calib] - Not enough yaw motion to compute a valid magnetic calibration"); - } -} - -void SbgDevice::exportMagCalibrationResults(void) const -{ - SbgEComMagCalibMode mag_calib_mode; - SbgEComMagCalibBandwidth mag_calib_bandwidth; - ostringstream mag_results_stream; - string output_filename; - - mag_calib_mode = m_config_store_.getMagnetometerCalibMode(); - mag_calib_bandwidth = m_config_store_.getMagnetometerCalibBandwidth(); - - mag_results_stream << "SBG DRIVER [Mag Calib]" << endl; - mag_results_stream << "======= Parameters =======" << endl; - mag_results_stream << "* CALIB_MODE = " << g_mag_calib_mode_[mag_calib_mode] << endl; - mag_results_stream << "* CALIB_BW = " << g_mag_calib_bandwidth[mag_calib_bandwidth] << endl; - - mag_results_stream << "======= Results =======" << endl; - mag_results_stream << g_mag_calib_quality_[m_magCalibResults.quality] << endl; - mag_results_stream << g_mag_calib_confidence_[m_magCalibResults.confidence] << endl; - mag_results_stream << "======= Infos =======" << endl; - mag_results_stream << "* Used points : " << m_magCalibResults.numPoints << "/" << m_magCalibResults.maxNumPoints << endl; - mag_results_stream << "* Mean, Std, Max" << endl; - mag_results_stream << "[Before]\t" << m_magCalibResults.beforeMeanError << "\t" << m_magCalibResults.beforeStdError << "\t" << m_magCalibResults.beforeMaxError << endl; - mag_results_stream << "[After]\t" << m_magCalibResults.afterMeanError << "\t" << m_magCalibResults.afterStdError << "\t" << m_magCalibResults.afterMaxError << endl; - mag_results_stream << "[Accuracy]\t" << sbgRadToDegF(m_magCalibResults.meanAccuracy) << "\t" << sbgRadToDegF(m_magCalibResults.stdAccuracy) << "\t" << sbgRadToDegF(m_magCalibResults.maxAccuracy) << endl; - mag_results_stream << "* Offset\t" << m_magCalibResults.offset[0] << "\t" << m_magCalibResults.offset[1] << "\t" << m_magCalibResults.offset[2] << endl; - - mag_results_stream << "* Matrix" << endl; - mag_results_stream << m_magCalibResults.matrix[0] << "\t" << m_magCalibResults.matrix[1] << "\t" << m_magCalibResults.matrix[2] << endl; - mag_results_stream << m_magCalibResults.matrix[3] << "\t" << m_magCalibResults.matrix[4] << "\t" << m_magCalibResults.matrix[5] << endl; - mag_results_stream << m_magCalibResults.matrix[6] << "\t" << m_magCalibResults.matrix[7] << "\t" << m_magCalibResults.matrix[8] << endl; - - output_filename = "mag_calib_" + timeToStr(ros::WallTime::now()) + ".txt"; - ofstream output_file(output_filename); - output_file << mag_results_stream.str(); - output_file.close(); - - ROS_INFO("%s", mag_results_stream.str().c_str()); - ROS_INFO("SBG DRIVER [Mag Calib] - Magnetometers calibration results saved to file %s", output_filename.c_str()); -} - -//---------------------------------------------------------------------// -//- Parameters -// -//---------------------------------------------------------------------// - -uint32_t SbgDevice::getUpdateFrequency(void) const -{ - return m_rate_frequency_; -} - -//---------------------------------------------------------------------// -//- Public methods -// -//---------------------------------------------------------------------// - -void SbgDevice::initDeviceForReceivingData(void) -{ - SbgErrorCode error_code; - initPublishers(); - configure(); - - error_code = sbgEComSetReceiveLogCallback(&m_com_handle_, onLogReceivedCallback, this); - - if (error_code != SBG_NO_ERROR) - { - throw ros::Exception("SBG_DRIVER - [Init] Unable to set the callback function - " + std::string(sbgErrorCodeToString(error_code))); - } -} - -void SbgDevice::initDeviceForMagCalibration(void) -{ - m_calib_service_ = m_ref_node_.advertiseService("sbg/mag_calibration", &SbgDevice::processMagCalibration, this); - m_calib_save_service_ = m_ref_node_.advertiseService("sbg/mag_calibration_save", &SbgDevice::saveMagCalibration, this); - - ROS_INFO("SBG DRIVER [Init] - SBG device is initialized for magnetometers calibration."); -} - -void SbgDevice::periodicHandle(void) -{ - m_ros_processing_time_ = ros::Time::now(); - - sbgEComHandle(&m_com_handle_); -} \ No newline at end of file